diff --git a/meta-arm/.gitignore b/meta-arm/.gitignore
new file mode 100644
index 0000000..bee8a64
--- /dev/null
+++ b/meta-arm/.gitignore
@@ -0,0 +1 @@
+__pycache__
diff --git a/meta-arm/.gitlab-ci.yml b/meta-arm/.gitlab-ci.yml
new file mode 100644
index 0000000..840a650
--- /dev/null
+++ b/meta-arm/.gitlab-ci.yml
@@ -0,0 +1,266 @@
+image: ghcr.io/siemens/kas/kas:latest-release
+
+stages:
+  - prep
+  - build
+
+# Common job fragment to get a worker ready
+.setup:
+  stage: build
+  interruptible: true
+  variables:
+    KAS_WORK_DIR: $CI_PROJECT_DIR/work
+    KAS_REPO_REF_DIR: $CI_BUILDS_DIR/persist/repos
+    SSTATE_DIR: $CI_BUILDS_DIR/persist/sstate
+    DL_DIR: $CI_BUILDS_DIR/persist/downloads
+    BB_LOGCONFIG: $CI_PROJECT_DIR/ci/logging.yml
+    TOOLCHAIN_DIR: $CI_BUILDS_DIR/persist/toolchains
+    IMAGE_DIR: $CI_PROJECT_DIR/work/build/tmp/deploy/images
+    TOOLCHAIN_LINK_DIR: $CI_PROJECT_DIR/work/build/toolchains
+  before_script:
+    - echo KAS_WORK_DIR = $KAS_WORK_DIR
+    - echo SSTATE_DIR = $SSTATE_DIR
+    - echo DL_DIR = $DL_DIR
+    - rm -rf $KAS_WORK_DIR
+    - mkdir --verbose --parents $KAS_WORK_DIR $KAS_REPO_REF_DIR $SSTATE_DIR $DL_DIR $TOOLCHAIN_DIR $TOOLCHAIN_LINK_DIR
+    # Must do this here, as it's the only way to make sure the toolchain is installed on the same builder
+    - ./ci/get-binary-toolchains $DL_DIR $TOOLCHAIN_DIR $TOOLCHAIN_LINK_DIR
+    - sudo apt-get update && sudo apt-get install --yes telnet python3-subunit
+
+# Generalised fragment to do a Kas build
+.build:
+  extends: .setup
+  script:
+    - KASFILES=$(./ci/jobs-to-kas "$CI_JOB_NAME")
+    - kas shell --update --force-checkout $KASFILES -c 'cat conf/*.conf'
+    - kas build $KASFILES
+    - ./ci/check-warnings $KAS_WORK_DIR/build/warnings.log
+  artifacts:
+    name: "logs"
+    when: on_failure
+    paths:
+      - $CI_PROJECT_DIR/work/build/tmp/work*/**/temp/log.do_*.*
+
+#
+# Prep stage, update repositories once
+#
+update-repos:
+  extends: .setup
+  stage: prep
+  script:
+    - flock --verbose --timeout 60 $KAS_REPO_REF_DIR ./ci/update-repos
+
+#
+# Build stage, the actual build jobs
+#
+# Available options for building are
+#  TOOLCHAINS: [gcc, clang, armgcc, external-gccarm]
+#  TCLIBC: [glibc, musl]
+#  FIRMWARE: [uboot, edk2]
+#  VIRT: [none, xen]
+#  TESTING: testimage
+
+corstone500:
+  extends: .build
+  parallel:
+    matrix:
+      - TESTING: testimage
+  tags:
+    - x86_64
+
+corstone1000-fvp:
+  extends: .build
+  parallel:
+    matrix:
+      - TESTING: [testimage,tftf]
+  tags:
+    - x86_64
+
+corstone1000-mps3:
+  extends: .build
+
+fvp-base:
+  extends: .build
+  parallel:
+    matrix:
+      - TESTING: testimage
+  tags:
+    - x86_64
+
+fvp-base-arm32:
+  extends: .build
+  parallel:
+    matrix:
+      - TOOLCHAINS: [gcc, external-gccarm]
+        TESTING: testimage
+  tags:
+    - x86_64
+
+fvp-baser-aemv8r64:
+  extends: .build
+  parallel:
+    matrix:
+      - TESTING: testimage
+  tags:
+    - x86_64
+
+fvps:
+  extends: .build
+
+gem5-arm64:
+  extends: .build
+  parallel:
+    matrix:
+      - VIRT: [none, xen]
+
+gem5-atp-arm64:
+  extends: .build
+
+generic-arm64:
+  extends: .build
+
+juno:
+  extends: .build
+  parallel:
+    matrix:
+      - TOOLCHAINS: [gcc, clang]
+        FIRMWARE: [uboot, edk2]
+
+microbit-v1:
+  extends: .build
+  parallel:
+    matrix:
+      - TESTING: testimage-zephyr
+
+musca-b1:
+  extends: .build
+
+musca-s1:
+  extends: .build
+
+n1sdp:
+  extends: .build
+  parallel:
+    matrix:
+      - TOOLCHAINS: [gcc, armgcc]
+
+qemu-cortex-a53:
+  extends: .build
+
+qemu-cortex-m3:
+  extends: .build
+  parallel:
+    matrix:
+      - TESTING: testimage-zephyr
+
+qemu-cortex-r5:
+  extends: .build
+
+qemu-generic-arm64:
+  extends: .build
+  parallel:
+    matrix:
+      - TOOLCHAINS: [gcc, clang]
+        TESTING: testimage
+
+qemuarm64-secureboot:
+  extends: .build
+  parallel:
+    matrix:
+      - TOOLCHAINS: [gcc, clang]
+        TCLIBC: [glibc, musl]
+        TESTING: testimage
+
+qemuarm64:
+  extends: .build
+  parallel:
+    matrix:
+      - TOOLCHAINS: [gcc, clang]
+        EFI: [uboot, edk2]
+        TESTING: testimage
+      - VIRT: xen
+
+qemuarm-secureboot:
+  extends: .build
+  parallel:
+    matrix:
+      - TOOLCHAINS: [gcc, clang]
+        TESTING: testimage
+
+qemuarm:
+  extends: .build
+  parallel:
+    matrix:
+      - TOOLCHAINS: [gcc, clang]
+        EFI: [uboot, edk2]
+        TESTING: testimage
+      - VIRT: xen
+
+qemuarmv5:
+  extends: .build
+  parallel:
+    matrix:
+      - TESTING: testimage
+
+sgi575:
+  extends: .build
+
+tc0:
+  extends: .build
+  tags:
+    - x86_64
+
+tc1:
+  extends: .build
+  tags:
+    - x86_64
+
+toolchains:
+  extends: .build
+
+selftest:
+  extends: .setup
+  script:
+    - KASFILES=./ci/qemuarm64.yml:./ci/selftest.yml
+    - kas shell --update --force-checkout $KASFILES -c 'oe-selftest --num-processes 1 --run-tests runfvp'
+
+# Validate layers are Yocto Project Compatible
+check-layers:
+  extends: .setup
+  script:
+    - kas shell --update --force-checkout ci/base.yml:ci/meta-openembedded.yml --command \
+      "yocto-check-layer-wrapper $CI_PROJECT_DIR/$LAYER --dependency $CI_PROJECT_DIR/meta-* $KAS_WORK_DIR/meta-openembedded/meta-oe --no-auto-dependency"
+  parallel:
+    matrix:
+      - LAYER: [meta-arm, meta-arm-bsp, meta-arm-toolchain, meta-gem5]
+
+pending-updates:
+  extends: .setup
+  artifacts:
+    paths:
+      - update-report
+  script:
+    - rm -fr update-report
+    # This configuration has all of the layers we need enabled
+    - kas shell ci/gem5-arm64.yml --command \
+      "$CI_PROJECT_DIR/scripts/machine-summary.py -t report -o $CI_PROJECT_DIR/update-report $($CI_PROJECT_DIR/ci/listmachines.py meta-arm meta-arm-bsp meta-gem5)"
+  # Do this on x86 whilst the compilers are x86-only
+  tags:
+    - x86_64
+
+# What percentage of machines in the layer do we build
+machine-coverage:
+  stage: build
+  interruptible: true
+  script:
+    - ./ci/check-machine-coverage
+  coverage: '/Coverage: \d+/'
+
+metrics:
+  extends: .setup
+  artifacts:
+    reports:
+      metrics: metrics.txt
+  script:
+    - kas shell --update --force-checkout ci/base.yml --command \
+      "$CI_PROJECT_DIR/ci/patchreview $CI_PROJECT_DIR/meta-* --verbose --metrics $CI_PROJECT_DIR/metrics.txt"
diff --git a/meta-arm/COPYING.MIT b/meta-arm/COPYING.MIT
new file mode 100644
index 0000000..89de354
--- /dev/null
+++ b/meta-arm/COPYING.MIT
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
diff --git a/meta-arm/README.md b/meta-arm/README.md
new file mode 100644
index 0000000..221633e
--- /dev/null
+++ b/meta-arm/README.md
@@ -0,0 +1,73 @@
+Introduction
+------------
+This repository contains the Arm layers for OpenEmbedded.
+
+* meta-arm
+
+  This layer contains general recipes for the Arm architecture, such as firmware, FVPs, and Arm-specific integration.
+
+* meta-arm-autonomy
+
+  This layer is the distribution for a reference stack for autonomous systems.
+
+* meta-arm-bsp
+
+  This layer contains machines for Arm reference platforms, for example FVP Base, N1SDP, and Juno.
+
+* meta-arm-toolchain
+
+  This layer contains recipes for Arm's binary toolchains (GCC and Clang for -A and -M), and a recipe to build Arm's GCC.
+
+* meta-atp
+
+  This layer contains recipes for the Adaptive Traffic Generation integration into meta-gem5.
+
+* meta-gem5
+
+  This layer contains recipes and machines for gem5, a system-level and processor simulator.
+
+
+Other Directories
+-----------------
+
+* ci
+
+  This directory contains gitlab continuous integration configuration files (KAS yaml files) as well as scripts needed for this
+
+* kas
+
+  This directory contains KAS yaml files to describe builds for systems not used in CI
+
+* scripts
+
+  This directory contains scripts used in running the CI tests
+
+Contributing
+------------
+Currently, we only accept patches from the meta-arm mailing list.  For general
+information on how to submit a patch, please read
+https://www.openembedded.org/wiki/How_to_submit_a_patch_to_OpenEmbedded
+
+E-mail meta-arm@lists.yoctoproject.org with patches created using this process. You can configure git-send-email to automatically use this address for the meta-arm repository with the following git command:
+
+$ git config --local --add sendemail.to meta-arm@lists.yoctoproject.org
+
+Commits and patches added should follow the OpenEmbedded patch guidelines:
+
+https://www.openembedded.org/wiki/Commit_Patch_Message_Guidelines
+
+The component being changed in the shortlog should be prefixed with the layer name (without the meta- prefix), for example:
+
+  arm-bsp/trusted-firmware-a: decrease frobbing level
+
+  arm-toolchain/gcc: enable foobar v2
+
+Reporting bugs
+--------------
+E-mail meta-arm@lists.yoctoproject.org with the error encountered and the steps
+to reproduce the issue.
+
+Maintainer(s)
+-------------
+* Jon Mason <jon.mason@arm.com>
+* Ross Burton <ross.burton@arm.com>
diff --git a/meta-arm/ci/armgcc.yml b/meta-arm/ci/armgcc.yml
new file mode 100644
index 0000000..d46f0b3
--- /dev/null
+++ b/meta-arm/ci/armgcc.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+
+local_conf_header:
+  cc: |
+    GCCVERSION = "arm-11.2"
diff --git a/meta-arm/ci/base.yml b/meta-arm/ci/base.yml
new file mode 100644
index 0000000..eff466e
--- /dev/null
+++ b/meta-arm/ci/base.yml
@@ -0,0 +1,53 @@
+header:
+  version: 11
+
+distro: poky
+
+defaults:
+  repos:
+    refspec: master
+
+repos:
+  meta-arm:
+    layers:
+      meta-arm:
+      meta-arm-bsp:
+      meta-arm-toolchain:
+
+  poky:
+    url: https://git.yoctoproject.org/git/poky
+    layers:
+      meta:
+      meta-poky:
+
+env:
+  BB_LOGCONFIG: ""
+  TOOLCHAIN_DIR: ""
+
+local_conf_header:
+  base: |
+    BB_SERVER_TIMEOUT = "60"
+    CONF_VERSION = "2"
+    BB_NUMBER_THREADS = "16"
+    PARALLEL_MAKE = "-j16"
+    LICENSE_FLAGS_ACCEPTED += "Arm-FVP-EULA"
+  setup: |
+    PACKAGE_CLASSES = "package_ipk"
+    PACKAGECONFIG:remove:pn-qemu-system-native = "gtk+ sdl"
+    EXTRA_IMAGE_FEATURES:append = " debug-tweaks"
+    PACKAGECONFIG:append:pn-perf = " coresight"
+    INHERIT += "rm_work"
+    DISTRO_FEATURES:remove = "ptest"
+  kvm: |
+    QEMU_USE_KVM = ""
+  perf: |
+    CORE_IMAGE_EXTRA_INSTALL += "perf"
+  sshkeys: |
+    CORE_IMAGE_EXTRA_INSTALL += "ssh-pregen-hostkeys"
+  universally_failing_tests: |
+    TEST_SUITES:remove = "opkg"
+
+machine: unset
+
+target:
+  - core-image-sato
diff --git a/meta-arm/ci/check-machine-coverage b/meta-arm/ci/check-machine-coverage
new file mode 100755
index 0000000..f329fce
--- /dev/null
+++ b/meta-arm/ci/check-machine-coverage
@@ -0,0 +1,28 @@
+#! /usr/bin/env python3
+
+from pathlib import Path
+import sys
+from listmachines import list_machines
+
+metaarm = Path.cwd()
+
+if metaarm.name != "meta-arm":
+    print("Not running inside meta-arm")
+    sys.exit(1)
+
+# Find all layers
+layers = (p.name for p in metaarm.glob("meta-*") if p.is_dir())
+# All machine configurations
+machines = list_machines(layers)
+
+# All kas files
+kas = metaarm.glob("ci/*.yml")
+kas = set(p.stem for p in kas)
+
+missing = machines - kas
+print(f"The following machines are missing: {', '.join(sorted(missing))}.")
+
+covered = len(machines) - len(missing)
+total = len(machines)
+percent = int(covered / total * 100)
+print(f"Coverage: {percent}%")
diff --git a/meta-arm/ci/check-warnings b/meta-arm/ci/check-warnings
new file mode 100755
index 0000000..89ae955
--- /dev/null
+++ b/meta-arm/ci/check-warnings
@@ -0,0 +1,19 @@
+#! /bin/bash
+
+# Expects the path to a log file as $1, and if this file has any content
+# then display the contents and exit with an error code.
+
+set -e -u
+
+LOGFILE=$1
+
+LINES=$(grep --invert-match "attempting MIRRORS if available" $LOGFILE | wc -l)
+if test "$LINES" -ne 0; then
+    echo ==============================
+    echo The build had warnings/errors:
+    echo ==============================
+    cat $LOGFILE
+    exit 1
+fi
+
+exit 0
diff --git a/meta-arm/ci/clang.yml b/meta-arm/ci/clang.yml
new file mode 100644
index 0000000..80b9a4e
--- /dev/null
+++ b/meta-arm/ci/clang.yml
@@ -0,0 +1,16 @@
+header:
+  version: 11
+
+repos:
+  meta-clang:
+    url: https://github.com/kraj/meta-clang
+
+local_conf_header:
+  clang: |
+    TOOLCHAIN = "clang"
+    # Clang causes more binaries to have buildpaths in the debug symbols
+    # https://github.com/llvm/llvm-project/issues/56609
+    WARN_QA:remove = "buildpaths"
+
+target:
+  - core-image-base
diff --git a/meta-arm/ci/corstone1000-common.yml b/meta-arm/ci/corstone1000-common.yml
new file mode 100644
index 0000000..6599d97
--- /dev/null
+++ b/meta-arm/ci/corstone1000-common.yml
@@ -0,0 +1,13 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+    - ci/meta-openembedded.yml
+local_conf_header:
+    perf: |
+
+distro: poky-tiny
+
+target:
+  - perf
+  - corstone1000-image
diff --git a/meta-arm/ci/corstone1000-fvp.yml b/meta-arm/ci/corstone1000-fvp.yml
new file mode 100644
index 0000000..1586504
--- /dev/null
+++ b/meta-arm/ci/corstone1000-fvp.yml
@@ -0,0 +1,12 @@
+header:
+  version: 11
+  includes:
+    - ci/corstone1000-common.yml
+
+local_conf_header:
+    fvp-config: |
+        # Remove Dropbear SSH as it will not fit into the corstone1000 image.
+        IMAGE_FEATURES:remove = " ssh-server-dropbear"
+        INHERIT += "fvpboot"
+
+machine: corstone1000-fvp
diff --git a/meta-arm/ci/corstone1000-mps3.yml b/meta-arm/ci/corstone1000-mps3.yml
new file mode 100644
index 0000000..2df7d97
--- /dev/null
+++ b/meta-arm/ci/corstone1000-mps3.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+  includes:
+    - ci/corstone1000-common.yml
+
+machine: corstone1000-mps3
diff --git a/meta-arm/ci/corstone500.yml b/meta-arm/ci/corstone500.yml
new file mode 100644
index 0000000..ef78cac
--- /dev/null
+++ b/meta-arm/ci/corstone500.yml
@@ -0,0 +1,18 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+local_conf_header:
+  testimagefvp: |
+    INHERIT += "fvpboot"
+    IMAGE_FEATURES:remove = " ssh-server-dropbear"
+  perf: |
+
+machine: corstone500
+
+distro: poky-tiny
+
+target:
+  - core-image-minimal
+  - perf
diff --git a/meta-arm/ci/edk2.yml b/meta-arm/ci/edk2.yml
new file mode 100644
index 0000000..1261bf1
--- /dev/null
+++ b/meta-arm/ci/edk2.yml
@@ -0,0 +1,17 @@
+header:
+  version: 11
+
+local_conf_header:
+  bootfirmware: |
+    PREFERRED_PROVIDER_virtual/bootloader = "edk2-firmware"
+    MACHINE_FEATURES += "efi"
+    TFA_UBOOT = "0"
+    TFA_UEFI = "1"
+
+    EXTRA_IMAGEDEPENDS += "edk2-firmware"
+    EFI_PROVIDER ?= "grub-efi"
+
+    QB_DEFAULT_BIOS = "QEMU_EFI.fd"
+    WKS_FILE ?= "efi-disk.wks.in"
+  failing_tests: |
+    TEST_SUITES:remove = "xorg"
diff --git a/meta-arm/ci/external-gccarm.yml b/meta-arm/ci/external-gccarm.yml
new file mode 100644
index 0000000..2af8b5e
--- /dev/null
+++ b/meta-arm/ci/external-gccarm.yml
@@ -0,0 +1,8 @@
+header:
+  version: 11
+
+local_conf_header:
+  cc: |
+    SKIP_RECIPE[gcc-cross-arm] = "Using external toolchain"
+    TCMODE = "external-arm"
+    EXTERNAL_TOOLCHAIN = "${TOPDIR}/toolchains/${TARGET_ARCH}"
diff --git a/meta-arm/ci/fvp-base-arm32.yml b/meta-arm/ci/fvp-base-arm32.yml
new file mode 100644
index 0000000..be4b008
--- /dev/null
+++ b/meta-arm/ci/fvp-base-arm32.yml
@@ -0,0 +1,18 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: fvp-base-arm32
+
+local_conf_header:
+  testimagefvp: |
+    INHERIT = "fvpboot"
+    # This fails but we can't add to the ignorelist from meta-arm yet
+    # https://bugzilla.yoctoproject.org/show_bug.cgi?id=14604
+    TEST_SUITES:remove = "parselogs"
+    # Tell testimage to connect to localhost:8122, and forward that to SSH in the FVP.
+    TEST_TARGET_IP = "127.0.0.1:8122"
+    FVP_CONFIG[bp.virtio_net.hostbridge.userNetPorts] = "8122=22"
+  failing_tests: |
+    TEST_SUITES:remove = "xorg"
diff --git a/meta-arm/ci/fvp-base.yml b/meta-arm/ci/fvp-base.yml
new file mode 100644
index 0000000..fa2ddb3
--- /dev/null
+++ b/meta-arm/ci/fvp-base.yml
@@ -0,0 +1,18 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: fvp-base
+
+local_conf_header:
+  testimagefvp: |
+    INHERIT += "fvpboot"
+    # This fails but we can't add to the ignorelist from meta-arm yet
+    # https://bugzilla.yoctoproject.org/show_bug.cgi?id=14604
+    TEST_SUITES:remove = "parselogs"
+    # Tell testimage to connect to localhost:8022, and forward that to SSH in the FVP.
+    TEST_TARGET_IP = "localhost:8022"
+    FVP_CONFIG[bp.virtio_net.hostbridge.userNetPorts] ?= "8022=22"
+  failing_tests: |
+    TEST_SUITES:remove = "xorg"
diff --git a/meta-arm/ci/fvp-baser-aemv8r64.yml b/meta-arm/ci/fvp-baser-aemv8r64.yml
new file mode 100644
index 0000000..40818bc
--- /dev/null
+++ b/meta-arm/ci/fvp-baser-aemv8r64.yml
@@ -0,0 +1,7 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: fvp-baser-aemv8r64
+
diff --git a/meta-arm/ci/fvps.yml b/meta-arm/ci/fvps.yml
new file mode 100644
index 0000000..576faa3
--- /dev/null
+++ b/meta-arm/ci/fvps.yml
@@ -0,0 +1,20 @@
+# Simple target to build the FVPs that are publically available
+
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: qemuarm64
+
+local_conf_header:
+  sdk: |
+    SDKMACHINE = "x86_64"
+
+target:
+  - nativesdk-fvp-base-a-aem
+  - nativesdk-fvp-n1-edge
+  - nativesdk-fvp-sgi575
+  - nativesdk-fvp-corstone500
+  - nativesdk-fvp-corstone1000
+  - nativesdk-fvp-tc0
diff --git a/meta-arm/ci/gem5-arm64.yml b/meta-arm/ci/gem5-arm64.yml
new file mode 100644
index 0000000..90913f3
--- /dev/null
+++ b/meta-arm/ci/gem5-arm64.yml
@@ -0,0 +1,16 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+    - ci/meta-openembedded.yml
+
+repos:
+  meta-arm:
+    layers:
+      meta-gem5:
+
+machine: gem5-arm64
+
+target:
+  - core-image-minimal
+  - gem5-aarch64-native
diff --git a/meta-arm/ci/gem5-atp-arm64.yml b/meta-arm/ci/gem5-atp-arm64.yml
new file mode 100644
index 0000000..626947b
--- /dev/null
+++ b/meta-arm/ci/gem5-atp-arm64.yml
@@ -0,0 +1,15 @@
+header:
+  version: 11
+  includes:
+    - ci/gem5-arm64.yml
+
+repos:
+  meta-arm:
+    layers:
+      meta-atp:
+
+machine: gem5-atp-arm64
+
+target:
+  - atp-native
+  - core-image-minimal
diff --git a/meta-arm/ci/generic-arm64.yml b/meta-arm/ci/generic-arm64.yml
new file mode 100644
index 0000000..873c9fd
--- /dev/null
+++ b/meta-arm/ci/generic-arm64.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: generic-arm64
diff --git a/meta-arm/ci/get-binary-toolchains b/meta-arm/ci/get-binary-toolchains
new file mode 100755
index 0000000..bfdd8c5
--- /dev/null
+++ b/meta-arm/ci/get-binary-toolchains
@@ -0,0 +1,46 @@
+#!/bin/bash
+set -u
+
+HOST_ARCH=$(uname -m)
+VER="11.2-2022.02"
+
+DOWNLOAD_DIR=$1
+TOOLCHAIN_DIR=$2
+TOOLCHAIN_LINK_DIR=$3
+
+# These should be already created by .gitlab-ci.yml, but do here if run outside of that env
+mkdir -p $DOWNLOAD_DIR $TOOLCHAIN_DIR $TOOLCHAIN_LINK_DIR
+
+if [ $HOST_ARCH = "aarch64" ]; then
+	#AArch64 Linux hosted cross compilers
+
+	#AArch32 target with hard float (arm-none-linux-gnueabihf)
+	wget -P $DOWNLOAD_DIR -nc https://developer.arm.com/-/media/Files/downloads/gnu/$VER/binrel/gcc-arm-$VER-$HOST_ARCH-arm-none-linux-gnueabihf.tar.xz
+elif [ $HOST_ARCH = "x86_64" ]; then
+	#x86_64 Linux hosted cross compilers
+
+	#AArch32 target with hard float (arm-linux-none-gnueabihf)
+	wget -P $DOWNLOAD_DIR -nc https://developer.arm.com/-/media/Files/downloads/gnu/$VER/binrel/gcc-arm-$VER-$HOST_ARCH-arm-none-linux-gnueabihf.tar.xz
+
+	#AArch64 GNU/Linux target (aarch64-none-linux-gnu)
+	wget -P $DOWNLOAD_DIR -nc https://developer.arm.com/-/media/Files/downloads/gnu/$VER/binrel/gcc-arm-$VER-$HOST_ARCH-aarch64-none-linux-gnu.tar.xz
+
+	#AArch64 GNU/Linux target (aarch64_be-none-linux-gnu)
+	wget -P $DOWNLOAD_DIR -nc https://developer.arm.com/-/media/Files/downloads/gnu/$VER/binrel/gcc-arm-$VER-$HOST_ARCH-aarch64_be-none-linux-gnu.tar.xz
+else
+	echo "ERROR - Unknown build arch of $HOST_ARCH"
+	exit 1
+fi
+
+for i in arm aarch64 aarch64_be; do
+	if [ ! -d $TOOLCHAIN_DIR/gcc-arm-$VER-$HOST_ARCH-$i-none-linux-gnu*/ ]; then
+		if [ ! -f $DOWNLOAD_DIR/gcc-arm-$VER-$HOST_ARCH-$i-none-linux-gnu*.tar.xz ]; then
+			continue
+		fi
+
+		tar -C $TOOLCHAIN_DIR -axvf $DOWNLOAD_DIR/gcc-arm-$VER-$HOST_ARCH-$i-none-linux-gnu*.tar.xz
+	fi
+
+	# Setup a link for the toolchain to use local to the building machine (e.g., not in a shared location)
+	ln -s $TOOLCHAIN_DIR/gcc-arm-$VER-$HOST_ARCH-$i-none-linux-gnu* $TOOLCHAIN_LINK_DIR/$i
+done
diff --git a/meta-arm/ci/jobs-to-kas b/meta-arm/ci/jobs-to-kas
new file mode 100755
index 0000000..d6896b7
--- /dev/null
+++ b/meta-arm/ci/jobs-to-kas
@@ -0,0 +1,27 @@
+#! /bin/bash
+
+# This script is expecting an input of machine name, optionally followed by a
+# colon and a list of one or more parameters separated by commas between
+# brackets.  For example, the following are acceptable:
+# corstone500
+# fvp-base: [testimage]
+# qemuarm64-secureboot: [clang, glibc, testimage]
+#
+# Turn this list into a series of yml files separated by colons to pass to kas
+
+set -e -u
+
+FILES="ci/$(echo $1 | cut -d ':' -f 1).yml"
+
+for i in $(echo $1 | cut -s -d ':' -f 2 | sed 's/[][,]//g'); do
+	# Given that there are no yml files for gcc or glibc, as those are the
+	# defaults, we can simply ignore those parameters.  They are necessary
+	# to pass in so that matrix can correctly setup all of the permutations
+	# of each individual run.
+	if [[ $i == 'none' || $i == 'gcc' || $i == 'glibc' || $i == 'uboot' ]]; then
+		continue
+	fi
+	FILES+=":ci/$i.yml"
+done
+
+echo $FILES
diff --git a/meta-arm/ci/juno.yml b/meta-arm/ci/juno.yml
new file mode 100644
index 0000000..b2ee60a
--- /dev/null
+++ b/meta-arm/ci/juno.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: juno
diff --git a/meta-arm/ci/listmachines.py b/meta-arm/ci/listmachines.py
new file mode 100755
index 0000000..04d207e
--- /dev/null
+++ b/meta-arm/ci/listmachines.py
@@ -0,0 +1,29 @@
+#! /usr/bin/env python3
+
+import pathlib
+import typing
+import sys
+
+"""
+List all of the machines available under the listed sub-layers of meta-arm.
+"""
+def list_machines(layers: typing.Sequence[str]) -> typing.Set[str]:
+    machines = set()
+
+    # We know we're in meta-arm/scripts, so find the top-level directory
+    metaarm = pathlib.Path(__file__).resolve().parent.parent
+    if metaarm.name != "meta-arm":
+        raise Exception("Not running inside meta-arm")
+    
+    for layer in layers:
+        machines |= set(p.stem for p in (metaarm / layer / "conf" / "machine").glob("*.conf"))
+    return machines
+
+if __name__ == "__main__":
+    if len(sys.argv) > 1:
+        machines = list_machines(sys.argv[1:])
+        print(" ".join(sorted(machines)))
+        sys.exit(0)
+    else:
+        print("Usage:\n$ %s [layer name ...] " % sys.argv[0])
+        sys.exit(1)
diff --git a/meta-arm/ci/logging.yml b/meta-arm/ci/logging.yml
new file mode 100644
index 0000000..3af1029
--- /dev/null
+++ b/meta-arm/ci/logging.yml
@@ -0,0 +1,13 @@
+# Python logging configuration to write all warnings to a separate file
+version: 1
+
+handlers:
+  warnings:
+    class: logging.FileHandler
+    level: WARNING
+    filename: warnings.log
+    formatter: BitBake.logfileFormatter
+
+loggers:
+  BitBake:
+    handlers: [warnings]
diff --git a/meta-arm/ci/meta-openembedded.yml b/meta-arm/ci/meta-openembedded.yml
new file mode 100644
index 0000000..bed338d
--- /dev/null
+++ b/meta-arm/ci/meta-openembedded.yml
@@ -0,0 +1,11 @@
+header:
+  version: 11
+
+repos:
+  meta-openembedded:
+    url: https://git.openembedded.org/meta-openembedded
+    layers:
+      meta-filesystems:
+      meta-networking:
+      meta-oe:
+      meta-python:
diff --git a/meta-arm/ci/meta-virtualization.yml b/meta-arm/ci/meta-virtualization.yml
new file mode 100644
index 0000000..1cd0e21
--- /dev/null
+++ b/meta-arm/ci/meta-virtualization.yml
@@ -0,0 +1,8 @@
+header:
+  version: 11
+  includes:
+    - ci/meta-openembedded.yml
+
+repos:
+  meta-virtualization:
+    url: git://git.yoctoproject.org/meta-virtualization
diff --git a/meta-arm/ci/meta-zephyr.yml b/meta-arm/ci/meta-zephyr.yml
new file mode 100644
index 0000000..bdd0e2b
--- /dev/null
+++ b/meta-arm/ci/meta-zephyr.yml
@@ -0,0 +1,13 @@
+header:
+  version: 11
+  includes:
+    - ci/meta-openembedded.yml
+
+repos:
+  meta-zephyr:
+    url: https://git.yoctoproject.org/git/meta-zephyr
+    layers:
+      meta-zephyr-core:
+
+target:
+  - zephyr-kernel-test-all
diff --git a/meta-arm/ci/microbit-v1.yml b/meta-arm/ci/microbit-v1.yml
new file mode 100644
index 0000000..b850148
--- /dev/null
+++ b/meta-arm/ci/microbit-v1.yml
@@ -0,0 +1,7 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+    - ci/meta-zephyr.yml
+
+machine: microbit-v1
diff --git a/meta-arm/ci/musca-b1.yml b/meta-arm/ci/musca-b1.yml
new file mode 100644
index 0000000..dc9814d
--- /dev/null
+++ b/meta-arm/ci/musca-b1.yml
@@ -0,0 +1,15 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+    - ci/meta-zephyr.yml
+
+local_conf_header:
+  nonbuilding_tests: |
+    ZEPHYRTESTS:remove = "common sleep poll device queue"
+
+machine: musca-b1
+
+target:
+  - trusted-firmware-m
+  - zephyr-kernel-test-all
diff --git a/meta-arm/ci/musca-s1.yml b/meta-arm/ci/musca-s1.yml
new file mode 100644
index 0000000..80a59c4
--- /dev/null
+++ b/meta-arm/ci/musca-s1.yml
@@ -0,0 +1,15 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+    - ci/meta-zephyr.yml
+
+local_conf_header:
+  nonbuilding_tests: |
+    ZEPHYRTESTS:remove = "common sleep poll device queue"
+
+machine: musca-s1
+
+target:
+  - trusted-firmware-m
+  - zephyr-kernel-test-all
diff --git a/meta-arm/ci/musl.yml b/meta-arm/ci/musl.yml
new file mode 100644
index 0000000..ee7905e
--- /dev/null
+++ b/meta-arm/ci/musl.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+
+local_conf_header:
+  libc: |
+    TCLIBC = "musl"
diff --git a/meta-arm/ci/n1sdp.yml b/meta-arm/ci/n1sdp.yml
new file mode 100644
index 0000000..797a522
--- /dev/null
+++ b/meta-arm/ci/n1sdp.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: n1sdp
diff --git a/meta-arm/ci/patchreview b/meta-arm/ci/patchreview
new file mode 100755
index 0000000..b23eda1
--- /dev/null
+++ b/meta-arm/ci/patchreview
@@ -0,0 +1,286 @@
+#! /usr/bin/env python3
+#
+# SPDX-License-Identifier: GPL-2.0-only
+#
+
+# TODO
+# - option to just list all broken files
+# - test suite
+# - validate signed-off-by
+
+import argparse
+import collections
+import json
+import os
+import re
+import subprocess
+
+status_values = (
+    "accepted",
+    "pending",
+    "inappropriate",
+    "backport",
+    "submitted",
+    "denied",
+)
+
+
+class PatchResult:
+    # Whether the patch has an Upstream-Status or not
+    missing_upstream_status = False
+    # If the Upstream-Status tag is malformed in some way (string for bad bit)
+    malformed_upstream_status = None
+    # If the Upstream-Status value is unknown (boolean)
+    unknown_upstream_status = False
+    # The upstream status value (Pending, etc)
+    upstream_status = None
+    # Whether the patch has a Signed-off-by or not
+    missing_sob = False
+    # Whether the Signed-off-by tag is malformed in some way
+    malformed_sob = False
+    # The Signed-off-by tag value
+    sob = None
+    # Whether a patch looks like a CVE but doesn't have a CVE tag
+    missing_cve = False
+
+
+class Summary:
+    total = 0
+    cve_missing = 0
+    sob_missing = 0
+    sob_malformed = 0
+    status_missing = 0
+    status_malformed = 0
+    status_pending = 0
+
+def blame_patch(patch):
+    """
+    From a patch filename, return a list of "commit summary (author name <author
+    email>)" strings representing the history.
+    """
+    return subprocess.check_output(("git", "log",
+                                    "--follow", "--find-renames", "--diff-filter=A",
+                                    "--format=%s (%aN <%aE>)",
+                                    "--", patch)).decode("utf-8").splitlines()
+
+def patchreview(patches):
+    # General pattern: start of line, optional whitespace, tag with optional
+    # hyphen or spaces, maybe a colon, some whitespace, then the value, all case
+    # insensitive.
+    sob_re = re.compile(r"^[\t ]*(Signed[-_ ]off[-_ ]by:?)[\t ]*(.+)", re.IGNORECASE | re.MULTILINE)
+    status_re = re.compile(r"^[\t ]*(Upstream[-_ ]Status:?)[\t ]*(\w*)", re.IGNORECASE | re.MULTILINE)
+    cve_tag_re = re.compile(r"^[\t ]*(CVE:)[\t ]*(.*)", re.IGNORECASE | re.MULTILINE)
+    cve_re = re.compile(r"cve-[0-9]{4}-[0-9]{4,6}", re.IGNORECASE)
+
+    results = {}
+
+    for patch in patches:
+
+        result = PatchResult()
+        results[patch] = result
+
+        content = open(patch, encoding="ascii", errors="ignore").read()
+
+        # Find the Signed-off-by tag
+        match = sob_re.search(content)
+        if match:
+            value = match.group(1)
+            if value != "Signed-off-by:":
+                result.malformed_sob = value
+            result.sob = match.group(2)
+        else:
+            result.missing_sob = True
+
+        # Find the Upstream-Status tag
+        match = status_re.search(content)
+        if match:
+            value = match.group(1)
+            if value != "Upstream-Status:":
+                result.malformed_upstream_status = value
+
+            value = match.group(2).lower()
+            # TODO: check case
+            if value not in status_values:
+                result.unknown_upstream_status = True
+            result.upstream_status = value
+        else:
+            result.missing_upstream_status = True
+
+        # Check that patches which looks like CVEs have CVE tags
+        if cve_re.search(patch) or cve_re.search(content):
+            if not cve_tag_re.search(content):
+                result.missing_cve = True
+        # TODO: extract CVE list
+
+    return results
+
+
+def analyse(results, want_blame=False, verbose=True):
+    """
+    want_blame: display blame data for each malformed patch
+    verbose: display per-file results instead of just summary
+    """
+
+    # want_blame requires verbose, so disable blame if we're not verbose
+    if want_blame and not verbose:
+        want_blame = False
+
+    summary = Summary()
+
+    for patch in sorted(results):
+        r = results[patch]
+        summary.total += 1
+        need_blame = False
+
+        # Build statistics
+        if r.missing_sob:
+            summary.sob_missing += 1
+        if r.malformed_sob:
+            summary.sob_malformed += 1
+        if r.missing_upstream_status:
+            summary.status_missing += 1
+        if r.malformed_upstream_status or r.unknown_upstream_status:
+            summary.status_malformed += 1
+            # Count patches with no status as pending
+            summary.status_pending += 1
+        if r.missing_cve:
+            summary.cve_missing += 1
+        if r.upstream_status == "pending":
+            summary.status_pending += 1
+
+        # Output warnings
+        if r.missing_sob:
+            need_blame = True
+            if verbose:
+                print("Missing Signed-off-by tag (%s)" % patch)
+        if r.malformed_sob:
+            need_blame = True
+            if verbose:
+                print("Malformed Signed-off-by '%s' (%s)" % (r.malformed_sob, patch))
+        if r.missing_cve:
+            need_blame = True
+            if verbose:
+                print("Missing CVE tag (%s)" % patch)
+        if r.missing_upstream_status:
+            need_blame = True
+            if verbose:
+                print("Missing Upstream-Status tag (%s)" % patch)
+        if r.malformed_upstream_status:
+            need_blame = True
+            if verbose:
+                print("Malformed Upstream-Status '%s' (%s)" % (r.malformed_upstream_status, patch))
+        if r.unknown_upstream_status:
+            need_blame = True
+            if verbose:
+                print("Unknown Upstream-Status value '%s' (%s)" % (r.upstream_status, patch))
+
+        if want_blame and need_blame:
+            print("\n".join(blame_patch(patch)) + "\n")
+
+    return summary
+
+
+def display_summary(summary, verbose):
+    def percent(num):
+        try:
+            return "%d (%d%%)" % (num, round(num * 100.0 / summary.total))
+        except ZeroDivisionError:
+            return "N/A"
+
+    if verbose:
+        print()
+
+    print("""Total patches found: %d
+Patches missing Signed-off-by: %s
+Patches with malformed Signed-off-by: %s
+Patches missing CVE: %s
+Patches missing Upstream-Status: %s
+Patches with malformed Upstream-Status: %s
+Patches in Pending state: %s""" % (summary.total,
+                                   percent(summary.sob_missing),
+                                   percent(summary.sob_malformed),
+                                   percent(summary.cve_missing),
+                                   percent(summary.status_missing),
+                                   percent(summary.status_malformed),
+                                   percent(summary.status_pending)))
+
+
+def generate_metrics(summary, output):
+    # https://github.com/OpenObservability/OpenMetrics/blob/main/specification/OpenMetrics.md
+    # Summary attribute name, MetricPoint help
+    mapping = (
+        ("total", "Total patches"),
+        ("cve_missing", "Patches missing CVE tag"),
+        ("sob_malformed", "Patches with malformed Signed-off-by"),
+        ("sob_missing", "Patches with missing Signed-off-by"),
+        ("status_malformed", "Patches with malformed Upstream-Status"),
+        ("status_missing", "Patches with missing Upstream-Status"),
+        ("status_pending", "Patches with Pending Upstream-Status")
+    )
+    for attr, help in mapping:
+        metric = f"patch_check_{attr}"
+        value = getattr(summary, attr)
+        output.write(f"""
+# TYPE {metric} gauge
+# HELP {help}
+{metric} {value}
+""")
+    output.write("\n# EOF\n")
+
+def histogram(results):
+    import math
+
+    from toolz import dicttoolz, recipes
+    counts = recipes.countby(lambda r: r.upstream_status, results.values())
+    bars = dicttoolz.valmap(lambda v: "#" * int(math.ceil(float(v) / len(results) * 100)), counts)
+    for k in bars:
+        print("%-20s %s (%d)" % (k.capitalize() if k else "No status", bars[k], counts[k]))
+
+def gather_patches(directories):
+    patches = []
+    for directory in directories:
+        filenames = subprocess.check_output(("git", "-C", directory, "ls-files", "recipes-*/**/*.patch", "recipes-*/**/*.diff")).decode("utf-8").split()
+        patches += [os.path.join(directory, f) for f in filenames]
+    return patches
+
+if __name__ == "__main__":
+    args = argparse.ArgumentParser(description="Patch Review Tool")
+    args.add_argument("-b", "--blame", action="store_true", help="show blame for malformed patches")
+    args.add_argument("-v", "--verbose", action="store_true", help="show per-patch results")
+    args.add_argument("-g", "--histogram", action="store_true", help="show patch histogram")
+    args.add_argument("-j", "--json", help="update JSON")
+    args.add_argument("-m", "--metrics", type=argparse.FileType('w'), help="write OpenMetrics")
+    args.add_argument("dirs", metavar="DIRECTORY", nargs="+", help="directory to scan")
+    args = args.parse_args()
+
+    patches = gather_patches(args.dirs)
+    results = patchreview(patches)
+    summary = analyse(results, want_blame=args.blame, verbose=args.verbose)
+    display_summary(summary, verbose=args.verbose)
+
+    if args.json:
+        if os.path.isfile(args.json):
+            data = json.load(open(args.json))
+        else:
+            data = []
+
+        row = collections.Counter()
+        row["total"] = len(results)
+        row["date"] = subprocess.check_output(["git", "-C", args.dirs[0], "show", "-s", "--pretty=format:%cd", "--date=format:%s"]).decode("utf-8").strip()
+        for r in results.values():
+            if r.upstream_status in status_values:
+                row[r.upstream_status] += 1
+            if r.malformed_upstream_status or r.missing_upstream_status:
+                row["malformed-upstream-status"] += 1
+            if r.malformed_sob or r.missing_sob:
+                row["malformed-sob"] += 1
+
+        data.append(row)
+        json.dump(data, open(args.json, "w"))
+
+    if args.metrics:
+        generate_metrics(summary, args.metrics)
+
+    if args.histogram:
+        print()
+        histogram(results)
diff --git a/meta-arm/ci/qemu-cortex-a53.yml b/meta-arm/ci/qemu-cortex-a53.yml
new file mode 100644
index 0000000..3eec580
--- /dev/null
+++ b/meta-arm/ci/qemu-cortex-a53.yml
@@ -0,0 +1,9 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+    - ci/meta-zephyr.yml
+
+# FIXME - testimage fails all tests currently, but all the tests build
+
+machine: qemu-cortex-a53
diff --git a/meta-arm/ci/qemu-cortex-m3.yml b/meta-arm/ci/qemu-cortex-m3.yml
new file mode 100644
index 0000000..f45cb9e
--- /dev/null
+++ b/meta-arm/ci/qemu-cortex-m3.yml
@@ -0,0 +1,20 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+    - ci/meta-zephyr.yml
+
+repos:
+  meta-zephyr:
+    layers:
+      meta-zephyr-bsp:
+
+local_conf_header:
+  tclibc: |
+    TCLIBC = "newlib"
+  nonbuilding_tests: |
+    ZEPHYRTESTS:remove = "common context pending poll sleep"
+  qemu_opts: |
+    QB_OPT_APPEND = "-icount shift=3,align=off,sleep=on -rtc clock=vm"
+
+machine: qemu-cortex-m3
diff --git a/meta-arm/ci/qemu-cortex-r5.yml b/meta-arm/ci/qemu-cortex-r5.yml
new file mode 100644
index 0000000..cfcb7b9
--- /dev/null
+++ b/meta-arm/ci/qemu-cortex-r5.yml
@@ -0,0 +1,11 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+    - ci/meta-zephyr.yml
+
+local_conf_header:
+  nonbuilding_tests: |
+    ZEPHYRTESTS:remove = "common poll sleep queue device"
+
+machine: qemu-cortex-r5
diff --git a/meta-arm/ci/qemu-generic-arm64.yml b/meta-arm/ci/qemu-generic-arm64.yml
new file mode 100644
index 0000000..87cbd2f
--- /dev/null
+++ b/meta-arm/ci/qemu-generic-arm64.yml
@@ -0,0 +1,14 @@
+header:
+  version: 11
+  includes:
+    - ci/generic-arm64.yml
+
+local_conf_header:
+  failing_tests: |
+    DEFAULT_TEST_SUITES:remove = "parselogs"
+
+machine: qemu-generic-arm64
+
+target:
+  - core-image-base
+  - sbsa-acs
diff --git a/meta-arm/ci/qemuarm-secureboot.yml b/meta-arm/ci/qemuarm-secureboot.yml
new file mode 100644
index 0000000..044661c
--- /dev/null
+++ b/meta-arm/ci/qemuarm-secureboot.yml
@@ -0,0 +1,12 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: qemuarm-secureboot
+
+target:
+  - core-image-base
+  - optee-examples
+  - optee-test
+  - optee-os-tadevkit
diff --git a/meta-arm/ci/qemuarm.yml b/meta-arm/ci/qemuarm.yml
new file mode 100644
index 0000000..4155847
--- /dev/null
+++ b/meta-arm/ci/qemuarm.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: qemuarm
diff --git a/meta-arm/ci/qemuarm64-secureboot.yml b/meta-arm/ci/qemuarm64-secureboot.yml
new file mode 100644
index 0000000..7e15a76
--- /dev/null
+++ b/meta-arm/ci/qemuarm64-secureboot.yml
@@ -0,0 +1,18 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: qemuarm64-secureboot
+
+local_conf_header:
+  failing_tests: |
+    # software IO TLB: Cannot allocate buffer
+    DEFAULT_TEST_SUITES:remove = "parselogs"
+
+target:
+  - core-image-base
+  - optee-examples
+  - optee-test
+  - optee-spdevkit
+  - optee-os-tadevkit
diff --git a/meta-arm/ci/qemuarm64.yml b/meta-arm/ci/qemuarm64.yml
new file mode 100644
index 0000000..6639034
--- /dev/null
+++ b/meta-arm/ci/qemuarm64.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: qemuarm64
diff --git a/meta-arm/ci/qemuarmv5.yml b/meta-arm/ci/qemuarmv5.yml
new file mode 100644
index 0000000..18c7a15
--- /dev/null
+++ b/meta-arm/ci/qemuarmv5.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: qemuarmv5
diff --git a/meta-arm/ci/selftest.yml b/meta-arm/ci/selftest.yml
new file mode 100644
index 0000000..9a58735
--- /dev/null
+++ b/meta-arm/ci/selftest.yml
@@ -0,0 +1,7 @@
+header:
+  version: 11
+
+local_conf_header:
+  setup: |
+    BB_LOGCONFIG = ""
+    SANITY_TESTED_DISTROS = ""
diff --git a/meta-arm/ci/sgi575.yml b/meta-arm/ci/sgi575.yml
new file mode 100644
index 0000000..1895fc5
--- /dev/null
+++ b/meta-arm/ci/sgi575.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: sgi575
diff --git a/meta-arm/ci/tc0.yml b/meta-arm/ci/tc0.yml
new file mode 100644
index 0000000..b2f75d5
--- /dev/null
+++ b/meta-arm/ci/tc0.yml
@@ -0,0 +1,9 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: tc0
+
+target:
+  - tc-artifacts-image
diff --git a/meta-arm/ci/tc1.yml b/meta-arm/ci/tc1.yml
new file mode 100644
index 0000000..fd9acbd
--- /dev/null
+++ b/meta-arm/ci/tc1.yml
@@ -0,0 +1,9 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+machine: tc1
+
+target:
+  - tc-artifacts-image
diff --git a/meta-arm/ci/testimage-zephyr.yml b/meta-arm/ci/testimage-zephyr.yml
new file mode 100644
index 0000000..83e17a7
--- /dev/null
+++ b/meta-arm/ci/testimage-zephyr.yml
@@ -0,0 +1,9 @@
+header:
+  version: 11
+
+local_conf_header:
+  testimage: |
+    IMAGE_CLASSES += "testimage"
+    TEST_TARGET = "QemuTargetZephyr"
+    TEST_SUITES = "zephyr"
+    TESTIMAGE_AUTO = "1"
diff --git a/meta-arm/ci/testimage.yml b/meta-arm/ci/testimage.yml
new file mode 100644
index 0000000..a26dcaf
--- /dev/null
+++ b/meta-arm/ci/testimage.yml
@@ -0,0 +1,13 @@
+header:
+  version: 11
+
+local_conf_header:
+  testimage: |
+    IMAGE_CLASSES += "testimage"
+    TESTIMAGE_AUTO = "1"
+  slirp: |
+    TEST_RUNQEMUPARAMS = "slirp"
+    TEST_SERVER_IP = "127.0.0.1"
+    QEMU_USE_SLIRP = "1"
+  sshd: |
+    IMAGE_FEATURES:append = " ssh-server-dropbear"
diff --git a/meta-arm/ci/tftf.yml b/meta-arm/ci/tftf.yml
new file mode 100644
index 0000000..6e42d9c
--- /dev/null
+++ b/meta-arm/ci/tftf.yml
@@ -0,0 +1,7 @@
+header:
+  version: 9
+
+local_conf_header:
+  tftf: |
+    TFA_UBOOT = "0"
+    TFTF_TESTS = "1"
diff --git a/meta-arm/ci/toolchains.yml b/meta-arm/ci/toolchains.yml
new file mode 100644
index 0000000..080d8d9
--- /dev/null
+++ b/meta-arm/ci/toolchains.yml
@@ -0,0 +1,19 @@
+header:
+  version: 11
+  includes:
+    - ci/base.yml
+
+# Target is arm64 and SDK is x86-64 to ensure that we exercise both
+# architectures
+
+machine: qemuarm64
+local_conf_header:
+  toolchains: |
+    SDKMACHINE = "x86_64"
+
+target:
+  - gcc-aarch64-none-elf
+  - nativesdk-gcc-aarch64-none-elf
+  - gcc-arm-none-eabi
+  - nativesdk-gcc-arm-none-eabi
+  - nativesdk-androidclang
diff --git a/meta-arm/ci/update-repos b/meta-arm/ci/update-repos
new file mode 100755
index 0000000..a68257b
--- /dev/null
+++ b/meta-arm/ci/update-repos
@@ -0,0 +1,43 @@
+#! /usr/bin/env python3
+
+# Update clones of the repositories we need in KAS_REPO_REF_DIR to speed up fetches
+
+import sys
+import os
+import subprocess
+import pathlib
+
+def repo_shortname(url):
+    # Taken from Kas (Repo.__getattr__) to ensure the logic is right
+    from urllib.parse import urlparse
+    url = urlparse(url)
+    return ('{url.netloc}{url.path}'
+            .format(url=url)
+            .replace('@', '.')
+            .replace(':', '.')
+            .replace('/', '.')
+            .replace('*', '.'))
+
+repositories = (
+    "https://git.yoctoproject.org/git/poky",
+    "https://git.openembedded.org/meta-openembedded",
+    "https://git.yoctoproject.org/git/meta-virtualization",
+    "https://git.yoctoproject.org/git/meta-zephyr",
+    "https://github.com/kraj/meta-clang",
+)
+
+if __name__ == "__main__":
+    if "KAS_REPO_REF_DIR" not in os.environ:
+        print("KAS_REPO_REF_DIR needs to be set")
+        sys.exit(1)
+
+    base_repodir = pathlib.Path(os.environ["KAS_REPO_REF_DIR"])
+
+    for repo in repositories:
+        repodir = base_repodir / repo_shortname(repo)
+        if repodir.exists():
+            print("Updating %s..." % repo)
+            subprocess.run(["git", "-C", repodir, "fetch"], check=True)
+        else:
+            print("Cloning %s..." % repo)
+            subprocess.run(["git", "clone", "--bare", repo, repodir], check=True)
diff --git a/meta-arm/ci/xen.yml b/meta-arm/ci/xen.yml
new file mode 100644
index 0000000..d8b75d4
--- /dev/null
+++ b/meta-arm/ci/xen.yml
@@ -0,0 +1,11 @@
+header:
+  version: 11
+  includes:
+    - ci/meta-virtualization.yml
+
+local_conf_header:
+  meta-virt: |
+    DISTRO_FEATURES:append = " virtualization xen"
+
+target:
+  - xen-image-minimal
diff --git a/meta-arm/documentation/runfvp.md b/meta-arm/documentation/runfvp.md
new file mode 100644
index 0000000..b9e007b
--- /dev/null
+++ b/meta-arm/documentation/runfvp.md
@@ -0,0 +1,120 @@
+# Running Images with a FVP
+
+The `runfvp` tool in meta-arm makes it easy to run Yocto Project disk images inside a [Fixed Virtual Platform (FVP)][FVP].  Some FVPs, such as the [Arm Architecture Models][AEM], are available free to download, but others need registration or are only available commercially.  The `fvp-base` machine in meta-arm-bsp uses one of these AEM models.
+
+## Running images with `runfvp`
+
+To build images with the FVP integration, the `fvpboot` class needs to be inherited.  If the machine does not do this explicitly it can be done in `local.conf`:
+
+```
+INHERIT += "fvpboot"
+```
+
+The class will download the correct FVP and write a `.fvpconf` configuration file when an image is built.
+
+To run an image in a FVP, pass either a machine name or a `.fvpconf` path to `runfvp`.
+
+```
+$ ./meta-arm/scripts/runfvp tmp/deploy/images/fvp-base/core-image-minimal-fvp-base.fvpconf
+```
+
+When a machine name is passed, `runfvp` will start the latest image that has been built for that machine. This requires that the BitBake environment has been initialized (using `oe-init-build-env` or similar) as it will start BitBake to determine where the images are.
+
+```
+$ ./meta-arm/scripts/runfvp fvp-base
+```
+
+Note that currently meta-arm's `scripts` directory isn't in `PATH`, so a full path needs to be used.
+
+`runfvp` will automatically start terminals connected to each of the serial ports that the machine specifies.  This can be controlled by using the `--terminals` option, for example `--terminals=none` will mean no terminals are started, and `--terminals=tmux` will start the terminals in [`tmux`][tmux] sessions.  Alternatively, passing `--console` will connect the serial port directly to the current session, without needing to open further windows.
+
+The default terminal can also be configured by writing a [INI-style][INI] configuration file to `~/.config/runfvp.conf`:
+
+```
+[RunFVP]
+Terminal=tmux
+```
+
+Arbitrary options can be passed directly to the FVP by specifying them after a double dash, for example this will list all of the FVP parameters:
+
+```
+$ runfvp fvp-base -- --list-params
+```
+
+## Configuring machines with `fvpboot`
+
+To configure a machine so that it can be ran inside `runfvp`, a number of variables need to be set in the machine configuration file (such as `meta-arm-bsp/conf/machine/fvp-base.conf`).
+
+Note that at present these variables are not stable and their behaviour may be changed in the future.
+
+### `FVP_EXE`
+
+The name of the FVP binary itself, for example `fvp-base` uses `FVP_Base_RevC-2xAEMvA`.
+
+### `FVP_PROVIDER`
+
+The name of the recipe that provides the FVP executable set in `FVP_EXE`, for example `fvp-base` uses `fvp-base-a-aem-native`.  This *must* be a `-native` recipe as the binary will be executed on the build host.
+
+There are recipes for common FVPs in meta-arm already, and writing new recipes is trivial.  For FVPs which are free to download `fvp-base-a-aem.bb` is a good example. Some FVPs must be downloaded separately as they need an account on Arm's website, `fvp-base-r-aem.bb` is a good example of those.
+
+If `FVP_PROVIDER` is not set then it is assumed that `FVP_EXE` is installed on the host already.
+
+### `FVP_CONFIG`
+
+Parameters passed to the FVP with the `--parameter`/`-C` option.  These are expressed as variable flags so individual parameters can be altered easily. For example:
+
+```
+FVP_CONFIG[bp.flashloader0.fname] = "${DEPLOY_DIR_IMAGE}/fip-fvp.bin"
+```
+
+### `FVP_DATA`
+
+Specify raw data to load at the specified address, passed to the FVP with the `--data` option.  This is a space-separated list of parameters in the format `[INST=]FILE@[MEMSPACE:]ADDRESS`. For example:
+
+```
+FVP_DATA = "cluster0.cpu0=${DEPLOY_DIR_IMAGE}/Image@0x80080000 \
+            cluster0.cpu0=${DEPLOY_DIR_IMAGE}/fvp-base-revc.dtb@0x83000000"
+```
+
+### `FVP_APPLICATIONS`
+
+Applications to load on the cores, passed to the FVP with the `--application` option.  These are expressed as variable flags with the flag name being the instance and flag value the filename, for example:
+
+```
+FVP_APPLICATIONS[cluster0] = "${DEPLOY_DIR_IMAGE}/linux-system.axf"
+```
+
+Note that symbols are not allowed in flag names, so if you need to use a wildcard in the instance then you'll need to use `FVP_EXTRA_ARGS` and `--application` directly.
+
+### `FVP_TERMINALS`
+
+Map hardware serial ports to abstract names. For example the `FVP_Base_RevC-2xAEMvA` FVP exposes four serial ports, `terminal_0` to `terminal_3`.  Typically only `terminal_0` is used in the `fvp-base` machine so this can be named `"Console"` and the others `""`.  When runfvp starts terminals it will only start named serial ports, so instead of opening four windows where only one is useful, it will only open one.
+
+For example:
+```
+FVP_TERMINALS[bp.terminal_0] = "Console"
+FVP_TERMINALS[bp.terminal_1] = ""
+FVP_TERMINALS[bp.terminal_2] = ""
+FVP_TERMINALS[bp.terminal_3] = ""
+```
+
+### `FVP_CONSOLE`
+
+This specifies what serial port is used when `--console` is passed to runfvp. Note that this has to be the FVP identifier but without the board prefix, for example:
+```
+FVP_CONSOLE = "terminal_0"
+```
+
+### `FVP_EXTRA_ARGS`
+
+Arbitrary extra arguments that are passed directly to the FVP.  For example:
+
+```
+FVP_EXTRA_ARGS = "--simlimit 60"
+```
+
+
+[AEM]: https://developer.arm.com/tools-and-software/simulation-models/fixed-virtual-platforms/arm-ecosystem-models
+[FVP]: https://developer.arm.com/tools-and-software/simulation-models/fixed-virtual-platforms
+[tmux]: https://tmux.github.io/
+[INI]: https://docs.python.org/3/library/configparser.html
diff --git a/meta-arm/kas/corstone1000-base.yml b/meta-arm/kas/corstone1000-base.yml
new file mode 100644
index 0000000..21e5280
--- /dev/null
+++ b/meta-arm/kas/corstone1000-base.yml
@@ -0,0 +1,44 @@
+header:
+  version: 11
+
+distro: poky-tiny
+
+defaults:
+  repos:
+    refspec: master
+
+repos:
+  meta-arm:
+    layers:
+      meta-arm:
+      meta-arm-bsp:
+      meta-arm-toolchain:
+
+  poky:
+    url: https://git.yoctoproject.org/git/poky
+    refspec: master
+    layers:
+      meta:
+      meta-poky:
+      meta-yocto-bsp:
+
+  meta-openembedded:
+    url: https://git.openembedded.org/meta-openembedded
+    refspec: master
+    layers:
+      meta-oe:
+      meta-python:
+
+local_conf_header:
+  base: |
+    CONF_VERSION = "2"
+    PACKAGE_CLASSES = "package_ipk"
+    LICENSE_FLAGS_ACCEPTED += "armcompiler"
+    BB_NUMBER_THREADS ?= "16"
+    PARALLEL_MAKE ?= "-j16"
+    PACKAGECONFIG:append:pn-perf = " coresight"
+
+machine: unset
+
+target:
+  - corstone1000-image
diff --git a/meta-arm/kas/corstone1000-fvp.yml b/meta-arm/kas/corstone1000-fvp.yml
new file mode 100644
index 0000000..d0d10a7
--- /dev/null
+++ b/meta-arm/kas/corstone1000-fvp.yml
@@ -0,0 +1,16 @@
+header:
+  version: 11
+  includes:
+    - kas/corstone1000-base.yml
+
+machine: corstone1000-fvp
+
+local_conf_header:
+    fvp-config: |
+        # Remove Dropbear SSH as it will not fit into the corstone1000 image.
+        IMAGE_FEATURES:remove = " ssh-server-dropbear"
+        INHERIT = " ${@bb.utils.contains('BUILD_ARCH', 'x86_64', 'fvpboot', '', d)}"
+        LICENSE_FLAGS_ACCEPTED:append = " Arm-FVP-EULA"
+
+target:
+  - corstone1000-image
diff --git a/meta-arm/kas/corstone1000-mps3.yml b/meta-arm/kas/corstone1000-mps3.yml
new file mode 100644
index 0000000..53be438
--- /dev/null
+++ b/meta-arm/kas/corstone1000-mps3.yml
@@ -0,0 +1,6 @@
+header:
+  version: 11
+  includes:
+    - kas/corstone1000-base.yml
+
+machine: corstone1000-mps3
diff --git a/meta-arm/kas/corstone500.yml b/meta-arm/kas/corstone500.yml
new file mode 100644
index 0000000..a454a46
--- /dev/null
+++ b/meta-arm/kas/corstone500.yml
@@ -0,0 +1,47 @@
+header:
+  version: 11
+
+distro: poky-tiny
+
+defaults:
+  repos:
+    refspec: master
+
+repos:
+  meta-arm:
+    layers:
+      meta-arm:
+      meta-arm-bsp:
+      meta-arm-toolchain:
+
+  poky:
+    url: https://git.yoctoproject.org/git/poky
+    refspec: master
+    layers:
+      meta:
+      meta-poky:
+      meta-yocto-bsp:
+
+  meta-openembedded:
+    url: https://git.openembedded.org/meta-openembedded
+    refspec: master
+    layers:
+      meta-oe:
+      meta-python:
+
+local_conf_header:
+  base: |
+    CONF_VERSION = "2"
+    PACKAGE_CLASSES = "package_ipk"
+    LICENSE_FLAGS_ACCEPTED += "armcompiler"
+    BB_NUMBER_THREADS ?= "16"
+    PARALLEL_MAKE ?= "-j16"
+    PACKAGECONFIG:append:pn-perf = " coresight"
+  fvp-config: |
+    IMAGE_CLASSES:append = " ${@bb.utils.contains('BUILD_ARCH', 'x86_64', 'fvpboot', '', d)}"
+    LICENSE_FLAGS_ACCEPTED:append = " Arm-FVP-EULA"
+
+machine: corstone500
+
+target:
+  - core-image-minimal
diff --git a/meta-arm/kas/fvp-baser-aemv8r64-bsp.yml b/meta-arm/kas/fvp-baser-aemv8r64-bsp.yml
new file mode 100644
index 0000000..dd175d0
--- /dev/null
+++ b/meta-arm/kas/fvp-baser-aemv8r64-bsp.yml
@@ -0,0 +1,41 @@
+header:
+  version: 9
+
+distro: poky
+machine: fvp-baser-aemv8r64
+
+defaults:
+  repos:
+    refspec: master
+
+repos:
+  meta-arm:
+    url: https://git.yoctoproject.org/git/meta-arm
+    path: layers/meta-arm
+    layers:
+      meta-arm:
+      meta-arm-bsp:
+      meta-arm-toolchain:
+
+  poky:
+    url: https://git.yoctoproject.org/git/poky
+    path: layers/poky
+    layers:
+      meta:
+      meta-poky:
+
+env:
+  FVP_BASE_R_ARM_EULA_ACCEPT: "False"
+
+local_conf_header:
+  base: |
+    CONF_VERSION = "2"
+    PACKAGE_CLASSES = "package_ipk"
+    PACKAGECONFIG:remove:pn-qemu-system-native = "gtk+ sdl"
+    EXTRA_IMAGE_FEATURES:append = " debug-tweaks ssh-server-openssh"
+    CORE_IMAGE_EXTRA_INSTALL:append = " ssh-pregen-hostkeys"
+    LICENSE_FLAGS_ACCEPTED:append = " ${@oe.utils.vartrue('FVP_BASE_R_ARM_EULA_ACCEPT', 'Arm-FVP-EULA', '', d)}"
+    IMAGE_CLASSES:append = " testimage"
+
+target:
+  - core-image-minimal
diff --git a/meta-arm/kas/fvp-baser-aemv8r64-rt-bsp.yml b/meta-arm/kas/fvp-baser-aemv8r64-rt-bsp.yml
new file mode 100644
index 0000000..19ee747
--- /dev/null
+++ b/meta-arm/kas/fvp-baser-aemv8r64-rt-bsp.yml
@@ -0,0 +1,8 @@
+header:
+  version: 9
+  includes:
+    - kas/fvp-baser-aemv8r64-bsp.yml
+
+local_conf_header:
+  base-rt: |
+    PREFERRED_PROVIDER_virtual/kernel = "linux-yocto-rt"
diff --git a/meta-arm/kas/tftf.yml b/meta-arm/kas/tftf.yml
new file mode 100644
index 0000000..6e42d9c
--- /dev/null
+++ b/meta-arm/kas/tftf.yml
@@ -0,0 +1,7 @@
+header:
+  version: 9
+
+local_conf_header:
+  tftf: |
+    TFA_UBOOT = "0"
+    TFTF_TESTS = "1"
diff --git a/meta-arm/meta-arm-bsp/README.md b/meta-arm/meta-arm-bsp/README.md
new file mode 100644
index 0000000..490ddca
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/README.md
@@ -0,0 +1 @@
+See ../README.md
diff --git a/meta-arm/meta-arm-bsp/classes/wic_nopt.bbclass b/meta-arm/meta-arm-bsp/classes/wic_nopt.bbclass
new file mode 100644
index 0000000..322be49
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/classes/wic_nopt.bbclass
@@ -0,0 +1,9 @@
+# This class removes the empty partition table header
+# in the WIC file when --no-table WKS option is used
+
+IMAGE_TYPES += "wic.nopt"
+
+CONVERSIONTYPES += "nopt"
+
+# 1024 bytes are skipped which corresponds to the size of the partition table header to remove
+CONVERSION_CMD:nopt = "tail -c +1025 ${IMAGE_NAME}${IMAGE_NAME_SUFFIX}.${type} >  ${IMAGE_NAME}${IMAGE_NAME_SUFFIX}.${type}.nopt"
diff --git a/meta-arm/meta-arm-bsp/conf/layer.conf b/meta-arm/meta-arm-bsp/conf/layer.conf
new file mode 100644
index 0000000..a1e22c0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/layer.conf
@@ -0,0 +1,21 @@
+# We have a conf and classes directory, add to BBPATH
+BBPATH .= ":${LAYERDIR}"
+
+# We have recipes-* directories, add to BBFILES
+BBFILES += "${LAYERDIR}/recipes-*/*/*.bb \
+            ${LAYERDIR}/recipes-*/*/*.bbappend"
+
+BBFILE_COLLECTIONS += "meta-arm-bsp"
+BBFILE_PATTERN_meta-arm-bsp = "^${LAYERDIR}/"
+BBFILE_PRIORITY_meta-arm-bsp = "5"
+
+LAYERSERIES_COMPAT_meta-arm-bsp = "kirkstone"
+
+LAYERDEPENDS_meta-arm-bsp = "core meta-arm"
+# This won't be used by layerindex-fetch, but works everywhere else
+LAYERDEPENDS_meta-arm-bsp:append:corstone1000 = " meta-python openembedded-layer"
+LAYERDEPENDS_meta-arm-bsp:append:musca-b1 = " meta-python"
+LAYERDEPENDS_meta-arm-bsp:append:musca-s1 = " meta-python"
+
+# Additional license directories.
+LICENSE_PATH += "${LAYERDIR}/custom-licenses"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/corstone1000-fvp.conf b/meta-arm/meta-arm-bsp/conf/machine/corstone1000-fvp.conf
new file mode 100644
index 0000000..e79373d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/corstone1000-fvp.conf
@@ -0,0 +1,41 @@
+#@TYPE: Machine
+#@NAME: corstone1000-fvp machine
+#@DESCRIPTION: Machine configuration for Corstone1000 64-bit FVP
+require conf/machine/include/corstone1000.inc
+
+TFA_TARGET_PLATFORM = "fvp"
+
+TFM_PLATFORM_IS_FVP = "TRUE"
+
+# testimage config
+TEST_TARGET = "OEFVPSerialTarget"
+TEST_SUITES = "linuxboot"
+
+# FVP Config
+FVP_PROVIDER ?= "fvp-corstone1000-native"
+FVP_EXE ?= "FVP_Corstone-1000"
+FVP_CONSOLE ?= "host_terminal_0"
+
+# FVP Parameters
+FVP_CONFIG[se.trustedBootROMloader.fname] ?= "${DEPLOY_DIR_IMAGE}/bl1.bin"
+FVP_CONFIG[board.xnvm_size] ?= "64"
+FVP_CONFIG[se.trustedSRAM_config] ?= "6"
+FVP_CONFIG[se.BootROM_config] ?= "3"
+FVP_CONFIG[board.hostbridge.interfaceName] ?= "tap0"
+FVP_CONFIG[board.smsc_91c111.enabled] ?= "1"
+FVP_CONFIG[board.hostbridge.userNetworking] ?= "true"
+FVP_CONFIG[board.hostbridge.userNetPorts] ?= "5555=5555,8080=80,8022=22"
+FVP_CONFIG[board.se_flash_size] ?= "8192"
+FVP_CONFIG[diagnostics] ?= "4"
+FVP_CONFIG[disable_visualisation] ?= "true"
+FVP_CONFIG[se.nvm.update_raw_image] ?= "0"
+
+# Boot image
+FVP_DATA ?= "board.flash0=${DEPLOY_DIR_IMAGE}/${IMAGE_NAME}.rootfs.wic.nopt@0x68100000"
+
+# FVP Terminals
+FVP_TERMINALS[host.host_terminal_0] ?= "Normal World Console"
+FVP_TERMINALS[host.host_terminal_1] ?= "Secure World Console"
+FVP_TERMINALS[se.secenc_terminal] ?= "Secure Enclave Console"
+FVP_TERMINALS[extsys0.extsys_terminal] ?= "Cortex M3"
+
diff --git a/meta-arm/meta-arm-bsp/conf/machine/corstone1000-mps3.conf b/meta-arm/meta-arm-bsp/conf/machine/corstone1000-mps3.conf
new file mode 100644
index 0000000..88f3d9d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/corstone1000-mps3.conf
@@ -0,0 +1,9 @@
+#@TYPE: Machine
+#@NAME: corstone1000-mps3 machine
+#@DESCRIPTION: Machine configuration for Corstone1000 64-bit MPS3 FPGA board
+
+require conf/machine/include/corstone1000.inc
+
+TFA_TARGET_PLATFORM = "fpga"
+
+PLATFORM_IS_FVP = "FALSE"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/corstone500.conf b/meta-arm/meta-arm-bsp/conf/machine/corstone500.conf
new file mode 100644
index 0000000..1d25471
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/corstone500.conf
@@ -0,0 +1,45 @@
+#@TYPE: Machine
+#@NAME: Corstone-500 machine
+#@DESCRIPTION: Machine configuration for the Corstone-500 platform
+
+require conf/machine/include/arm/armv7a/tune-cortexa5.inc
+
+# Corstone-500 is built against poky-tiny distro.
+# poky-tiny sets PREFERRED_PROVIDER_virtual/kernel to linux-yocto-tiny.
+# Since distro config is evaluated after the machine config, we need to
+# use the strongest override possible (forcevariable) so the
+# PREFERRED_PROVIDER_virtual/kernel specified in the machine config will
+# apply.
+#
+PREFERRED_PROVIDER_virtual/kernel:forcevariable = "linux-yocto"
+PREFERRED_VERSION_linux-yocto ?= "5.15%"
+
+EXTRA_IMAGEDEPENDS += "trusted-firmware-a u-boot"
+
+IMAGE_CLASSES += "wic_nopt"
+IMAGE_FEATURES += "debug-tweaks"
+IMAGE_FSTYPES:forcevariable = "cpio.gz squashfs wic wic.nopt"
+
+SERIAL_CONSOLES = "115200;ttyAMA0"
+
+# Corstone-500 u-boot configuration
+UBOOT_MACHINE = "corstone500_defconfig"
+UBOOT_IMAGE_ENTRYPOINT = "0x84000000"
+UBOOT_IMAGE_LOADADDRESS = "0x84000000"
+PREFERRED_VERSION_u-boot ?= "2022.04"
+
+# making sure EXTRA_IMAGEDEPENDS will be used while creating the image
+WKS_FILE_DEPENDS:append = " ${EXTRA_IMAGEDEPENDS}"
+
+WKS_FILE ?= "core-image-minimal.corstone500.wks"
+
+TEST_TARGET = "OEFVPSerialTarget"
+TEST_SUITES = "linuxboot"
+
+FVP_PROVIDER ?= "fvp-corstone500-native"
+FVP_EXE ?= "FVP_Corstone-500"
+FVP_CONFIG[board.flashloader0.fname] ?= "${DEPLOY_DIR_IMAGE}/bl1.bin"
+FVP_DATA ?= "css.cluster.cpu0=${DEPLOY_DIR_IMAGE}/${IMAGE_NAME}.rootfs.wic.nopt@0x80000000"
+FVP_CONSOLE ?= "terminal_0"
+FVP_TERMINALS[css.terminal_0] ?= "console"
+FVP_TERMINALS[css.terminal_1] ?= ""
diff --git a/meta-arm/meta-arm-bsp/conf/machine/fvp-base-arm32.conf b/meta-arm/meta-arm-bsp/conf/machine/fvp-base-arm32.conf
new file mode 100644
index 0000000..7343aed
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/fvp-base-arm32.conf
@@ -0,0 +1,23 @@
+# Configuration for Armv7-A Base Platform FVP
+
+#@TYPE: Machine
+#@NAME: Armv7-A Base Platform FVP machine
+#@DESCRIPTION: Machine configuration for Armv7-A Base Platform FVP model
+
+require conf/machine/fvp-common.inc
+require conf/machine/include/arm/arch-armv7a.inc
+
+# FVP u-boot configuration
+PREFERRED_VERSION_u-boot ?= "2022.04"
+UBOOT_MACHINE = "vexpress_aemv8a_aarch32_defconfig"
+
+KERNEL_IMAGETYPE = "zImage"
+
+FVP_CONFIG[cluster0.cpu0.CONFIG64] = "0"
+FVP_CONFIG[cluster0.cpu1.CONFIG64] = "0"
+FVP_CONFIG[cluster0.cpu2.CONFIG64] = "0"
+FVP_CONFIG[cluster0.cpu3.CONFIG64] = "0"
+FVP_CONFIG[cluster1.cpu0.CONFIG64] = "0"
+FVP_CONFIG[cluster1.cpu1.CONFIG64] = "0"
+FVP_CONFIG[cluster1.cpu2.CONFIG64] = "0"
+FVP_CONFIG[cluster1.cpu3.CONFIG64] = "0"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/fvp-base.conf b/meta-arm/meta-arm-bsp/conf/machine/fvp-base.conf
new file mode 100644
index 0000000..434812b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/fvp-base.conf
@@ -0,0 +1,17 @@
+# Configuration for Armv8-A Base Platform FVP
+
+#@TYPE: Machine
+#@NAME: Armv8-A Base Platform FVP machine
+#@DESCRIPTION: Machine configuration for Armv8-A Base Platform FVP model
+
+require conf/machine/fvp-common.inc
+require conf/machine/include/arm/arch-armv8a.inc
+
+TUNE_FEATURES = "aarch64"
+
+PREFERRED_VERSION_u-boot ?= "2022.04"
+
+# FVP u-boot configuration
+UBOOT_MACHINE = "vexpress_aemv8a_semi_defconfig"
+
+KERNEL_IMAGETYPE = "Image"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/fvp-baser-aemv8r64.conf b/meta-arm/meta-arm-bsp/conf/machine/fvp-baser-aemv8r64.conf
new file mode 100644
index 0000000..8119cb6
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/fvp-baser-aemv8r64.conf
@@ -0,0 +1,67 @@
+# Configuration for Fixed Virtual Platform BaseR AEMv8r64 Machine
+
+#@TYPE: Machine
+#@NAME: FVP BaseR AEMv8r64 Machine
+#@DESCRIPTION: Machine configuration for FVP BaseR AEMv8r64
+
+require conf/machine/include/arm/armv8r/arch-armv8r64.inc
+
+EXTRA_IMAGEDEPENDS += "boot-wrapper-aarch64"
+
+PREFERRED_PROVIDER_virtual/kernel ?= "linux-yocto"
+PREFERRED_VERSION_linux-yocto ?= "5.15%"
+PREFERRED_VERSION_linux-yocto-rt ?= "5.15%"
+PREFERRED_VERSION_u-boot ?= "2022.07"
+
+KERNEL_IMAGETYPE = "Image"
+KERNEL_DEVICETREE = "arm/fvp-baser-aemv8r64.dtb"
+
+UBOOT_MACHINE ?= "vexpress_aemv8r_defconfig"
+
+SERIAL_CONSOLES = "115200;ttyAMA0"
+
+IMAGE_CLASSES:append = " fvpboot"
+IMAGE_FSTYPES += "wic"
+WKS_FILE ?= "efi-disk.wks.in"
+EFI_PROVIDER ?= "grub-efi"
+MACHINE_FEATURES:append = " efi"
+
+# As this is a virtual target that will not be used in the real world there is
+# no need for real SSH keys.  Disable rng-tools (which takes too long to
+# initialise) and install the pre-generated keys.
+PACKAGECONFIG:remove:pn-openssh = "rng-tools"
+MACHINE_EXTRA_RRECOMMENDS += "ssh-pregen-hostkeys"
+
+# testimage configuration
+TEST_TARGET = "OEFVPSerialTarget"
+TEST_SUITES = "linuxboot"
+TEST_TARGET_IP ?= "127.0.0.1:8022"
+TEST_SERVER_IP ?= "127.0.1.1"
+
+FVP_EXTRA_ARGS = "-a cluster0*=${DEPLOY_DIR_IMAGE}/linux-system.axf"
+FVP_PROVIDER ?= "fvp-base-r-aem-native"
+FVP_EXE ?= "FVP_BaseR_AEMv8R"
+FVP_CONSOLE ?= "terminal_0"
+
+# FVP parameters
+FVP_CONFIG[bp.exclusive_monitor.monitor_access_level] ?= "2"
+FVP_CONFIG[bp.refcounter.non_arch_start_at_default] ?= "1"
+FVP_CONFIG[bp.refcounter.use_real_time] ?= "1"
+FVP_CONFIG[bp.ve_sysregs.exit_on_shutdown] ?= "1"
+FVP_CONFIG[bp.virtio_net.enabled] ?= "1"
+FVP_CONFIG[bp.virtio_net.hostbridge.userNetPorts] ?= "8022=22"
+FVP_CONFIG[bp.virtio_net.hostbridge.userNetworking] ?= "1"
+FVP_CONFIG[bp.virtio_net.secure_accesses] = "1"
+FVP_CONFIG[bp.virtio_rng.enabled] ?= "1"
+FVP_CONFIG[bp.virtio_rng.secure_accesses] = "1"
+FVP_CONFIG[bp.virtioblockdevice.image_path] ?= "${DEPLOY_DIR_IMAGE}/${IMAGE_NAME}.rootfs.wic"
+FVP_CONFIG[bp.virtioblockdevice.secure_accesses] = "1"
+FVP_CONFIG[cache_state_modelled] ?= "0"
+FVP_CONFIG[cci400.force_on_from_start] = "1"
+FVP_CONFIG[cluster0.gicv3.cpuintf-mmap-access-level] ?= "2"
+FVP_CONFIG[cluster0.gicv3.extended-interrupt-range-support] ?= "1"
+FVP_CONFIG[cluster0.gicv3.SRE-EL2-enable-RAO] ?= "1"
+FVP_CONFIG[cluster0.gicv3.SRE-enable-action-on-mmap] ?= "2"
+FVP_CONFIG[cluster0.has_aarch64] ?= "1"
+FVP_CONFIG[gic_distributor.GICD_CTLR-DS-1-means-secure-only] ?= "1"
+FVP_CONFIG[gic_distributor.has-two-security-states] ?= "0"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/fvp-common.inc b/meta-arm/meta-arm-bsp/conf/machine/fvp-common.inc
new file mode 100644
index 0000000..c834f24
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/fvp-common.inc
@@ -0,0 +1,47 @@
+# FVP common parameters
+
+#
+# Capturing FVP common configurations (Armv8-A Base Platform FVP,
+# Armv8-A Foundation Platform and Armv7-A Base Platform FVP).
+#
+
+MACHINE_FEATURES = "optee"
+
+IMAGE_FSTYPES += "wic"
+WKS_FILE ?= "fvp-base.wks"
+
+SERIAL_CONSOLES = "115200;ttyAMA0"
+
+PREFERRED_PROVIDER_virtual/kernel ?= "linux-yocto"
+
+KERNEL_DEVICETREE = "arm/fvp-base-revc.dtb"
+
+EXTRA_IMAGEDEPENDS += "trusted-firmware-a u-boot"
+
+# As this is a virtual target that will not be used in the real world there is
+# no need for real SSH keys.  Disable rng-tools (which takes too long to
+# initialise) and install the pre-generated keys.
+PACKAGECONFIG:remove:pn-openssh = "rng-tools"
+MACHINE_EXTRA_RRECOMMENDS += "ssh-pregen-hostkeys"
+
+TEST_TARGET = "OEFVPTarget"
+
+FVP_PROVIDER ?= "fvp-base-a-aem-native"
+FVP_EXE ?= "FVP_Base_RevC-2xAEMvA"
+FVP_CONFIG[bp.ve_sysregs.exit_on_shutdown] ?= "1"
+FVP_CONFIG[bp.virtio_net.enabled] ?= "1"
+FVP_CONFIG[bp.virtio_net.hostbridge.userNetworking] ?= "1"
+FVP_CONFIG[cache_state_modelled] ?= "0"
+FVP_CONFIG[bp.secureflashloader.fname] ?= "${DEPLOY_DIR_IMAGE}/bl1-fvp.bin"
+FVP_CONFIG[bp.flashloader0.fname] ?= "${DEPLOY_DIR_IMAGE}/fip-fvp.bin"
+FVP_CONFIG[bp.virtioblockdevice.image_path] ?= "${DEPLOY_DIR_IMAGE}/${IMAGE_NAME}.rootfs.wic"
+# Set the baseline to ARMv8.4, as the default is 8.0.
+FVP_CONFIG[cluster0.has_arm_v8-4] = "1"
+FVP_CONFIG[cluster1.has_arm_v8-4] = "1"
+FVP_CONSOLE ?= "terminal_0"
+FVP_DATA ?= "cluster0.cpu0=${DEPLOY_DIR_IMAGE}/${KERNEL_IMAGETYPE}@0x80080000 \
+             cluster0.cpu0=${DEPLOY_DIR_IMAGE}/fvp-base-revc.dtb@0x8fc00000"
+FVP_TERMINALS[bp.terminal_0] ?= "Console"
+FVP_TERMINALS[bp.terminal_1] ?= ""
+FVP_TERMINALS[bp.terminal_2] ?= ""
+FVP_TERMINALS[bp.terminal_3] ?= ""
diff --git a/meta-arm/meta-arm-bsp/conf/machine/include/arm/armv8r/arch-armv8r64.inc b/meta-arm/meta-arm-bsp/conf/machine/include/arm/armv8r/arch-armv8r64.inc
new file mode 100644
index 0000000..5db12e2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/include/arm/armv8r/arch-armv8r64.inc
@@ -0,0 +1,10 @@
+require conf/machine/include/arm/arch-armv8r.inc
+
+TUNE_FEATURES:tune-armv8r            =+ "aarch64"
+PACKAGE_EXTRA_ARCHS:tune-armv8r      =+ "aarch64"
+BASE_LIB:tune-armv8r                 = "lib64"
+BASE_LIB:tune-armv8r-crc             = "lib64"
+BASE_LIB:tune-armv8r-crypto          = "lib64"
+BASE_LIB:tune-armv8r-simd            = "lib64"
+BASE_LIB:tune-armv8r-crc-simd        = "lib64"
+BASE_LIB:tune-armv8r-crc-crypto-simd = "lib64"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/include/corstone1000.inc b/meta-arm/meta-arm-bsp/conf/machine/include/corstone1000.inc
new file mode 100644
index 0000000..91dbbfd
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/include/corstone1000.inc
@@ -0,0 +1,65 @@
+require conf/machine/include/arm/armv8a/tune-cortexa35.inc
+
+MACHINEOVERRIDES =. "corstone1000:"
+
+# TF-A
+TFA_PLATFORM = "corstone1000"
+EXTRA_IMAGEDEPENDS += "trusted-firmware-a"
+
+TFA_BL2_BINARY = "bl2-corstone1000.bin"
+TFA_FIP_BINARY = "fip-corstone1000.bin"
+
+# TF-M
+EXTRA_IMAGEDEPENDS += "virtual/trusted-firmware-m"
+
+# TF-M settings for signing host images
+TFA_BL2_RE_IMAGE_LOAD_ADDRESS = "0x62353000"
+TFA_BL2_RE_SIGN_BIN_SIZE = "0x2d000"
+TFA_FIP_RE_IMAGE_LOAD_ADDRESS = "0x68130000"
+TFA_FIP_RE_SIGN_BIN_SIZE = "0x00200000"
+RE_LAYOUT_WRAPPER_VERSION = "0.0.7"
+TFM_SIGN_PRIVATE_KEY = "${S}/bl2/ext/mcuboot/root-RSA-3072_1.pem"
+RE_IMAGE_OFFSET = "0x1000"
+
+# u-boot
+PREFERRED_VERSION_u-boot ?= "2022.04"
+EXTRA_IMAGEDEPENDS += "u-boot"
+
+UBOOT_CONFIG ??= "EFI"
+UBOOT_CONFIG[EFI] = "corstone1000_defconfig"
+UBOOT_ENTRYPOINT  = "0x80000000"
+UBOOT_LOADADDRESS = "0x80000000"
+UBOOT_BOOTARGS = "earlycon=pl011,0x1a510000 console=ttyAMA0 loglevel=9"
+UBOOT_ARCH = "arm"
+UBOOT_EXTLINUX = "0"
+
+# optee
+PREFERRED_VERSION_optee-os ?= "3.10.%"
+PREFERRED_VERSION_optee-client ?= "3.14.%"
+EXTRA_IMAGEDEPENDS += "optee-os"
+OPTEE_ARCH = "arm64"
+OPTEE_BINARY = "tee-pager_v2.bin"
+
+# Trusted Services(TS)
+EXTRA_IMAGEDEPENDS += "secure-partitions"
+
+# Linux kernel
+PREFERRED_PROVIDER_virtual/kernel:forcevariable = "linux-yocto"
+PREFERRED_VERSION_linux-yocto = "5.15%"
+KERNEL_IMAGETYPE = "Image"
+
+INITRAMFS_IMAGE_BUNDLE ?= "1"
+
+#telling the build system which image is responsible of the generation of the initramfs rootfs
+INITRAMFS_IMAGE = "corstone1000-initramfs-image"
+
+# enable this feature for kernel debugging
+# MACHINE_FEATURES += "corstone1000_kernel_debug"
+
+# login terminal serial port settings
+SERIAL_CONSOLES ?= "115200;ttyAMA0"
+
+# making sure EXTRA_IMAGEDEPENDS will be used while creating the image
+WKS_FILE_DEPENDS:append = " ${EXTRA_IMAGEDEPENDS}"
+
+WKS_FILE ?= "corstone1000-image.corstone1000.wks"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/include/tc.inc b/meta-arm/meta-arm-bsp/conf/machine/include/tc.inc
new file mode 100644
index 0000000..88eda55
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/include/tc.inc
@@ -0,0 +1,41 @@
+TUNE_FEATURES = "aarch64"
+
+require conf/machine/include/arm/arch-armv8a.inc
+
+MACHINEOVERRIDES =. "tc:"
+
+# Das U-boot
+UBOOT_MACHINE ?= "total_compute_defconfig"
+PREFERRED_VERSION_u-boot ?= "2022.04"
+UBOOT_RD_LOADADDRESS = "0x88000000"
+UBOOT_RD_ENTRYPOINT = "0x88000000"
+UBOOT_LOADADDRESS = "0x80080000"
+UBOOT_ENTRYPOINT = "0x80080000"
+# Below options will generate a key to sign the kernel Image and INITRAMFS_IMAGE
+# according to the default parameters of kernel-fitimage.bbclass. If the user
+# would prefer to use their own keys, disable the key generation using the
+# FIT_GENERATE_KEYS parameter and specify the location of the keys using the
+# below paramters.
+UBOOT_SIGN_ENABLE = "1"
+UBOOT_MKIMAGE_DTCOPTS = "-I dts -O dtb"
+UBOOT_SIGN_KEYNAME = "dev_key"
+UBOOT_SIGN_KEYDIR = "${DEPLOY_DIR_IMAGE}/keys"
+FIT_GENERATE_KEYS = "1"
+
+PREFERRED_PROVIDER_virtual/kernel ?= "linux-arm64-ack"
+PREFERRED_VERSION_linux-arm64-ack ?= "5.10"
+
+# OP-TEE
+PREFERRED_VERSION_optee-os ?= "3.14%"
+PREFERRED_VERSION_optee-client ?= "3.14%"
+PREFERRED_VERSION_optee-test ?= "3.14%"
+
+# Cannot use the default zImage on arm64
+KERNEL_IMAGETYPE = "Image"
+KERNEL_IMAGETYPES += "fitImage"
+KERNEL_CLASSES = " kernel-fitimage "
+
+IMAGE_FSTYPES += "cpio.gz"
+INITRAMFS_IMAGE ?= "core-image-minimal"
+
+SERIAL_CONSOLES = "115200;ttyAMA0"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/juno.conf b/meta-arm/meta-arm-bsp/conf/machine/juno.conf
new file mode 100644
index 0000000..c002ed6
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/juno.conf
@@ -0,0 +1,27 @@
+# Configuration for juno development board
+
+#@TYPE: Machine
+#@NAME: Juno machine
+#@DESCRIPTION: Machine configuration for Juno
+
+TUNE_FEATURES = "aarch64"
+
+require conf/machine/include/arm/arch-armv8a.inc
+
+MACHINE_FEATURES = "usbhost usbgadget alsa screen wifi bluetooth optee pci"
+
+KERNEL_IMAGETYPE = "Image"
+KERNEL_DEVICETREE = "arm/juno.dtb arm/juno-r1.dtb arm/juno-r2.dtb"
+
+IMAGE_FSTYPES += "tar.bz2 ext4"
+
+SERIAL_CONSOLES = "115200;ttyAMA0"
+
+PREFERRED_PROVIDER_virtual/kernel ?= "linux-yocto"
+PREFERRED_VERSION_u-boot ?= "2022.04"
+PREFERRED_PROVIDER_virtual/bootloader ?= "u-boot"
+
+EXTRA_IMAGEDEPENDS += "trusted-firmware-a virtual/bootloader firmware-image-juno"
+
+# Juno u-boot configuration
+UBOOT_MACHINE = "vexpress_aemv8a_juno_defconfig"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/musca-b1.conf b/meta-arm/meta-arm-bsp/conf/machine/musca-b1.conf
new file mode 100644
index 0000000..a7895b8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/musca-b1.conf
@@ -0,0 +1,28 @@
+# Configuration for Musca-B1 development board
+
+#@TYPE: Machine
+#@NAME: Musca-B1 machine
+#@DESCRIPTION: Machine configuration for Musca-B1
+
+DEFAULTTUNE ?= "armv8m-main"
+require conf/machine/include/arm/armv8-m/tune-cortexm33.inc
+
+# GLIBC will not work with Cortex-M.
+TCLIBC = "newlib"
+
+# For runqemu
+IMAGE_FSTYPES += "ext4"
+IMAGE_CLASSES += "qemuboot"
+QB_SYSTEM_NAME = "qemu-system-arm"
+QB_MACHINE = "-machine musca-b1"
+QB_CPU = "-cpu cortex-m33"
+QB_GRAPHICS = "-nographic -vga none"
+QB_MEM = "512k"
+QB_RNG = ""
+
+# Zephyr RTOS settings
+ZEPHYR_BOARD = "v2m_musca_b1"
+ZEPHYR_INHERIT_CLASSES += "zephyr-qemuboot"
+ARCH:musca-b1 = "arm"
+
+TFM_PLATFORM = "arm/musca_b1/sse_200"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/musca-s1.conf b/meta-arm/meta-arm-bsp/conf/machine/musca-s1.conf
new file mode 100644
index 0000000..0cfd56e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/musca-s1.conf
@@ -0,0 +1,27 @@
+# Configuration for Musca-S1 development board
+
+#@TYPE: Machine
+#@NAME: Musca-S1 machine
+#@DESCRIPTION: Machine configuration for Musca-S1
+
+require conf/machine/include/arm/armv8-m/tune-cortexm33.inc
+
+# GLIBC will not work with Cortex-M.
+TCLIBC = "newlib"
+
+# For runqemu
+IMAGE_FSTYPES += "ext4"
+IMAGE_CLASSES += "qemuboot"
+QB_SYSTEM_NAME = "qemu-system-arm"
+QB_MACHINE = "-machine musca-s1"
+QB_CPU = "-cpu cortex-m33"
+QB_GRAPHICS = "-nographic -vga none"
+QB_MEM = "512k"
+QB_RNG = ""
+
+# Zephyr RTOS settings
+ZEPHYR_BOARD = "v2m_musca_s1"
+ZEPHYR_INHERIT_CLASSES += "zephyr-qemuboot"
+ARCH:musca-s1 = "arm"
+
+TFM_PLATFORM = "arm/musca_s1"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/n1sdp.conf b/meta-arm/meta-arm-bsp/conf/machine/n1sdp.conf
new file mode 100644
index 0000000..5e87e61
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/n1sdp.conf
@@ -0,0 +1,38 @@
+# Configuration for Arm N1SDP development board
+
+#@TYPE: Machine
+#@NAME: N1SDP machine
+#@DESCRIPTION: Machine configuration for N1SDP
+
+require conf/machine/include/arm/armv8-2a/tune-neoversen1.inc
+
+KERNEL_IMAGETYPE = "Image"
+
+IMAGE_FSTYPES += "wic wic.gz wic.bmap tar.bz2 ext4"
+
+SERIAL_CONSOLES = "115200;ttyAMA0"
+
+# Set default WKS
+WKS_FILE ?= "n1sdp-efidisk.wks"
+IMAGE_EFI_BOOT_FILES ?= "n1sdp-multi-chip.dtb n1sdp-single-chip.dtb"
+WKS_FILE_DEPENDS:append = " ${EXTRA_IMAGEDEPENDS}"
+
+# Use kernel provided by yocto
+PREFERRED_PROVIDER_virtual/kernel ?= "linux-yocto"
+PREFERRED_VERSION_linux-yocto ?= "5.15%"
+
+# RTL8168E Gigabit Ethernet Controller is attached to the PCIe interface
+MACHINE_ESSENTIAL_EXTRA_RDEPENDS += "linux-firmware-rtl8168"
+
+EXTRA_IMAGEDEPENDS += "trusted-firmware-a"
+EXTRA_IMAGEDEPENDS += "virtual/control-processor-firmware"
+
+#UEFI EDK2 firmware
+EXTRA_IMAGEDEPENDS += "edk2-firmware"
+
+#grub-efi
+EFI_PROVIDER ?= "grub-efi"
+MACHINE_FEATURES += "efi"
+
+# SD-Card firmware
+EXTRA_IMAGEDEPENDS += "sdcard-image-n1sdp"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/qemu-cortex-r5.conf b/meta-arm/meta-arm-bsp/conf/machine/qemu-cortex-r5.conf
new file mode 100644
index 0000000..5a0ea69
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/qemu-cortex-r5.conf
@@ -0,0 +1,22 @@
+#@TYPE: Machine
+#@NAME: qemu_cortex_r5
+#@DESCRIPTION: Machine for Zephyr BOARD qemu_cortex_r5
+
+require conf/machine/include/qemu.inc
+require conf/machine/include/arm/armv7r/tune-cortexr5.inc
+
+# GLIBC will not work with Cortex-R.
+TCLIBC = "newlib"
+
+# For runqemu
+QB_SYSTEM_NAME = "qemu-system-aarch64"
+QB_MACHINE = "-machine xlnx-zcu102"
+QB_CPU = "-cpu cortex-r5"
+QB_MEM = "-m 64k"
+QB_GRAPHICS = "-nographic -vga none"
+QB_RNG = ""
+
+# Zephyr RTOS settings
+ZEPHYR_BOARD = "qemu_cortex_r5"
+ZEPHYR_INHERIT_CLASSES += "zephyr-qemuboot"
+ARCH:qemu-cortex-r5 = "arm"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/sgi575.conf b/meta-arm/meta-arm-bsp/conf/machine/sgi575.conf
new file mode 100644
index 0000000..3c2c94b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/sgi575.conf
@@ -0,0 +1,24 @@
+# Configuration for Arm SGI575 development board
+
+#@TYPE: Machine
+#@NAME: SGI575
+#@DESCRIPTION: Machine configuration for SGI575
+
+require conf/machine/include/arm/armv8-2a/tune-cortexa75.inc
+
+EXTRA_IMAGEDEPENDS += "virtual/control-processor-firmware"
+
+EXTRA_IMAGEDEPENDS += "trusted-firmware-a"
+
+KERNEL_IMAGETYPE ?= "Image"
+PREFERRED_PROVIDER_virtual/kernel ?= "linux-yocto"
+SERIAL_CONSOLES = "115200;ttyAMA0"
+
+#grub-efi
+EFI_PROVIDER ?= "grub-efi"
+MACHINE_FEATURES += "efi"
+
+IMAGE_FSTYPES += "cpio.gz wic"
+
+WKS_FILE ?= "sgi575-efidisk.wks"
+WKS_FILE_DEPENDS:append = " ${EXTRA_IMAGEDEPENDS}"
diff --git a/meta-arm/meta-arm-bsp/conf/machine/tc0.conf b/meta-arm/meta-arm-bsp/conf/machine/tc0.conf
new file mode 100644
index 0000000..b9c762d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/tc0.conf
@@ -0,0 +1,7 @@
+# Configuration for TC0
+
+#@TYPE: Machine
+#@NAME: TC0
+#@DESCRIPTION: Machine configuration for TC0
+
+require conf/machine/include/tc.inc
diff --git a/meta-arm/meta-arm-bsp/conf/machine/tc1.conf b/meta-arm/meta-arm-bsp/conf/machine/tc1.conf
new file mode 100644
index 0000000..42c5d8d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/conf/machine/tc1.conf
@@ -0,0 +1,7 @@
+# Configuration for TC1
+
+#@TYPE: Machine
+#@NAME: TC1
+#@DESCRIPTION: Machine configuration for TC1
+
+require conf/machine/include/tc.inc
diff --git a/meta-arm/meta-arm-bsp/custom-licenses/STM-SLA0044-Rev5 b/meta-arm/meta-arm-bsp/custom-licenses/STM-SLA0044-Rev5
new file mode 100644
index 0000000..edb5cd5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/custom-licenses/STM-SLA0044-Rev5
@@ -0,0 +1,18 @@
+SLA0044 Rev5/February 2018
+
+BY INSTALLING COPYING, DOWNLOADING, ACCESSING OR OTHERWISE USING THIS SOFTWARE OR ANY PART THEREOF (AND THE RELATED DOCUMENTATION) FROM STMICROELECTRONICS INTERNATIONAL N.V, SWISS BRANCH AND/OR ITS AFFILIATED COMPANIES (STMICROELECTRONICS), THE RECIPIENT, ON BEHALF OF HIMSELF OR HERSELF, OR ON BEHALF OF ANY ENTITY BY WHICH SUCH RECIPIENT IS EMPLOYED AND/OR ENGAGED AGREES TO BE BOUND BY THIS SOFTWARE LICENSE AGREEMENT.
+
+Under STMicroelectronics’ intellectual property rights, the redistribution, reproduction and use in source and binary forms of the software or any part thereof, with or without modification, are permitted provided that the following conditions are met:
+1.	Redistribution of source code (modified or not) must retain any copyright notice, this list of conditions and the disclaimer set forth below as items 10 and 11.
+2.	Redistributions in binary form, except as embedded into microcontroller or microprocessor device manufactured by or for STMicroelectronics or a software update for such device, must reproduce any copyright notice provided with the binary code, this list of conditions, and the disclaimer set forth below as items 10 and 11, in documentation and/or other materials provided with the distribution.
+3.	Neither the name of STMicroelectronics nor the names of other contributors to this software may be used to endorse or promote products derived from this software or part thereof without specific written permission.
+4.	This software or any part thereof, including modifications and/or derivative works of this software, must be used and execute solely and exclusively on or in combination with a microcontroller or microprocessor device manufactured by or for STMicroelectronics.
+5.	No use, reproduction or redistribution of this software partially or totally may be done in any manner that would subject this software to any Open Source Terms. “Open Source Terms” shall mean any open source license which requires as part of distribution of software that the source code of such software is distributed therewith or otherwise made available, or open source license that substantially complies with the Open Source definition specified at www.opensource.org and any other comparable open source license such as for example GNU General Public License (GPL), Eclipse Public License (EPL), Apache Software License, BSD license or MIT license.
+6.	STMicroelectronics has no obligation to provide any maintenance, support or updates for the software.
+7.	The software is and will remain the exclusive property of STMicroelectronics and its licensors. The recipient will not take any action that jeopardizes STMicroelectronics and its licensors' proprietary rights or acquire any rights in the software, except the limited rights specified hereunder.
+8.	The recipient shall comply with all applicable laws and regulations affecting the use of the software or any part thereof including any applicable export control law or regulation.
+9.	Redistribution and use of this software or any part thereof other than as permitted under this license is void and will automatically terminate your rights under this license. 
+10.	THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY RIGHTS, WHICH ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+11.	EXCEPT AS EXPRESSLY PERMITTED HEREUNDER, NO LICENSE OR OTHER RIGHTS, WHETHER EXPRESS OR IMPLIED, ARE GRANTED UNDER ANY PATENT OR OTHER INTELLECTUAL PROPERTY RIGHTS OF STMICROELECTRONICS OR ANY THIRD PARTY.
+ 
+
diff --git a/meta-arm/meta-arm-bsp/documentation/corstone500.md b/meta-arm/meta-arm-bsp/documentation/corstone500.md
new file mode 100644
index 0000000..0f01961
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/documentation/corstone500.md
@@ -0,0 +1,28 @@
+# Corstone-500 Platform Support in meta-arm-bsp
+
+## Howto Build and Run
+
+### Configuration:
+
+Use the kas
+
+### Build:
+
+``bash$ kas build kas/corstone500.yml
+
+### Run:
+
+Building using kas should have fetch the Fixed Virtual Platform for this
+platform and installed at:
+
+build/tmp/sysroots-components/x86_64/fvp-corstone500-native/usr/bin/./FVP_Corstone-500
+
+with this in place is possible to launch the FVP using the runfvp inside the
+scripts directory:
+
+cd scripts
+
+./runfvp ../build/tmp/deploy/images/corstone500/core-image-minimal-corstone500.fvpconf --console
+
+this will output the console in the launching terminal
+
diff --git a/meta-arm/meta-arm-bsp/documentation/fvp-base-arm32.md b/meta-arm/meta-arm-bsp/documentation/fvp-base-arm32.md
new file mode 100644
index 0000000..54408b0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/documentation/fvp-base-arm32.md
@@ -0,0 +1,61 @@
+# Armv7-A Base Platform FVP Support in meta-arm-bsp
+
+## How to build and run
+
+### Configuration:
+In the local.conf file, MACHINE should be set as follows:
+MACHINE ?= "fvp-base-arm32"
+
+### Build:
+```bash$ bitbake core-image-minimal```
+
+### Run:
+To Run the Fixed Virtual Platform simulation tool you must download "Armv8-A
+Base Platform FVP" from Arm developer (This might require the user to
+register) from this address:
+https://developer.arm.com/tools-and-software/simulation-models/fixed-virtual-platforms
+and install it on your host PC.
+
+Fast Models Fixed Virtual Platforms (FVP) Reference Guide:
+https://developer.arm.com/docs/100966/latest
+
+Armv8‑A Foundation Platform User Guide:
+https://developer.arm.com/docs/100961/latest/
+
+
+Once done, do the following to build and run an image:
+```bash$ bitbake core-image-minimal```
+```bash$ export YOCTO_DEPLOY_IMGS_DIR="<yocto-build-dir/tmp/deploy/images/fvp-base-arm32>"```
+```bash$ cd <path-to-Base_RevC_AEMv8A_pkg-dir/models/Linux64_GCC-X.X/>```
+```
+bash$ ./FVP_Base_RevC-2xAEMv8A -C bp.virtio_net.enabled=1 \
+         -C cache_state_modelled=0 \
+         -C bp.secureflashloader.fname=${YOCTO_DEPLOY_IMGS_DIR}/bl1-fvp.bin \
+         -C bp.flashloader0.fname=${YOCTO_DEPLOY_IMGS_DIR}/fip-fvp.bin \
+         --data cluster0.cpu0=${YOCTO_DEPLOY_IMGS_DIR}/Image@0x80080000 \
+         -C bp.virtioblockdevice.image_path=${YOCTO_DEPLOY_IMGS_DIR}/core-image-minimal-fvp-base-arm32.wic \
+         -C cluster0.cpu0.CONFIG64=0 \
+         -C cluster0.cpu1.CONFIG64=0 \
+         -C cluster0.cpu2.CONFIG64=0 \
+         -C cluster0.cpu3.CONFIG64=0 \
+         -C cluster1.cpu0.CONFIG64=0 \
+         -C cluster1.cpu1.CONFIG64=0 \
+         -C cluster1.cpu2.CONFIG64=0 \
+         -C cluster1.cpu3.CONFIG64=0 \
+
+```
+
+
+If you have built a configuration without a ramdisk, you can use the following
+command in U-boot to start Linux:
+```fvp32# bootz 0x80080000 - 0x82000000```
+
+## Devices supported in the kernel
+- serial
+- virtio disk
+- network
+- watchdog
+- rtc
+
+## Devices not supported or not functional
+None
diff --git a/meta-arm/meta-arm-bsp/documentation/fvp-base.md b/meta-arm/meta-arm-bsp/documentation/fvp-base.md
new file mode 100644
index 0000000..b0026de
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/documentation/fvp-base.md
@@ -0,0 +1,53 @@
+# Armv8-A Base Platform FVP Support in meta-arm-bsp
+
+## Howto Build and Run
+
+### Configuration:
+In the local.conf file, MACHINE should be set as follow:
+MACHINE ?= "fvp-base"
+
+### Build:
+```bash$ bitbake core-image-minimal```
+
+### Run:
+To Run the Fixed Virtual Platform simulation tool you must download "Armv8-A
+Base Platform FVP" from Arm developer (This might require the user to
+register) from this address:
+https://developer.arm.com/tools-and-software/simulation-models/fixed-virtual-platforms
+and install it on your host PC.
+
+Fast Models Fixed Virtual Platforms (FVP) Reference Guide:
+https://developer.arm.com/docs/100966/latest
+
+Armv8‑A Foundation Platform User Guide:
+https://developer.arm.com/docs/100961/latest/
+
+
+Once done, do the following to build and run an image:
+```bash$ bitbake core-image-minimal```
+```bash$ export YOCTO_DEPLOY_IMGS_DIR="<yocto-build-dir/tmp/deploy/images/fvp-base>"```
+```bash$ cd <path-to-Base_RevC_AEMv8A_pkg-dir/models/Linux64_GCC-X.X/>```
+```
+bash$ ./FVP_Base_RevC-2xAEMv8A -C bp.virtio_net.enabled=1 \
+         -C cache_state_modelled=0 \
+         -C bp.secureflashloader.fname=${YOCTO_DEPLOY_IMGS_DIR}/bl1-fvp.bin \
+         -C bp.flashloader0.fname=${YOCTO_DEPLOY_IMGS_DIR}/fip-fvp.bin \
+         --data cluster0.cpu0=${YOCTO_DEPLOY_IMGS_DIR}/Image@0x80080000 \
+         --data cluster0.cpu0=${YOCTO_DEPLOY_IMGS_DIR}/fvp-base-gicv3-psci-custom.dtb@0x83000000 \
+         -C bp.virtioblockdevice.image_path=${YOCTO_DEPLOY_IMGS_DIR}/core-image-minimal-fvp-base.wic
+```
+
+
+If you have built a configuration without a ramdisk, you can use the following
+command in U-boot to start Linux:
+```VExpress64# booti 0x80080000 - 0x83000000```
+
+## Devices supported in the kernel
+- serial
+- virtio disk
+- network
+- watchdog
+- rtc
+
+## Devices not supported or not functional
+None
diff --git a/meta-arm/meta-arm-bsp/documentation/fvp-baser-aemv8r64.md b/meta-arm/meta-arm-bsp/documentation/fvp-baser-aemv8r64.md
new file mode 100644
index 0000000..6986afd
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/documentation/fvp-baser-aemv8r64.md
@@ -0,0 +1,264 @@
+Armv8-R AArch64 AEM FVP Support in meta-arm-bsp
+===============================================
+
+Overview
+--------
+
+Fixed Virtual Platforms (FVP) are complete simulations of an Arm system,
+including processor, memory and peripherals. These are set out in a
+"programmer's view", which gives you a comprehensive model on which to build
+and test your software.
+
+The Armv8-R AEM FVP is a free of charge Armv8-R Fixed Virtual Platform. It
+supports the latest Armv8-R feature set.
+
+This BSP implements a reference stack for the AArch64 support in the R-class
+first announced with the Cortex-R82 processor:
+https://developer.arm.com/ip-products/processors/cortex-r/cortex-r82
+
+Fast Models Fixed Virtual Platforms (FVP) Reference Guide:
+https://developer.arm.com/docs/100966/latest
+
+
+BSP Support
+-----------
+
+The fvp-baser-aemv8r64 Yocto MACHINE supports the following BSP components,
+where either a standard or Real-Time Linux kernel (PREEMPT\_RT) can be built
+and run:
+
+ - FVP_Base_AEMv8R: v11.18.16
+ - boot-wrapper-aarch64: provides PSCI support
+ - U-Boot: v2022.04 - provides UEFI services
+ - Linux kernel: linux-yocto-5.15
+ - Linux kernel with PREEMPT\_RT support: linux-yocto-rt-5.15
+
+Note that the Real-Time Linux kernel (PREEMPT\_RT) does not use the real-time
+architectural extensions of the Armv8-R feature set.
+
+High-Level Architecture
+-----------------------
+
+The diagram below shows the current boot flow:
+
+    +---------------------------------------------------------------+
+    |                         Linux kernel                          |
+    +---------------------------------------------------------------+
+               /|\                                     /|\
+                |                                       |
+                | UEFI services                         |
+                |                         PSCI services |
+               \|/                                      |
+        +----------------+                              |     S-EL1
+    ----|     U-Boot     |------------------------------|-----------
+        +----------------+                              |     S-EL2
+               /|\                                      |
+                |                                       |
+                |                                       |
+                |                                       |
+    +--------------------------------------------------\|/----------+
+    |                        +----------------+ +----------------+  |
+    |  boot-wrapper-aarch64  |  Device tree   | |  PSCI handler  |  |
+    |                        +----------------+ +----------------+  |
+    +---------------------------------------------------------------+
+
+
+The firmware binary (generated as `linux-system.axf`) includes
+boot-wrapper-aarch64, the flattened device tree and U-Boot. U-Boot is configured
+to automatically detect a virtio block device and boot the UEFI payload at the
+path `/efi/boot/bootaa64.efi`. Using the standard build, the first partition
+contains a Grub image at this path, which boots the Linux kernel at `/Image` on
+the same partition. The second partition of the image contains the Linux root
+file system.
+
+There is no EL3 or non-secure world in the Armv8-R AArch64 architecture, so the
+reset vector starts boot-wrapper-aarch64 at S-EL2. Boot-wrapper-aarch64 is
+compiled with the `--enable-keep-el` flag, which causes it to boot U-Boot at
+S-EL2 too. U-Boot is compiled with the `CONFIG_ARMV8_SWITCH_TO_EL1` flag, which
+causes it to switch to S-EL1 before booting Linux.
+
+The bundled device tree is passed to U-Boot via register x0. U-Boot passes the
+same device tree to Linux via the UEFI system table.
+
+Power state management is provided by PSCI services in boot-wrapper-aarch64.
+Linux accesses the PSCI handler via HVC calls to S-EL2. U-Boot has been patched
+to prevent it from overriding the exception vector at S-EL2. The PSCI handler
+memory region is added to a `/memreserve/` node in the device tree.
+
+Please note that the final firmware architecture for the fvp-baser-aemv8r64 is
+not yet stabilized. The patches in this layer are provided for development and
+evaluation purposes only, and should not be used in production firmware.
+
+Quick start: Howto Build and Run
+--------------------------------
+
+### Host environment setup
+The following instructions have been tested on hosts running Ubuntu 18.04 and
+Ubuntu 20.04.
+Install the required packages for the build host:
+https://docs.yoctoproject.org/singleindex.html#required-packages-for-the-build-host
+
+Kas is a setup tool for bitbake based projects. The minimal supported version
+is 3.0, install it like so:
+
+    pip3 install --user --upgrade kas
+
+For more details on kas, see https://kas.readthedocs.io/.
+
+To build the images for the fvp-baser-aemv8r64 machine, you also need to accept
+the EULA at
+https://developer.arm.com/downloads/-/arm-ecosystem-fvps/eula
+by setting the following environment variable:
+
+    FVP_BASE_R_ARM_EULA_ACCEPT="True"
+
+**Note:** The host machine should have at least 50 GBytes of free disk space
+for the next steps to work correctly.
+
+### Fetch sources
+To fetch and build the ongoing development of the software stack follow the
+instructions on this document.
+
+To fetch and build the version 1 (single core) find instructions at https://community.arm.com/developer/tools-software/oss-platforms/w/docs/633/release-1-single-core
+
+To fetch and build the version 2 (linux smp) find instructions at https://community.arm.com/developer/tools-software/oss-platforms/w/docs/634/release-2---smp
+
+Fetch the meta-arm repository into a build directory:
+
+    mkdir -p ~/fvp-baser-aemv8r64-build
+    cd ~/fvp-baser-aemv8r64-build
+    git clone https://git.yoctoproject.org/git/meta-arm
+
+
+### Build
+Building with the standard Linux kernel:
+
+    cd ~/fvp-baser-aemv8r64-build
+    export FVP_BASE_R_ARM_EULA_ACCEPT="True"
+    kas build meta-arm/kas/fvp-baser-aemv8r64-bsp.yml
+
+Building with the Real-Time Linux kernel (PREEMPT\_RT):
+
+    cd ~/fvp-baser-aemv8r64-build
+    export FVP_BASE_R_ARM_EULA_ACCEPT="True"
+    kas build meta-arm/kas/fvp-baser-aemv8r64-rt-bsp.yml
+
+### Run
+To run an image after the build is done with the standard Linux kernel:
+
+    kas shell --keep-config-unchanged \
+       meta-arm/kas/fvp-baser-aemv8r64-bsp.yml \
+           --command "../layers/meta-arm/scripts/runfvp \
+           --console "
+
+To run an image after the build is done with the Real-Time Linux kernel
+(PREEMPT\_RT):
+
+    kas shell --keep-config-unchanged \
+       meta-arm/kas/fvp-baser-aemv8r64-rt-bsp.yml \
+           --command "../layers/meta-arm/scripts/runfvp \
+           --console "
+
+**Note:** The terminal console login is `root` without password.
+
+To finish the fvp emulation, you need to close the telnet session:
+
+ - Escape to telnet console with ``ctrl+]``.
+ - Run ``quit`` to close the session.
+
+### Networking
+The FVP is configured by default to use "user-mode networking", which simulates
+an IP router and DHCP server to avoid additional host dependencies and
+networking configuration. Outbound connections work automatically, e.g. by
+running:
+
+    wget www.arm.com
+
+Inbound connections require an explicit port mapping from the host. By default,
+port 8022 on the host is mapped to port 22 on the FVP, so that the following
+command will connect to an ssh server running on the FVP:
+
+    ssh root@localhost -p 8022
+
+Note that user-mode networking does not support ICMP, so `ping` will not work.
+For more information about user-mode networking, please see
+https://developer.arm.com/documentation/100964/1117/Introduction-to-Fast-Models/User-mode-networking?lang=en
+
+### File sharing between host and fvp
+It is possible to share a directory between the host machine and the fvp using
+the virtio P9 device component included in the kernel. To do so, create a
+directory to be mounted from the host machine:
+
+    mkdir /path/to/host-mount-dir
+
+Then, add the following parameter containing the path to the directory when
+launching the model:
+
+    --parameter 'bp.virtiop9device.root_path=/path/to/host-mount-dir'
+
+e.g. for the standard Linux kernel:
+
+    kas shell --keep-config-unchanged \
+       meta-arm/kas/fvp-baser-aemv8r64-bsp.yml \
+           --command "../layers/meta-arm/scripts/runfvp \
+           --console -- --parameter \
+           'bp.virtiop9device.root_path=/path/to/host-mount-dir'"
+
+Once you are logged into the fvp, the host directory can be mounted in a
+directory on the model using the following command:
+
+    mount -t 9p -o trans=virtio,version=9p2000.L FM /path/to/fvp-mount-dir
+
+Devices supported in the kernel
+-------------------------------
+
+- serial
+- virtio 9p
+- virtio disk
+- virtio network
+- virtio rng
+- watchdog
+- rtc
+
+Known Issues and Limitations
+----------------------------
+
+- Only PSCI CPU\_ON and CPU\_OFF functions are supported
+- Linux kernel does not support booting from secure EL2 on Armv8-R AArch64
+- Linux KVM does not support Armv8-R AArch64
+- Device DMA memory cache-coherence issue: the FVP `cache_state_modelled`
+  parameter will affect the cache coherence behavior of peripherals’ DMA. When
+  users set `cache_state_modelled=1`, they also have to set
+  `cci400.force_on_from_start=1` to force the FVP to enable snooping on upstream
+  ports.
+
+Change Log
+----------
+- Enabled the ability for U-Boot to apply device tree overlays
+- Fixed bug in U-Boot that caused changes to the `memory` node in the device
+  tree to be ignored.
+- Added boot-wrapper-aarch64 support for booting SMP payloads at S-EL2.
+- Enabled testimage support by default.
+- Added virtio\_rng to improve random number generation.
+- Added U-Boot v2022.01 for UEFI support.
+- Updated Linux kernel version from 5.14 to 5.15 for both standard and
+  Real-Time (PREEMPT\_RT) builds.
+- Updated boot-wrapper-aarch64 revision and added support for booting U-Boot.
+- Included boot-wrapper-aarch64 PSCI services in `/memreserve/` region.
+- Fixed the counter frequency initialization in boot-wrapper-aarch64.
+- Configured the FVP to use the default RAM size of 4 Gb
+- Fixed PL011 and SP805 register sizes in the device tree.
+- Added virtio\_net User Networking mode by default and removed instructions
+  about tap networking setup.
+- Updated Linux kernel version from 5.10 to 5.14 for both standard and
+  Real-Time (PREEMPT\_RT) builds.
+- Enabled SMP support via boot-wrapper-aarch64 providing the PSCI CPU\_ON and
+  CPU\_OFF functions.
+- Introduced Armv8-R64 compiler flags.
+- Added Linux PREEMPT\_RT support via linux-yocto-rt-5.10.
+- Added support for file sharing with the host machine using Virtio P9.
+- Added support for runfvp.
+- Added performance event support (PMU) in the Linux device tree.
+- Introduced the fvp-baser-aemv8r64 machine and its BSP composed of
+  boot-wrapper-aarch64 and linux-yocto-5.10 supporting serial, virtio disk,
+  virtio network, watchdog and rtc.
diff --git a/meta-arm/meta-arm-bsp/documentation/juno.md b/meta-arm/meta-arm-bsp/documentation/juno.md
new file mode 100644
index 0000000..69e56bc
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/documentation/juno.md
@@ -0,0 +1,75 @@
+# Juno Development Platform Support in meta-arm-bsp
+
+## Howto Build and Run
+
+### Configuration:
+In the local.conf file, MACHINE should be set as follow:
+MACHINE ?= "juno"
+
+Juno is using a USB hard drive for root filesystem by default. The distribution
+used must have ```usbhost``` and ```usbgadget``` in DISTRO_FEATURES (this is
+the case in poky distribution).
+
+### Build:
+```bash$ bitbake core-image-minimal```
+
+### Update Juno SD card:
+
+The SD card content is generated during the build here:
+tmp/deploy/images/juno/firmware-image-juno.tar.gz
+
+Its content must be written on the Juno firmware SD card.
+To do this:
+- insert the sdcard of the Juno in an SD card reader and mount it:
+```bash$ sudo mount /dev/sdx1 /mnt```
+(replace sdx by the device of the SD card)
+
+- erase its content and put the new one:
+```bash$ sudo rm -rf /mnt/*```
+```bash$ sudo tar --no-same-owner -xzf tmp/deploy/images/juno/firmware-image-juno.tar.gz -C /mnt/```
+```bash$ sudo umount /mnt```
+
+- reinsert the SD card in the Juno board
+
+### Create an USB hard drive:
+
+Linux root file system should be stored on the second partition of an USB
+drive that must be plugged on the Juno Platform.
+
+This partition should be initialized with the content of the filesystem
+generated by yocto that you can find here:
+tmp/deploy/images/juno/core-image-minimal-juno.tar.bz2
+
+To do this
+- Format a USB disk, create two primary partitions (ext4).
+- mount the secondary partition
+- untar tmp/deploy/images/juno/core-image-minimal-juno.tar.bz2 on to the
+  secondary partition.
+
+### Run:
+You must insert the SD card and the USB drive and power-on the Juno board.
+The console should be available on the second serial line:
+screen -L /dev/tty.usbserial 115200
+
+On the first boot the images will be flashed which can take some time.
+
+## Devices supported in the kernel
+- serial
+- usb
+- network
+- watchdog
+- rtc
+- mmc
+
+### Untested:
+- i2c
+- dma
+- pci
+- sata
+- sound
+
+## Devices not supported or not functional
+- framebuffer: not functional
+    The HDMI is not properly detected.
+- GPU (no user land libraries).
+    The mali-midgard-kernel can be used to have a kernel driver
diff --git a/meta-arm/meta-arm-bsp/documentation/musca-b1.md b/meta-arm/meta-arm-bsp/documentation/musca-b1.md
new file mode 100644
index 0000000..a9af03f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/documentation/musca-b1.md
@@ -0,0 +1,26 @@
+# Musca B1
+
+## Overview
+For a description of the hardware, go to
+https://developer.arm.com/tools-and-software/development-boards/iot-test-chips-and-boards/musca-b-test-chip-board
+
+For current supported hardware by Zephyr, go to
+https://docs.zephyrproject.org/2.3.0/boards/arm/v2m_musca/doc/index.html
+
+For emulated hardware, go to
+https://www.qemu.org/docs/master/system/arm/musca.html
+
+## Building
+In the local.conf file, MACHINE should be set as follows:
+MACHINE ?= "musca-b1"
+
+To build for Zephyr:
+```bash$ bitbake-layers layerindex-fetch meta-zephyr```
+```bash$ bitbake zephyr-philosophers```
+
+To build the trusted firmware-m (and not Zephyr):
+```bash$ bitbake trusted-firmware-m```
+
+## Running
+To run Zephyr on the QEMU based machine, execute the following command
+```bash$ runqemu qemu-musca-b1```
diff --git a/meta-arm/meta-arm-bsp/documentation/n1sdp.md b/meta-arm/meta-arm-bsp/documentation/n1sdp.md
new file mode 100644
index 0000000..d8661a5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/documentation/n1sdp.md
@@ -0,0 +1,78 @@
+# N1SDP Development Platform Support in meta-arm-bsp
+
+## Overview
+The N1SDP provides access to the Arm Neoverse N1 SoC. The N1SDP enables software development for key enterprise technology
+and general Arm software development. The N1SDP consists of the N1 board containing the N1 SoC.
+The N1 SoC contains two dual-core Arm Neoverse N1 processor clusters.
+
+The system demonstrates Arm technology in the context of Cache-Coherent Interconnect for Accelerators (CCIX) protocol by:
+
+- Running coherent traffic between the N1 SoC and an accelerator card.
+- Coherent communication between two N1 SoCs.
+- Enabling development of CCIX-enabled FPGA accelerators.
+
+Further information on N1SDP can be found at
+https://community.arm.com/developer/tools-software/oss-platforms/w/docs/458/neoverse-n1-sdp
+
+## Configuration:
+In the local.conf file, MACHINE should be set as follow:
+MACHINE ?= "n1sdp"
+
+## Building
+```bash$ bitbake core-image-minimal```
+
+## Running
+
+# Update Firmware on SD card:
+
+(*) To use n1sdp board in single chip mode, flash:
+    n1sdp-board-firmware_primary.tar.gz firmware.
+
+(*) To use n1sdp board in multi chip mode, flash:
+    n1sdp-board-firmware_primary.tar.gz firmware to primary board,
+    n1sdp-board-firmware_secondary.tar.gz firmware to secondary board.
+
+The SD card content is generated during the build here:
+  tmp/deploy/images/n1sdp/n1sdp-board-firmware_primary.tar.gz
+  tmp/deploy/images/n1sdp/n1sdp-board-firmware_secondary.tar.gz
+
+
+Its content must be written on the N1SDP firmware SD card.
+To do this:
+- insert the sdcard of the N1SDP in an SD card reader and mount it:
+```bash$ sudo mount /dev/sdx1 /mnt```
+(replace sdx by the device of the SD card)
+
+- erase its content and put the new one:
+```bash$ sudo rm -rf /mnt/*```
+```bash$ sudo tar --no-same-owner -xzf tmp/deploy/images/n1sdp/n1sdp-board-firmware_primary.tar.gz -C /mnt/```
+```bash$ sudo umount /mnt```
+
+- reinsert the SD card in the N1SDP board
+
+Firmware tarball contains iofpga configuration files, scp and uefi binaries.
+
+**NOTE**:
+If the N1SDP board was manufactured after November 2019 (Serial Number greater
+than 36253xxx), a different PMIC firmware image must be used to prevent
+potential damage to the board. More details can be found in [1].
+The `MB/HBI0316A/io_v123f.txt` file located in the microSD needs to be updated.
+To update it, set the PMIC image (300k_8c2.bin) to be used in the newer models
+by running the following commands on your host PC:
+
+    $ sudo umount /dev/sdx1
+    $ sudo mount /dev/sdx1 /mnt
+    $ sudo sed -i '/^MBPMIC: pms_0V85.bin/s/^/;/g' /mnt/MB/HBI0316A/io_v123f.txt
+    $ sudo sed -i '/^;MBPMIC: 300k_8c2.bin/s/^;//g' /mnt/MB/HBI0316A/io_v123f.txt
+    $ sudo umount /mnt
+
+# Prepare an USB hard drive:
+
+Grub boot partition is placed on first partition of the *.wic image,
+Linux root file system is placed on the second partition of the *.wic image:
+  tmp/deploy/images/n1sdp/core-image-minimal-n1sdp.wic
+
+This *.wic image should be copied to USB stick with simple dd call.
+
+
+[1]: https://community.arm.com/developer/tools-software/oss-platforms/w/docs/604/notice-potential-damage-to-n1sdp-boards-if-using-latest-firmware-release
diff --git a/meta-arm/meta-arm-bsp/documentation/tc0.md b/meta-arm/meta-arm-bsp/documentation/tc0.md
new file mode 100644
index 0000000..2ae2592
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/documentation/tc0.md
@@ -0,0 +1,32 @@
+# TC0 Platform Support in meta-arm-bsp
+
+## Overview
+The Total Compute platform provides an envelope for all of Arm's latest IP and
+software solutions, optimised to work together. Further information can be
+found on the Total Compute community page:
+https://community.arm.com/developer/tools-software/oss-platforms/w/docs/606/total-compute
+
+The user guide for TC0 platform with detailed instructions for
+syncing and building the source code and running on TC0 Fixed Virtual Platform
+for poky and android distributions is available at:
+https://git.linaro.org/landing-teams/working/arm/arm-reference-platforms.git/tree/docs/tc0/user-guide.rst
+
+## Building
+In the local.conf file, MACHINE should be set as follows:
+MACHINE = "tc0"
+
+To build the required binaries for tc0, run the commmand:
+```bash$ bitbake tc-artifacts-image```
+
+Trusted-firmware-a is the final component to be built with the rest of the
+components dependent of it, therefore building tc-artifacts-image which depends
+on trusted-firmware-a will build all the required binaries.
+
+## Running
+To run the produced binaries in a TC0 Fixed Virtual Platform please get
+the run scripts at:
+https://git.linaro.org/landing-teams/working/arm/model-scripts.git/
+
+and follow the instructions in the user-guide.rst available in:
+https://git.linaro.org/landing-teams/working/arm/arm-reference-platforms.git/tree/docs/tc0/user-guide.rst
+
diff --git a/meta-arm/meta-arm-bsp/documentation/tc1.md b/meta-arm/meta-arm-bsp/documentation/tc1.md
new file mode 100644
index 0000000..464f77e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/documentation/tc1.md
@@ -0,0 +1,32 @@
+# TC1 Platform Support in meta-arm-bsp
+
+## Overview
+The Total Compute platform provides an envelope for all of Arm's latest IP and
+software solutions, optimised to work together. Further information can be
+found on the Total Compute community page:
+https://community.arm.com/developer/tools-software/oss-platforms/w/docs/606/total-compute
+
+The user guide for TC1 platform with detailed instructions for
+syncing and building the source code and running on TC1 Fixed Virtual Platform
+for poky and android distributions is available at:
+https://git.linaro.org/landing-teams/working/arm/arm-reference-platforms.git/tree/docs/tc1/user-guide.rst
+
+## Building
+In the local.conf file, MACHINE should be set as follows:
+MACHINE = "tc1"
+
+To build the required binaries for tc1, run the commmand:
+```bash$ bitbake tc-artifacts-image```
+
+Trusted-firmware-a is the final component to be built with the rest of the
+components dependent of it, therefore building tc-artifacts-image which depends
+on trusted-firmware-a will build all the required binaries.
+
+## Running
+To run the produced binaries in a TC1 Fixed Virtual Platform please get
+the run scripts at:
+https://git.linaro.org/landing-teams/working/arm/model-scripts.git/
+
+and follow the instructions in the user-guide.rst available in:
+https://git.linaro.org/landing-teams/working/arm/arm-reference-platforms.git/tree/docs/tc1/user-guide.rst
+
diff --git a/meta-arm/meta-arm-bsp/documentation/template.md b/meta-arm/meta-arm-bsp/documentation/template.md
new file mode 100644
index 0000000..7beeb59
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/documentation/template.md
@@ -0,0 +1,19 @@
+# *Hardware Name*
+
+## Overview
+
+*Brief summary of the hardware*
+
+*Link to reference documentation*
+
+## Building
+
+*Any special steps required to build successfully beyond setting MACHINE*
+
+*For example: corstone700 needs DISTRO=poky-tiny, musca only supports TF-M*
+
+## Running
+
+*A summary of how to deploy or execute the image*
+
+*For example, an overview of the N1SDP SD structure, or FVP arguments*
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64-fvp-baser-aemv8r64.inc b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64-fvp-baser-aemv8r64.inc
new file mode 100644
index 0000000..8ffa0aa
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64-fvp-baser-aemv8r64.inc
@@ -0,0 +1,36 @@
+COMPATIBLE_MACHINE = "fvp-baser-aemv8r64"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/${MACHINE}:"
+SRC_URI:append = " \
+    file://0001-aarch64-Rename-labels-and-prepare-for-lower-EL-booti.patch \
+    file://0002-aarch64-Prepare-for-EL1-booting.patch \
+    file://0003-aarch64-Prepare-for-lower-EL-booting.patch \
+    file://0004-gic-v3-Prepare-for-gicv3-with-EL2.patch \
+    file://0005-aarch64-Prepare-for-booting-with-EL2.patch \
+    file://0006-aarch64-Introduce-EL2-boot-code-for-Armv8-R-AArch64.patch \
+    file://0007-Allow-enable-psci-to-choose-between-smc-and-hvc.patch \
+    file://0008-aarch64-Disable-CNTPCT_EL0-trap-for-v8-R64.patch \
+    file://0009-lds-Mark-the-mem-range.patch \
+    file://0010-common-Introduce-the-libfdt.patch \
+    file://0011-common-Add-essential-libc-functions.patch \
+    file://0012-Makefile-Add-the-libfdt-to-the-Makefile-system.patch \
+    file://0013-platform-Add-print_hex-func.patch \
+    file://0014-common-Add-mem-usage-to-memreserve.patch \
+    file://0015-boot-Add-the-enable-keep-el-compile-option.patch \
+    file://0016-Makefile-Change-COUNTER_FREQ-to-100-MHz.patch \
+    file://0017-PSCI-Apply-flush-cache-after-setting-branch_data.patch \
+    file://0018-PSCI-Add-function-call-entry-point.patch \
+    file://0019-lds-Rearrange-and-mark-the-sections.patch \
+    file://0020-common-Provide-firmware-info-using-libfdt.patch \
+    file://0021-boot-Enable-firmware-node-initialization.patch \
+    "
+
+BOOT_WRAPPER_AARCH64_CMDLINE = "\
+earlycon console=ttyAMA0 loglevel=8 rootfstype=ext4 root=/dev/vda1 rw"
+
+EXTRA_OECONF += "--enable-psci=hvc --enable-keep-el"
+
+TUNE_CCARGS = ""
+
+BOOT_WRAPPER_AARCH64_KERNEL = "u-boot.bin"
+do_deploy[depends] += "u-boot:do_deploy"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_%.bbappend
new file mode 100644
index 0000000..c2e7e6e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_%.bbappend
@@ -0,0 +1,4 @@
+MACHINE_BOOT_WRAPPER_AARCH64_REQUIRE ?= ""
+MACHINE_BOOT_WRAPPER_AARCH64_REQUIRE:fvp-baser-aemv8r64 ?= "boot-wrapper-aarch64-fvp-baser-aemv8r64.inc"
+
+require ${MACHINE_BOOT_WRAPPER_AARCH64_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0001-aarch64-Rename-labels-and-prepare-for-lower-EL-booti.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0001-aarch64-Rename-labels-and-prepare-for-lower-EL-booti.patch
new file mode 100644
index 0000000..566070a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0001-aarch64-Rename-labels-and-prepare-for-lower-EL-booti.patch
@@ -0,0 +1,135 @@
+From 3e7cfbe39a2a053d2a6b0d928cc172ed9d1c6da8 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] aarch64: Rename labels and prepare for lower EL booting
+
+Prepare for booting from lower EL. Rename *_el3 relavant labels with
+*_el_max and *_no_el3 with *_keep_el. Since the original _no_el3 means
+"We neither do init sequence at this highest EL nor drop to lower EL
+when entering to kernel", we rename it with _keep_el to make it more
+clear for lower EL initialisation.
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+---
+ arch/aarch64/boot.S | 28 ++++++++++++++++++++--------
+ arch/aarch64/psci.S |  9 +++++----
+ arch/aarch64/spin.S |  4 ++--
+ 3 files changed, 27 insertions(+), 14 deletions(-)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 27ba449..84e1646 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -21,18 +21,30 @@ ASM_FUNC(_start)
+ 
+ 	/*
+ 	 * EL3 initialisation
++	 * Boot sequence
++	 * If CurrentEL == EL3, then goto EL3 initialisation and drop to
++	 *   lower EL before entering the kernel.
++	 * Else, no initialisation and keep the current EL before
++	 *   entering the kernel.
+ 	 */
+ 	mrs	x0, CurrentEL
+ 	cmp	x0, #CURRENTEL_EL3
+-	b.eq	1f
++	b.eq	el3_init
+ 
++	/*
++	 * We stay in the current EL for entering the kernel
++	 */
+ 	mov	w0, #1
+-	ldr	x1, =flag_no_el3
++	ldr	x1, =flag_keep_el
+ 	str	w0, [x1]
+ 
+-	b	start_no_el3
++	b	start_keep_el
+ 
+-1:	mov	x0, #0x30			// RES1
++	/*
++	 * EL3 initialisation
++	 */
++el3_init:
++	mov	x0, #0x30			// RES1
+ 	orr	x0, x0, #(1 << 0)		// Non-secure EL1
+ 	orr	x0, x0, #(1 << 8)		// HVC enable
+ 
+@@ -124,7 +136,7 @@ ASM_FUNC(_start)
+ 
+ 	bl	gic_secure_init
+ 
+-	b	start_el3
++	b	start_el_max
+ 
+ err_invalid_id:
+ 	b	.
+@@ -151,7 +163,7 @@ ASM_FUNC(jump_kernel)
+ 	bl	find_logical_id
+ 	bl	setup_stack		// Reset stack pointer
+ 
+-	ldr	w0, flag_no_el3
++	ldr	w0, flag_keep_el
+ 	cmp	w0, #0			// Prepare Z flag
+ 
+ 	mov	x0, x20
+@@ -160,7 +172,7 @@ ASM_FUNC(jump_kernel)
+ 	mov	x3, x23
+ 
+ 	b.eq	1f
+-	br	x19			// No EL3
++	br	x19			// Keep current EL
+ 
+ 1:	mov	x4, #SPSR_KERNEL
+ 
+@@ -178,5 +190,5 @@ ASM_FUNC(jump_kernel)
+ 
+ 	.data
+ 	.align 3
+-flag_no_el3:
++flag_keep_el:
+ 	.long 0
+diff --git a/arch/aarch64/psci.S b/arch/aarch64/psci.S
+index 8bd224b..7b8919a 100644
+--- a/arch/aarch64/psci.S
++++ b/arch/aarch64/psci.S
+@@ -79,7 +79,7 @@ smc_exit:
+ 	ldp	x18, x19, [sp], #16
+ 	eret
+ 
+-ASM_FUNC(start_el3)
++ASM_FUNC(start_el_max)
+ 	ldr	x0, =vector
+ 	bl	setup_vector
+ 
+@@ -89,10 +89,11 @@ ASM_FUNC(start_el3)
+ 	b	psci_first_spin
+ 
+ /*
+- * This PSCI implementation requires EL3. Without EL3 we'll only boot the
+- * primary cpu, all others will be trapped in an infinite loop.
++ * This PSCI implementation requires the highest EL(EL3 or Armv8-R EL2).
++ * Without the highest EL, we'll only boot the primary cpu, all othersr
++ * will be trapped in an infinite loop.
+  */
+-ASM_FUNC(start_no_el3)
++ASM_FUNC(start_keep_el)
+ 	cpuid	x0, x1
+ 	bl	find_logical_id
+ 	cbz	x0, psci_first_spin
+diff --git a/arch/aarch64/spin.S b/arch/aarch64/spin.S
+index 1ea1c0b..bfb1d47 100644
+--- a/arch/aarch64/spin.S
++++ b/arch/aarch64/spin.S
+@@ -12,8 +12,8 @@
+ 
+ 	.text
+ 
+-ASM_FUNC(start_el3)
+-ASM_FUNC(start_no_el3)
++ASM_FUNC(start_el_max)
++ASM_FUNC(start_keep_el)
+ 	cpuid	x0, x1
+ 	bl	find_logical_id
+ 
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0002-aarch64-Prepare-for-EL1-booting.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0002-aarch64-Prepare-for-EL1-booting.patch
new file mode 100644
index 0000000..46447b8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0002-aarch64-Prepare-for-EL1-booting.patch
@@ -0,0 +1,48 @@
+From 26f9b5354c2de9cc052531096ff92b04c3a3846f Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] aarch64: Prepare for EL1 booting
+
+When booting from EL1, add a check and skip the init of
+sctlr_el2 in jump_kernel
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Reviewed-by: Andre Przywara <andre.przywara@arm.com>
+---
+ arch/aarch64/boot.S            | 6 +++++-
+ arch/aarch64/include/asm/cpu.h | 1 +
+ 2 files changed, 6 insertions(+), 1 deletion(-)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 84e1646..b589744 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -156,10 +156,14 @@ ASM_FUNC(jump_kernel)
+ 	ldr	x0, =SCTLR_EL1_KERNEL
+ 	msr	sctlr_el1, x0
+ 
++	mrs	x0, CurrentEL
++	cmp	x0, #CURRENTEL_EL2
++	b.lt	1f
++
+ 	ldr	x0, =SCTLR_EL2_KERNEL
+ 	msr	sctlr_el2, x0
+ 
+-	cpuid	x0, x1
++1:	cpuid	x0, x1
+ 	bl	find_logical_id
+ 	bl	setup_stack		// Reset stack pointer
+ 
+diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
+index 63eb1c3..b1003f4 100644
+--- a/arch/aarch64/include/asm/cpu.h
++++ b/arch/aarch64/include/asm/cpu.h
+@@ -11,6 +11,7 @@
+ 
+ #define MPIDR_ID_BITS		0xff00ffffff
+ 
++#define CURRENTEL_EL2		(2 << 2)
+ #define CURRENTEL_EL3		(3 << 2)
+ 
+ /*
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0003-aarch64-Prepare-for-lower-EL-booting.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0003-aarch64-Prepare-for-lower-EL-booting.patch
new file mode 100644
index 0000000..db81355
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0003-aarch64-Prepare-for-lower-EL-booting.patch
@@ -0,0 +1,55 @@
+From ce628de7699dd6401ddf713efaa49872e2733619 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] aarch64: Prepare for lower EL booting
+
+Save SPSR_KERNEL into spsr_to_elx during el3_init.
+The jump_kernel will load spsr_to_elx into spsr_el3.
+
+This change will make it easier to control whether drop to lower EL
+before jumping to the kernel.
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Reviewed-by: Andre Przywara <andre.przywara@arm.com>
+---
+ arch/aarch64/boot.S | 15 +++++++++++++--
+ 1 file changed, 13 insertions(+), 2 deletions(-)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index b589744..6b45afc 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -130,7 +130,16 @@ el3_init:
+ 	mov	x0, #ZCR_EL3_LEN_MASK		// SVE: Enable full vector len
+ 	msr	ZCR_EL3, x0			// for EL2.
+ 
+-1:
++	/*
++	 * Save SPSR_KERNEL into spsr_to_elx.
++	 * The jump_kernel will load spsr_to_elx into spsr_el3
++	 */
++1:	mov	w0, #SPSR_KERNEL
++	ldr	x1, =spsr_to_elx
++	str	w0, [x1]
++	b	el_max_init
++
++el_max_init:
+ 	ldr	x0, =COUNTER_FREQ
+ 	msr	cntfrq_el0, x0
+ 
+@@ -178,7 +187,7 @@ ASM_FUNC(jump_kernel)
+ 	b.eq	1f
+ 	br	x19			// Keep current EL
+ 
+-1:	mov	x4, #SPSR_KERNEL
++1:	ldr	w4, spsr_to_elx
+ 
+ 	/*
+ 	 * If bit 0 of the kernel address is set, we're entering in AArch32
+@@ -196,3 +205,5 @@ ASM_FUNC(jump_kernel)
+ 	.align 3
+ flag_keep_el:
+ 	.long 0
++spsr_to_elx:
++	.long 0
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0004-gic-v3-Prepare-for-gicv3-with-EL2.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0004-gic-v3-Prepare-for-gicv3-with-EL2.patch
new file mode 100644
index 0000000..e10182e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0004-gic-v3-Prepare-for-gicv3-with-EL2.patch
@@ -0,0 +1,105 @@
+From 483d363bf825082b6db6de3c57d169e741861891 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] gic-v3: Prepare for gicv3 with EL2
+
+This is a preparation for allowing boot-wrapper configuring the gicv3
+with EL2.
+
+When confiuring with EL2, since there is no ICC_CTLR_EL2, the
+ICC_CTLR_EL3 cannot be replaced with ICC_CTLR_EL2 simply.
+See [https://developer.arm.com/documentation/ihi0069/latest/].
+
+As the caller, gic_secure_init expects the ICC_CTLR to be written,
+we change the function into gic_init_icc_ctlr(). In the GIC spec,
+the r/w bits in this register ([6:0]) either affect EL3 IRQ routing
+(not applicable since no EL3), non-secure IRQ handling (not applicable
+since only secure state in Armv8-R aarch64), or are aliased to
+ICC_CTLR_EL1 bits.
+So, based on this, the new gic_init_icc_ctlr() would be:
+When currentEL is EL3, init ICC_CTLR_EL3 as before.
+When currentEL is not EL3, init ICC_CTLR_EL1 with ICC_CTLR_EL1_RESET.
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Reviewed-by: Andre Przywara <andre.przywara@arm.com>
+---
+ arch/aarch32/include/asm/gic-v3.h |  7 +++++++
+ arch/aarch64/include/asm/gic-v3.h | 23 ++++++++++++++++++++---
+ common/gic-v3.c                   |  2 +-
+ 3 files changed, 28 insertions(+), 4 deletions(-)
+
+diff --git a/arch/aarch32/include/asm/gic-v3.h b/arch/aarch32/include/asm/gic-v3.h
+index 65f38de..11e7bc7 100644
+--- a/arch/aarch32/include/asm/gic-v3.h
++++ b/arch/aarch32/include/asm/gic-v3.h
+@@ -9,6 +9,8 @@
+ #ifndef __ASM_AARCH32_GICV3_H
+ #define __ASM_AARCH32_GICV3_H
+ 
++#define ICC_CTLR_RESET (0UL)
++
+ static inline void gic_write_icc_sre(uint32_t val)
+ {
+ 	asm volatile ("mcr p15, 6, %0, c12, c12, 5" : : "r" (val));
+@@ -19,4 +21,9 @@ static inline void gic_write_icc_ctlr(uint32_t val)
+ 	asm volatile ("mcr p15, 6, %0, c12, c12, 4" : : "r" (val));
+ }
+ 
++static inline void gic_init_icc_ctlr()
++{
++	gic_write_icc_ctlr(ICC_CTLR_RESET);
++}
++
+ #endif
+diff --git a/arch/aarch64/include/asm/gic-v3.h b/arch/aarch64/include/asm/gic-v3.h
+index 5b32380..090ab0b 100644
+--- a/arch/aarch64/include/asm/gic-v3.h
++++ b/arch/aarch64/include/asm/gic-v3.h
+@@ -15,14 +15,31 @@
+ #define ICC_CTLR_EL3	"S3_6_C12_C12_4"
+ #define ICC_PMR_EL1	"S3_0_C4_C6_0"
+ 
++#define ICC_CTLR_EL3_RESET     (0UL)
++#define ICC_CTLR_EL1_RESET     (0UL)
++
++static inline uint32_t current_el(void)
++{
++	uint32_t val;
++
++	asm volatile ("mrs %0, CurrentEL" : "=r" (val));
++	return val;
++}
++
+ static inline void gic_write_icc_sre(uint32_t val)
+ {
+-	asm volatile ("msr " ICC_SRE_EL3 ", %0" : : "r" (val));
++	if (current_el() == CURRENTEL_EL3)
++		asm volatile ("msr " ICC_SRE_EL3 ", %0" : : "r" (val));
++	else
++		asm volatile ("msr " ICC_SRE_EL2 ", %0" : : "r" (val));
+ }
+ 
+-static inline void gic_write_icc_ctlr(uint32_t val)
++static inline void gic_init_icc_ctlr()
+ {
+-	asm volatile ("msr " ICC_CTLR_EL3 ", %0" : : "r" (val));
++	if (current_el() == CURRENTEL_EL3)
++		asm volatile ("msr " ICC_CTLR_EL3 ", %0" : : "r" (ICC_CTLR_EL3_RESET));
++	else
++		asm volatile ("msr " ICC_CTLR_EL1 ", %0" : : "r" (ICC_CTLR_EL1_RESET));
+ }
+ 
+ #endif
+diff --git a/common/gic-v3.c b/common/gic-v3.c
+index 6207007..a0fe564 100644
+--- a/common/gic-v3.c
++++ b/common/gic-v3.c
+@@ -117,6 +117,6 @@ void gic_secure_init(void)
+ 	gic_write_icc_sre(ICC_SRE_Enable | ICC_SRE_DIB | ICC_SRE_DFB | ICC_SRE_SRE);
+ 	isb();
+ 
+-	gic_write_icc_ctlr(0);
++	gic_init_icc_ctlr();
+ 	isb();
+ }
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0005-aarch64-Prepare-for-booting-with-EL2.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0005-aarch64-Prepare-for-booting-with-EL2.patch
new file mode 100644
index 0000000..3b6f78a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0005-aarch64-Prepare-for-booting-with-EL2.patch
@@ -0,0 +1,63 @@
+From be814863cdd5f61d9a16eec012d500550053c8c6 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] aarch64: Prepare for booting with EL2
+
+Prepare for allowing boot-wrapper to be entered in EL2.
+Detect current EL and set the corresponding EL registers.
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Reviewed-by: Andre Przywara <andre.przywara@arm.com>
+---
+ arch/aarch64/boot.S  |  8 ++++++++
+ arch/aarch64/utils.S | 10 +++++++++-
+ 2 files changed, 17 insertions(+), 1 deletion(-)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 6b45afc..908764a 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -195,10 +195,18 @@ ASM_FUNC(jump_kernel)
+ 	 */
+ 	bfi	x4, x19, #5, #1
+ 
++	mrs	x5, CurrentEL
++	cmp	x5, #CURRENTEL_EL2
++	b.eq	1f
++
+ 	msr	elr_el3, x19
+ 	msr	spsr_el3, x4
+ 	eret
+ 
++1:	msr	elr_el2, x19
++	msr	spsr_el2, x4
++	eret
++
+ 	.ltorg
+ 
+ 	.data
+diff --git a/arch/aarch64/utils.S b/arch/aarch64/utils.S
+index 85c7f8a..f02a249 100644
+--- a/arch/aarch64/utils.S
++++ b/arch/aarch64/utils.S
+@@ -34,10 +34,18 @@ ASM_FUNC(find_logical_id)
+ 	ret
+ 
+ /*
+- * Setup EL3 vectors
++ * Setup EL3/EL2 vectors
+  * x0: vector address
+  */
+ ASM_FUNC(setup_vector)
++	mrs	x1, CurrentEL
++	cmp	x1, #CURRENTEL_EL2
++	b.eq    1f
++
+ 	msr	VBAR_EL3, x0
+ 	isb
+ 	ret
++
++1:	msr	VBAR_EL2, x0
++	isb
++	ret
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0006-aarch64-Introduce-EL2-boot-code-for-Armv8-R-AArch64.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0006-aarch64-Introduce-EL2-boot-code-for-Armv8-R-AArch64.patch
new file mode 100644
index 0000000..aaacc72
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0006-aarch64-Introduce-EL2-boot-code-for-Armv8-R-AArch64.patch
@@ -0,0 +1,182 @@
+From 81df76f8d94cb6c31c01739b078a72bdb8497441 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 May 2021 07:25:00 +0100
+Subject: [PATCH] aarch64: Introduce EL2 boot code for Armv8-R AArch64
+
+The Armv8-R AArch64 profile does not support the EL3 exception level.
+The Armv8-R AArch64 profile allows for an (optional) VMSAv8-64 MMU
+at EL1, which allows to run off-the-shelf Linux. However EL2 only
+supports a PMSA, which is not supported by Linux, so we need to drop
+into EL1 before entering the kernel.
+
+We add a new err_invalid_arch symbol as a dead loop. If we detect the
+current Armv8-R aarch64 only supports with PMSA, meaning we cannot boot
+Linux anymore, then we jump to err_invalid_arch.
+
+During Armv8-R aarch64 init, to make sure nothing unexpected traps into
+EL2, we auto-detect and config FIEN and EnSCXT in HCR_EL2.
+
+The boot sequence is:
+If CurrentEL == EL3, then goto EL3 initialisation and drop to lower EL
+  before entering the kernel.
+If CurrentEL == EL2 && id_aa64mmfr0_el1.MSA == 0xf (Armv8-R aarch64),
+  if id_aa64mmfr0_el1.MSA_frac == 0x2,
+    then goto Armv8-R AArch64 initialisation and drop to EL1 before
+    entering the kernel.
+  else, which means VMSA unsupported and cannot boot Linux,
+    goto err_invalid_arch (dead loop).
+Else, no initialisation and keep the current EL before entering the
+  kernel.
+
+Upstream-Status: Pending
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+---
+ arch/aarch64/boot.S            | 92 +++++++++++++++++++++++++++++++++-
+ arch/aarch64/include/asm/cpu.h |  2 +
+ 2 files changed, 92 insertions(+), 2 deletions(-)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 908764a..def9192 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -24,16 +24,24 @@ ASM_FUNC(_start)
+ 	 * Boot sequence
+ 	 * If CurrentEL == EL3, then goto EL3 initialisation and drop to
+ 	 *   lower EL before entering the kernel.
++	 * If CurrentEL == EL2 && id_aa64mmfr0_el1.MSA == 0xf, then
++	 *   If id_aa64mmfr0_el1.MSA_frac == 0x2, then goto
++	 *     Armv8-R AArch64 initialisation and drop to EL1 before
++	 *     entering the kernel.
++	 *   Else, which means VMSA unsupported and cannot boot Linux,
++	 *     goto err_invalid_arch (dead loop).
+ 	 * Else, no initialisation and keep the current EL before
+ 	 *   entering the kernel.
+ 	 */
+ 	mrs	x0, CurrentEL
+-	cmp	x0, #CURRENTEL_EL3
+-	b.eq	el3_init
++	cmp	x0, #CURRENTEL_EL2
++	bgt	el3_init
++	beq	el2_init
+ 
+ 	/*
+ 	 * We stay in the current EL for entering the kernel
+ 	 */
++keep_el:
+ 	mov	w0, #1
+ 	ldr	x1, =flag_keep_el
+ 	str	w0, [x1]
+@@ -139,6 +147,85 @@ el3_init:
+ 	str	w0, [x1]
+ 	b	el_max_init
+ 
++	/*
++	 * EL2 Armv8-R AArch64 initialisation
++	 */
++el2_init:
++	/* Detect Armv8-R AArch64 */
++	mrs	x1, id_aa64mmfr0_el1
++	/*
++	 * Check MSA, bits [51:48]:
++	 * 0xf means Armv8-R AArch64.
++	 * If not 0xf, proceed in Armv8-A EL2.
++	 */
++	ubfx	x0, x1, #48, #4			// MSA
++	cmp	x0, 0xf
++	bne	keep_el
++	/*
++	 * Check MSA_frac, bits [55:52]:
++	 * 0x2 means EL1&0 translation regime also supports VMSAv8-64.
++	 */
++	ubfx	x0, x1, #52, #4			// MSA_frac
++	cmp	x0, 0x2
++	/*
++	 * If not 0x2, no VMSA, so cannot boot Linux and dead loop.
++	 * Also, since the architecture guarantees that those CPUID
++	 * fields never lose features when the value in a field
++	 * increases, we use blt to cover it.
++	*/
++	blt	err_invalid_arch
++
++	mrs	x0, midr_el1
++	msr	vpidr_el2, x0
++
++	mrs	x0, mpidr_el1
++	msr	vmpidr_el2, x0
++
++	mov	x0, #(1 << 31)			// VTCR_MSA: VMSAv8-64 support
++	msr	vtcr_el2, x0
++
++	/* Init HCR_EL2 */
++	mov	x0, #(1 << 31)			// RES1: Armv8-R aarch64 only
++
++	mrs	x1, id_aa64pfr0_el1
++	ubfx	x2, x1, #56, 4			// ID_AA64PFR0_EL1.CSV2
++	cmp	x2, 0x2
++	b.lt	1f
++	/*
++	 * Disable trap when accessing SCTXNUM_EL0 or SCTXNUM_EL1
++	 * if FEAT_CSV2.
++	 */
++	orr	x0, x0, #(1 << 53)		// HCR_EL2.EnSCXT
++
++1:	ubfx	x2, x1, #28, 4			// ID_AA64PFR0_EL1.RAS
++	cmp	x2, 0x2
++	b.lt	1f
++	/* Disable trap when accessing ERXPFGCDN_EL1 if FEAT_RASv1p1. */
++	orr	x0, x0, #(1 << 47)		// HCR_EL2.FIEN
++
++	/* Enable pointer authentication if present */
++1:	mrs	x1, id_aa64isar1_el1
++	/*
++	 * If ID_AA64ISAR1_EL1.{GPI, GPA, API, APA} == {0000, 0000, 0000, 0000}
++	 *   then HCR_EL2.APK and HCR_EL2.API are RES 0.
++	 * Else
++	 *   set HCR_EL2.APK and HCR_EL2.API.
++	 */
++	ldr	x2, =(((0xff) << 24) | (0xff << 4))
++	and	x1, x1, x2
++	cbz	x1, 1f
++
++	orr	x0, x0, #(1 << 40)		// HCR_EL2.APK
++	orr	x0, x0, #(1 << 41)		// HCR_EL2.API
++
++1:	msr	hcr_el2, x0
++	isb
++
++	mov	w0, #SPSR_KERNEL_EL1
++	ldr	x1, =spsr_to_elx
++	str	w0, [x1]
++	// fall through
++
+ el_max_init:
+ 	ldr	x0, =COUNTER_FREQ
+ 	msr	cntfrq_el0, x0
+@@ -148,6 +235,7 @@ el_max_init:
+ 	b	start_el_max
+ 
+ err_invalid_id:
++err_invalid_arch:
+ 	b	.
+ 
+ 	/*
+diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
+index b1003f4..91f803c 100644
+--- a/arch/aarch64/include/asm/cpu.h
++++ b/arch/aarch64/include/asm/cpu.h
+@@ -25,6 +25,7 @@
+ #define SPSR_I			(1 << 7)	/* IRQ masked */
+ #define SPSR_F			(1 << 6)	/* FIQ masked */
+ #define SPSR_T			(1 << 5)	/* Thumb */
++#define SPSR_EL1H		(5 << 0)	/* EL1 Handler mode */
+ #define SPSR_EL2H		(9 << 0)	/* EL2 Handler mode */
+ #define SPSR_HYP		(0x1a << 0)	/* M[3:0] = hyp, M[4] = AArch32 */
+ 
+@@ -43,6 +44,7 @@
+ #else
+ #define SCTLR_EL1_KERNEL	SCTLR_EL1_RES1
+ #define SPSR_KERNEL		(SPSR_A | SPSR_D | SPSR_I | SPSR_F | SPSR_EL2H)
++#define SPSR_KERNEL_EL1		(SPSR_A | SPSR_D | SPSR_I | SPSR_F | SPSR_EL1H)
+ #endif
+ 
+ #ifndef __ASSEMBLY__
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0007-Allow-enable-psci-to-choose-between-smc-and-hvc.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0007-Allow-enable-psci-to-choose-between-smc-and-hvc.patch
new file mode 100644
index 0000000..b130854
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0007-Allow-enable-psci-to-choose-between-smc-and-hvc.patch
@@ -0,0 +1,89 @@
+From f5a31b4f4ea8daaa0d337d5a2322ddb1912083fc Mon Sep 17 00:00:00 2001
+From: Qi Feng <qi.feng@arm.com>
+Date: Wed, 26 May 2021 17:52:01 +0800
+Subject: [PATCH] Allow --enable-psci to choose between smc and hvc
+
+According to Armv8-R AArch64 manual [1], Armv8-R AArch64 does not
+support smc:
+
+- Pseudocode for AArch64.CheckForSMCUndefOrTrap has this snippet:
+
+      if !HaveEL(EL3) || PSTATE.EL == EL0 then
+          UNDEFINED;
+
+  And Armv8-R AArch64 does not have EL3.
+
+- In the document of HCR_EL2 TSC bit:
+  If EL3 is not implemented and HCR_EL2.NV is 0, it is IMPLEMENTATION
+  DEFINED whether this bit is:
+  - RES0.
+  - Implemented with the functionality as described in HCR_EL2.TSC.
+
+So hvc is needed in this situation. And due to the lack of libfdt, the
+psci method cannot be modified at runtime.
+
+To use smc, use --enable-psci or --enable-psci=smc.
+To use hvc, use --enable-psci=hvc.
+
+[1]: https://developer.arm.com/documentation/ddi0600/latest/
+
+Issue-Id: SCM-2654
+Upstream-Status: Pending
+Signed-off-by: Qi Feng <qi.feng@arm.com>
+Change-Id: Ib8afabdad2d98bc37371d165bbb6f1f9b88bfc87
+
+Upstream-Status: Pending
+Signed-off-by: Huifeng Zhang <Huifeng.Zhang@arm.com>
+---
+ Makefile.am  | 10 +++++-----
+ configure.ac | 14 +++++++++-----
+ 2 files changed, 14 insertions(+), 10 deletions(-)
+
+diff --git a/Makefile.am b/Makefile.am
+index f941b07..88a27de 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -50,11 +50,11 @@ endif
+ if PSCI
+ ARCH_OBJ	+= psci.o
+ COMMON_OBJ	+= psci.o
+-PSCI_NODE	:= psci {				\
+-			compatible = \"arm,psci\";	\
+-			method = \"smc\";		\
+-			cpu_on = <$(PSCI_CPU_ON)>;	\
+-			cpu_off = <$(PSCI_CPU_OFF)>;	\
++PSCI_NODE	:= psci {						\
++			compatible = \"arm,psci\";			\
++			method = \"$(PSCI_METHOD)\";			\
++			cpu_on = <$(PSCI_CPU_ON)>;			\
++			cpu_off = <$(PSCI_CPU_OFF)>;			\
+ 		   };
+ CPU_NODES	:= $(shell perl -I $(SCRIPT_DIR) $(SCRIPT_DIR)/addpsci.pl $(KERNEL_DTB))
+ else
+diff --git a/configure.ac b/configure.ac
+index 9e3b722..53e51be 100644
+--- a/configure.ac
++++ b/configure.ac
+@@ -83,13 +83,17 @@ AS_IF([test "x$X_IMAGE" != "x"],
+ # Allow a user to pass --enable-psci
+ AC_ARG_ENABLE([psci],
+ 	AS_HELP_STRING([--disable-psci], [disable the psci boot method]),
+-	[USE_PSCI=$enableval], [USE_PSCI="yes"])
+-AM_CONDITIONAL([PSCI], [test "x$USE_PSCI" = "xyes"])
+-AS_IF([test "x$USE_PSCI" = "xyes"], [], [USE_PSCI=no])
+-
+-AS_IF([test "x$USE_PSCI" != "xyes" -a "x$KERNEL_ES" = "x32"],
++	[case "${enableval}" in
++		yes|smc) USE_PSCI=smc ;;
++		hvc) USE_PSCI=hvc ;;
++		*) AC_MSG_ERROR([Bad value "${enableval}" for --enable-psci. Use "smc" or "hvc"]) ;;
++	esac], [USE_PSCI="yes"])
++AM_CONDITIONAL([PSCI], [test "x$USE_PSCI" = "xyes" -o "x$USE_PSCI" = "xsmc" -o "x$USE_PSCI" = "xhvc"])
++
++AS_IF([test "x$USE_PSCI" = "xno" -a "x$KERNEL_ES" = "x32"],
+ 	[AC_MSG_ERROR([With an AArch32 kernel, boot method must be PSCI.])]
+ )
++AC_SUBST([PSCI_METHOD], [$USE_PSCI])
+ 
+ # Allow a user to pass --with-initrd
+ AC_ARG_WITH([initrd],
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0008-aarch64-Disable-CNTPCT_EL0-trap-for-v8-R64.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0008-aarch64-Disable-CNTPCT_EL0-trap-for-v8-R64.patch
new file mode 100644
index 0000000..2ce28b7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0008-aarch64-Disable-CNTPCT_EL0-trap-for-v8-R64.patch
@@ -0,0 +1,48 @@
+From 3f4614e02f0f8d2522510578da2752f8e3511bb3 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Mon, 25 Oct 2021 17:09:13 +0800
+Subject: [PATCH] aarch64: Disable CNTPCT_EL0 trap for v8-R64
+
+To allow EL1 to access CNTPCT_EL0 without traping into EL2, we need to
+set CNTHCTL_EL2.EL1PCTEN to 1.
+
+For v8-R64, the CNTHCTL_EL2 register follows the v8-A architecture.
+However, as described in the v8-A architecture profile, the
+CNTHCTL_EL2's bit assignments are different according to whether the
+FEAT_VHE is implemented.
+
+Since v8-R64 does not support FEAT_VHE, we do not need to detect
+FEAT_VHE. We can simply set CNTHCTL_EL2.EL1PCTEN to 1.
+
+Issue-ID: SCM-3508
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I4147e66341c8153312021e6f2ab67d0037246da1
+---
+ arch/aarch64/boot.S | 12 ++++++++++++
+ 1 file changed, 12 insertions(+)
+
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index def9192..6dbd5cc 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -219,6 +219,18 @@ el2_init:
+ 	orr	x0, x0, #(1 << 41)		// HCR_EL2.API
+ 
+ 1:	msr	hcr_el2, x0
++
++	/*
++	 * To disable trap when accessing CNTPCT_EL0, we need to set
++	 * CNTHCTL_EL2.EL1PCTEN to 1. However, the CNTHCTL_EL2 bit assignments
++	 * are different according to whether the FEAT_VHE is implemented.
++	 *
++	 * For Armv8-R AArch64, FEAT_VHE is not supported, so we do not need to
++	 * detect FEAT_VHE(ID_AA64MMFR1_EL1.VH) and simply set
++	 * CNTHCTL_EL2.EL1PCTEN to 1.
++	 */
++	mov	x0, #1				// CNTHCTL_EL2.EL1PCTEN
++	msr	cnthctl_el2, x0
+ 	isb
+ 
+ 	mov	w0, #SPSR_KERNEL_EL1
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0009-lds-Mark-the-mem-range.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0009-lds-Mark-the-mem-range.patch
new file mode 100644
index 0000000..0c310eb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0009-lds-Mark-the-mem-range.patch
@@ -0,0 +1,38 @@
+From 2851f0e6c1216894b9498d7b91256bb1ef49e544 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 2 Nov 2021 15:10:28 +0800
+Subject: [PATCH] lds: Mark the mem range
+
+Add firmware_start and firmware_end, so that we can use them to
+calculate the mem range of boot-wrapper and then set the range to
+/memreserve/ of dtb.
+
+Issue-ID: SCM-3815
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: Idc5a2894e193c75381049a0f359b4b2a51c567ee
+---
+ model.lds.S | 2 ++
+ 1 file changed, 2 insertions(+)
+
+diff --git a/model.lds.S b/model.lds.S
+index d4e7e13..ab98ddf 100644
+--- a/model.lds.S
++++ b/model.lds.S
+@@ -64,6 +64,7 @@ SECTIONS
+ #endif
+ 
+ 	.boot PHYS_OFFSET: {
++		PROVIDE(firmware_start = .);
+ 		*(.init)
+ 		*(.text*)
+ 		*(.data* .rodata* .bss* COMMON)
+@@ -76,6 +77,7 @@ SECTIONS
+ 		mbox = .;
+ 		QUAD(0x0)
+ 	}
++	PROVIDE(firmware_end = .);
+ 
+ 	ASSERT(etext <= (PHYS_OFFSET + TEXT_LIMIT), ".text overflow!")
+ }
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0010-common-Introduce-the-libfdt.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0010-common-Introduce-the-libfdt.patch
new file mode 100644
index 0000000..0305f8b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0010-common-Introduce-the-libfdt.patch
@@ -0,0 +1,6044 @@
+From fadf04f44b679d85e55b2e5f220fecbebb52ad03 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 28 Dec 2021 17:02:17 +0800
+Subject: [PATCH] common: Introduce the libfdt
+
+We introduce libfdt (v1.6.1) [1] to boot-wrapper, so we can dynamically
+add the firmware node.
+
+According to [2]: The libfdt is GPL/BSD dual-licensed which means it can
+be used either under the terms of GPL, or under the terms of BSD.
+We choose BSD because the boot-wrapper is under BSD.
+
+[1]: https://github.com/dgibson/dtc/tree/v1.6.1/libfdt
+[2]: https://github.com/dgibson/dtc/blob/v1.6.1/README.license
+
+Issue-Id: SCM-3814
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: Iec2f469053c8ac0ed38838c597b21a42bdf67b38
+---
+ common/libfdt/README.license    |   56 +
+ common/libfdt/fdt.c             |  335 +++++
+ common/libfdt/fdt_addresses.c   |  101 ++
+ common/libfdt/fdt_check.c       |   93 ++
+ common/libfdt/fdt_empty_tree.c  |   38 +
+ common/libfdt/fdt_overlay.c     |  882 +++++++++++++
+ common/libfdt/fdt_ro.c          |  859 +++++++++++++
+ common/libfdt/fdt_rw.c          |  500 +++++++
+ common/libfdt/fdt_strerror.c    |   59 +
+ common/libfdt/fdt_sw.c          |  384 ++++++
+ common/libfdt/fdt_wip.c         |   94 ++
+ common/libfdt/libfdt_internal.h |  192 +++
+ include/fdt.h                   |   66 +
+ include/libfdt.h                | 2147 +++++++++++++++++++++++++++++++
+ include/libfdt_env.h            |   95 ++
+ 15 files changed, 5901 insertions(+)
+ create mode 100644 common/libfdt/README.license
+ create mode 100644 common/libfdt/fdt.c
+ create mode 100644 common/libfdt/fdt_addresses.c
+ create mode 100644 common/libfdt/fdt_check.c
+ create mode 100644 common/libfdt/fdt_empty_tree.c
+ create mode 100644 common/libfdt/fdt_overlay.c
+ create mode 100644 common/libfdt/fdt_ro.c
+ create mode 100644 common/libfdt/fdt_rw.c
+ create mode 100644 common/libfdt/fdt_strerror.c
+ create mode 100644 common/libfdt/fdt_sw.c
+ create mode 100644 common/libfdt/fdt_wip.c
+ create mode 100644 common/libfdt/libfdt_internal.h
+ create mode 100644 include/fdt.h
+ create mode 100644 include/libfdt.h
+ create mode 100644 include/libfdt_env.h
+
+diff --git a/common/libfdt/README.license b/common/libfdt/README.license
+new file mode 100644
+index 0000000..102b004
+--- /dev/null
++++ b/common/libfdt/README.license
+@@ -0,0 +1,56 @@
++Licensing and contribution policy of dtc and libfdt
++===================================================
++
++This dtc package contains two pieces of software: dtc itself, and
++libfdt which comprises the files in the libfdt/ subdirectory.  These
++two pieces of software, although closely related, are quite distinct.
++dtc does not incorporate or rely on libfdt for its operation, nor vice
++versa.  It is important that these two pieces of software have
++different license conditions.
++
++As SPDX license tags in each source file attest, dtc is licensed
++under the GNU GPL.  The full text of the GPL can be found in the file
++entitled 'GPL' which should be included in this package.  dtc code,
++therefore, may not be incorporated into works which do not have a GPL
++compatible license.
++
++libfdt, however, is GPL/BSD dual-licensed.  That is, it may be used
++either under the terms of the GPL, or under the terms of the 2-clause
++BSD license (aka the ISC license).  The full terms of that license can
++be found are in the file entitled 'BSD-2-Clause'. This is, in
++practice, equivalent to being BSD licensed, since the terms of the BSD
++license are strictly more permissive than the GPL.
++
++I made the decision to license libfdt in this way because I want to
++encourage widespread and correct usage of flattened device trees,
++including by proprietary or otherwise GPL-incompatible firmware or
++tools.  Allowing libfdt to be used under the terms of the BSD license
++makes that it easier for vendors or authors of such software to do so.
++
++This does mean that libfdt code could be "stolen" - say, included in a
++proprietary fimware and extended without contributing those extensions
++back to the libfdt mainline.  While I hope that doesn't happen, I
++believe the goal of allowing libfdt to be widely used is more
++important than avoiding that.  libfdt is quite small, and hardly
++rocket science; so the incentive for such impolite behaviour is small,
++and the inconvenience caused thereby is not dire.
++
++Licenses such as the LGPL which would allow code to be used in non-GPL
++software, but also require contributions to be returned were
++considered.  However, libfdt is designed to be used in firmwares and
++other environments with unusual technical constraints.  It's difficult
++to anticipate all possible changes which might be needed to meld
++libfdt into such environments and so difficult to suitably word a
++license that puts the boundary between what is and isn't permitted in
++the intended place.  Again, I judged encouraging widespread use of
++libfdt by keeping the license terms simple and familiar to be the more
++important goal.
++
++**IMPORTANT** It's intended that all of libfdt as released remain
++permissively licensed this way.  Therefore only contributions which
++are released under these terms can be merged into the libfdt mainline.
++
++
++David Gibson <david@gibson.dropbear.id.au>
++(principal original author of dtc and libfdt)
++2 November 2007
+diff --git a/common/libfdt/fdt.c b/common/libfdt/fdt.c
+new file mode 100644
+index 0000000..9fe7cf4
+--- /dev/null
++++ b/common/libfdt/fdt.c
+@@ -0,0 +1,335 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++/*
++ * Minimal sanity check for a read-only tree. fdt_ro_probe_() checks
++ * that the given buffer contains what appears to be a flattened
++ * device tree with sane information in its header.
++ */
++int32_t fdt_ro_probe_(const void *fdt)
++{
++	uint32_t totalsize = fdt_totalsize(fdt);
++
++	if (can_assume(VALID_DTB))
++		return totalsize;
++
++	/* The device tree must be at an 8-byte aligned address */
++	if ((uintptr_t)fdt & 7)
++		return -FDT_ERR_ALIGNMENT;
++
++	if (fdt_magic(fdt) == FDT_MAGIC) {
++		/* Complete tree */
++		if (!can_assume(LATEST)) {
++			if (fdt_version(fdt) < FDT_FIRST_SUPPORTED_VERSION)
++				return -FDT_ERR_BADVERSION;
++			if (fdt_last_comp_version(fdt) >
++					FDT_LAST_SUPPORTED_VERSION)
++				return -FDT_ERR_BADVERSION;
++		}
++	} else if (fdt_magic(fdt) == FDT_SW_MAGIC) {
++		/* Unfinished sequential-write blob */
++		if (!can_assume(VALID_INPUT) && fdt_size_dt_struct(fdt) == 0)
++			return -FDT_ERR_BADSTATE;
++	} else {
++		return -FDT_ERR_BADMAGIC;
++	}
++
++	if (totalsize < INT32_MAX)
++		return totalsize;
++	else
++		return -FDT_ERR_TRUNCATED;
++}
++
++static int check_off_(uint32_t hdrsize, uint32_t totalsize, uint32_t off)
++{
++	return (off >= hdrsize) && (off <= totalsize);
++}
++
++static int check_block_(uint32_t hdrsize, uint32_t totalsize,
++			uint32_t base, uint32_t size)
++{
++	if (!check_off_(hdrsize, totalsize, base))
++		return 0; /* block start out of bounds */
++	if ((base + size) < base)
++		return 0; /* overflow */
++	if (!check_off_(hdrsize, totalsize, base + size))
++		return 0; /* block end out of bounds */
++	return 1;
++}
++
++size_t fdt_header_size_(uint32_t version)
++{
++	if (version <= 1)
++		return FDT_V1_SIZE;
++	else if (version <= 2)
++		return FDT_V2_SIZE;
++	else if (version <= 3)
++		return FDT_V3_SIZE;
++	else if (version <= 16)
++		return FDT_V16_SIZE;
++	else
++		return FDT_V17_SIZE;
++}
++
++size_t fdt_header_size(const void *fdt)
++{
++	return can_assume(LATEST) ? FDT_V17_SIZE :
++		fdt_header_size_(fdt_version(fdt));
++}
++
++int fdt_check_header(const void *fdt)
++{
++	size_t hdrsize;
++
++	/* The device tree must be at an 8-byte aligned address */
++	if ((uintptr_t)fdt & 7)
++		return -FDT_ERR_ALIGNMENT;
++
++	if (fdt_magic(fdt) != FDT_MAGIC)
++		return -FDT_ERR_BADMAGIC;
++	if (!can_assume(LATEST)) {
++		if ((fdt_version(fdt) < FDT_FIRST_SUPPORTED_VERSION)
++		    || (fdt_last_comp_version(fdt) >
++			FDT_LAST_SUPPORTED_VERSION))
++			return -FDT_ERR_BADVERSION;
++		if (fdt_version(fdt) < fdt_last_comp_version(fdt))
++			return -FDT_ERR_BADVERSION;
++	}
++	hdrsize = fdt_header_size(fdt);
++	if (!can_assume(VALID_DTB)) {
++
++		if ((fdt_totalsize(fdt) < hdrsize)
++		    || (fdt_totalsize(fdt) > INT_MAX))
++			return -FDT_ERR_TRUNCATED;
++
++		/* Bounds check memrsv block */
++		if (!check_off_(hdrsize, fdt_totalsize(fdt),
++				fdt_off_mem_rsvmap(fdt)))
++			return -FDT_ERR_TRUNCATED;
++	}
++
++	if (!can_assume(VALID_DTB)) {
++		/* Bounds check structure block */
++		if (!can_assume(LATEST) && fdt_version(fdt) < 17) {
++			if (!check_off_(hdrsize, fdt_totalsize(fdt),
++					fdt_off_dt_struct(fdt)))
++				return -FDT_ERR_TRUNCATED;
++		} else {
++			if (!check_block_(hdrsize, fdt_totalsize(fdt),
++					  fdt_off_dt_struct(fdt),
++					  fdt_size_dt_struct(fdt)))
++				return -FDT_ERR_TRUNCATED;
++		}
++
++		/* Bounds check strings block */
++		if (!check_block_(hdrsize, fdt_totalsize(fdt),
++				  fdt_off_dt_strings(fdt),
++				  fdt_size_dt_strings(fdt)))
++			return -FDT_ERR_TRUNCATED;
++	}
++
++	return 0;
++}
++
++const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int len)
++{
++	unsigned int uoffset = offset;
++	unsigned int absoffset = offset + fdt_off_dt_struct(fdt);
++
++	if (offset < 0)
++		return NULL;
++
++	if (!can_assume(VALID_INPUT))
++		if ((absoffset < uoffset)
++		    || ((absoffset + len) < absoffset)
++		    || (absoffset + len) > fdt_totalsize(fdt))
++			return NULL;
++
++	if (can_assume(LATEST) || fdt_version(fdt) >= 0x11)
++		if (((uoffset + len) < uoffset)
++		    || ((offset + len) > fdt_size_dt_struct(fdt)))
++			return NULL;
++
++	return fdt_offset_ptr_(fdt, offset);
++}
++
++uint32_t fdt_next_tag(const void *fdt, int startoffset, int *nextoffset)
++{
++	const fdt32_t *tagp, *lenp;
++	uint32_t tag;
++	int offset = startoffset;
++	const char *p;
++
++	*nextoffset = -FDT_ERR_TRUNCATED;
++	tagp = fdt_offset_ptr(fdt, offset, FDT_TAGSIZE);
++	if (!can_assume(VALID_DTB) && !tagp)
++		return FDT_END; /* premature end */
++	tag = fdt32_to_cpu(*tagp);
++	offset += FDT_TAGSIZE;
++
++	*nextoffset = -FDT_ERR_BADSTRUCTURE;
++	switch (tag) {
++	case FDT_BEGIN_NODE:
++		/* skip name */
++		do {
++			p = fdt_offset_ptr(fdt, offset++, 1);
++		} while (p && (*p != '\0'));
++		if (!can_assume(VALID_DTB) && !p)
++			return FDT_END; /* premature end */
++		break;
++
++	case FDT_PROP:
++		lenp = fdt_offset_ptr(fdt, offset, sizeof(*lenp));
++		if (!can_assume(VALID_DTB) && !lenp)
++			return FDT_END; /* premature end */
++		/* skip-name offset, length and value */
++		offset += sizeof(struct fdt_property) - FDT_TAGSIZE
++			+ fdt32_to_cpu(*lenp);
++		if (!can_assume(LATEST) &&
++		    fdt_version(fdt) < 0x10 && fdt32_to_cpu(*lenp) >= 8 &&
++		    ((offset - fdt32_to_cpu(*lenp)) % 8) != 0)
++			offset += 4;
++		break;
++
++	case FDT_END:
++	case FDT_END_NODE:
++	case FDT_NOP:
++		break;
++
++	default:
++		return FDT_END;
++	}
++
++	if (!fdt_offset_ptr(fdt, startoffset, offset - startoffset))
++		return FDT_END; /* premature end */
++
++	*nextoffset = FDT_TAGALIGN(offset);
++	return tag;
++}
++
++int fdt_check_node_offset_(const void *fdt, int offset)
++{
++	if (!can_assume(VALID_INPUT)
++	    && ((offset < 0) || (offset % FDT_TAGSIZE)))
++		return -FDT_ERR_BADOFFSET;
++
++	if (fdt_next_tag(fdt, offset, &offset) != FDT_BEGIN_NODE)
++		return -FDT_ERR_BADOFFSET;
++
++	return offset;
++}
++
++int fdt_check_prop_offset_(const void *fdt, int offset)
++{
++	if (!can_assume(VALID_INPUT)
++	    && ((offset < 0) || (offset % FDT_TAGSIZE)))
++		return -FDT_ERR_BADOFFSET;
++
++	if (fdt_next_tag(fdt, offset, &offset) != FDT_PROP)
++		return -FDT_ERR_BADOFFSET;
++
++	return offset;
++}
++
++int fdt_next_node(const void *fdt, int offset, int *depth)
++{
++	int nextoffset = 0;
++	uint32_t tag;
++
++	if (offset >= 0)
++		if ((nextoffset = fdt_check_node_offset_(fdt, offset)) < 0)
++			return nextoffset;
++
++	do {
++		offset = nextoffset;
++		tag = fdt_next_tag(fdt, offset, &nextoffset);
++
++		switch (tag) {
++		case FDT_PROP:
++		case FDT_NOP:
++			break;
++
++		case FDT_BEGIN_NODE:
++			if (depth)
++				(*depth)++;
++			break;
++
++		case FDT_END_NODE:
++			if (depth && ((--(*depth)) < 0))
++				return nextoffset;
++			break;
++
++		case FDT_END:
++			if ((nextoffset >= 0)
++			    || ((nextoffset == -FDT_ERR_TRUNCATED) && !depth))
++				return -FDT_ERR_NOTFOUND;
++			else
++				return nextoffset;
++		}
++	} while (tag != FDT_BEGIN_NODE);
++
++	return offset;
++}
++
++int fdt_first_subnode(const void *fdt, int offset)
++{
++	int depth = 0;
++
++	offset = fdt_next_node(fdt, offset, &depth);
++	if (offset < 0 || depth != 1)
++		return -FDT_ERR_NOTFOUND;
++
++	return offset;
++}
++
++int fdt_next_subnode(const void *fdt, int offset)
++{
++	int depth = 1;
++
++	/*
++	 * With respect to the parent, the depth of the next subnode will be
++	 * the same as the last.
++	 */
++	do {
++		offset = fdt_next_node(fdt, offset, &depth);
++		if (offset < 0 || depth < 1)
++			return -FDT_ERR_NOTFOUND;
++	} while (depth > 1);
++
++	return offset;
++}
++
++const char *fdt_find_string_(const char *strtab, int tabsize, const char *s)
++{
++	int len = strlen(s) + 1;
++	const char *last = strtab + tabsize - len;
++	const char *p;
++
++	for (p = strtab; p <= last; p++)
++		if (memcmp(p, s, len) == 0)
++			return p;
++	return NULL;
++}
++
++int fdt_move(const void *fdt, void *buf, int bufsize)
++{
++	if (!can_assume(VALID_INPUT) && bufsize < 0)
++		return -FDT_ERR_NOSPACE;
++
++	FDT_RO_PROBE(fdt);
++
++	if (fdt_totalsize(fdt) > (unsigned int)bufsize)
++		return -FDT_ERR_NOSPACE;
++
++	memmove(buf, fdt, fdt_totalsize(fdt));
++	return 0;
++}
+diff --git a/common/libfdt/fdt_addresses.c b/common/libfdt/fdt_addresses.c
+new file mode 100644
+index 0000000..9a82cd0
+--- /dev/null
++++ b/common/libfdt/fdt_addresses.c
+@@ -0,0 +1,101 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2014 David Gibson <david@gibson.dropbear.id.au>
++ * Copyright (C) 2018 embedded brains GmbH
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++static int fdt_cells(const void *fdt, int nodeoffset, const char *name)
++{
++	const fdt32_t *c;
++	uint32_t val;
++	int len;
++
++	c = fdt_getprop(fdt, nodeoffset, name, &len);
++	if (!c)
++		return len;
++
++	if (len != sizeof(*c))
++		return -FDT_ERR_BADNCELLS;
++
++	val = fdt32_to_cpu(*c);
++	if (val > FDT_MAX_NCELLS)
++		return -FDT_ERR_BADNCELLS;
++
++	return (int)val;
++}
++
++int fdt_address_cells(const void *fdt, int nodeoffset)
++{
++	int val;
++
++	val = fdt_cells(fdt, nodeoffset, "#address-cells");
++	if (val == 0)
++		return -FDT_ERR_BADNCELLS;
++	if (val == -FDT_ERR_NOTFOUND)
++		return 2;
++	return val;
++}
++
++int fdt_size_cells(const void *fdt, int nodeoffset)
++{
++	int val;
++
++	val = fdt_cells(fdt, nodeoffset, "#size-cells");
++	if (val == -FDT_ERR_NOTFOUND)
++		return 1;
++	return val;
++}
++
++/* This function assumes that [address|size]_cells is 1 or 2 */
++int fdt_appendprop_addrrange(void *fdt, int parent, int nodeoffset,
++			     const char *name, uint64_t addr, uint64_t size)
++{
++	int addr_cells, size_cells, ret;
++	uint8_t data[sizeof(fdt64_t) * 2], *prop;
++
++	ret = fdt_address_cells(fdt, parent);
++	if (ret < 0)
++		return ret;
++	addr_cells = ret;
++
++	ret = fdt_size_cells(fdt, parent);
++	if (ret < 0)
++		return ret;
++	size_cells = ret;
++
++	/* check validity of address */
++	prop = data;
++	if (addr_cells == 1) {
++		if ((addr > UINT32_MAX) || ((UINT32_MAX + 1 - addr) < size))
++			return -FDT_ERR_BADVALUE;
++
++		fdt32_st(prop, (uint32_t)addr);
++	} else if (addr_cells == 2) {
++		fdt64_st(prop, addr);
++	} else {
++		return -FDT_ERR_BADNCELLS;
++	}
++
++	/* check validity of size */
++	prop += addr_cells * sizeof(fdt32_t);
++	if (size_cells == 1) {
++		if (size > UINT32_MAX)
++			return -FDT_ERR_BADVALUE;
++
++		fdt32_st(prop, (uint32_t)size);
++	} else if (size_cells == 2) {
++		fdt64_st(prop, size);
++	} else {
++		return -FDT_ERR_BADNCELLS;
++	}
++
++	return fdt_appendprop(fdt, nodeoffset, name, data,
++			      (addr_cells + size_cells) * sizeof(fdt32_t));
++}
+diff --git a/common/libfdt/fdt_check.c b/common/libfdt/fdt_check.c
+new file mode 100644
+index 0000000..fa410a8
+--- /dev/null
++++ b/common/libfdt/fdt_check.c
+@@ -0,0 +1,93 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++int fdt_check_full(const void *fdt, size_t bufsize)
++{
++	int err;
++	int num_memrsv;
++	int offset, nextoffset = 0;
++	uint32_t tag;
++	unsigned int depth = 0;
++	const void *prop;
++	const char *propname;
++	bool expect_end = false;
++
++	if (bufsize < FDT_V1_SIZE)
++		return -FDT_ERR_TRUNCATED;
++	if (bufsize < fdt_header_size(fdt))
++		return -FDT_ERR_TRUNCATED;
++	err = fdt_check_header(fdt);
++	if (err != 0)
++		return err;
++	if (bufsize < fdt_totalsize(fdt))
++		return -FDT_ERR_TRUNCATED;
++
++	num_memrsv = fdt_num_mem_rsv(fdt);
++	if (num_memrsv < 0)
++		return num_memrsv;
++
++	while (1) {
++		offset = nextoffset;
++		tag = fdt_next_tag(fdt, offset, &nextoffset);
++
++		if (nextoffset < 0)
++			return nextoffset;
++
++		/* If we see two root nodes, something is wrong */
++		if (expect_end && tag != FDT_END)
++			return -FDT_ERR_BADSTRUCTURE;
++
++		switch (tag) {
++		case FDT_NOP:
++			break;
++
++		case FDT_END:
++			if (depth != 0)
++				return -FDT_ERR_BADSTRUCTURE;
++			return 0;
++
++		case FDT_BEGIN_NODE:
++			depth++;
++			if (depth > INT_MAX)
++				return -FDT_ERR_BADSTRUCTURE;
++
++			/* The root node must have an empty name */
++			if (depth == 1) {
++				const char *name;
++				int len;
++
++				name = fdt_get_name(fdt, offset, &len);
++				if (*name || len)
++					return -FDT_ERR_BADSTRUCTURE;
++			}
++			break;
++
++		case FDT_END_NODE:
++			if (depth == 0)
++				return -FDT_ERR_BADSTRUCTURE;
++			depth--;
++			if (depth == 0)
++				expect_end = true;
++			break;
++
++		case FDT_PROP:
++			prop = fdt_getprop_by_offset(fdt, offset, &propname,
++						     &err);
++			if (!prop)
++				return err;
++			break;
++
++		default:
++			return -FDT_ERR_INTERNAL;
++		}
++	}
++}
+diff --git a/common/libfdt/fdt_empty_tree.c b/common/libfdt/fdt_empty_tree.c
+new file mode 100644
+index 0000000..49d54d4
+--- /dev/null
++++ b/common/libfdt/fdt_empty_tree.c
+@@ -0,0 +1,38 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2012 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++int fdt_create_empty_tree(void *buf, int bufsize)
++{
++	int err;
++
++	err = fdt_create(buf, bufsize);
++	if (err)
++		return err;
++
++	err = fdt_finish_reservemap(buf);
++	if (err)
++		return err;
++
++	err = fdt_begin_node(buf, "");
++	if (err)
++		return err;
++
++	err =  fdt_end_node(buf);
++	if (err)
++		return err;
++
++	err = fdt_finish(buf);
++	if (err)
++		return err;
++
++	return fdt_open_into(buf, buf, bufsize);
++}
+diff --git a/common/libfdt/fdt_overlay.c b/common/libfdt/fdt_overlay.c
+new file mode 100644
+index 0000000..d217e79
+--- /dev/null
++++ b/common/libfdt/fdt_overlay.c
+@@ -0,0 +1,882 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2016 Free Electrons
++ * Copyright (C) 2016 NextThing Co.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++/**
++ * overlay_get_target_phandle - retrieves the target phandle of a fragment
++ * @fdto: pointer to the device tree overlay blob
++ * @fragment: node offset of the fragment in the overlay
++ *
++ * overlay_get_target_phandle() retrieves the target phandle of an
++ * overlay fragment when that fragment uses a phandle (target
++ * property) instead of a path (target-path property).
++ *
++ * returns:
++ *      the phandle pointed by the target property
++ *      0, if the phandle was not found
++ *	-1, if the phandle was malformed
++ */
++static uint32_t overlay_get_target_phandle(const void *fdto, int fragment)
++{
++	const fdt32_t *val;
++	int len;
++
++	val = fdt_getprop(fdto, fragment, "target", &len);
++	if (!val)
++		return 0;
++
++	if ((len != sizeof(*val)) || (fdt32_to_cpu(*val) == (uint32_t)-1))
++		return (uint32_t)-1;
++
++	return fdt32_to_cpu(*val);
++}
++
++/**
++ * overlay_get_target - retrieves the offset of a fragment's target
++ * @fdt: Base device tree blob
++ * @fdto: Device tree overlay blob
++ * @fragment: node offset of the fragment in the overlay
++ * @pathp: pointer which receives the path of the target (or NULL)
++ *
++ * overlay_get_target() retrieves the target offset in the base
++ * device tree of a fragment, no matter how the actual targeting is
++ * done (through a phandle or a path)
++ *
++ * returns:
++ *      the targeted node offset in the base device tree
++ *      Negative error code on error
++ */
++static int overlay_get_target(const void *fdt, const void *fdto,
++			      int fragment, char const **pathp)
++{
++	uint32_t phandle;
++	const char *path = NULL;
++	int path_len = 0, ret;
++
++	/* Try first to do a phandle based lookup */
++	phandle = overlay_get_target_phandle(fdto, fragment);
++	if (phandle == (uint32_t)-1)
++		return -FDT_ERR_BADPHANDLE;
++
++	/* no phandle, try path */
++	if (!phandle) {
++		/* And then a path based lookup */
++		path = fdt_getprop(fdto, fragment, "target-path", &path_len);
++		if (path)
++			ret = fdt_path_offset(fdt, path);
++		else
++			ret = path_len;
++	} else
++		ret = fdt_node_offset_by_phandle(fdt, phandle);
++
++	/*
++	* If we haven't found either a target or a
++	* target-path property in a node that contains a
++	* __overlay__ subnode (we wouldn't be called
++	* otherwise), consider it a improperly written
++	* overlay
++	*/
++	if (ret < 0 && path_len == -FDT_ERR_NOTFOUND)
++		ret = -FDT_ERR_BADOVERLAY;
++
++	/* return on error */
++	if (ret < 0)
++		return ret;
++
++	/* return pointer to path (if available) */
++	if (pathp)
++		*pathp = path ? path : NULL;
++
++	return ret;
++}
++
++/**
++ * overlay_phandle_add_offset - Increases a phandle by an offset
++ * @fdt: Base device tree blob
++ * @node: Device tree overlay blob
++ * @name: Name of the property to modify (phandle or linux,phandle)
++ * @delta: offset to apply
++ *
++ * overlay_phandle_add_offset() increments a node phandle by a given
++ * offset.
++ *
++ * returns:
++ *      0 on success.
++ *      Negative error code on error
++ */
++static int overlay_phandle_add_offset(void *fdt, int node,
++				      const char *name, uint32_t delta)
++{
++	const fdt32_t *val;
++	uint32_t adj_val;
++	int len;
++
++	val = fdt_getprop(fdt, node, name, &len);
++	if (!val)
++		return len;
++
++	if (len != sizeof(*val))
++		return -FDT_ERR_BADPHANDLE;
++
++	adj_val = fdt32_to_cpu(*val);
++	if ((adj_val + delta) < adj_val)
++		return -FDT_ERR_NOPHANDLES;
++
++	adj_val += delta;
++	if (adj_val == (uint32_t)-1)
++		return -FDT_ERR_NOPHANDLES;
++
++	return fdt_setprop_inplace_u32(fdt, node, name, adj_val);
++}
++
++/**
++ * overlay_adjust_node_phandles - Offsets the phandles of a node
++ * @fdto: Device tree overlay blob
++ * @node: Offset of the node we want to adjust
++ * @delta: Offset to shift the phandles of
++ *
++ * overlay_adjust_node_phandles() adds a constant to all the phandles
++ * of a given node. This is mainly use as part of the overlay
++ * application process, when we want to update all the overlay
++ * phandles to not conflict with the overlays of the base device tree.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_adjust_node_phandles(void *fdto, int node,
++					uint32_t delta)
++{
++	int child;
++	int ret;
++
++	ret = overlay_phandle_add_offset(fdto, node, "phandle", delta);
++	if (ret && ret != -FDT_ERR_NOTFOUND)
++		return ret;
++
++	ret = overlay_phandle_add_offset(fdto, node, "linux,phandle", delta);
++	if (ret && ret != -FDT_ERR_NOTFOUND)
++		return ret;
++
++	fdt_for_each_subnode(child, fdto, node) {
++		ret = overlay_adjust_node_phandles(fdto, child, delta);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++/**
++ * overlay_adjust_local_phandles - Adjust the phandles of a whole overlay
++ * @fdto: Device tree overlay blob
++ * @delta: Offset to shift the phandles of
++ *
++ * overlay_adjust_local_phandles() adds a constant to all the
++ * phandles of an overlay. This is mainly use as part of the overlay
++ * application process, when we want to update all the overlay
++ * phandles to not conflict with the overlays of the base device tree.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_adjust_local_phandles(void *fdto, uint32_t delta)
++{
++	/*
++	 * Start adjusting the phandles from the overlay root
++	 */
++	return overlay_adjust_node_phandles(fdto, 0, delta);
++}
++
++/**
++ * overlay_update_local_node_references - Adjust the overlay references
++ * @fdto: Device tree overlay blob
++ * @tree_node: Node offset of the node to operate on
++ * @fixup_node: Node offset of the matching local fixups node
++ * @delta: Offset to shift the phandles of
++ *
++ * overlay_update_local_nodes_references() update the phandles
++ * pointing to a node within the device tree overlay by adding a
++ * constant delta.
++ *
++ * This is mainly used as part of a device tree application process,
++ * where you want the device tree overlays phandles to not conflict
++ * with the ones from the base device tree before merging them.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_update_local_node_references(void *fdto,
++						int tree_node,
++						int fixup_node,
++						uint32_t delta)
++{
++	int fixup_prop;
++	int fixup_child;
++	int ret;
++
++	fdt_for_each_property_offset(fixup_prop, fdto, fixup_node) {
++		const fdt32_t *fixup_val;
++		const char *tree_val;
++		const char *name;
++		int fixup_len;
++		int tree_len;
++		int i;
++
++		fixup_val = fdt_getprop_by_offset(fdto, fixup_prop,
++						  &name, &fixup_len);
++		if (!fixup_val)
++			return fixup_len;
++
++		if (fixup_len % sizeof(uint32_t))
++			return -FDT_ERR_BADOVERLAY;
++		fixup_len /= sizeof(uint32_t);
++
++		tree_val = fdt_getprop(fdto, tree_node, name, &tree_len);
++		if (!tree_val) {
++			if (tree_len == -FDT_ERR_NOTFOUND)
++				return -FDT_ERR_BADOVERLAY;
++
++			return tree_len;
++		}
++
++		for (i = 0; i < fixup_len; i++) {
++			fdt32_t adj_val;
++			uint32_t poffset;
++
++			poffset = fdt32_to_cpu(fixup_val[i]);
++
++			/*
++			 * phandles to fixup can be unaligned.
++			 *
++			 * Use a memcpy for the architectures that do
++			 * not support unaligned accesses.
++			 */
++			memcpy(&adj_val, tree_val + poffset, sizeof(adj_val));
++
++			adj_val = cpu_to_fdt32(fdt32_to_cpu(adj_val) + delta);
++
++			ret = fdt_setprop_inplace_namelen_partial(fdto,
++								  tree_node,
++								  name,
++								  strlen(name),
++								  poffset,
++								  &adj_val,
++								  sizeof(adj_val));
++			if (ret == -FDT_ERR_NOSPACE)
++				return -FDT_ERR_BADOVERLAY;
++
++			if (ret)
++				return ret;
++		}
++	}
++
++	fdt_for_each_subnode(fixup_child, fdto, fixup_node) {
++		const char *fixup_child_name = fdt_get_name(fdto, fixup_child,
++							    NULL);
++		int tree_child;
++
++		tree_child = fdt_subnode_offset(fdto, tree_node,
++						fixup_child_name);
++		if (tree_child == -FDT_ERR_NOTFOUND)
++			return -FDT_ERR_BADOVERLAY;
++		if (tree_child < 0)
++			return tree_child;
++
++		ret = overlay_update_local_node_references(fdto,
++							   tree_child,
++							   fixup_child,
++							   delta);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++/**
++ * overlay_update_local_references - Adjust the overlay references
++ * @fdto: Device tree overlay blob
++ * @delta: Offset to shift the phandles of
++ *
++ * overlay_update_local_references() update all the phandles pointing
++ * to a node within the device tree overlay by adding a constant
++ * delta to not conflict with the base overlay.
++ *
++ * This is mainly used as part of a device tree application process,
++ * where you want the device tree overlays phandles to not conflict
++ * with the ones from the base device tree before merging them.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_update_local_references(void *fdto, uint32_t delta)
++{
++	int fixups;
++
++	fixups = fdt_path_offset(fdto, "/__local_fixups__");
++	if (fixups < 0) {
++		/* There's no local phandles to adjust, bail out */
++		if (fixups == -FDT_ERR_NOTFOUND)
++			return 0;
++
++		return fixups;
++	}
++
++	/*
++	 * Update our local references from the root of the tree
++	 */
++	return overlay_update_local_node_references(fdto, 0, fixups,
++						    delta);
++}
++
++/**
++ * overlay_fixup_one_phandle - Set an overlay phandle to the base one
++ * @fdt: Base Device Tree blob
++ * @fdto: Device tree overlay blob
++ * @symbols_off: Node offset of the symbols node in the base device tree
++ * @path: Path to a node holding a phandle in the overlay
++ * @path_len: number of path characters to consider
++ * @name: Name of the property holding the phandle reference in the overlay
++ * @name_len: number of name characters to consider
++ * @poffset: Offset within the overlay property where the phandle is stored
++ * @label: Label of the node referenced by the phandle
++ *
++ * overlay_fixup_one_phandle() resolves an overlay phandle pointing to
++ * a node in the base device tree.
++ *
++ * This is part of the device tree overlay application process, when
++ * you want all the phandles in the overlay to point to the actual
++ * base dt nodes.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_fixup_one_phandle(void *fdt, void *fdto,
++				     int symbols_off,
++				     const char *path, uint32_t path_len,
++				     const char *name, uint32_t name_len,
++				     int poffset, const char *label)
++{
++	const char *symbol_path;
++	uint32_t phandle;
++	fdt32_t phandle_prop;
++	int symbol_off, fixup_off;
++	int prop_len;
++
++	if (symbols_off < 0)
++		return symbols_off;
++
++	symbol_path = fdt_getprop(fdt, symbols_off, label,
++				  &prop_len);
++	if (!symbol_path)
++		return prop_len;
++
++	symbol_off = fdt_path_offset(fdt, symbol_path);
++	if (symbol_off < 0)
++		return symbol_off;
++
++	phandle = fdt_get_phandle(fdt, symbol_off);
++	if (!phandle)
++		return -FDT_ERR_NOTFOUND;
++
++	fixup_off = fdt_path_offset_namelen(fdto, path, path_len);
++	if (fixup_off == -FDT_ERR_NOTFOUND)
++		return -FDT_ERR_BADOVERLAY;
++	if (fixup_off < 0)
++		return fixup_off;
++
++	phandle_prop = cpu_to_fdt32(phandle);
++	return fdt_setprop_inplace_namelen_partial(fdto, fixup_off,
++						   name, name_len, poffset,
++						   &phandle_prop,
++						   sizeof(phandle_prop));
++};
++
++/**
++ * overlay_fixup_phandle - Set an overlay phandle to the base one
++ * @fdt: Base Device Tree blob
++ * @fdto: Device tree overlay blob
++ * @symbols_off: Node offset of the symbols node in the base device tree
++ * @property: Property offset in the overlay holding the list of fixups
++ *
++ * overlay_fixup_phandle() resolves all the overlay phandles pointed
++ * to in a __fixups__ property, and updates them to match the phandles
++ * in use in the base device tree.
++ *
++ * This is part of the device tree overlay application process, when
++ * you want all the phandles in the overlay to point to the actual
++ * base dt nodes.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_fixup_phandle(void *fdt, void *fdto, int symbols_off,
++				 int property)
++{
++	const char *value;
++	const char *label;
++	int len;
++
++	value = fdt_getprop_by_offset(fdto, property,
++				      &label, &len);
++	if (!value) {
++		if (len == -FDT_ERR_NOTFOUND)
++			return -FDT_ERR_INTERNAL;
++
++		return len;
++	}
++
++	do {
++		const char *path, *name, *fixup_end;
++		const char *fixup_str = value;
++		uint32_t path_len, name_len;
++		uint32_t fixup_len;
++		char *sep, *endptr;
++		int poffset, ret;
++
++		fixup_end = memchr(value, '\0', len);
++		if (!fixup_end)
++			return -FDT_ERR_BADOVERLAY;
++		fixup_len = fixup_end - fixup_str;
++
++		len -= fixup_len + 1;
++		value += fixup_len + 1;
++
++		path = fixup_str;
++		sep = memchr(fixup_str, ':', fixup_len);
++		if (!sep || *sep != ':')
++			return -FDT_ERR_BADOVERLAY;
++
++		path_len = sep - path;
++		if (path_len == (fixup_len - 1))
++			return -FDT_ERR_BADOVERLAY;
++
++		fixup_len -= path_len + 1;
++		name = sep + 1;
++		sep = memchr(name, ':', fixup_len);
++		if (!sep || *sep != ':')
++			return -FDT_ERR_BADOVERLAY;
++
++		name_len = sep - name;
++		if (!name_len)
++			return -FDT_ERR_BADOVERLAY;
++
++		poffset = strtoul(sep + 1, &endptr, 10);
++		if ((*endptr != '\0') || (endptr <= (sep + 1)))
++			return -FDT_ERR_BADOVERLAY;
++
++		ret = overlay_fixup_one_phandle(fdt, fdto, symbols_off,
++						path, path_len, name, name_len,
++						poffset, label);
++		if (ret)
++			return ret;
++	} while (len > 0);
++
++	return 0;
++}
++
++/**
++ * overlay_fixup_phandles - Resolve the overlay phandles to the base
++ *                          device tree
++ * @fdt: Base Device Tree blob
++ * @fdto: Device tree overlay blob
++ *
++ * overlay_fixup_phandles() resolves all the overlay phandles pointing
++ * to nodes in the base device tree.
++ *
++ * This is one of the steps of the device tree overlay application
++ * process, when you want all the phandles in the overlay to point to
++ * the actual base dt nodes.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_fixup_phandles(void *fdt, void *fdto)
++{
++	int fixups_off, symbols_off;
++	int property;
++
++	/* We can have overlays without any fixups */
++	fixups_off = fdt_path_offset(fdto, "/__fixups__");
++	if (fixups_off == -FDT_ERR_NOTFOUND)
++		return 0; /* nothing to do */
++	if (fixups_off < 0)
++		return fixups_off;
++
++	/* And base DTs without symbols */
++	symbols_off = fdt_path_offset(fdt, "/__symbols__");
++	if ((symbols_off < 0 && (symbols_off != -FDT_ERR_NOTFOUND)))
++		return symbols_off;
++
++	fdt_for_each_property_offset(property, fdto, fixups_off) {
++		int ret;
++
++		ret = overlay_fixup_phandle(fdt, fdto, symbols_off, property);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++/**
++ * overlay_apply_node - Merges a node into the base device tree
++ * @fdt: Base Device Tree blob
++ * @target: Node offset in the base device tree to apply the fragment to
++ * @fdto: Device tree overlay blob
++ * @node: Node offset in the overlay holding the changes to merge
++ *
++ * overlay_apply_node() merges a node into a target base device tree
++ * node pointed.
++ *
++ * This is part of the final step in the device tree overlay
++ * application process, when all the phandles have been adjusted and
++ * resolved and you just have to merge overlay into the base device
++ * tree.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_apply_node(void *fdt, int target,
++			      void *fdto, int node)
++{
++	int property;
++	int subnode;
++
++	fdt_for_each_property_offset(property, fdto, node) {
++		const char *name;
++		const void *prop;
++		int prop_len;
++		int ret;
++
++		prop = fdt_getprop_by_offset(fdto, property, &name,
++					     &prop_len);
++		if (prop_len == -FDT_ERR_NOTFOUND)
++			return -FDT_ERR_INTERNAL;
++		if (prop_len < 0)
++			return prop_len;
++
++		ret = fdt_setprop(fdt, target, name, prop, prop_len);
++		if (ret)
++			return ret;
++	}
++
++	fdt_for_each_subnode(subnode, fdto, node) {
++		const char *name = fdt_get_name(fdto, subnode, NULL);
++		int nnode;
++		int ret;
++
++		nnode = fdt_add_subnode(fdt, target, name);
++		if (nnode == -FDT_ERR_EXISTS) {
++			nnode = fdt_subnode_offset(fdt, target, name);
++			if (nnode == -FDT_ERR_NOTFOUND)
++				return -FDT_ERR_INTERNAL;
++		}
++
++		if (nnode < 0)
++			return nnode;
++
++		ret = overlay_apply_node(fdt, nnode, fdto, subnode);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++/**
++ * overlay_merge - Merge an overlay into its base device tree
++ * @fdt: Base Device Tree blob
++ * @fdto: Device tree overlay blob
++ *
++ * overlay_merge() merges an overlay into its base device tree.
++ *
++ * This is the next to last step in the device tree overlay application
++ * process, when all the phandles have been adjusted and resolved and
++ * you just have to merge overlay into the base device tree.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_merge(void *fdt, void *fdto)
++{
++	int fragment;
++
++	fdt_for_each_subnode(fragment, fdto, 0) {
++		int overlay;
++		int target;
++		int ret;
++
++		/*
++		 * Each fragments will have an __overlay__ node. If
++		 * they don't, it's not supposed to be merged
++		 */
++		overlay = fdt_subnode_offset(fdto, fragment, "__overlay__");
++		if (overlay == -FDT_ERR_NOTFOUND)
++			continue;
++
++		if (overlay < 0)
++			return overlay;
++
++		target = overlay_get_target(fdt, fdto, fragment, NULL);
++		if (target < 0)
++			return target;
++
++		ret = overlay_apply_node(fdt, target, fdto, overlay);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++static int get_path_len(const void *fdt, int nodeoffset)
++{
++	int len = 0, namelen;
++	const char *name;
++
++	FDT_RO_PROBE(fdt);
++
++	for (;;) {
++		name = fdt_get_name(fdt, nodeoffset, &namelen);
++		if (!name)
++			return namelen;
++
++		/* root? we're done */
++		if (namelen == 0)
++			break;
++
++		nodeoffset = fdt_parent_offset(fdt, nodeoffset);
++		if (nodeoffset < 0)
++			return nodeoffset;
++		len += namelen + 1;
++	}
++
++	/* in case of root pretend it's "/" */
++	if (len == 0)
++		len++;
++	return len;
++}
++
++/**
++ * overlay_symbol_update - Update the symbols of base tree after a merge
++ * @fdt: Base Device Tree blob
++ * @fdto: Device tree overlay blob
++ *
++ * overlay_symbol_update() updates the symbols of the base tree with the
++ * symbols of the applied overlay
++ *
++ * This is the last step in the device tree overlay application
++ * process, allowing the reference of overlay symbols by subsequent
++ * overlay operations.
++ *
++ * returns:
++ *      0 on success
++ *      Negative error code on failure
++ */
++static int overlay_symbol_update(void *fdt, void *fdto)
++{
++	int root_sym, ov_sym, prop, path_len, fragment, target;
++	int len, frag_name_len, ret, rel_path_len;
++	const char *s, *e;
++	const char *path;
++	const char *name;
++	const char *frag_name;
++	const char *rel_path;
++	const char *target_path;
++	char *buf;
++	void *p;
++
++	ov_sym = fdt_subnode_offset(fdto, 0, "__symbols__");
++
++	/* if no overlay symbols exist no problem */
++	if (ov_sym < 0)
++		return 0;
++
++	root_sym = fdt_subnode_offset(fdt, 0, "__symbols__");
++
++	/* it no root symbols exist we should create them */
++	if (root_sym == -FDT_ERR_NOTFOUND)
++		root_sym = fdt_add_subnode(fdt, 0, "__symbols__");
++
++	/* any error is fatal now */
++	if (root_sym < 0)
++		return root_sym;
++
++	/* iterate over each overlay symbol */
++	fdt_for_each_property_offset(prop, fdto, ov_sym) {
++		path = fdt_getprop_by_offset(fdto, prop, &name, &path_len);
++		if (!path)
++			return path_len;
++
++		/* verify it's a string property (terminated by a single \0) */
++		if (path_len < 1 || memchr(path, '\0', path_len) != &path[path_len - 1])
++			return -FDT_ERR_BADVALUE;
++
++		/* keep end marker to avoid strlen() */
++		e = path + path_len;
++
++		if (*path != '/')
++			return -FDT_ERR_BADVALUE;
++
++		/* get fragment name first */
++		s = strchr(path + 1, '/');
++		if (!s) {
++			/* Symbol refers to something that won't end
++			 * up in the target tree */
++			continue;
++		}
++
++		frag_name = path + 1;
++		frag_name_len = s - path - 1;
++
++		/* verify format; safe since "s" lies in \0 terminated prop */
++		len = sizeof("/__overlay__/") - 1;
++		if ((e - s) > len && (memcmp(s, "/__overlay__/", len) == 0)) {
++			/* /<fragment-name>/__overlay__/<relative-subnode-path> */
++			rel_path = s + len;
++			rel_path_len = e - rel_path - 1;
++		} else if ((e - s) == len
++			   && (memcmp(s, "/__overlay__", len - 1) == 0)) {
++			/* /<fragment-name>/__overlay__ */
++			rel_path = "";
++			rel_path_len = 0;
++		} else {
++			/* Symbol refers to something that won't end
++			 * up in the target tree */
++			continue;
++		}
++
++		/* find the fragment index in which the symbol lies */
++		ret = fdt_subnode_offset_namelen(fdto, 0, frag_name,
++					       frag_name_len);
++		/* not found? */
++		if (ret < 0)
++			return -FDT_ERR_BADOVERLAY;
++		fragment = ret;
++
++		/* an __overlay__ subnode must exist */
++		ret = fdt_subnode_offset(fdto, fragment, "__overlay__");
++		if (ret < 0)
++			return -FDT_ERR_BADOVERLAY;
++
++		/* get the target of the fragment */
++		ret = overlay_get_target(fdt, fdto, fragment, &target_path);
++		if (ret < 0)
++			return ret;
++		target = ret;
++
++		/* if we have a target path use */
++		if (!target_path) {
++			ret = get_path_len(fdt, target);
++			if (ret < 0)
++				return ret;
++			len = ret;
++		} else {
++			len = strlen(target_path);
++		}
++
++		ret = fdt_setprop_placeholder(fdt, root_sym, name,
++				len + (len > 1) + rel_path_len + 1, &p);
++		if (ret < 0)
++			return ret;
++
++		if (!target_path) {
++			/* again in case setprop_placeholder changed it */
++			ret = overlay_get_target(fdt, fdto, fragment, &target_path);
++			if (ret < 0)
++				return ret;
++			target = ret;
++		}
++
++		buf = p;
++		if (len > 1) { /* target is not root */
++			if (!target_path) {
++				ret = fdt_get_path(fdt, target, buf, len + 1);
++				if (ret < 0)
++					return ret;
++			} else
++				memcpy(buf, target_path, len + 1);
++
++		} else
++			len--;
++
++		buf[len] = '/';
++		memcpy(buf + len + 1, rel_path, rel_path_len);
++		buf[len + 1 + rel_path_len] = '\0';
++	}
++
++	return 0;
++}
++
++int fdt_overlay_apply(void *fdt, void *fdto)
++{
++	uint32_t delta;
++	int ret;
++
++	FDT_RO_PROBE(fdt);
++	FDT_RO_PROBE(fdto);
++
++	ret = fdt_find_max_phandle(fdt, &delta);
++	if (ret)
++		goto err;
++
++	ret = overlay_adjust_local_phandles(fdto, delta);
++	if (ret)
++		goto err;
++
++	ret = overlay_update_local_references(fdto, delta);
++	if (ret)
++		goto err;
++
++	ret = overlay_fixup_phandles(fdt, fdto);
++	if (ret)
++		goto err;
++
++	ret = overlay_merge(fdt, fdto);
++	if (ret)
++		goto err;
++
++	ret = overlay_symbol_update(fdt, fdto);
++	if (ret)
++		goto err;
++
++	/*
++	 * The overlay has been damaged, erase its magic.
++	 */
++	fdt_set_magic(fdto, ~0);
++
++	return 0;
++
++err:
++	/*
++	 * The overlay might have been damaged, erase its magic.
++	 */
++	fdt_set_magic(fdto, ~0);
++
++	/*
++	 * The base device tree might have been damaged, erase its
++	 * magic.
++	 */
++	fdt_set_magic(fdt, ~0);
++
++	return ret;
++}
+diff --git a/common/libfdt/fdt_ro.c b/common/libfdt/fdt_ro.c
+new file mode 100644
+index 0000000..17584da
+--- /dev/null
++++ b/common/libfdt/fdt_ro.c
+@@ -0,0 +1,859 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++static int fdt_nodename_eq_(const void *fdt, int offset,
++			    const char *s, int len)
++{
++	int olen;
++	const char *p = fdt_get_name(fdt, offset, &olen);
++
++	if (!p || olen < len)
++		/* short match */
++		return 0;
++
++	if (memcmp(p, s, len) != 0)
++		return 0;
++
++	if (p[len] == '\0')
++		return 1;
++	else if (!memchr(s, '@', len) && (p[len] == '@'))
++		return 1;
++	else
++		return 0;
++}
++
++const char *fdt_get_string(const void *fdt, int stroffset, int *lenp)
++{
++	int32_t totalsize;
++	uint32_t absoffset;
++	size_t len;
++	int err;
++	const char *s, *n;
++
++	if (can_assume(VALID_INPUT)) {
++		s = (const char *)fdt + fdt_off_dt_strings(fdt) + stroffset;
++
++		if (lenp)
++			*lenp = strlen(s);
++		return s;
++	}
++	totalsize = fdt_ro_probe_(fdt);
++	err = totalsize;
++	if (totalsize < 0)
++		goto fail;
++
++	err = -FDT_ERR_BADOFFSET;
++	absoffset = stroffset + fdt_off_dt_strings(fdt);
++	if (absoffset >= (unsigned)totalsize)
++		goto fail;
++	len = totalsize - absoffset;
++
++	if (fdt_magic(fdt) == FDT_MAGIC) {
++		if (stroffset < 0)
++			goto fail;
++		if (can_assume(LATEST) || fdt_version(fdt) >= 17) {
++			if ((unsigned)stroffset >= fdt_size_dt_strings(fdt))
++				goto fail;
++			if ((fdt_size_dt_strings(fdt) - stroffset) < len)
++				len = fdt_size_dt_strings(fdt) - stroffset;
++		}
++	} else if (fdt_magic(fdt) == FDT_SW_MAGIC) {
++		unsigned int sw_stroffset = -stroffset;
++
++		if ((stroffset >= 0) ||
++		    (sw_stroffset > fdt_size_dt_strings(fdt)))
++			goto fail;
++		if (sw_stroffset < len)
++			len = sw_stroffset;
++	} else {
++		err = -FDT_ERR_INTERNAL;
++		goto fail;
++	}
++
++	s = (const char *)fdt + absoffset;
++	n = memchr(s, '\0', len);
++	if (!n) {
++		/* missing terminating NULL */
++		err = -FDT_ERR_TRUNCATED;
++		goto fail;
++	}
++
++	if (lenp)
++		*lenp = n - s;
++	return s;
++
++fail:
++	if (lenp)
++		*lenp = err;
++	return NULL;
++}
++
++const char *fdt_string(const void *fdt, int stroffset)
++{
++	return fdt_get_string(fdt, stroffset, NULL);
++}
++
++static int fdt_string_eq_(const void *fdt, int stroffset,
++			  const char *s, int len)
++{
++	int slen;
++	const char *p = fdt_get_string(fdt, stroffset, &slen);
++
++	return p && (slen == len) && (memcmp(p, s, len) == 0);
++}
++
++int fdt_find_max_phandle(const void *fdt, uint32_t *phandle)
++{
++	uint32_t max = 0;
++	int offset = -1;
++
++	while (true) {
++		uint32_t value;
++
++		offset = fdt_next_node(fdt, offset, NULL);
++		if (offset < 0) {
++			if (offset == -FDT_ERR_NOTFOUND)
++				break;
++
++			return offset;
++		}
++
++		value = fdt_get_phandle(fdt, offset);
++
++		if (value > max)
++			max = value;
++	}
++
++	if (phandle)
++		*phandle = max;
++
++	return 0;
++}
++
++int fdt_generate_phandle(const void *fdt, uint32_t *phandle)
++{
++	uint32_t max;
++	int err;
++
++	err = fdt_find_max_phandle(fdt, &max);
++	if (err < 0)
++		return err;
++
++	if (max == FDT_MAX_PHANDLE)
++		return -FDT_ERR_NOPHANDLES;
++
++	if (phandle)
++		*phandle = max + 1;
++
++	return 0;
++}
++
++static const struct fdt_reserve_entry *fdt_mem_rsv(const void *fdt, int n)
++{
++	unsigned int offset = n * sizeof(struct fdt_reserve_entry);
++	unsigned int absoffset = fdt_off_mem_rsvmap(fdt) + offset;
++
++	if (!can_assume(VALID_INPUT)) {
++		if (absoffset < fdt_off_mem_rsvmap(fdt))
++			return NULL;
++		if (absoffset > fdt_totalsize(fdt) -
++		    sizeof(struct fdt_reserve_entry))
++			return NULL;
++	}
++	return fdt_mem_rsv_(fdt, n);
++}
++
++int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size)
++{
++	const struct fdt_reserve_entry *re;
++
++	FDT_RO_PROBE(fdt);
++	re = fdt_mem_rsv(fdt, n);
++	if (!can_assume(VALID_INPUT) && !re)
++		return -FDT_ERR_BADOFFSET;
++
++	*address = fdt64_ld_(&re->address);
++	*size = fdt64_ld_(&re->size);
++	return 0;
++}
++
++int fdt_num_mem_rsv(const void *fdt)
++{
++	int i;
++	const struct fdt_reserve_entry *re;
++
++	for (i = 0; (re = fdt_mem_rsv(fdt, i)) != NULL; i++) {
++		if (fdt64_ld_(&re->size) == 0)
++			return i;
++	}
++	return -FDT_ERR_TRUNCATED;
++}
++
++static int nextprop_(const void *fdt, int offset)
++{
++	uint32_t tag;
++	int nextoffset;
++
++	do {
++		tag = fdt_next_tag(fdt, offset, &nextoffset);
++
++		switch (tag) {
++		case FDT_END:
++			if (nextoffset >= 0)
++				return -FDT_ERR_BADSTRUCTURE;
++			else
++				return nextoffset;
++
++		case FDT_PROP:
++			return offset;
++		}
++		offset = nextoffset;
++	} while (tag == FDT_NOP);
++
++	return -FDT_ERR_NOTFOUND;
++}
++
++int fdt_subnode_offset_namelen(const void *fdt, int offset,
++			       const char *name, int namelen)
++{
++	int depth;
++
++	FDT_RO_PROBE(fdt);
++
++	for (depth = 0;
++	     (offset >= 0) && (depth >= 0);
++	     offset = fdt_next_node(fdt, offset, &depth))
++		if ((depth == 1)
++		    && fdt_nodename_eq_(fdt, offset, name, namelen))
++			return offset;
++
++	if (depth < 0)
++		return -FDT_ERR_NOTFOUND;
++	return offset; /* error */
++}
++
++int fdt_subnode_offset(const void *fdt, int parentoffset,
++		       const char *name)
++{
++	return fdt_subnode_offset_namelen(fdt, parentoffset, name, strlen(name));
++}
++
++int fdt_path_offset_namelen(const void *fdt, const char *path, int namelen)
++{
++	const char *end = path + namelen;
++	const char *p = path;
++	int offset = 0;
++
++	FDT_RO_PROBE(fdt);
++
++	/* see if we have an alias */
++	if (*path != '/') {
++		const char *q = memchr(path, '/', end - p);
++
++		if (!q)
++			q = end;
++
++		p = fdt_get_alias_namelen(fdt, p, q - p);
++		if (!p)
++			return -FDT_ERR_BADPATH;
++		offset = fdt_path_offset(fdt, p);
++
++		p = q;
++	}
++
++	while (p < end) {
++		const char *q;
++
++		while (*p == '/') {
++			p++;
++			if (p == end)
++				return offset;
++		}
++		q = memchr(p, '/', end - p);
++		if (! q)
++			q = end;
++
++		offset = fdt_subnode_offset_namelen(fdt, offset, p, q-p);
++		if (offset < 0)
++			return offset;
++
++		p = q;
++	}
++
++	return offset;
++}
++
++int fdt_path_offset(const void *fdt, const char *path)
++{
++	return fdt_path_offset_namelen(fdt, path, strlen(path));
++}
++
++const char *fdt_get_name(const void *fdt, int nodeoffset, int *len)
++{
++	const struct fdt_node_header *nh = fdt_offset_ptr_(fdt, nodeoffset);
++	const char *nameptr;
++	int err;
++
++	if (((err = fdt_ro_probe_(fdt)) < 0)
++	    || ((err = fdt_check_node_offset_(fdt, nodeoffset)) < 0))
++			goto fail;
++
++	nameptr = nh->name;
++
++	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10) {
++		/*
++		 * For old FDT versions, match the naming conventions of V16:
++		 * give only the leaf name (after all /). The actual tree
++		 * contents are loosely checked.
++		 */
++		const char *leaf;
++		leaf = strrchr(nameptr, '/');
++		if (leaf == NULL) {
++			err = -FDT_ERR_BADSTRUCTURE;
++			goto fail;
++		}
++		nameptr = leaf+1;
++	}
++
++	if (len)
++		*len = strlen(nameptr);
++
++	return nameptr;
++
++ fail:
++	if (len)
++		*len = err;
++	return NULL;
++}
++
++int fdt_first_property_offset(const void *fdt, int nodeoffset)
++{
++	int offset;
++
++	if ((offset = fdt_check_node_offset_(fdt, nodeoffset)) < 0)
++		return offset;
++
++	return nextprop_(fdt, offset);
++}
++
++int fdt_next_property_offset(const void *fdt, int offset)
++{
++	if ((offset = fdt_check_prop_offset_(fdt, offset)) < 0)
++		return offset;
++
++	return nextprop_(fdt, offset);
++}
++
++static const struct fdt_property *fdt_get_property_by_offset_(const void *fdt,
++						              int offset,
++						              int *lenp)
++{
++	int err;
++	const struct fdt_property *prop;
++
++	if (!can_assume(VALID_INPUT) &&
++	    (err = fdt_check_prop_offset_(fdt, offset)) < 0) {
++		if (lenp)
++			*lenp = err;
++		return NULL;
++	}
++
++	prop = fdt_offset_ptr_(fdt, offset);
++
++	if (lenp)
++		*lenp = fdt32_ld_(&prop->len);
++
++	return prop;
++}
++
++const struct fdt_property *fdt_get_property_by_offset(const void *fdt,
++						      int offset,
++						      int *lenp)
++{
++	/* Prior to version 16, properties may need realignment
++	 * and this API does not work. fdt_getprop_*() will, however. */
++
++	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10) {
++		if (lenp)
++			*lenp = -FDT_ERR_BADVERSION;
++		return NULL;
++	}
++
++	return fdt_get_property_by_offset_(fdt, offset, lenp);
++}
++
++static const struct fdt_property *fdt_get_property_namelen_(const void *fdt,
++						            int offset,
++						            const char *name,
++						            int namelen,
++							    int *lenp,
++							    int *poffset)
++{
++	for (offset = fdt_first_property_offset(fdt, offset);
++	     (offset >= 0);
++	     (offset = fdt_next_property_offset(fdt, offset))) {
++		const struct fdt_property *prop;
++
++		prop = fdt_get_property_by_offset_(fdt, offset, lenp);
++		if (!can_assume(LIBFDT_FLAWLESS) && !prop) {
++			offset = -FDT_ERR_INTERNAL;
++			break;
++		}
++		if (fdt_string_eq_(fdt, fdt32_ld_(&prop->nameoff),
++				   name, namelen)) {
++			if (poffset)
++				*poffset = offset;
++			return prop;
++		}
++	}
++
++	if (lenp)
++		*lenp = offset;
++	return NULL;
++}
++
++
++const struct fdt_property *fdt_get_property_namelen(const void *fdt,
++						    int offset,
++						    const char *name,
++						    int namelen, int *lenp)
++{
++	/* Prior to version 16, properties may need realignment
++	 * and this API does not work. fdt_getprop_*() will, however. */
++	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10) {
++		if (lenp)
++			*lenp = -FDT_ERR_BADVERSION;
++		return NULL;
++	}
++
++	return fdt_get_property_namelen_(fdt, offset, name, namelen, lenp,
++					 NULL);
++}
++
++
++const struct fdt_property *fdt_get_property(const void *fdt,
++					    int nodeoffset,
++					    const char *name, int *lenp)
++{
++	return fdt_get_property_namelen(fdt, nodeoffset, name,
++					strlen(name), lenp);
++}
++
++const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
++				const char *name, int namelen, int *lenp)
++{
++	int poffset;
++	const struct fdt_property *prop;
++
++	prop = fdt_get_property_namelen_(fdt, nodeoffset, name, namelen, lenp,
++					 &poffset);
++	if (!prop)
++		return NULL;
++
++	/* Handle realignment */
++	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10 &&
++	    (poffset + sizeof(*prop)) % 8 && fdt32_ld_(&prop->len) >= 8)
++		return prop->data + 4;
++	return prop->data;
++}
++
++const void *fdt_getprop_by_offset(const void *fdt, int offset,
++				  const char **namep, int *lenp)
++{
++	const struct fdt_property *prop;
++
++	prop = fdt_get_property_by_offset_(fdt, offset, lenp);
++	if (!prop)
++		return NULL;
++	if (namep) {
++		const char *name;
++		int namelen;
++
++		if (!can_assume(VALID_INPUT)) {
++			name = fdt_get_string(fdt, fdt32_ld_(&prop->nameoff),
++					      &namelen);
++			if (!name) {
++				if (lenp)
++					*lenp = namelen;
++				return NULL;
++			}
++			*namep = name;
++		} else {
++			*namep = fdt_string(fdt, fdt32_ld_(&prop->nameoff));
++		}
++	}
++
++	/* Handle realignment */
++	if (!can_assume(LATEST) && fdt_version(fdt) < 0x10 &&
++	    (offset + sizeof(*prop)) % 8 && fdt32_ld_(&prop->len) >= 8)
++		return prop->data + 4;
++	return prop->data;
++}
++
++const void *fdt_getprop(const void *fdt, int nodeoffset,
++			const char *name, int *lenp)
++{
++	return fdt_getprop_namelen(fdt, nodeoffset, name, strlen(name), lenp);
++}
++
++uint32_t fdt_get_phandle(const void *fdt, int nodeoffset)
++{
++	const fdt32_t *php;
++	int len;
++
++	/* FIXME: This is a bit sub-optimal, since we potentially scan
++	 * over all the properties twice. */
++	php = fdt_getprop(fdt, nodeoffset, "phandle", &len);
++	if (!php || (len != sizeof(*php))) {
++		php = fdt_getprop(fdt, nodeoffset, "linux,phandle", &len);
++		if (!php || (len != sizeof(*php)))
++			return 0;
++	}
++
++	return fdt32_ld_(php);
++}
++
++const char *fdt_get_alias_namelen(const void *fdt,
++				  const char *name, int namelen)
++{
++	int aliasoffset;
++
++	aliasoffset = fdt_path_offset(fdt, "/aliases");
++	if (aliasoffset < 0)
++		return NULL;
++
++	return fdt_getprop_namelen(fdt, aliasoffset, name, namelen, NULL);
++}
++
++const char *fdt_get_alias(const void *fdt, const char *name)
++{
++	return fdt_get_alias_namelen(fdt, name, strlen(name));
++}
++
++int fdt_get_path(const void *fdt, int nodeoffset, char *buf, int buflen)
++{
++	int pdepth = 0, p = 0;
++	int offset, depth, namelen;
++	const char *name;
++
++	FDT_RO_PROBE(fdt);
++
++	if (buflen < 2)
++		return -FDT_ERR_NOSPACE;
++
++	for (offset = 0, depth = 0;
++	     (offset >= 0) && (offset <= nodeoffset);
++	     offset = fdt_next_node(fdt, offset, &depth)) {
++		while (pdepth > depth) {
++			do {
++				p--;
++			} while (buf[p-1] != '/');
++			pdepth--;
++		}
++
++		if (pdepth >= depth) {
++			name = fdt_get_name(fdt, offset, &namelen);
++			if (!name)
++				return namelen;
++			if ((p + namelen + 1) <= buflen) {
++				memcpy(buf + p, name, namelen);
++				p += namelen;
++				buf[p++] = '/';
++				pdepth++;
++			}
++		}
++
++		if (offset == nodeoffset) {
++			if (pdepth < (depth + 1))
++				return -FDT_ERR_NOSPACE;
++
++			if (p > 1) /* special case so that root path is "/", not "" */
++				p--;
++			buf[p] = '\0';
++			return 0;
++		}
++	}
++
++	if ((offset == -FDT_ERR_NOTFOUND) || (offset >= 0))
++		return -FDT_ERR_BADOFFSET;
++	else if (offset == -FDT_ERR_BADOFFSET)
++		return -FDT_ERR_BADSTRUCTURE;
++
++	return offset; /* error from fdt_next_node() */
++}
++
++int fdt_supernode_atdepth_offset(const void *fdt, int nodeoffset,
++				 int supernodedepth, int *nodedepth)
++{
++	int offset, depth;
++	int supernodeoffset = -FDT_ERR_INTERNAL;
++
++	FDT_RO_PROBE(fdt);
++
++	if (supernodedepth < 0)
++		return -FDT_ERR_NOTFOUND;
++
++	for (offset = 0, depth = 0;
++	     (offset >= 0) && (offset <= nodeoffset);
++	     offset = fdt_next_node(fdt, offset, &depth)) {
++		if (depth == supernodedepth)
++			supernodeoffset = offset;
++
++		if (offset == nodeoffset) {
++			if (nodedepth)
++				*nodedepth = depth;
++
++			if (supernodedepth > depth)
++				return -FDT_ERR_NOTFOUND;
++			else
++				return supernodeoffset;
++		}
++	}
++
++	if (!can_assume(VALID_INPUT)) {
++		if ((offset == -FDT_ERR_NOTFOUND) || (offset >= 0))
++			return -FDT_ERR_BADOFFSET;
++		else if (offset == -FDT_ERR_BADOFFSET)
++			return -FDT_ERR_BADSTRUCTURE;
++	}
++
++	return offset; /* error from fdt_next_node() */
++}
++
++int fdt_node_depth(const void *fdt, int nodeoffset)
++{
++	int nodedepth;
++	int err;
++
++	err = fdt_supernode_atdepth_offset(fdt, nodeoffset, 0, &nodedepth);
++	if (err)
++		return (can_assume(LIBFDT_FLAWLESS) || err < 0) ? err :
++			-FDT_ERR_INTERNAL;
++	return nodedepth;
++}
++
++int fdt_parent_offset(const void *fdt, int nodeoffset)
++{
++	int nodedepth = fdt_node_depth(fdt, nodeoffset);
++
++	if (nodedepth < 0)
++		return nodedepth;
++	return fdt_supernode_atdepth_offset(fdt, nodeoffset,
++					    nodedepth - 1, NULL);
++}
++
++int fdt_node_offset_by_prop_value(const void *fdt, int startoffset,
++				  const char *propname,
++				  const void *propval, int proplen)
++{
++	int offset;
++	const void *val;
++	int len;
++
++	FDT_RO_PROBE(fdt);
++
++	/* FIXME: The algorithm here is pretty horrible: we scan each
++	 * property of a node in fdt_getprop(), then if that didn't
++	 * find what we want, we scan over them again making our way
++	 * to the next node.  Still it's the easiest to implement
++	 * approach; performance can come later. */
++	for (offset = fdt_next_node(fdt, startoffset, NULL);
++	     offset >= 0;
++	     offset = fdt_next_node(fdt, offset, NULL)) {
++		val = fdt_getprop(fdt, offset, propname, &len);
++		if (val && (len == proplen)
++		    && (memcmp(val, propval, len) == 0))
++			return offset;
++	}
++
++	return offset; /* error from fdt_next_node() */
++}
++
++int fdt_node_offset_by_phandle(const void *fdt, uint32_t phandle)
++{
++	int offset;
++
++	if ((phandle == 0) || (phandle == ~0U))
++		return -FDT_ERR_BADPHANDLE;
++
++	FDT_RO_PROBE(fdt);
++
++	/* FIXME: The algorithm here is pretty horrible: we
++	 * potentially scan each property of a node in
++	 * fdt_get_phandle(), then if that didn't find what
++	 * we want, we scan over them again making our way to the next
++	 * node.  Still it's the easiest to implement approach;
++	 * performance can come later. */
++	for (offset = fdt_next_node(fdt, -1, NULL);
++	     offset >= 0;
++	     offset = fdt_next_node(fdt, offset, NULL)) {
++		if (fdt_get_phandle(fdt, offset) == phandle)
++			return offset;
++	}
++
++	return offset; /* error from fdt_next_node() */
++}
++
++int fdt_stringlist_contains(const char *strlist, int listlen, const char *str)
++{
++	int len = strlen(str);
++	const char *p;
++
++	while (listlen >= len) {
++		if (memcmp(str, strlist, len+1) == 0)
++			return 1;
++		p = memchr(strlist, '\0', listlen);
++		if (!p)
++			return 0; /* malformed strlist.. */
++		listlen -= (p-strlist) + 1;
++		strlist = p + 1;
++	}
++	return 0;
++}
++
++int fdt_stringlist_count(const void *fdt, int nodeoffset, const char *property)
++{
++	const char *list, *end;
++	int length, count = 0;
++
++	list = fdt_getprop(fdt, nodeoffset, property, &length);
++	if (!list)
++		return length;
++
++	end = list + length;
++
++	while (list < end) {
++		length = strnlen(list, end - list) + 1;
++
++		/* Abort if the last string isn't properly NUL-terminated. */
++		if (list + length > end)
++			return -FDT_ERR_BADVALUE;
++
++		list += length;
++		count++;
++	}
++
++	return count;
++}
++
++int fdt_stringlist_search(const void *fdt, int nodeoffset, const char *property,
++			  const char *string)
++{
++	int length, len, idx = 0;
++	const char *list, *end;
++
++	list = fdt_getprop(fdt, nodeoffset, property, &length);
++	if (!list)
++		return length;
++
++	len = strlen(string) + 1;
++	end = list + length;
++
++	while (list < end) {
++		length = strnlen(list, end - list) + 1;
++
++		/* Abort if the last string isn't properly NUL-terminated. */
++		if (list + length > end)
++			return -FDT_ERR_BADVALUE;
++
++		if (length == len && memcmp(list, string, length) == 0)
++			return idx;
++
++		list += length;
++		idx++;
++	}
++
++	return -FDT_ERR_NOTFOUND;
++}
++
++const char *fdt_stringlist_get(const void *fdt, int nodeoffset,
++			       const char *property, int idx,
++			       int *lenp)
++{
++	const char *list, *end;
++	int length;
++
++	list = fdt_getprop(fdt, nodeoffset, property, &length);
++	if (!list) {
++		if (lenp)
++			*lenp = length;
++
++		return NULL;
++	}
++
++	end = list + length;
++
++	while (list < end) {
++		length = strnlen(list, end - list) + 1;
++
++		/* Abort if the last string isn't properly NUL-terminated. */
++		if (list + length > end) {
++			if (lenp)
++				*lenp = -FDT_ERR_BADVALUE;
++
++			return NULL;
++		}
++
++		if (idx == 0) {
++			if (lenp)
++				*lenp = length - 1;
++
++			return list;
++		}
++
++		list += length;
++		idx--;
++	}
++
++	if (lenp)
++		*lenp = -FDT_ERR_NOTFOUND;
++
++	return NULL;
++}
++
++int fdt_node_check_compatible(const void *fdt, int nodeoffset,
++			      const char *compatible)
++{
++	const void *prop;
++	int len;
++
++	prop = fdt_getprop(fdt, nodeoffset, "compatible", &len);
++	if (!prop)
++		return len;
++
++	return !fdt_stringlist_contains(prop, len, compatible);
++}
++
++int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
++				  const char *compatible)
++{
++	int offset, err;
++
++	FDT_RO_PROBE(fdt);
++
++	/* FIXME: The algorithm here is pretty horrible: we scan each
++	 * property of a node in fdt_node_check_compatible(), then if
++	 * that didn't find what we want, we scan over them again
++	 * making our way to the next node.  Still it's the easiest to
++	 * implement approach; performance can come later. */
++	for (offset = fdt_next_node(fdt, startoffset, NULL);
++	     offset >= 0;
++	     offset = fdt_next_node(fdt, offset, NULL)) {
++		err = fdt_node_check_compatible(fdt, offset, compatible);
++		if ((err < 0) && (err != -FDT_ERR_NOTFOUND))
++			return err;
++		else if (err == 0)
++			return offset;
++	}
++
++	return offset; /* error from fdt_next_node() */
++}
+diff --git a/common/libfdt/fdt_rw.c b/common/libfdt/fdt_rw.c
+new file mode 100644
+index 0000000..3621d36
+--- /dev/null
++++ b/common/libfdt/fdt_rw.c
+@@ -0,0 +1,500 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++static int fdt_blocks_misordered_(const void *fdt,
++				  int mem_rsv_size, int struct_size)
++{
++	return (fdt_off_mem_rsvmap(fdt) < FDT_ALIGN(sizeof(struct fdt_header), 8))
++		|| (fdt_off_dt_struct(fdt) <
++		    (fdt_off_mem_rsvmap(fdt) + mem_rsv_size))
++		|| (fdt_off_dt_strings(fdt) <
++		    (fdt_off_dt_struct(fdt) + struct_size))
++		|| (fdt_totalsize(fdt) <
++		    (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)));
++}
++
++static int fdt_rw_probe_(void *fdt)
++{
++	if (can_assume(VALID_DTB))
++		return 0;
++	FDT_RO_PROBE(fdt);
++
++	if (!can_assume(LATEST) && fdt_version(fdt) < 17)
++		return -FDT_ERR_BADVERSION;
++	if (fdt_blocks_misordered_(fdt, sizeof(struct fdt_reserve_entry),
++				   fdt_size_dt_struct(fdt)))
++		return -FDT_ERR_BADLAYOUT;
++	if (!can_assume(LATEST) && fdt_version(fdt) > 17)
++		fdt_set_version(fdt, 17);
++
++	return 0;
++}
++
++#define FDT_RW_PROBE(fdt) \
++	{ \
++		int err_; \
++		if ((err_ = fdt_rw_probe_(fdt)) != 0) \
++			return err_; \
++	}
++
++static inline unsigned int fdt_data_size_(void *fdt)
++{
++	return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
++}
++
++static int fdt_splice_(void *fdt, void *splicepoint, int oldlen, int newlen)
++{
++	char *p = splicepoint;
++	unsigned int dsize = fdt_data_size_(fdt);
++	size_t soff = p - (char *)fdt;
++
++	if ((oldlen < 0) || (soff + oldlen < soff) || (soff + oldlen > dsize))
++		return -FDT_ERR_BADOFFSET;
++	if ((p < (char *)fdt) || (dsize + newlen < (unsigned)oldlen))
++		return -FDT_ERR_BADOFFSET;
++	if (dsize - oldlen + newlen > fdt_totalsize(fdt))
++		return -FDT_ERR_NOSPACE;
++	memmove(p + newlen, p + oldlen, ((char *)fdt + dsize) - (p + oldlen));
++	return 0;
++}
++
++static int fdt_splice_mem_rsv_(void *fdt, struct fdt_reserve_entry *p,
++			       int oldn, int newn)
++{
++	int delta = (newn - oldn) * sizeof(*p);
++	int err;
++	err = fdt_splice_(fdt, p, oldn * sizeof(*p), newn * sizeof(*p));
++	if (err)
++		return err;
++	fdt_set_off_dt_struct(fdt, fdt_off_dt_struct(fdt) + delta);
++	fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
++	return 0;
++}
++
++static int fdt_splice_struct_(void *fdt, void *p,
++			      int oldlen, int newlen)
++{
++	int delta = newlen - oldlen;
++	int err;
++
++	if ((err = fdt_splice_(fdt, p, oldlen, newlen)))
++		return err;
++
++	fdt_set_size_dt_struct(fdt, fdt_size_dt_struct(fdt) + delta);
++	fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
++	return 0;
++}
++
++/* Must only be used to roll back in case of error */
++static void fdt_del_last_string_(void *fdt, const char *s)
++{
++	int newlen = strlen(s) + 1;
++
++	fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) - newlen);
++}
++
++static int fdt_splice_string_(void *fdt, int newlen)
++{
++	void *p = (char *)fdt
++		+ fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
++	int err;
++
++	if ((err = fdt_splice_(fdt, p, 0, newlen)))
++		return err;
++
++	fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) + newlen);
++	return 0;
++}
++
++/**
++ * fdt_find_add_string_() - Find or allocate a string
++ *
++ * @fdt: pointer to the device tree to check/adjust
++ * @s: string to find/add
++ * @allocated: Set to 0 if the string was found, 1 if not found and so
++ *	allocated. Ignored if can_assume(NO_ROLLBACK)
++ * @return offset of string in the string table (whether found or added)
++ */
++static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
++{
++	char *strtab = (char *)fdt + fdt_off_dt_strings(fdt);
++	const char *p;
++	char *new;
++	int len = strlen(s) + 1;
++	int err;
++
++	if (!can_assume(NO_ROLLBACK))
++		*allocated = 0;
++
++	p = fdt_find_string_(strtab, fdt_size_dt_strings(fdt), s);
++	if (p)
++		/* found it */
++		return (p - strtab);
++
++	new = strtab + fdt_size_dt_strings(fdt);
++	err = fdt_splice_string_(fdt, len);
++	if (err)
++		return err;
++
++	if (!can_assume(NO_ROLLBACK))
++		*allocated = 1;
++
++	memcpy(new, s, len);
++	return (new - strtab);
++}
++
++int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size)
++{
++	struct fdt_reserve_entry *re;
++	int err;
++
++	FDT_RW_PROBE(fdt);
++
++	re = fdt_mem_rsv_w_(fdt, fdt_num_mem_rsv(fdt));
++	err = fdt_splice_mem_rsv_(fdt, re, 0, 1);
++	if (err)
++		return err;
++
++	re->address = cpu_to_fdt64(address);
++	re->size = cpu_to_fdt64(size);
++	return 0;
++}
++
++int fdt_del_mem_rsv(void *fdt, int n)
++{
++	struct fdt_reserve_entry *re = fdt_mem_rsv_w_(fdt, n);
++
++	FDT_RW_PROBE(fdt);
++
++	if (n >= fdt_num_mem_rsv(fdt))
++		return -FDT_ERR_NOTFOUND;
++
++	return fdt_splice_mem_rsv_(fdt, re, 1, 0);
++}
++
++static int fdt_resize_property_(void *fdt, int nodeoffset, const char *name,
++				int len, struct fdt_property **prop)
++{
++	int oldlen;
++	int err;
++
++	*prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
++	if (!*prop)
++		return oldlen;
++
++	if ((err = fdt_splice_struct_(fdt, (*prop)->data, FDT_TAGALIGN(oldlen),
++				      FDT_TAGALIGN(len))))
++		return err;
++
++	(*prop)->len = cpu_to_fdt32(len);
++	return 0;
++}
++
++static int fdt_add_property_(void *fdt, int nodeoffset, const char *name,
++			     int len, struct fdt_property **prop)
++{
++	int proplen;
++	int nextoffset;
++	int namestroff;
++	int err;
++	int allocated;
++
++	if ((nextoffset = fdt_check_node_offset_(fdt, nodeoffset)) < 0)
++		return nextoffset;
++
++	namestroff = fdt_find_add_string_(fdt, name, &allocated);
++	if (namestroff < 0)
++		return namestroff;
++
++	*prop = fdt_offset_ptr_w_(fdt, nextoffset);
++	proplen = sizeof(**prop) + FDT_TAGALIGN(len);
++
++	err = fdt_splice_struct_(fdt, *prop, 0, proplen);
++	if (err) {
++		/* Delete the string if we failed to add it */
++		if (!can_assume(NO_ROLLBACK) && allocated)
++			fdt_del_last_string_(fdt, name);
++		return err;
++	}
++
++	(*prop)->tag = cpu_to_fdt32(FDT_PROP);
++	(*prop)->nameoff = cpu_to_fdt32(namestroff);
++	(*prop)->len = cpu_to_fdt32(len);
++	return 0;
++}
++
++int fdt_set_name(void *fdt, int nodeoffset, const char *name)
++{
++	char *namep;
++	int oldlen, newlen;
++	int err;
++
++	FDT_RW_PROBE(fdt);
++
++	namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen);
++	if (!namep)
++		return oldlen;
++
++	newlen = strlen(name);
++
++	err = fdt_splice_struct_(fdt, namep, FDT_TAGALIGN(oldlen+1),
++				 FDT_TAGALIGN(newlen+1));
++	if (err)
++		return err;
++
++	memcpy(namep, name, newlen+1);
++	return 0;
++}
++
++int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name,
++			    int len, void **prop_data)
++{
++	struct fdt_property *prop;
++	int err;
++
++	FDT_RW_PROBE(fdt);
++
++	err = fdt_resize_property_(fdt, nodeoffset, name, len, &prop);
++	if (err == -FDT_ERR_NOTFOUND)
++		err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
++	if (err)
++		return err;
++
++	*prop_data = prop->data;
++	return 0;
++}
++
++int fdt_setprop(void *fdt, int nodeoffset, const char *name,
++		const void *val, int len)
++{
++	void *prop_data;
++	int err;
++
++	err = fdt_setprop_placeholder(fdt, nodeoffset, name, len, &prop_data);
++	if (err)
++		return err;
++
++	if (len)
++		memcpy(prop_data, val, len);
++	return 0;
++}
++
++int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
++		   const void *val, int len)
++{
++	struct fdt_property *prop;
++	int err, oldlen, newlen;
++
++	FDT_RW_PROBE(fdt);
++
++	prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
++	if (prop) {
++		newlen = len + oldlen;
++		err = fdt_splice_struct_(fdt, prop->data,
++					 FDT_TAGALIGN(oldlen),
++					 FDT_TAGALIGN(newlen));
++		if (err)
++			return err;
++		prop->len = cpu_to_fdt32(newlen);
++		memcpy(prop->data + oldlen, val, len);
++	} else {
++		err = fdt_add_property_(fdt, nodeoffset, name, len, &prop);
++		if (err)
++			return err;
++		memcpy(prop->data, val, len);
++	}
++	return 0;
++}
++
++int fdt_delprop(void *fdt, int nodeoffset, const char *name)
++{
++	struct fdt_property *prop;
++	int len, proplen;
++
++	FDT_RW_PROBE(fdt);
++
++	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
++	if (!prop)
++		return len;
++
++	proplen = sizeof(*prop) + FDT_TAGALIGN(len);
++	return fdt_splice_struct_(fdt, prop, proplen, 0);
++}
++
++int fdt_add_subnode_namelen(void *fdt, int parentoffset,
++			    const char *name, int namelen)
++{
++	struct fdt_node_header *nh;
++	int offset, nextoffset;
++	int nodelen;
++	int err;
++	uint32_t tag;
++	fdt32_t *endtag;
++
++	FDT_RW_PROBE(fdt);
++
++	offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen);
++	if (offset >= 0)
++		return -FDT_ERR_EXISTS;
++	else if (offset != -FDT_ERR_NOTFOUND)
++		return offset;
++
++	/* Try to place the new node after the parent's properties */
++	tag = fdt_next_tag(fdt, parentoffset, &nextoffset);
++	/* the fdt_subnode_offset_namelen() should ensure this never hits */
++	if (!can_assume(LIBFDT_FLAWLESS) && (tag != FDT_BEGIN_NODE))
++		return -FDT_ERR_INTERNAL;
++	do {
++		offset = nextoffset;
++		tag = fdt_next_tag(fdt, offset, &nextoffset);
++	} while ((tag == FDT_PROP) || (tag == FDT_NOP));
++
++	nh = fdt_offset_ptr_w_(fdt, offset);
++	nodelen = sizeof(*nh) + FDT_TAGALIGN(namelen+1) + FDT_TAGSIZE;
++
++	err = fdt_splice_struct_(fdt, nh, 0, nodelen);
++	if (err)
++		return err;
++
++	nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
++	memset(nh->name, 0, FDT_TAGALIGN(namelen+1));
++	memcpy(nh->name, name, namelen);
++	endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE);
++	*endtag = cpu_to_fdt32(FDT_END_NODE);
++
++	return offset;
++}
++
++int fdt_add_subnode(void *fdt, int parentoffset, const char *name)
++{
++	return fdt_add_subnode_namelen(fdt, parentoffset, name, strlen(name));
++}
++
++int fdt_del_node(void *fdt, int nodeoffset)
++{
++	int endoffset;
++
++	FDT_RW_PROBE(fdt);
++
++	endoffset = fdt_node_end_offset_(fdt, nodeoffset);
++	if (endoffset < 0)
++		return endoffset;
++
++	return fdt_splice_struct_(fdt, fdt_offset_ptr_w_(fdt, nodeoffset),
++				  endoffset - nodeoffset, 0);
++}
++
++static void fdt_packblocks_(const char *old, char *new,
++			    int mem_rsv_size,
++			    int struct_size,
++			    int strings_size)
++{
++	int mem_rsv_off, struct_off, strings_off;
++
++	mem_rsv_off = FDT_ALIGN(sizeof(struct fdt_header), 8);
++	struct_off = mem_rsv_off + mem_rsv_size;
++	strings_off = struct_off + struct_size;
++
++	memmove(new + mem_rsv_off, old + fdt_off_mem_rsvmap(old), mem_rsv_size);
++	fdt_set_off_mem_rsvmap(new, mem_rsv_off);
++
++	memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size);
++	fdt_set_off_dt_struct(new, struct_off);
++	fdt_set_size_dt_struct(new, struct_size);
++
++	memmove(new + strings_off, old + fdt_off_dt_strings(old), strings_size);
++	fdt_set_off_dt_strings(new, strings_off);
++	fdt_set_size_dt_strings(new, fdt_size_dt_strings(old));
++}
++
++int fdt_open_into(const void *fdt, void *buf, int bufsize)
++{
++	int err;
++	int mem_rsv_size, struct_size;
++	int newsize;
++	const char *fdtstart = fdt;
++	const char *fdtend = fdtstart + fdt_totalsize(fdt);
++	char *tmp;
++
++	FDT_RO_PROBE(fdt);
++
++	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
++		* sizeof(struct fdt_reserve_entry);
++
++	if (can_assume(LATEST) || fdt_version(fdt) >= 17) {
++		struct_size = fdt_size_dt_struct(fdt);
++	} else if (fdt_version(fdt) == 16) {
++		struct_size = 0;
++		while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END)
++			;
++		if (struct_size < 0)
++			return struct_size;
++	} else {
++		return -FDT_ERR_BADVERSION;
++	}
++
++	if (can_assume(LIBFDT_ORDER) ||
++	    !fdt_blocks_misordered_(fdt, mem_rsv_size, struct_size)) {
++		/* no further work necessary */
++		err = fdt_move(fdt, buf, bufsize);
++		if (err)
++			return err;
++		fdt_set_version(buf, 17);
++		fdt_set_size_dt_struct(buf, struct_size);
++		fdt_set_totalsize(buf, bufsize);
++		return 0;
++	}
++
++	/* Need to reorder */
++	newsize = FDT_ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size
++		+ struct_size + fdt_size_dt_strings(fdt);
++
++	if (bufsize < newsize)
++		return -FDT_ERR_NOSPACE;
++
++	/* First attempt to build converted tree at beginning of buffer */
++	tmp = buf;
++	/* But if that overlaps with the old tree... */
++	if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) {
++		/* Try right after the old tree instead */
++		tmp = (char *)(uintptr_t)fdtend;
++		if ((tmp + newsize) > ((char *)buf + bufsize))
++			return -FDT_ERR_NOSPACE;
++	}
++
++	fdt_packblocks_(fdt, tmp, mem_rsv_size, struct_size,
++			fdt_size_dt_strings(fdt));
++	memmove(buf, tmp, newsize);
++
++	fdt_set_magic(buf, FDT_MAGIC);
++	fdt_set_totalsize(buf, bufsize);
++	fdt_set_version(buf, 17);
++	fdt_set_last_comp_version(buf, 16);
++	fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt));
++
++	return 0;
++}
++
++int fdt_pack(void *fdt)
++{
++	int mem_rsv_size;
++
++	FDT_RW_PROBE(fdt);
++
++	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
++		* sizeof(struct fdt_reserve_entry);
++	fdt_packblocks_(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt),
++			fdt_size_dt_strings(fdt));
++	fdt_set_totalsize(fdt, fdt_data_size_(fdt));
++
++	return 0;
++}
+diff --git a/common/libfdt/fdt_strerror.c b/common/libfdt/fdt_strerror.c
+new file mode 100644
+index 0000000..b435693
+--- /dev/null
++++ b/common/libfdt/fdt_strerror.c
+@@ -0,0 +1,59 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ *     EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++struct fdt_errtabent {
++	const char *str;
++};
++
++#define FDT_ERRTABENT(val) \
++	[(val)] = { .str = #val, }
++
++static struct fdt_errtabent fdt_errtable[] = {
++	FDT_ERRTABENT(FDT_ERR_NOTFOUND),
++	FDT_ERRTABENT(FDT_ERR_EXISTS),
++	FDT_ERRTABENT(FDT_ERR_NOSPACE),
++
++	FDT_ERRTABENT(FDT_ERR_BADOFFSET),
++	FDT_ERRTABENT(FDT_ERR_BADPATH),
++	FDT_ERRTABENT(FDT_ERR_BADPHANDLE),
++	FDT_ERRTABENT(FDT_ERR_BADSTATE),
++
++	FDT_ERRTABENT(FDT_ERR_TRUNCATED),
++	FDT_ERRTABENT(FDT_ERR_BADMAGIC),
++	FDT_ERRTABENT(FDT_ERR_BADVERSION),
++	FDT_ERRTABENT(FDT_ERR_BADSTRUCTURE),
++	FDT_ERRTABENT(FDT_ERR_BADLAYOUT),
++	FDT_ERRTABENT(FDT_ERR_INTERNAL),
++	FDT_ERRTABENT(FDT_ERR_BADNCELLS),
++	FDT_ERRTABENT(FDT_ERR_BADVALUE),
++	FDT_ERRTABENT(FDT_ERR_BADOVERLAY),
++	FDT_ERRTABENT(FDT_ERR_NOPHANDLES),
++	FDT_ERRTABENT(FDT_ERR_BADFLAGS),
++};
++#define FDT_ERRTABSIZE	((int)(sizeof(fdt_errtable) / sizeof(fdt_errtable[0])))
++
++const char *fdt_strerror(int errval)
++{
++	if (errval > 0)
++		return "<valid offset/length>";
++	else if (errval == 0)
++		return "<no error>";
++	else if (-errval < FDT_ERRTABSIZE) {
++		const char *s = fdt_errtable[-errval].str;
++
++		if (s)
++			return s;
++	}
++
++	return "<unknown error>";
++}
+diff --git a/common/libfdt/fdt_sw.c b/common/libfdt/fdt_sw.c
+new file mode 100644
+index 0000000..4c569ee
+--- /dev/null
++++ b/common/libfdt/fdt_sw.c
+@@ -0,0 +1,384 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++static int fdt_sw_probe_(void *fdt)
++{
++	if (!can_assume(VALID_INPUT)) {
++		if (fdt_magic(fdt) == FDT_MAGIC)
++			return -FDT_ERR_BADSTATE;
++		else if (fdt_magic(fdt) != FDT_SW_MAGIC)
++			return -FDT_ERR_BADMAGIC;
++	}
++
++	return 0;
++}
++
++#define FDT_SW_PROBE(fdt) \
++	{ \
++		int err; \
++		if ((err = fdt_sw_probe_(fdt)) != 0) \
++			return err; \
++	}
++
++/* 'memrsv' state:	Initial state after fdt_create()
++ *
++ * Allowed functions:
++ *	fdt_add_reservemap_entry()
++ *	fdt_finish_reservemap()		[moves to 'struct' state]
++ */
++static int fdt_sw_probe_memrsv_(void *fdt)
++{
++	int err = fdt_sw_probe_(fdt);
++	if (err)
++		return err;
++
++	if (!can_assume(VALID_INPUT) && fdt_off_dt_strings(fdt) != 0)
++		return -FDT_ERR_BADSTATE;
++	return 0;
++}
++
++#define FDT_SW_PROBE_MEMRSV(fdt) \
++	{ \
++		int err; \
++		if ((err = fdt_sw_probe_memrsv_(fdt)) != 0) \
++			return err; \
++	}
++
++/* 'struct' state:	Enter this state after fdt_finish_reservemap()
++ *
++ * Allowed functions:
++ *	fdt_begin_node()
++ *	fdt_end_node()
++ *	fdt_property*()
++ *	fdt_finish()			[moves to 'complete' state]
++ */
++static int fdt_sw_probe_struct_(void *fdt)
++{
++	int err = fdt_sw_probe_(fdt);
++	if (err)
++		return err;
++
++	if (!can_assume(VALID_INPUT) &&
++	    fdt_off_dt_strings(fdt) != fdt_totalsize(fdt))
++		return -FDT_ERR_BADSTATE;
++	return 0;
++}
++
++#define FDT_SW_PROBE_STRUCT(fdt) \
++	{ \
++		int err; \
++		if ((err = fdt_sw_probe_struct_(fdt)) != 0) \
++			return err; \
++	}
++
++static inline uint32_t sw_flags(void *fdt)
++{
++	/* assert: (fdt_magic(fdt) == FDT_SW_MAGIC) */
++	return fdt_last_comp_version(fdt);
++}
++
++/* 'complete' state:	Enter this state after fdt_finish()
++ *
++ * Allowed functions: none
++ */
++
++static void *fdt_grab_space_(void *fdt, size_t len)
++{
++	unsigned int offset = fdt_size_dt_struct(fdt);
++	unsigned int spaceleft;
++
++	spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
++		- fdt_size_dt_strings(fdt);
++
++	if ((offset + len < offset) || (offset + len > spaceleft))
++		return NULL;
++
++	fdt_set_size_dt_struct(fdt, offset + len);
++	return fdt_offset_ptr_w_(fdt, offset);
++}
++
++int fdt_create_with_flags(void *buf, int bufsize, uint32_t flags)
++{
++	const int hdrsize = FDT_ALIGN(sizeof(struct fdt_header),
++				      sizeof(struct fdt_reserve_entry));
++	void *fdt = buf;
++
++	if (bufsize < hdrsize)
++		return -FDT_ERR_NOSPACE;
++
++	if (flags & ~FDT_CREATE_FLAGS_ALL)
++		return -FDT_ERR_BADFLAGS;
++
++	memset(buf, 0, bufsize);
++
++	/*
++	 * magic and last_comp_version keep intermediate state during the fdt
++	 * creation process, which is replaced with the proper FDT format by
++	 * fdt_finish().
++	 *
++	 * flags should be accessed with sw_flags().
++	 */
++	fdt_set_magic(fdt, FDT_SW_MAGIC);
++	fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
++	fdt_set_last_comp_version(fdt, flags);
++
++	fdt_set_totalsize(fdt,  bufsize);
++
++	fdt_set_off_mem_rsvmap(fdt, hdrsize);
++	fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
++	fdt_set_off_dt_strings(fdt, 0);
++
++	return 0;
++}
++
++int fdt_create(void *buf, int bufsize)
++{
++	return fdt_create_with_flags(buf, bufsize, 0);
++}
++
++int fdt_resize(void *fdt, void *buf, int bufsize)
++{
++	size_t headsize, tailsize;
++	char *oldtail, *newtail;
++
++	FDT_SW_PROBE(fdt);
++
++	if (bufsize < 0)
++		return -FDT_ERR_NOSPACE;
++
++	headsize = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
++	tailsize = fdt_size_dt_strings(fdt);
++
++	if (!can_assume(VALID_DTB) &&
++	    headsize + tailsize > fdt_totalsize(fdt))
++		return -FDT_ERR_INTERNAL;
++
++	if ((headsize + tailsize) > (unsigned)bufsize)
++		return -FDT_ERR_NOSPACE;
++
++	oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
++	newtail = (char *)buf + bufsize - tailsize;
++
++	/* Two cases to avoid clobbering data if the old and new
++	 * buffers partially overlap */
++	if (buf <= fdt) {
++		memmove(buf, fdt, headsize);
++		memmove(newtail, oldtail, tailsize);
++	} else {
++		memmove(newtail, oldtail, tailsize);
++		memmove(buf, fdt, headsize);
++	}
++
++	fdt_set_totalsize(buf, bufsize);
++	if (fdt_off_dt_strings(buf))
++		fdt_set_off_dt_strings(buf, bufsize);
++
++	return 0;
++}
++
++int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
++{
++	struct fdt_reserve_entry *re;
++	int offset;
++
++	FDT_SW_PROBE_MEMRSV(fdt);
++
++	offset = fdt_off_dt_struct(fdt);
++	if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
++		return -FDT_ERR_NOSPACE;
++
++	re = (struct fdt_reserve_entry *)((char *)fdt + offset);
++	re->address = cpu_to_fdt64(addr);
++	re->size = cpu_to_fdt64(size);
++
++	fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
++
++	return 0;
++}
++
++int fdt_finish_reservemap(void *fdt)
++{
++	int err = fdt_add_reservemap_entry(fdt, 0, 0);
++
++	if (err)
++		return err;
++
++	fdt_set_off_dt_strings(fdt, fdt_totalsize(fdt));
++	return 0;
++}
++
++int fdt_begin_node(void *fdt, const char *name)
++{
++	struct fdt_node_header *nh;
++	int namelen;
++
++	FDT_SW_PROBE_STRUCT(fdt);
++
++	namelen = strlen(name) + 1;
++	nh = fdt_grab_space_(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
++	if (! nh)
++		return -FDT_ERR_NOSPACE;
++
++	nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
++	memcpy(nh->name, name, namelen);
++	return 0;
++}
++
++int fdt_end_node(void *fdt)
++{
++	fdt32_t *en;
++
++	FDT_SW_PROBE_STRUCT(fdt);
++
++	en = fdt_grab_space_(fdt, FDT_TAGSIZE);
++	if (! en)
++		return -FDT_ERR_NOSPACE;
++
++	*en = cpu_to_fdt32(FDT_END_NODE);
++	return 0;
++}
++
++static int fdt_add_string_(void *fdt, const char *s)
++{
++	char *strtab = (char *)fdt + fdt_totalsize(fdt);
++	unsigned int strtabsize = fdt_size_dt_strings(fdt);
++	unsigned int len = strlen(s) + 1;
++	unsigned int struct_top, offset;
++
++	offset = strtabsize + len;
++	struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
++	if (fdt_totalsize(fdt) - offset < struct_top)
++		return 0; /* no more room :( */
++
++	memcpy(strtab - offset, s, len);
++	fdt_set_size_dt_strings(fdt, strtabsize + len);
++	return -offset;
++}
++
++/* Must only be used to roll back in case of error */
++static void fdt_del_last_string_(void *fdt, const char *s)
++{
++	int strtabsize = fdt_size_dt_strings(fdt);
++	int len = strlen(s) + 1;
++
++	fdt_set_size_dt_strings(fdt, strtabsize - len);
++}
++
++static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
++{
++	char *strtab = (char *)fdt + fdt_totalsize(fdt);
++	int strtabsize = fdt_size_dt_strings(fdt);
++	const char *p;
++
++	*allocated = 0;
++
++	p = fdt_find_string_(strtab - strtabsize, strtabsize, s);
++	if (p)
++		return p - strtab;
++
++	*allocated = 1;
++
++	return fdt_add_string_(fdt, s);
++}
++
++int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp)
++{
++	struct fdt_property *prop;
++	int nameoff;
++	int allocated;
++
++	FDT_SW_PROBE_STRUCT(fdt);
++
++	/* String de-duplication can be slow, _NO_NAME_DEDUP skips it */
++	if (sw_flags(fdt) & FDT_CREATE_FLAG_NO_NAME_DEDUP) {
++		allocated = 1;
++		nameoff = fdt_add_string_(fdt, name);
++	} else {
++		nameoff = fdt_find_add_string_(fdt, name, &allocated);
++	}
++	if (nameoff == 0)
++		return -FDT_ERR_NOSPACE;
++
++	prop = fdt_grab_space_(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
++	if (! prop) {
++		if (allocated)
++			fdt_del_last_string_(fdt, name);
++		return -FDT_ERR_NOSPACE;
++	}
++
++	prop->tag = cpu_to_fdt32(FDT_PROP);
++	prop->nameoff = cpu_to_fdt32(nameoff);
++	prop->len = cpu_to_fdt32(len);
++	*valp = prop->data;
++	return 0;
++}
++
++int fdt_property(void *fdt, const char *name, const void *val, int len)
++{
++	void *ptr;
++	int ret;
++
++	ret = fdt_property_placeholder(fdt, name, len, &ptr);
++	if (ret)
++		return ret;
++	memcpy(ptr, val, len);
++	return 0;
++}
++
++int fdt_finish(void *fdt)
++{
++	char *p = (char *)fdt;
++	fdt32_t *end;
++	int oldstroffset, newstroffset;
++	uint32_t tag;
++	int offset, nextoffset;
++
++	FDT_SW_PROBE_STRUCT(fdt);
++
++	/* Add terminator */
++	end = fdt_grab_space_(fdt, sizeof(*end));
++	if (! end)
++		return -FDT_ERR_NOSPACE;
++	*end = cpu_to_fdt32(FDT_END);
++
++	/* Relocate the string table */
++	oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
++	newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
++	memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
++	fdt_set_off_dt_strings(fdt, newstroffset);
++
++	/* Walk the structure, correcting string offsets */
++	offset = 0;
++	while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
++		if (tag == FDT_PROP) {
++			struct fdt_property *prop =
++				fdt_offset_ptr_w_(fdt, offset);
++			int nameoff;
++
++			nameoff = fdt32_to_cpu(prop->nameoff);
++			nameoff += fdt_size_dt_strings(fdt);
++			prop->nameoff = cpu_to_fdt32(nameoff);
++		}
++		offset = nextoffset;
++	}
++	if (nextoffset < 0)
++		return nextoffset;
++
++	/* Finally, adjust the header */
++	fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
++
++	/* And fix up fields that were keeping intermediate state. */
++	fdt_set_last_comp_version(fdt, FDT_LAST_COMPATIBLE_VERSION);
++	fdt_set_magic(fdt, FDT_MAGIC);
++
++	return 0;
++}
+diff --git a/common/libfdt/fdt_wip.c b/common/libfdt/fdt_wip.c
+new file mode 100644
+index 0000000..c2d7566
+--- /dev/null
++++ b/common/libfdt/fdt_wip.c
+@@ -0,0 +1,94 @@
++// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include "libfdt_env.h"
++
++#include <fdt.h>
++#include <libfdt.h>
++
++#include "libfdt_internal.h"
++
++int fdt_setprop_inplace_namelen_partial(void *fdt, int nodeoffset,
++					const char *name, int namelen,
++					uint32_t idx, const void *val,
++					int len)
++{
++	void *propval;
++	int proplen;
++
++	propval = fdt_getprop_namelen_w(fdt, nodeoffset, name, namelen,
++					&proplen);
++	if (!propval)
++		return proplen;
++
++	if ((unsigned)proplen < (len + idx))
++		return -FDT_ERR_NOSPACE;
++
++	memcpy((char *)propval + idx, val, len);
++	return 0;
++}
++
++int fdt_setprop_inplace(void *fdt, int nodeoffset, const char *name,
++			const void *val, int len)
++{
++	const void *propval;
++	int proplen;
++
++	propval = fdt_getprop(fdt, nodeoffset, name, &proplen);
++	if (!propval)
++		return proplen;
++
++	if (proplen != len)
++		return -FDT_ERR_NOSPACE;
++
++	return fdt_setprop_inplace_namelen_partial(fdt, nodeoffset, name,
++						   strlen(name), 0,
++						   val, len);
++}
++
++static void fdt_nop_region_(void *start, int len)
++{
++	fdt32_t *p;
++
++	for (p = start; (char *)p < ((char *)start + len); p++)
++		*p = cpu_to_fdt32(FDT_NOP);
++}
++
++int fdt_nop_property(void *fdt, int nodeoffset, const char *name)
++{
++	struct fdt_property *prop;
++	int len;
++
++	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
++	if (!prop)
++		return len;
++
++	fdt_nop_region_(prop, len + sizeof(*prop));
++
++	return 0;
++}
++
++int fdt_node_end_offset_(void *fdt, int offset)
++{
++	int depth = 0;
++
++	while ((offset >= 0) && (depth >= 0))
++		offset = fdt_next_node(fdt, offset, &depth);
++
++	return offset;
++}
++
++int fdt_nop_node(void *fdt, int nodeoffset)
++{
++	int endoffset;
++
++	endoffset = fdt_node_end_offset_(fdt, nodeoffset);
++	if (endoffset < 0)
++		return endoffset;
++
++	fdt_nop_region_(fdt_offset_ptr_w(fdt, nodeoffset, 0),
++			endoffset - nodeoffset);
++	return 0;
++}
+diff --git a/common/libfdt/libfdt_internal.h b/common/libfdt/libfdt_internal.h
+new file mode 100644
+index 0000000..16bda19
+--- /dev/null
++++ b/common/libfdt/libfdt_internal.h
+@@ -0,0 +1,192 @@
++/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
++#ifndef LIBFDT_INTERNAL_H
++#define LIBFDT_INTERNAL_H
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++#include <fdt.h>
++
++#define FDT_ALIGN(x, a)		(((x) + (a) - 1) & ~((a) - 1))
++#define FDT_TAGALIGN(x)		(FDT_ALIGN((x), FDT_TAGSIZE))
++
++int32_t fdt_ro_probe_(const void *fdt);
++#define FDT_RO_PROBE(fdt)					\
++	{							\
++		int32_t totalsize_;				\
++		if ((totalsize_ = fdt_ro_probe_(fdt)) < 0)	\
++			return totalsize_;			\
++	}
++
++int fdt_check_node_offset_(const void *fdt, int offset);
++int fdt_check_prop_offset_(const void *fdt, int offset);
++const char *fdt_find_string_(const char *strtab, int tabsize, const char *s);
++int fdt_node_end_offset_(void *fdt, int nodeoffset);
++
++static inline const void *fdt_offset_ptr_(const void *fdt, int offset)
++{
++	return (const char *)fdt + fdt_off_dt_struct(fdt) + offset;
++}
++
++static inline void *fdt_offset_ptr_w_(void *fdt, int offset)
++{
++	return (void *)(uintptr_t)fdt_offset_ptr_(fdt, offset);
++}
++
++static inline const struct fdt_reserve_entry *fdt_mem_rsv_(const void *fdt, int n)
++{
++	const struct fdt_reserve_entry *rsv_table =
++		(const struct fdt_reserve_entry *)
++		((const char *)fdt + fdt_off_mem_rsvmap(fdt));
++
++	return rsv_table + n;
++}
++static inline struct fdt_reserve_entry *fdt_mem_rsv_w_(void *fdt, int n)
++{
++	return (void *)(uintptr_t)fdt_mem_rsv_(fdt, n);
++}
++
++/*
++ * Internal helpers to access tructural elements of the device tree
++ * blob (rather than for exaple reading integers from within property
++ * values).  We assume that we are either given a naturally aligned
++ * address for the platform or if we are not, we are on a platform
++ * where unaligned memory reads will be handled in a graceful manner.
++ * If not the external helpers fdtXX_ld() from libfdt.h can be used
++ * instead.
++ */
++static inline uint32_t fdt32_ld_(const fdt32_t *p)
++{
++	return fdt32_to_cpu(*p);
++}
++
++static inline uint64_t fdt64_ld_(const fdt64_t *p)
++{
++	return fdt64_to_cpu(*p);
++}
++
++#define FDT_SW_MAGIC		(~FDT_MAGIC)
++
++/**********************************************************************/
++/* Checking controls                                                  */
++/**********************************************************************/
++
++#ifndef FDT_ASSUME_MASK
++#define FDT_ASSUME_MASK 0
++#endif
++
++/*
++ * Defines assumptions which can be enabled. Each of these can be enabled
++ * individually. For maximum safety, don't enable any assumptions!
++ *
++ * For minimal code size and no safety, use ASSUME_PERFECT at your own risk.
++ * You should have another method of validating the device tree, such as a
++ * signature or hash check before using libfdt.
++ *
++ * For situations where security is not a concern it may be safe to enable
++ * ASSUME_SANE.
++ */
++enum {
++	/*
++	 * This does essentially no checks. Only the latest device-tree
++	 * version is correctly handled. Inconsistencies or errors in the device
++	 * tree may cause undefined behaviour or crashes. Invalid parameters
++	 * passed to libfdt may do the same.
++	 *
++	 * If an error occurs when modifying the tree it may leave the tree in
++	 * an intermediate (but valid) state. As an example, adding a property
++	 * where there is insufficient space may result in the property name
++	 * being added to the string table even though the property itself is
++	 * not added to the struct section.
++	 *
++	 * Only use this if you have a fully validated device tree with
++	 * the latest supported version and wish to minimise code size.
++	 */
++	ASSUME_PERFECT		= 0xff,
++
++	/*
++	 * This assumes that the device tree is sane. i.e. header metadata
++	 * and basic hierarchy are correct.
++	 *
++	 * With this assumption enabled, normal device trees produced by libfdt
++	 * and the compiler should be handled safely. Malicious device trees and
++	 * complete garbage may cause libfdt to behave badly or crash. Truncated
++	 * device trees (e.g. those only partially loaded) can also cause
++	 * problems.
++	 *
++	 * Note: Only checks that relate exclusively to the device tree itself
++	 * (not the parameters passed to libfdt) are disabled by this
++	 * assumption. This includes checking headers, tags and the like.
++	 */
++	ASSUME_VALID_DTB	= 1 << 0,
++
++	/*
++	 * This builds on ASSUME_VALID_DTB and further assumes that libfdt
++	 * functions are called with valid parameters, i.e. not trigger
++	 * FDT_ERR_BADOFFSET or offsets that are out of bounds. It disables any
++	 * extensive checking of parameters and the device tree, making various
++	 * assumptions about correctness.
++	 *
++	 * It doesn't make sense to enable this assumption unless
++	 * ASSUME_VALID_DTB is also enabled.
++	 */
++	ASSUME_VALID_INPUT	= 1 << 1,
++
++	/*
++	 * This disables checks for device-tree version and removes all code
++	 * which handles older versions.
++	 *
++	 * Only enable this if you know you have a device tree with the latest
++	 * version.
++	 */
++	ASSUME_LATEST		= 1 << 2,
++
++	/*
++	 * This assumes that it is OK for a failed addition to the device tree,
++	 * due to lack of space or some other problem, to skip any rollback
++	 * steps (such as dropping the property name from the string table).
++	 * This is safe to enable in most circumstances, even though it may
++	 * leave the tree in a sub-optimal state.
++	 */
++	ASSUME_NO_ROLLBACK	= 1 << 3,
++
++	/*
++	 * This assumes that the device tree components appear in a 'convenient'
++	 * order, i.e. the memory reservation block first, then the structure
++	 * block and finally the string block.
++	 *
++	 * This order is not specified by the device-tree specification,
++	 * but is expected by libfdt. The device-tree compiler always created
++	 * device trees with this order.
++	 *
++	 * This assumption disables a check in fdt_open_into() and removes the
++	 * ability to fix the problem there. This is safe if you know that the
++	 * device tree is correctly ordered. See fdt_blocks_misordered_().
++	 */
++	ASSUME_LIBFDT_ORDER	= 1 << 4,
++
++	/*
++	 * This assumes that libfdt itself does not have any internal bugs. It
++	 * drops certain checks that should never be needed unless libfdt has an
++	 * undiscovered bug.
++	 *
++	 * This can generally be considered safe to enable.
++	 */
++	ASSUME_LIBFDT_FLAWLESS	= 1 << 5,
++};
++
++/**
++ * can_assume_() - check if a particular assumption is enabled
++ *
++ * @mask: Mask to check (ASSUME_...)
++ * @return true if that assumption is enabled, else false
++ */
++static inline bool can_assume_(int mask)
++{
++	return FDT_ASSUME_MASK & mask;
++}
++
++/** helper macros for checking assumptions */
++#define can_assume(_assume)	can_assume_(ASSUME_ ## _assume)
++
++#endif /* LIBFDT_INTERNAL_H */
+diff --git a/include/fdt.h b/include/fdt.h
+new file mode 100644
+index 0000000..f2e6880
+--- /dev/null
++++ b/include/fdt.h
+@@ -0,0 +1,66 @@
++/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
++#ifndef FDT_H
++#define FDT_H
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ * Copyright 2012 Kim Phillips, Freescale Semiconductor.
++ */
++
++#ifndef __ASSEMBLY__
++
++struct fdt_header {
++	fdt32_t magic;			 /* magic word FDT_MAGIC */
++	fdt32_t totalsize;		 /* total size of DT block */
++	fdt32_t off_dt_struct;		 /* offset to structure */
++	fdt32_t off_dt_strings;		 /* offset to strings */
++	fdt32_t off_mem_rsvmap;		 /* offset to memory reserve map */
++	fdt32_t version;		 /* format version */
++	fdt32_t last_comp_version;	 /* last compatible version */
++
++	/* version 2 fields below */
++	fdt32_t boot_cpuid_phys;	 /* Which physical CPU id we're
++					    booting on */
++	/* version 3 fields below */
++	fdt32_t size_dt_strings;	 /* size of the strings block */
++
++	/* version 17 fields below */
++	fdt32_t size_dt_struct;		 /* size of the structure block */
++};
++
++struct fdt_reserve_entry {
++	fdt64_t address;
++	fdt64_t size;
++};
++
++struct fdt_node_header {
++	fdt32_t tag;
++	char name[0];
++};
++
++struct fdt_property {
++	fdt32_t tag;
++	fdt32_t len;
++	fdt32_t nameoff;
++	char data[0];
++};
++
++#endif /* !__ASSEMBLY */
++
++#define FDT_MAGIC	0xd00dfeed	/* 4: version, 4: total size */
++#define FDT_TAGSIZE	sizeof(fdt32_t)
++
++#define FDT_BEGIN_NODE	0x1		/* Start node: full name */
++#define FDT_END_NODE	0x2		/* End node */
++#define FDT_PROP	0x3		/* Property: name off,
++					   size, content */
++#define FDT_NOP		0x4		/* nop */
++#define FDT_END		0x9
++
++#define FDT_V1_SIZE	(7*sizeof(fdt32_t))
++#define FDT_V2_SIZE	(FDT_V1_SIZE + sizeof(fdt32_t))
++#define FDT_V3_SIZE	(FDT_V2_SIZE + sizeof(fdt32_t))
++#define FDT_V16_SIZE	FDT_V3_SIZE
++#define FDT_V17_SIZE	(FDT_V16_SIZE + sizeof(fdt32_t))
++
++#endif /* FDT_H */
+diff --git a/include/libfdt.h b/include/libfdt.h
+new file mode 100644
+index 0000000..a7f432c
+--- /dev/null
++++ b/include/libfdt.h
+@@ -0,0 +1,2147 @@
++/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
++#ifndef LIBFDT_H
++#define LIBFDT_H
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ */
++
++#include <libfdt_env.h>
++#include <fdt.h>
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++#define FDT_FIRST_SUPPORTED_VERSION	0x02
++#define FDT_LAST_COMPATIBLE_VERSION 0x10
++#define FDT_LAST_SUPPORTED_VERSION	0x11
++
++/* Error codes: informative error codes */
++#define FDT_ERR_NOTFOUND	1
++	/* FDT_ERR_NOTFOUND: The requested node or property does not exist */
++#define FDT_ERR_EXISTS		2
++	/* FDT_ERR_EXISTS: Attempted to create a node or property which
++	 * already exists */
++#define FDT_ERR_NOSPACE		3
++	/* FDT_ERR_NOSPACE: Operation needed to expand the device
++	 * tree, but its buffer did not have sufficient space to
++	 * contain the expanded tree. Use fdt_open_into() to move the
++	 * device tree to a buffer with more space. */
++
++/* Error codes: codes for bad parameters */
++#define FDT_ERR_BADOFFSET	4
++	/* FDT_ERR_BADOFFSET: Function was passed a structure block
++	 * offset which is out-of-bounds, or which points to an
++	 * unsuitable part of the structure for the operation. */
++#define FDT_ERR_BADPATH		5
++	/* FDT_ERR_BADPATH: Function was passed a badly formatted path
++	 * (e.g. missing a leading / for a function which requires an
++	 * absolute path) */
++#define FDT_ERR_BADPHANDLE	6
++	/* FDT_ERR_BADPHANDLE: Function was passed an invalid phandle.
++	 * This can be caused either by an invalid phandle property
++	 * length, or the phandle value was either 0 or -1, which are
++	 * not permitted. */
++#define FDT_ERR_BADSTATE	7
++	/* FDT_ERR_BADSTATE: Function was passed an incomplete device
++	 * tree created by the sequential-write functions, which is
++	 * not sufficiently complete for the requested operation. */
++
++/* Error codes: codes for bad device tree blobs */
++#define FDT_ERR_TRUNCATED	8
++	/* FDT_ERR_TRUNCATED: FDT or a sub-block is improperly
++	 * terminated (overflows, goes outside allowed bounds, or
++	 * isn't properly terminated).  */
++#define FDT_ERR_BADMAGIC	9
++	/* FDT_ERR_BADMAGIC: Given "device tree" appears not to be a
++	 * device tree at all - it is missing the flattened device
++	 * tree magic number. */
++#define FDT_ERR_BADVERSION	10
++	/* FDT_ERR_BADVERSION: Given device tree has a version which
++	 * can't be handled by the requested operation.  For
++	 * read-write functions, this may mean that fdt_open_into() is
++	 * required to convert the tree to the expected version. */
++#define FDT_ERR_BADSTRUCTURE	11
++	/* FDT_ERR_BADSTRUCTURE: Given device tree has a corrupt
++	 * structure block or other serious error (e.g. misnested
++	 * nodes, or subnodes preceding properties). */
++#define FDT_ERR_BADLAYOUT	12
++	/* FDT_ERR_BADLAYOUT: For read-write functions, the given
++	 * device tree has it's sub-blocks in an order that the
++	 * function can't handle (memory reserve map, then structure,
++	 * then strings).  Use fdt_open_into() to reorganize the tree
++	 * into a form suitable for the read-write operations. */
++
++/* "Can't happen" error indicating a bug in libfdt */
++#define FDT_ERR_INTERNAL	13
++	/* FDT_ERR_INTERNAL: libfdt has failed an internal assertion.
++	 * Should never be returned, if it is, it indicates a bug in
++	 * libfdt itself. */
++
++/* Errors in device tree content */
++#define FDT_ERR_BADNCELLS	14
++	/* FDT_ERR_BADNCELLS: Device tree has a #address-cells, #size-cells
++	 * or similar property with a bad format or value */
++
++#define FDT_ERR_BADVALUE	15
++	/* FDT_ERR_BADVALUE: Device tree has a property with an unexpected
++	 * value. For example: a property expected to contain a string list
++	 * is not NUL-terminated within the length of its value. */
++
++#define FDT_ERR_BADOVERLAY	16
++	/* FDT_ERR_BADOVERLAY: The device tree overlay, while
++	 * correctly structured, cannot be applied due to some
++	 * unexpected or missing value, property or node. */
++
++#define FDT_ERR_NOPHANDLES	17
++	/* FDT_ERR_NOPHANDLES: The device tree doesn't have any
++	 * phandle available anymore without causing an overflow */
++
++#define FDT_ERR_BADFLAGS	18
++	/* FDT_ERR_BADFLAGS: The function was passed a flags field that
++	 * contains invalid flags or an invalid combination of flags. */
++
++#define FDT_ERR_ALIGNMENT	19
++	/* FDT_ERR_ALIGNMENT: The device tree base address is not 8-byte
++	 * aligned. */
++
++#define FDT_ERR_MAX		19
++
++/* constants */
++#define FDT_MAX_PHANDLE 0xfffffffe
++	/* Valid values for phandles range from 1 to 2^32-2. */
++
++/**********************************************************************/
++/* Low-level functions (you probably don't need these)                */
++/**********************************************************************/
++
++#ifndef SWIG /* This function is not useful in Python */
++const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int checklen);
++#endif
++static inline void *fdt_offset_ptr_w(void *fdt, int offset, int checklen)
++{
++	return (void *)(uintptr_t)fdt_offset_ptr(fdt, offset, checklen);
++}
++
++uint32_t fdt_next_tag(const void *fdt, int offset, int *nextoffset);
++
++/*
++ * External helpers to access words from a device tree blob. They're built
++ * to work even with unaligned pointers on platforms (such as ARMv5) that don't
++ * like unaligned loads and stores.
++ */
++static inline uint16_t fdt16_ld(const fdt16_t *p)
++{
++	const uint8_t *bp = (const uint8_t *)p;
++
++	return ((uint16_t)bp[0] << 8) | bp[1];
++}
++
++static inline uint32_t fdt32_ld(const fdt32_t *p)
++{
++	const uint8_t *bp = (const uint8_t *)p;
++
++	return ((uint32_t)bp[0] << 24)
++		| ((uint32_t)bp[1] << 16)
++		| ((uint32_t)bp[2] << 8)
++		| bp[3];
++}
++
++static inline void fdt32_st(void *property, uint32_t value)
++{
++	uint8_t *bp = (uint8_t *)property;
++
++	bp[0] = value >> 24;
++	bp[1] = (value >> 16) & 0xff;
++	bp[2] = (value >> 8) & 0xff;
++	bp[3] = value & 0xff;
++}
++
++static inline uint64_t fdt64_ld(const fdt64_t *p)
++{
++	const uint8_t *bp = (const uint8_t *)p;
++
++	return ((uint64_t)bp[0] << 56)
++		| ((uint64_t)bp[1] << 48)
++		| ((uint64_t)bp[2] << 40)
++		| ((uint64_t)bp[3] << 32)
++		| ((uint64_t)bp[4] << 24)
++		| ((uint64_t)bp[5] << 16)
++		| ((uint64_t)bp[6] << 8)
++		| bp[7];
++}
++
++static inline void fdt64_st(void *property, uint64_t value)
++{
++	uint8_t *bp = (uint8_t *)property;
++
++	bp[0] = value >> 56;
++	bp[1] = (value >> 48) & 0xff;
++	bp[2] = (value >> 40) & 0xff;
++	bp[3] = (value >> 32) & 0xff;
++	bp[4] = (value >> 24) & 0xff;
++	bp[5] = (value >> 16) & 0xff;
++	bp[6] = (value >> 8) & 0xff;
++	bp[7] = value & 0xff;
++}
++
++/**********************************************************************/
++/* Traversal functions                                                */
++/**********************************************************************/
++
++int fdt_next_node(const void *fdt, int offset, int *depth);
++
++/**
++ * fdt_first_subnode() - get offset of first direct subnode
++ * @fdt:	FDT blob
++ * @offset:	Offset of node to check
++ *
++ * Return: offset of first subnode, or -FDT_ERR_NOTFOUND if there is none
++ */
++int fdt_first_subnode(const void *fdt, int offset);
++
++/**
++ * fdt_next_subnode() - get offset of next direct subnode
++ * @fdt:	FDT blob
++ * @offset:	Offset of previous subnode
++ *
++ * After first calling fdt_first_subnode(), call this function repeatedly to
++ * get direct subnodes of a parent node.
++ *
++ * Return: offset of next subnode, or -FDT_ERR_NOTFOUND if there are no more
++ *         subnodes
++ */
++int fdt_next_subnode(const void *fdt, int offset);
++
++/**
++ * fdt_for_each_subnode - iterate over all subnodes of a parent
++ *
++ * @node:	child node (int, lvalue)
++ * @fdt:	FDT blob (const void *)
++ * @parent:	parent node (int)
++ *
++ * This is actually a wrapper around a for loop and would be used like so:
++ *
++ *	fdt_for_each_subnode(node, fdt, parent) {
++ *		Use node
++ *		...
++ *	}
++ *
++ *	if ((node < 0) && (node != -FDT_ERR_NOTFOUND)) {
++ *		Error handling
++ *	}
++ *
++ * Note that this is implemented as a macro and @node is used as
++ * iterator in the loop. The parent variable be constant or even a
++ * literal.
++ */
++#define fdt_for_each_subnode(node, fdt, parent)		\
++	for (node = fdt_first_subnode(fdt, parent);	\
++	     node >= 0;					\
++	     node = fdt_next_subnode(fdt, node))
++
++/**********************************************************************/
++/* General functions                                                  */
++/**********************************************************************/
++#define fdt_get_header(fdt, field) \
++	(fdt32_ld(&((const struct fdt_header *)(fdt))->field))
++#define fdt_magic(fdt)			(fdt_get_header(fdt, magic))
++#define fdt_totalsize(fdt)		(fdt_get_header(fdt, totalsize))
++#define fdt_off_dt_struct(fdt)		(fdt_get_header(fdt, off_dt_struct))
++#define fdt_off_dt_strings(fdt)		(fdt_get_header(fdt, off_dt_strings))
++#define fdt_off_mem_rsvmap(fdt)		(fdt_get_header(fdt, off_mem_rsvmap))
++#define fdt_version(fdt)		(fdt_get_header(fdt, version))
++#define fdt_last_comp_version(fdt)	(fdt_get_header(fdt, last_comp_version))
++#define fdt_boot_cpuid_phys(fdt)	(fdt_get_header(fdt, boot_cpuid_phys))
++#define fdt_size_dt_strings(fdt)	(fdt_get_header(fdt, size_dt_strings))
++#define fdt_size_dt_struct(fdt)		(fdt_get_header(fdt, size_dt_struct))
++
++#define fdt_set_hdr_(name) \
++	static inline void fdt_set_##name(void *fdt, uint32_t val) \
++	{ \
++		struct fdt_header *fdth = (struct fdt_header *)fdt; \
++		fdth->name = cpu_to_fdt32(val); \
++	}
++fdt_set_hdr_(magic);
++fdt_set_hdr_(totalsize);
++fdt_set_hdr_(off_dt_struct);
++fdt_set_hdr_(off_dt_strings);
++fdt_set_hdr_(off_mem_rsvmap);
++fdt_set_hdr_(version);
++fdt_set_hdr_(last_comp_version);
++fdt_set_hdr_(boot_cpuid_phys);
++fdt_set_hdr_(size_dt_strings);
++fdt_set_hdr_(size_dt_struct);
++#undef fdt_set_hdr_
++
++/**
++ * fdt_header_size - return the size of the tree's header
++ * @fdt: pointer to a flattened device tree
++ *
++ * Return: size of DTB header in bytes
++ */
++size_t fdt_header_size(const void *fdt);
++
++/**
++ * fdt_header_size_ - internal function to get header size from a version number
++ * @version: devicetree version number
++ *
++ * Return: size of DTB header in bytes
++ */
++size_t fdt_header_size_(uint32_t version);
++
++/**
++ * fdt_check_header - sanity check a device tree header
++ * @fdt: pointer to data which might be a flattened device tree
++ *
++ * fdt_check_header() checks that the given buffer contains what
++ * appears to be a flattened device tree, and that the header contains
++ * valid information (to the extent that can be determined from the
++ * header alone).
++ *
++ * returns:
++ *     0, if the buffer appears to contain a valid device tree
++ *     -FDT_ERR_BADMAGIC,
++ *     -FDT_ERR_BADVERSION,
++ *     -FDT_ERR_BADSTATE,
++ *     -FDT_ERR_TRUNCATED, standard meanings, as above
++ */
++int fdt_check_header(const void *fdt);
++
++/**
++ * fdt_move - move a device tree around in memory
++ * @fdt: pointer to the device tree to move
++ * @buf: pointer to memory where the device is to be moved
++ * @bufsize: size of the memory space at buf
++ *
++ * fdt_move() relocates, if possible, the device tree blob located at
++ * fdt to the buffer at buf of size bufsize.  The buffer may overlap
++ * with the existing device tree blob at fdt.  Therefore,
++ *     fdt_move(fdt, fdt, fdt_totalsize(fdt))
++ * should always succeed.
++ *
++ * returns:
++ *     0, on success
++ *     -FDT_ERR_NOSPACE, bufsize is insufficient to contain the device tree
++ *     -FDT_ERR_BADMAGIC,
++ *     -FDT_ERR_BADVERSION,
++ *     -FDT_ERR_BADSTATE, standard meanings
++ */
++int fdt_move(const void *fdt, void *buf, int bufsize);
++
++/**********************************************************************/
++/* Read-only functions                                                */
++/**********************************************************************/
++
++int fdt_check_full(const void *fdt, size_t bufsize);
++
++/**
++ * fdt_get_string - retrieve a string from the strings block of a device tree
++ * @fdt: pointer to the device tree blob
++ * @stroffset: offset of the string within the strings block (native endian)
++ * @lenp: optional pointer to return the string's length
++ *
++ * fdt_get_string() retrieves a pointer to a single string from the
++ * strings block of the device tree blob at fdt, and optionally also
++ * returns the string's length in *lenp.
++ *
++ * returns:
++ *     a pointer to the string, on success
++ *     NULL, if stroffset is out of bounds, or doesn't point to a valid string
++ */
++const char *fdt_get_string(const void *fdt, int stroffset, int *lenp);
++
++/**
++ * fdt_string - retrieve a string from the strings block of a device tree
++ * @fdt: pointer to the device tree blob
++ * @stroffset: offset of the string within the strings block (native endian)
++ *
++ * fdt_string() retrieves a pointer to a single string from the
++ * strings block of the device tree blob at fdt.
++ *
++ * returns:
++ *     a pointer to the string, on success
++ *     NULL, if stroffset is out of bounds, or doesn't point to a valid string
++ */
++const char *fdt_string(const void *fdt, int stroffset);
++
++/**
++ * fdt_find_max_phandle - find and return the highest phandle in a tree
++ * @fdt: pointer to the device tree blob
++ * @phandle: return location for the highest phandle value found in the tree
++ *
++ * fdt_find_max_phandle() finds the highest phandle value in the given device
++ * tree. The value returned in @phandle is only valid if the function returns
++ * success.
++ *
++ * returns:
++ *     0 on success or a negative error code on failure
++ */
++int fdt_find_max_phandle(const void *fdt, uint32_t *phandle);
++
++/**
++ * fdt_get_max_phandle - retrieves the highest phandle in a tree
++ * @fdt: pointer to the device tree blob
++ *
++ * fdt_get_max_phandle retrieves the highest phandle in the given
++ * device tree. This will ignore badly formatted phandles, or phandles
++ * with a value of 0 or -1.
++ *
++ * This function is deprecated in favour of fdt_find_max_phandle().
++ *
++ * returns:
++ *      the highest phandle on success
++ *      0, if no phandle was found in the device tree
++ *      -1, if an error occurred
++ */
++static inline uint32_t fdt_get_max_phandle(const void *fdt)
++{
++	uint32_t phandle;
++	int err;
++
++	err = fdt_find_max_phandle(fdt, &phandle);
++	if (err < 0)
++		return (uint32_t)-1;
++
++	return phandle;
++}
++
++/**
++ * fdt_generate_phandle - return a new, unused phandle for a device tree blob
++ * @fdt: pointer to the device tree blob
++ * @phandle: return location for the new phandle
++ *
++ * Walks the device tree blob and looks for the highest phandle value. On
++ * success, the new, unused phandle value (one higher than the previously
++ * highest phandle value in the device tree blob) will be returned in the
++ * @phandle parameter.
++ *
++ * Return: 0 on success or a negative error-code on failure
++ */
++int fdt_generate_phandle(const void *fdt, uint32_t *phandle);
++
++/**
++ * fdt_num_mem_rsv - retrieve the number of memory reserve map entries
++ * @fdt: pointer to the device tree blob
++ *
++ * Returns the number of entries in the device tree blob's memory
++ * reservation map.  This does not include the terminating 0,0 entry
++ * or any other (0,0) entries reserved for expansion.
++ *
++ * returns:
++ *     the number of entries
++ */
++int fdt_num_mem_rsv(const void *fdt);
++
++/**
++ * fdt_get_mem_rsv - retrieve one memory reserve map entry
++ * @fdt: pointer to the device tree blob
++ * @n: index of reserve map entry
++ * @address: pointer to 64-bit variable to hold the start address
++ * @size: pointer to 64-bit variable to hold the size of the entry
++ *
++ * On success, @address and @size will contain the address and size of
++ * the n-th reserve map entry from the device tree blob, in
++ * native-endian format.
++ *
++ * returns:
++ *     0, on success
++ *     -FDT_ERR_BADMAGIC,
++ *     -FDT_ERR_BADVERSION,
++ *     -FDT_ERR_BADSTATE, standard meanings
++ */
++int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size);
++
++/**
++ * fdt_subnode_offset_namelen - find a subnode based on substring
++ * @fdt: pointer to the device tree blob
++ * @parentoffset: structure block offset of a node
++ * @name: name of the subnode to locate
++ * @namelen: number of characters of name to consider
++ *
++ * Identical to fdt_subnode_offset(), but only examine the first
++ * namelen characters of name for matching the subnode name.  This is
++ * useful for finding subnodes based on a portion of a larger string,
++ * such as a full path.
++ *
++ * Return: offset of the subnode or -FDT_ERR_NOTFOUND if name not found.
++ */
++#ifndef SWIG /* Not available in Python */
++int fdt_subnode_offset_namelen(const void *fdt, int parentoffset,
++			       const char *name, int namelen);
++#endif
++/**
++ * fdt_subnode_offset - find a subnode of a given node
++ * @fdt: pointer to the device tree blob
++ * @parentoffset: structure block offset of a node
++ * @name: name of the subnode to locate
++ *
++ * fdt_subnode_offset() finds a subnode of the node at structure block
++ * offset parentoffset with the given name.  name may include a unit
++ * address, in which case fdt_subnode_offset() will find the subnode
++ * with that unit address, or the unit address may be omitted, in
++ * which case fdt_subnode_offset() will find an arbitrary subnode
++ * whose name excluding unit address matches the given name.
++ *
++ * returns:
++ *	structure block offset of the requested subnode (>=0), on success
++ *	-FDT_ERR_NOTFOUND, if the requested subnode does not exist
++ *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE
++ *		tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings.
++ */
++int fdt_subnode_offset(const void *fdt, int parentoffset, const char *name);
++
++/**
++ * fdt_path_offset_namelen - find a tree node by its full path
++ * @fdt: pointer to the device tree blob
++ * @path: full path of the node to locate
++ * @namelen: number of characters of path to consider
++ *
++ * Identical to fdt_path_offset(), but only consider the first namelen
++ * characters of path as the path name.
++ *
++ * Return: offset of the node or negative libfdt error value otherwise
++ */
++#ifndef SWIG /* Not available in Python */
++int fdt_path_offset_namelen(const void *fdt, const char *path, int namelen);
++#endif
++
++/**
++ * fdt_path_offset - find a tree node by its full path
++ * @fdt: pointer to the device tree blob
++ * @path: full path of the node to locate
++ *
++ * fdt_path_offset() finds a node of a given path in the device tree.
++ * Each path component may omit the unit address portion, but the
++ * results of this are undefined if any such path component is
++ * ambiguous (that is if there are multiple nodes at the relevant
++ * level matching the given component, differentiated only by unit
++ * address).
++ *
++ * returns:
++ *	structure block offset of the node with the requested path (>=0), on
++ *		success
++ *	-FDT_ERR_BADPATH, given path does not begin with '/' or is invalid
++ *	-FDT_ERR_NOTFOUND, if the requested node does not exist
++ *      -FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings.
++ */
++int fdt_path_offset(const void *fdt, const char *path);
++
++/**
++ * fdt_get_name - retrieve the name of a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: structure block offset of the starting node
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_get_name() retrieves the name (including unit address) of the
++ * device tree node at structure block offset nodeoffset.  If lenp is
++ * non-NULL, the length of this name is also returned, in the integer
++ * pointed to by lenp.
++ *
++ * returns:
++ *	pointer to the node's name, on success
++ *		If lenp is non-NULL, *lenp contains the length of that name
++ *			(>=0)
++ *	NULL, on error
++ *		if lenp is non-NULL *lenp contains an error code (<0):
++ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE
++ *			tag
++ *		-FDT_ERR_BADMAGIC,
++ *		-FDT_ERR_BADVERSION,
++ *		-FDT_ERR_BADSTATE, standard meanings
++ */
++const char *fdt_get_name(const void *fdt, int nodeoffset, int *lenp);
++
++/**
++ * fdt_first_property_offset - find the offset of a node's first property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: structure block offset of a node
++ *
++ * fdt_first_property_offset() finds the first property of the node at
++ * the given structure block offset.
++ *
++ * returns:
++ *	structure block offset of the property (>=0), on success
++ *	-FDT_ERR_NOTFOUND, if the requested node has no properties
++ *	-FDT_ERR_BADOFFSET, if nodeoffset did not point to an FDT_BEGIN_NODE tag
++ *      -FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings.
++ */
++int fdt_first_property_offset(const void *fdt, int nodeoffset);
++
++/**
++ * fdt_next_property_offset - step through a node's properties
++ * @fdt: pointer to the device tree blob
++ * @offset: structure block offset of a property
++ *
++ * fdt_next_property_offset() finds the property immediately after the
++ * one at the given structure block offset.  This will be a property
++ * of the same node as the given property.
++ *
++ * returns:
++ *	structure block offset of the next property (>=0), on success
++ *	-FDT_ERR_NOTFOUND, if the given property is the last in its node
++ *	-FDT_ERR_BADOFFSET, if nodeoffset did not point to an FDT_PROP tag
++ *      -FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings.
++ */
++int fdt_next_property_offset(const void *fdt, int offset);
++
++/**
++ * fdt_for_each_property_offset - iterate over all properties of a node
++ *
++ * @property:	property offset (int, lvalue)
++ * @fdt:	FDT blob (const void *)
++ * @node:	node offset (int)
++ *
++ * This is actually a wrapper around a for loop and would be used like so:
++ *
++ *	fdt_for_each_property_offset(property, fdt, node) {
++ *		Use property
++ *		...
++ *	}
++ *
++ *	if ((property < 0) && (property != -FDT_ERR_NOTFOUND)) {
++ *		Error handling
++ *	}
++ *
++ * Note that this is implemented as a macro and property is used as
++ * iterator in the loop. The node variable can be constant or even a
++ * literal.
++ */
++#define fdt_for_each_property_offset(property, fdt, node)	\
++	for (property = fdt_first_property_offset(fdt, node);	\
++	     property >= 0;					\
++	     property = fdt_next_property_offset(fdt, property))
++
++/**
++ * fdt_get_property_by_offset - retrieve the property at a given offset
++ * @fdt: pointer to the device tree blob
++ * @offset: offset of the property to retrieve
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_get_property_by_offset() retrieves a pointer to the
++ * fdt_property structure within the device tree blob at the given
++ * offset.  If lenp is non-NULL, the length of the property value is
++ * also returned, in the integer pointed to by lenp.
++ *
++ * Note that this code only works on device tree versions >= 16. fdt_getprop()
++ * works on all versions.
++ *
++ * returns:
++ *	pointer to the structure representing the property
++ *		if lenp is non-NULL, *lenp contains the length of the property
++ *		value (>=0)
++ *	NULL, on error
++ *		if lenp is non-NULL, *lenp contains an error code (<0):
++ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_PROP tag
++ *		-FDT_ERR_BADMAGIC,
++ *		-FDT_ERR_BADVERSION,
++ *		-FDT_ERR_BADSTATE,
++ *		-FDT_ERR_BADSTRUCTURE,
++ *		-FDT_ERR_TRUNCATED, standard meanings
++ */
++const struct fdt_property *fdt_get_property_by_offset(const void *fdt,
++						      int offset,
++						      int *lenp);
++
++/**
++ * fdt_get_property_namelen - find a property based on substring
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to find
++ * @name: name of the property to find
++ * @namelen: number of characters of name to consider
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * Identical to fdt_get_property(), but only examine the first namelen
++ * characters of name for matching the property name.
++ *
++ * Return: pointer to the structure representing the property, or NULL
++ *         if not found
++ */
++#ifndef SWIG /* Not available in Python */
++const struct fdt_property *fdt_get_property_namelen(const void *fdt,
++						    int nodeoffset,
++						    const char *name,
++						    int namelen, int *lenp);
++#endif
++
++/**
++ * fdt_get_property - find a given property in a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to find
++ * @name: name of the property to find
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_get_property() retrieves a pointer to the fdt_property
++ * structure within the device tree blob corresponding to the property
++ * named 'name' of the node at offset nodeoffset.  If lenp is
++ * non-NULL, the length of the property value is also returned, in the
++ * integer pointed to by lenp.
++ *
++ * returns:
++ *	pointer to the structure representing the property
++ *		if lenp is non-NULL, *lenp contains the length of the property
++ *		value (>=0)
++ *	NULL, on error
++ *		if lenp is non-NULL, *lenp contains an error code (<0):
++ *		-FDT_ERR_NOTFOUND, node does not have named property
++ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE
++ *			tag
++ *		-FDT_ERR_BADMAGIC,
++ *		-FDT_ERR_BADVERSION,
++ *		-FDT_ERR_BADSTATE,
++ *		-FDT_ERR_BADSTRUCTURE,
++ *		-FDT_ERR_TRUNCATED, standard meanings
++ */
++const struct fdt_property *fdt_get_property(const void *fdt, int nodeoffset,
++					    const char *name, int *lenp);
++static inline struct fdt_property *fdt_get_property_w(void *fdt, int nodeoffset,
++						      const char *name,
++						      int *lenp)
++{
++	return (struct fdt_property *)(uintptr_t)
++		fdt_get_property(fdt, nodeoffset, name, lenp);
++}
++
++/**
++ * fdt_getprop_by_offset - retrieve the value of a property at a given offset
++ * @fdt: pointer to the device tree blob
++ * @offset: offset of the property to read
++ * @namep: pointer to a string variable (will be overwritten) or NULL
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_getprop_by_offset() retrieves a pointer to the value of the
++ * property at structure block offset 'offset' (this will be a pointer
++ * to within the device blob itself, not a copy of the value).  If
++ * lenp is non-NULL, the length of the property value is also
++ * returned, in the integer pointed to by lenp.  If namep is non-NULL,
++ * the property's namne will also be returned in the char * pointed to
++ * by namep (this will be a pointer to within the device tree's string
++ * block, not a new copy of the name).
++ *
++ * returns:
++ *	pointer to the property's value
++ *		if lenp is non-NULL, *lenp contains the length of the property
++ *		value (>=0)
++ *		if namep is non-NULL *namep contiains a pointer to the property
++ *		name.
++ *	NULL, on error
++ *		if lenp is non-NULL, *lenp contains an error code (<0):
++ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_PROP tag
++ *		-FDT_ERR_BADMAGIC,
++ *		-FDT_ERR_BADVERSION,
++ *		-FDT_ERR_BADSTATE,
++ *		-FDT_ERR_BADSTRUCTURE,
++ *		-FDT_ERR_TRUNCATED, standard meanings
++ */
++#ifndef SWIG /* This function is not useful in Python */
++const void *fdt_getprop_by_offset(const void *fdt, int offset,
++				  const char **namep, int *lenp);
++#endif
++
++/**
++ * fdt_getprop_namelen - get property value based on substring
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to find
++ * @name: name of the property to find
++ * @namelen: number of characters of name to consider
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * Identical to fdt_getprop(), but only examine the first namelen
++ * characters of name for matching the property name.
++ *
++ * Return: pointer to the property's value or NULL on error
++ */
++#ifndef SWIG /* Not available in Python */
++const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
++				const char *name, int namelen, int *lenp);
++static inline void *fdt_getprop_namelen_w(void *fdt, int nodeoffset,
++					  const char *name, int namelen,
++					  int *lenp)
++{
++	return (void *)(uintptr_t)fdt_getprop_namelen(fdt, nodeoffset, name,
++						      namelen, lenp);
++}
++#endif
++
++/**
++ * fdt_getprop - retrieve the value of a given property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to find
++ * @name: name of the property to find
++ * @lenp: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_getprop() retrieves a pointer to the value of the property
++ * named @name of the node at offset @nodeoffset (this will be a
++ * pointer to within the device blob itself, not a copy of the value).
++ * If @lenp is non-NULL, the length of the property value is also
++ * returned, in the integer pointed to by @lenp.
++ *
++ * returns:
++ *	pointer to the property's value
++ *		if lenp is non-NULL, *lenp contains the length of the property
++ *		value (>=0)
++ *	NULL, on error
++ *		if lenp is non-NULL, *lenp contains an error code (<0):
++ *		-FDT_ERR_NOTFOUND, node does not have named property
++ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE
++ *			tag
++ *		-FDT_ERR_BADMAGIC,
++ *		-FDT_ERR_BADVERSION,
++ *		-FDT_ERR_BADSTATE,
++ *		-FDT_ERR_BADSTRUCTURE,
++ *		-FDT_ERR_TRUNCATED, standard meanings
++ */
++const void *fdt_getprop(const void *fdt, int nodeoffset,
++			const char *name, int *lenp);
++static inline void *fdt_getprop_w(void *fdt, int nodeoffset,
++				  const char *name, int *lenp)
++{
++	return (void *)(uintptr_t)fdt_getprop(fdt, nodeoffset, name, lenp);
++}
++
++/**
++ * fdt_get_phandle - retrieve the phandle of a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: structure block offset of the node
++ *
++ * fdt_get_phandle() retrieves the phandle of the device tree node at
++ * structure block offset nodeoffset.
++ *
++ * returns:
++ *	the phandle of the node at nodeoffset, on success (!= 0, != -1)
++ *	0, if the node has no phandle, or another error occurs
++ */
++uint32_t fdt_get_phandle(const void *fdt, int nodeoffset);
++
++/**
++ * fdt_get_alias_namelen - get alias based on substring
++ * @fdt: pointer to the device tree blob
++ * @name: name of the alias th look up
++ * @namelen: number of characters of name to consider
++ *
++ * Identical to fdt_get_alias(), but only examine the first @namelen
++ * characters of @name for matching the alias name.
++ *
++ * Return: a pointer to the expansion of the alias named @name, if it exists,
++ *	   NULL otherwise
++ */
++#ifndef SWIG /* Not available in Python */
++const char *fdt_get_alias_namelen(const void *fdt,
++				  const char *name, int namelen);
++#endif
++
++/**
++ * fdt_get_alias - retrieve the path referenced by a given alias
++ * @fdt: pointer to the device tree blob
++ * @name: name of the alias th look up
++ *
++ * fdt_get_alias() retrieves the value of a given alias.  That is, the
++ * value of the property named @name in the node /aliases.
++ *
++ * returns:
++ *	a pointer to the expansion of the alias named 'name', if it exists
++ *	NULL, if the given alias or the /aliases node does not exist
++ */
++const char *fdt_get_alias(const void *fdt, const char *name);
++
++/**
++ * fdt_get_path - determine the full path of a node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose path to find
++ * @buf: character buffer to contain the returned path (will be overwritten)
++ * @buflen: size of the character buffer at buf
++ *
++ * fdt_get_path() computes the full path of the node at offset
++ * nodeoffset, and records that path in the buffer at buf.
++ *
++ * NOTE: This function is expensive, as it must scan the device tree
++ * structure from the start to nodeoffset.
++ *
++ * returns:
++ *	0, on success
++ *		buf contains the absolute path of the node at
++ *		nodeoffset, as a NUL-terminated string.
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_NOSPACE, the path of the given node is longer than (bufsize-1)
++ *		characters and will not fit in the given buffer.
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_get_path(const void *fdt, int nodeoffset, char *buf, int buflen);
++
++/**
++ * fdt_supernode_atdepth_offset - find a specific ancestor of a node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose parent to find
++ * @supernodedepth: depth of the ancestor to find
++ * @nodedepth: pointer to an integer variable (will be overwritten) or NULL
++ *
++ * fdt_supernode_atdepth_offset() finds an ancestor of the given node
++ * at a specific depth from the root (where the root itself has depth
++ * 0, its immediate subnodes depth 1 and so forth).  So
++ *	fdt_supernode_atdepth_offset(fdt, nodeoffset, 0, NULL);
++ * will always return 0, the offset of the root node.  If the node at
++ * nodeoffset has depth D, then:
++ *	fdt_supernode_atdepth_offset(fdt, nodeoffset, D, NULL);
++ * will return nodeoffset itself.
++ *
++ * NOTE: This function is expensive, as it must scan the device tree
++ * structure from the start to nodeoffset.
++ *
++ * returns:
++ *	structure block offset of the node at node offset's ancestor
++ *		of depth supernodedepth (>=0), on success
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_NOTFOUND, supernodedepth was greater than the depth of
++ *		nodeoffset
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_supernode_atdepth_offset(const void *fdt, int nodeoffset,
++				 int supernodedepth, int *nodedepth);
++
++/**
++ * fdt_node_depth - find the depth of a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose parent to find
++ *
++ * fdt_node_depth() finds the depth of a given node.  The root node
++ * has depth 0, its immediate subnodes depth 1 and so forth.
++ *
++ * NOTE: This function is expensive, as it must scan the device tree
++ * structure from the start to nodeoffset.
++ *
++ * returns:
++ *	depth of the node at nodeoffset (>=0), on success
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_node_depth(const void *fdt, int nodeoffset);
++
++/**
++ * fdt_parent_offset - find the parent of a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose parent to find
++ *
++ * fdt_parent_offset() locates the parent node of a given node (that
++ * is, it finds the offset of the node which contains the node at
++ * nodeoffset as a subnode).
++ *
++ * NOTE: This function is expensive, as it must scan the device tree
++ * structure from the start to nodeoffset, *twice*.
++ *
++ * returns:
++ *	structure block offset of the parent of the node at nodeoffset
++ *		(>=0), on success
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_parent_offset(const void *fdt, int nodeoffset);
++
++/**
++ * fdt_node_offset_by_prop_value - find nodes with a given property value
++ * @fdt: pointer to the device tree blob
++ * @startoffset: only find nodes after this offset
++ * @propname: property name to check
++ * @propval: property value to search for
++ * @proplen: length of the value in propval
++ *
++ * fdt_node_offset_by_prop_value() returns the offset of the first
++ * node after startoffset, which has a property named propname whose
++ * value is of length proplen and has value equal to propval; or if
++ * startoffset is -1, the very first such node in the tree.
++ *
++ * To iterate through all nodes matching the criterion, the following
++ * idiom can be used:
++ *	offset = fdt_node_offset_by_prop_value(fdt, -1, propname,
++ *					       propval, proplen);
++ *	while (offset != -FDT_ERR_NOTFOUND) {
++ *		// other code here
++ *		offset = fdt_node_offset_by_prop_value(fdt, offset, propname,
++ *						       propval, proplen);
++ *	}
++ *
++ * Note the -1 in the first call to the function, if 0 is used here
++ * instead, the function will never locate the root node, even if it
++ * matches the criterion.
++ *
++ * returns:
++ *	structure block offset of the located node (>= 0, >startoffset),
++ *		 on success
++ *	-FDT_ERR_NOTFOUND, no node matching the criterion exists in the
++ *		tree after startoffset
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_node_offset_by_prop_value(const void *fdt, int startoffset,
++				  const char *propname,
++				  const void *propval, int proplen);
++
++/**
++ * fdt_node_offset_by_phandle - find the node with a given phandle
++ * @fdt: pointer to the device tree blob
++ * @phandle: phandle value
++ *
++ * fdt_node_offset_by_phandle() returns the offset of the node
++ * which has the given phandle value.  If there is more than one node
++ * in the tree with the given phandle (an invalid tree), results are
++ * undefined.
++ *
++ * returns:
++ *	structure block offset of the located node (>= 0), on success
++ *	-FDT_ERR_NOTFOUND, no node with that phandle exists
++ *	-FDT_ERR_BADPHANDLE, given phandle value was invalid (0 or -1)
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_node_offset_by_phandle(const void *fdt, uint32_t phandle);
++
++/**
++ * fdt_node_check_compatible - check a node's compatible property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of a tree node
++ * @compatible: string to match against
++ *
++ * fdt_node_check_compatible() returns 0 if the given node contains a
++ * @compatible property with the given string as one of its elements,
++ * it returns non-zero otherwise, or on error.
++ *
++ * returns:
++ *	0, if the node has a 'compatible' property listing the given string
++ *	1, if the node has a 'compatible' property, but it does not list
++ *		the given string
++ *	-FDT_ERR_NOTFOUND, if the given node has no 'compatible' property
++ *	-FDT_ERR_BADOFFSET, if nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_node_check_compatible(const void *fdt, int nodeoffset,
++			      const char *compatible);
++
++/**
++ * fdt_node_offset_by_compatible - find nodes with a given 'compatible' value
++ * @fdt: pointer to the device tree blob
++ * @startoffset: only find nodes after this offset
++ * @compatible: 'compatible' string to match against
++ *
++ * fdt_node_offset_by_compatible() returns the offset of the first
++ * node after startoffset, which has a 'compatible' property which
++ * lists the given compatible string; or if startoffset is -1, the
++ * very first such node in the tree.
++ *
++ * To iterate through all nodes matching the criterion, the following
++ * idiom can be used:
++ *	offset = fdt_node_offset_by_compatible(fdt, -1, compatible);
++ *	while (offset != -FDT_ERR_NOTFOUND) {
++ *		// other code here
++ *		offset = fdt_node_offset_by_compatible(fdt, offset, compatible);
++ *	}
++ *
++ * Note the -1 in the first call to the function, if 0 is used here
++ * instead, the function will never locate the root node, even if it
++ * matches the criterion.
++ *
++ * returns:
++ *	structure block offset of the located node (>= 0, >startoffset),
++ *		 on success
++ *	-FDT_ERR_NOTFOUND, no node matching the criterion exists in the
++ *		tree after startoffset
++ *	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE, standard meanings
++ */
++int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
++				  const char *compatible);
++
++/**
++ * fdt_stringlist_contains - check a string list property for a string
++ * @strlist: Property containing a list of strings to check
++ * @listlen: Length of property
++ * @str: String to search for
++ *
++ * This is a utility function provided for convenience. The list contains
++ * one or more strings, each terminated by \0, as is found in a device tree
++ * "compatible" property.
++ *
++ * Return: 1 if the string is found in the list, 0 not found, or invalid list
++ */
++int fdt_stringlist_contains(const char *strlist, int listlen, const char *str);
++
++/**
++ * fdt_stringlist_count - count the number of strings in a string list
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of a tree node
++ * @property: name of the property containing the string list
++ *
++ * Return:
++ *   the number of strings in the given property
++ *   -FDT_ERR_BADVALUE if the property value is not NUL-terminated
++ *   -FDT_ERR_NOTFOUND if the property does not exist
++ */
++int fdt_stringlist_count(const void *fdt, int nodeoffset, const char *property);
++
++/**
++ * fdt_stringlist_search - find a string in a string list and return its index
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of a tree node
++ * @property: name of the property containing the string list
++ * @string: string to look up in the string list
++ *
++ * Note that it is possible for this function to succeed on property values
++ * that are not NUL-terminated. That's because the function will stop after
++ * finding the first occurrence of @string. This can for example happen with
++ * small-valued cell properties, such as #address-cells, when searching for
++ * the empty string.
++ *
++ * return:
++ *   the index of the string in the list of strings
++ *   -FDT_ERR_BADVALUE if the property value is not NUL-terminated
++ *   -FDT_ERR_NOTFOUND if the property does not exist or does not contain
++ *                     the given string
++ */
++int fdt_stringlist_search(const void *fdt, int nodeoffset, const char *property,
++			  const char *string);
++
++/**
++ * fdt_stringlist_get() - obtain the string at a given index in a string list
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of a tree node
++ * @property: name of the property containing the string list
++ * @index: index of the string to return
++ * @lenp: return location for the string length or an error code on failure
++ *
++ * Note that this will successfully extract strings from properties with
++ * non-NUL-terminated values. For example on small-valued cell properties
++ * this function will return the empty string.
++ *
++ * If non-NULL, the length of the string (on success) or a negative error-code
++ * (on failure) will be stored in the integer pointer to by lenp.
++ *
++ * Return:
++ *   A pointer to the string at the given index in the string list or NULL on
++ *   failure. On success the length of the string will be stored in the memory
++ *   location pointed to by the lenp parameter, if non-NULL. On failure one of
++ *   the following negative error codes will be returned in the lenp parameter
++ *   (if non-NULL):
++ *     -FDT_ERR_BADVALUE if the property value is not NUL-terminated
++ *     -FDT_ERR_NOTFOUND if the property does not exist
++ */
++const char *fdt_stringlist_get(const void *fdt, int nodeoffset,
++			       const char *property, int index,
++			       int *lenp);
++
++/**********************************************************************/
++/* Read-only functions (addressing related)                           */
++/**********************************************************************/
++
++/**
++ * FDT_MAX_NCELLS - maximum value for #address-cells and #size-cells
++ *
++ * This is the maximum value for #address-cells, #size-cells and
++ * similar properties that will be processed by libfdt.  IEE1275
++ * requires that OF implementations handle values up to 4.
++ * Implementations may support larger values, but in practice higher
++ * values aren't used.
++ */
++#define FDT_MAX_NCELLS		4
++
++/**
++ * fdt_address_cells - retrieve address size for a bus represented in the tree
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node to find the address size for
++ *
++ * When the node has a valid #address-cells property, returns its value.
++ *
++ * returns:
++ *	0 <= n < FDT_MAX_NCELLS, on success
++ *      2, if the node has no #address-cells property
++ *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid
++ *		#address-cells property
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_address_cells(const void *fdt, int nodeoffset);
++
++/**
++ * fdt_size_cells - retrieve address range size for a bus represented in the
++ *                  tree
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node to find the address range size for
++ *
++ * When the node has a valid #size-cells property, returns its value.
++ *
++ * returns:
++ *	0 <= n < FDT_MAX_NCELLS, on success
++ *      1, if the node has no #size-cells property
++ *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid
++ *		#size-cells property
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_size_cells(const void *fdt, int nodeoffset);
++
++
++/**********************************************************************/
++/* Write-in-place functions                                           */
++/**********************************************************************/
++
++/**
++ * fdt_setprop_inplace_namelen_partial - change a property's value,
++ *                                       but not its size
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @namelen: number of characters of name to consider
++ * @idx: index of the property to change in the array
++ * @val: pointer to data to replace the property value with
++ * @len: length of the property value
++ *
++ * Identical to fdt_setprop_inplace(), but modifies the given property
++ * starting from the given index, and using only the first characters
++ * of the name. It is useful when you want to manipulate only one value of
++ * an array and you have a string that doesn't end with \0.
++ *
++ * Return: 0 on success, negative libfdt error value otherwise
++ */
++#ifndef SWIG /* Not available in Python */
++int fdt_setprop_inplace_namelen_partial(void *fdt, int nodeoffset,
++					const char *name, int namelen,
++					uint32_t idx, const void *val,
++					int len);
++#endif
++
++/**
++ * fdt_setprop_inplace - change a property's value, but not its size
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: pointer to data to replace the property value with
++ * @len: length of the property value
++ *
++ * fdt_setprop_inplace() replaces the value of a given property with
++ * the data in val, of length len.  This function cannot change the
++ * size of a property, and so will only work if len is equal to the
++ * current length of the property.
++ *
++ * This function will alter only the bytes in the blob which contain
++ * the given property value, and will not alter or move any other part
++ * of the tree.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, if len is not equal to the property's current length
++ *	-FDT_ERR_NOTFOUND, node does not have the named property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++#ifndef SWIG /* Not available in Python */
++int fdt_setprop_inplace(void *fdt, int nodeoffset, const char *name,
++			const void *val, int len);
++#endif
++
++/**
++ * fdt_setprop_inplace_u32 - change the value of a 32-bit integer property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 32-bit integer value to replace the property with
++ *
++ * fdt_setprop_inplace_u32() replaces the value of a given property
++ * with the 32-bit integer value in val, converting val to big-endian
++ * if necessary.  This function cannot change the size of a property,
++ * and so will only work if the property already exists and has length
++ * 4.
++ *
++ * This function will alter only the bytes in the blob which contain
++ * the given property value, and will not alter or move any other part
++ * of the tree.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, if the property's length is not equal to 4
++ *	-FDT_ERR_NOTFOUND, node does not have the named property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_setprop_inplace_u32(void *fdt, int nodeoffset,
++					  const char *name, uint32_t val)
++{
++	fdt32_t tmp = cpu_to_fdt32(val);
++	return fdt_setprop_inplace(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_setprop_inplace_u64 - change the value of a 64-bit integer property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 64-bit integer value to replace the property with
++ *
++ * fdt_setprop_inplace_u64() replaces the value of a given property
++ * with the 64-bit integer value in val, converting val to big-endian
++ * if necessary.  This function cannot change the size of a property,
++ * and so will only work if the property already exists and has length
++ * 8.
++ *
++ * This function will alter only the bytes in the blob which contain
++ * the given property value, and will not alter or move any other part
++ * of the tree.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, if the property's length is not equal to 8
++ *	-FDT_ERR_NOTFOUND, node does not have the named property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_setprop_inplace_u64(void *fdt, int nodeoffset,
++					  const char *name, uint64_t val)
++{
++	fdt64_t tmp = cpu_to_fdt64(val);
++	return fdt_setprop_inplace(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_setprop_inplace_cell - change the value of a single-cell property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node containing the property
++ * @name: name of the property to change the value of
++ * @val: new value of the 32-bit cell
++ *
++ * This is an alternative name for fdt_setprop_inplace_u32()
++ * Return: 0 on success, negative libfdt error number otherwise.
++ */
++static inline int fdt_setprop_inplace_cell(void *fdt, int nodeoffset,
++					   const char *name, uint32_t val)
++{
++	return fdt_setprop_inplace_u32(fdt, nodeoffset, name, val);
++}
++
++/**
++ * fdt_nop_property - replace a property with nop tags
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to nop
++ * @name: name of the property to nop
++ *
++ * fdt_nop_property() will replace a given property's representation
++ * in the blob with FDT_NOP tags, effectively removing it from the
++ * tree.
++ *
++ * This function will alter only the bytes in the blob which contain
++ * the property, and will not alter or move any other part of the
++ * tree.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOTFOUND, node does not have the named property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_nop_property(void *fdt, int nodeoffset, const char *name);
++
++/**
++ * fdt_nop_node - replace a node (subtree) with nop tags
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node to nop
++ *
++ * fdt_nop_node() will replace a given node's representation in the
++ * blob, including all its subnodes, if any, with FDT_NOP tags,
++ * effectively removing it from the tree.
++ *
++ * This function will alter only the bytes in the blob which contain
++ * the node and its properties and subnodes, and will not alter or
++ * move any other part of the tree.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_nop_node(void *fdt, int nodeoffset);
++
++/**********************************************************************/
++/* Sequential write functions                                         */
++/**********************************************************************/
++
++/* fdt_create_with_flags flags */
++#define FDT_CREATE_FLAG_NO_NAME_DEDUP 0x1
++	/* FDT_CREATE_FLAG_NO_NAME_DEDUP: Do not try to de-duplicate property
++	 * names in the fdt. This can result in faster creation times, but
++	 * a larger fdt. */
++
++#define FDT_CREATE_FLAGS_ALL	(FDT_CREATE_FLAG_NO_NAME_DEDUP)
++
++/**
++ * fdt_create_with_flags - begin creation of a new fdt
++ * @buf: pointer to memory allocated where fdt will be created
++ * @bufsize: size of the memory space at fdt
++ * @flags: a valid combination of FDT_CREATE_FLAG_ flags, or 0.
++ *
++ * fdt_create_with_flags() begins the process of creating a new fdt with
++ * the sequential write interface.
++ *
++ * fdt creation process must end with fdt_finished() to produce a valid fdt.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, bufsize is insufficient for a minimal fdt
++ *	-FDT_ERR_BADFLAGS, flags is not valid
++ */
++int fdt_create_with_flags(void *buf, int bufsize, uint32_t flags);
++
++/**
++ * fdt_create - begin creation of a new fdt
++ * @buf: pointer to memory allocated where fdt will be created
++ * @bufsize: size of the memory space at fdt
++ *
++ * fdt_create() is equivalent to fdt_create_with_flags() with flags=0.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, bufsize is insufficient for a minimal fdt
++ */
++int fdt_create(void *buf, int bufsize);
++
++int fdt_resize(void *fdt, void *buf, int bufsize);
++int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size);
++int fdt_finish_reservemap(void *fdt);
++int fdt_begin_node(void *fdt, const char *name);
++int fdt_property(void *fdt, const char *name, const void *val, int len);
++static inline int fdt_property_u32(void *fdt, const char *name, uint32_t val)
++{
++	fdt32_t tmp = cpu_to_fdt32(val);
++	return fdt_property(fdt, name, &tmp, sizeof(tmp));
++}
++static inline int fdt_property_u64(void *fdt, const char *name, uint64_t val)
++{
++	fdt64_t tmp = cpu_to_fdt64(val);
++	return fdt_property(fdt, name, &tmp, sizeof(tmp));
++}
++
++#ifndef SWIG /* Not available in Python */
++static inline int fdt_property_cell(void *fdt, const char *name, uint32_t val)
++{
++	return fdt_property_u32(fdt, name, val);
++}
++#endif
++
++/**
++ * fdt_property_placeholder - add a new property and return a ptr to its value
++ *
++ * @fdt: pointer to the device tree blob
++ * @name: name of property to add
++ * @len: length of property value in bytes
++ * @valp: returns a pointer to where where the value should be placed
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_NOSPACE, standard meanings
++ */
++int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp);
++
++#define fdt_property_string(fdt, name, str) \
++	fdt_property(fdt, name, str, strlen(str)+1)
++int fdt_end_node(void *fdt);
++int fdt_finish(void *fdt);
++
++/**********************************************************************/
++/* Read-write functions                                               */
++/**********************************************************************/
++
++int fdt_create_empty_tree(void *buf, int bufsize);
++int fdt_open_into(const void *fdt, void *buf, int bufsize);
++int fdt_pack(void *fdt);
++
++/**
++ * fdt_add_mem_rsv - add one memory reserve map entry
++ * @fdt: pointer to the device tree blob
++ * @address: 64-bit start address of the reserve map entry
++ * @size: 64-bit size of the reserved region
++ *
++ * Adds a reserve map entry to the given blob reserving a region at
++ * address address of length size.
++ *
++ * This function will insert data into the reserve map and will
++ * therefore change the indexes of some entries in the table.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new reservation entry
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size);
++
++/**
++ * fdt_del_mem_rsv - remove a memory reserve map entry
++ * @fdt: pointer to the device tree blob
++ * @n: entry to remove
++ *
++ * fdt_del_mem_rsv() removes the n-th memory reserve map entry from
++ * the blob.
++ *
++ * This function will delete data from the reservation table and will
++ * therefore change the indexes of some entries in the table.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOTFOUND, there is no entry of the given index (i.e. there
++ *		are less than n+1 reserve map entries)
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_del_mem_rsv(void *fdt, int n);
++
++/**
++ * fdt_set_name - change the name of a given node
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: structure block offset of a node
++ * @name: name to give the node
++ *
++ * fdt_set_name() replaces the name (including unit address, if any)
++ * of the given node with the given string.  NOTE: this function can't
++ * efficiently check if the new name is unique amongst the given
++ * node's siblings; results are undefined if this function is invoked
++ * with a name equal to one of the given node's siblings.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob
++ *		to contain the new name
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE, standard meanings
++ */
++int fdt_set_name(void *fdt, int nodeoffset, const char *name);
++
++/**
++ * fdt_setprop - create or change a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: pointer to data to set the property value to
++ * @len: length of the property value
++ *
++ * fdt_setprop() sets the value of the named property in the given
++ * node to the given value and length, creating the property if it
++ * does not already exist.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_setprop(void *fdt, int nodeoffset, const char *name,
++		const void *val, int len);
++
++/**
++ * fdt_setprop_placeholder - allocate space for a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @len: length of the property value
++ * @prop_data: return pointer to property data
++ *
++ * fdt_setprop_placeholer() allocates the named property in the given node.
++ * If the property exists it is resized. In either case a pointer to the
++ * property data is returned.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name,
++			    int len, void **prop_data);
++
++/**
++ * fdt_setprop_u32 - set a property to a 32-bit integer
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 32-bit integer value for the property (native endian)
++ *
++ * fdt_setprop_u32() sets the value of the named property in the given
++ * node to the given 32-bit integer value (converting to big-endian if
++ * necessary), or creates a new property with that value if it does
++ * not already exist.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_setprop_u32(void *fdt, int nodeoffset, const char *name,
++				  uint32_t val)
++{
++	fdt32_t tmp = cpu_to_fdt32(val);
++	return fdt_setprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_setprop_u64 - set a property to a 64-bit integer
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 64-bit integer value for the property (native endian)
++ *
++ * fdt_setprop_u64() sets the value of the named property in the given
++ * node to the given 64-bit integer value (converting to big-endian if
++ * necessary), or creates a new property with that value if it does
++ * not already exist.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_setprop_u64(void *fdt, int nodeoffset, const char *name,
++				  uint64_t val)
++{
++	fdt64_t tmp = cpu_to_fdt64(val);
++	return fdt_setprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_setprop_cell - set a property to a single cell value
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 32-bit integer value for the property (native endian)
++ *
++ * This is an alternative name for fdt_setprop_u32()
++ *
++ * Return: 0 on success, negative libfdt error value otherwise.
++ */
++static inline int fdt_setprop_cell(void *fdt, int nodeoffset, const char *name,
++				   uint32_t val)
++{
++	return fdt_setprop_u32(fdt, nodeoffset, name, val);
++}
++
++/**
++ * fdt_setprop_string - set a property to a string value
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @str: string value for the property
++ *
++ * fdt_setprop_string() sets the value of the named property in the
++ * given node to the given string value (using the length of the
++ * string to determine the new length of the property), or creates a
++ * new property with that value if it does not already exist.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++#define fdt_setprop_string(fdt, nodeoffset, name, str) \
++	fdt_setprop((fdt), (nodeoffset), (name), (str), strlen(str)+1)
++
++
++/**
++ * fdt_setprop_empty - set a property to an empty value
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ *
++ * fdt_setprop_empty() sets the value of the named property in the
++ * given node to an empty (zero length) value, or creates a new empty
++ * property if it does not already exist.
++ *
++ * This function may insert or delete data from the blob, and will
++ * therefore change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++#define fdt_setprop_empty(fdt, nodeoffset, name) \
++	fdt_setprop((fdt), (nodeoffset), (name), NULL, 0)
++
++/**
++ * fdt_appendprop - append to or create a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to append to
++ * @val: pointer to data to append to the property value
++ * @len: length of the data to append to the property value
++ *
++ * fdt_appendprop() appends the value to the named property in the
++ * given node, creating the property if it does not already exist.
++ *
++ * This function may insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
++		   const void *val, int len);
++
++/**
++ * fdt_appendprop_u32 - append a 32-bit integer value to a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 32-bit integer value to append to the property (native endian)
++ *
++ * fdt_appendprop_u32() appends the given 32-bit integer value
++ * (converting to big-endian if necessary) to the value of the named
++ * property in the given node, or creates a new property with that
++ * value if it does not already exist.
++ *
++ * This function may insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_appendprop_u32(void *fdt, int nodeoffset,
++				     const char *name, uint32_t val)
++{
++	fdt32_t tmp = cpu_to_fdt32(val);
++	return fdt_appendprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_appendprop_u64 - append a 64-bit integer value to a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 64-bit integer value to append to the property (native endian)
++ *
++ * fdt_appendprop_u64() appends the given 64-bit integer value
++ * (converting to big-endian if necessary) to the value of the named
++ * property in the given node, or creates a new property with that
++ * value if it does not already exist.
++ *
++ * This function may insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++static inline int fdt_appendprop_u64(void *fdt, int nodeoffset,
++				     const char *name, uint64_t val)
++{
++	fdt64_t tmp = cpu_to_fdt64(val);
++	return fdt_appendprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
++}
++
++/**
++ * fdt_appendprop_cell - append a single cell value to a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @val: 32-bit integer value to append to the property (native endian)
++ *
++ * This is an alternative name for fdt_appendprop_u32()
++ *
++ * Return: 0 on success, negative libfdt error value otherwise.
++ */
++static inline int fdt_appendprop_cell(void *fdt, int nodeoffset,
++				      const char *name, uint32_t val)
++{
++	return fdt_appendprop_u32(fdt, nodeoffset, name, val);
++}
++
++/**
++ * fdt_appendprop_string - append a string to a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to change
++ * @name: name of the property to change
++ * @str: string value to append to the property
++ *
++ * fdt_appendprop_string() appends the given string to the value of
++ * the named property in the given node, or creates a new property
++ * with that value if it does not already exist.
++ *
++ * This function may insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain the new property value
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++#define fdt_appendprop_string(fdt, nodeoffset, name, str) \
++	fdt_appendprop((fdt), (nodeoffset), (name), (str), strlen(str)+1)
++
++/**
++ * fdt_appendprop_addrrange - append a address range property
++ * @fdt: pointer to the device tree blob
++ * @parent: offset of the parent node
++ * @nodeoffset: offset of the node to add a property at
++ * @name: name of property
++ * @addr: start address of a given range
++ * @size: size of a given range
++ *
++ * fdt_appendprop_addrrange() appends an address range value (start
++ * address and size) to the value of the named property in the given
++ * node, or creates a new property with that value if it does not
++ * already exist.
++ * If "name" is not specified, a default "reg" is used.
++ * Cell sizes are determined by parent's #address-cells and #size-cells.
++ *
++ * This function may insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid
++ *		#address-cells property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADVALUE, addr or size doesn't fit to respective cells size
++ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
++ *		contain a new property
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_appendprop_addrrange(void *fdt, int parent, int nodeoffset,
++			     const char *name, uint64_t addr, uint64_t size);
++
++/**
++ * fdt_delprop - delete a property
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node whose property to nop
++ * @name: name of the property to nop
++ *
++ * fdt_del_property() will delete the given property.
++ *
++ * This function will delete data from the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOTFOUND, node does not have the named property
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_delprop(void *fdt, int nodeoffset, const char *name);
++
++/**
++ * fdt_add_subnode_namelen - creates a new node based on substring
++ * @fdt: pointer to the device tree blob
++ * @parentoffset: structure block offset of a node
++ * @name: name of the subnode to create
++ * @namelen: number of characters of name to consider
++ *
++ * Identical to fdt_add_subnode(), but use only the first @namelen
++ * characters of @name as the name of the new node.  This is useful for
++ * creating subnodes based on a portion of a larger string, such as a
++ * full path.
++ *
++ * Return: structure block offset of the created subnode (>=0),
++ *	   negative libfdt error value otherwise
++ */
++#ifndef SWIG /* Not available in Python */
++int fdt_add_subnode_namelen(void *fdt, int parentoffset,
++			    const char *name, int namelen);
++#endif
++
++/**
++ * fdt_add_subnode - creates a new node
++ * @fdt: pointer to the device tree blob
++ * @parentoffset: structure block offset of a node
++ * @name: name of the subnode to locate
++ *
++ * fdt_add_subnode() creates a new node as a subnode of the node at
++ * structure block offset parentoffset, with the given name (which
++ * should include the unit address, if any).
++ *
++ * This function will insert data into the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	structure block offset of the created nodeequested subnode (>=0), on
++ *		success
++ *	-FDT_ERR_NOTFOUND, if the requested subnode does not exist
++ *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE
++ *		tag
++ *	-FDT_ERR_EXISTS, if the node at parentoffset already has a subnode of
++ *		the given name
++ *	-FDT_ERR_NOSPACE, if there is insufficient free space in the
++ *		blob to contain the new node
++ *	-FDT_ERR_NOSPACE
++ *	-FDT_ERR_BADLAYOUT
++ *      -FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings.
++ */
++int fdt_add_subnode(void *fdt, int parentoffset, const char *name);
++
++/**
++ * fdt_del_node - delete a node (subtree)
++ * @fdt: pointer to the device tree blob
++ * @nodeoffset: offset of the node to nop
++ *
++ * fdt_del_node() will remove the given node, including all its
++ * subnodes if any, from the blob.
++ *
++ * This function will delete data from the blob, and will therefore
++ * change the offsets of some existing nodes.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_del_node(void *fdt, int nodeoffset);
++
++/**
++ * fdt_overlay_apply - Applies a DT overlay on a base DT
++ * @fdt: pointer to the base device tree blob
++ * @fdto: pointer to the device tree overlay blob
++ *
++ * fdt_overlay_apply() will apply the given device tree overlay on the
++ * given base device tree.
++ *
++ * Expect the base device tree to be modified, even if the function
++ * returns an error.
++ *
++ * returns:
++ *	0, on success
++ *	-FDT_ERR_NOSPACE, there's not enough space in the base device tree
++ *	-FDT_ERR_NOTFOUND, the overlay points to some inexistant nodes or
++ *		properties in the base DT
++ *	-FDT_ERR_BADPHANDLE,
++ *	-FDT_ERR_BADOVERLAY,
++ *	-FDT_ERR_NOPHANDLES,
++ *	-FDT_ERR_INTERNAL,
++ *	-FDT_ERR_BADLAYOUT,
++ *	-FDT_ERR_BADMAGIC,
++ *	-FDT_ERR_BADOFFSET,
++ *	-FDT_ERR_BADPATH,
++ *	-FDT_ERR_BADVERSION,
++ *	-FDT_ERR_BADSTRUCTURE,
++ *	-FDT_ERR_BADSTATE,
++ *	-FDT_ERR_TRUNCATED, standard meanings
++ */
++int fdt_overlay_apply(void *fdt, void *fdto);
++
++/**
++ * fdt_overlay_target_offset - retrieves the offset of a fragment's target
++ * @fdt: Base device tree blob
++ * @fdto: Device tree overlay blob
++ * @fragment_offset: node offset of the fragment in the overlay
++ * @pathp: pointer which receives the path of the target (or NULL)
++ *
++ * fdt_overlay_target_offset() retrieves the target offset in the base
++ * device tree of a fragment, no matter how the actual targeting is
++ * done (through a phandle or a path)
++ *
++ * returns:
++ *      the targeted node offset in the base device tree
++ *      Negative error code on error
++ */
++int fdt_overlay_target_offset(const void *fdt, const void *fdto,
++			      int fragment_offset, char const **pathp);
++
++/**********************************************************************/
++/* Debugging / informational functions                                */
++/**********************************************************************/
++
++const char *fdt_strerror(int errval);
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* LIBFDT_H */
+diff --git a/include/libfdt_env.h b/include/libfdt_env.h
+new file mode 100644
+index 0000000..51b31d1
+--- /dev/null
++++ b/include/libfdt_env.h
+@@ -0,0 +1,95 @@
++/* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
++#ifndef LIBFDT_ENV_H
++#define LIBFDT_ENV_H
++/*
++ * libfdt - Flat Device Tree manipulation
++ * Copyright (C) 2006 David Gibson, IBM Corporation.
++ * Copyright 2012 Kim Phillips, Freescale Semiconductor.
++ */
++
++#include <stdbool.h>
++#include <stddef.h>
++#include <stdint.h>
++#include <limits.h>
++#include <string.h>
++
++#ifdef __CHECKER__
++#define FDT_FORCE __attribute__((force))
++#define FDT_BITWISE __attribute__((bitwise))
++#else
++#define FDT_FORCE
++#define FDT_BITWISE
++#endif
++
++typedef uint16_t FDT_BITWISE fdt16_t;
++typedef uint32_t FDT_BITWISE fdt32_t;
++typedef uint64_t FDT_BITWISE fdt64_t;
++
++#define EXTRACT_BYTE(x, n)	((unsigned long long)((uint8_t *)&x)[n])
++#define CPU_TO_FDT16(x) ((EXTRACT_BYTE(x, 0) << 8) | EXTRACT_BYTE(x, 1))
++#define CPU_TO_FDT32(x) ((EXTRACT_BYTE(x, 0) << 24) | (EXTRACT_BYTE(x, 1) << 16) | \
++			 (EXTRACT_BYTE(x, 2) << 8) | EXTRACT_BYTE(x, 3))
++#define CPU_TO_FDT64(x) ((EXTRACT_BYTE(x, 0) << 56) | (EXTRACT_BYTE(x, 1) << 48) | \
++			 (EXTRACT_BYTE(x, 2) << 40) | (EXTRACT_BYTE(x, 3) << 32) | \
++			 (EXTRACT_BYTE(x, 4) << 24) | (EXTRACT_BYTE(x, 5) << 16) | \
++			 (EXTRACT_BYTE(x, 6) << 8) | EXTRACT_BYTE(x, 7))
++
++static inline uint16_t fdt16_to_cpu(fdt16_t x)
++{
++	return (FDT_FORCE uint16_t)CPU_TO_FDT16(x);
++}
++static inline fdt16_t cpu_to_fdt16(uint16_t x)
++{
++	return (FDT_FORCE fdt16_t)CPU_TO_FDT16(x);
++}
++
++static inline uint32_t fdt32_to_cpu(fdt32_t x)
++{
++	return (FDT_FORCE uint32_t)CPU_TO_FDT32(x);
++}
++static inline fdt32_t cpu_to_fdt32(uint32_t x)
++{
++	return (FDT_FORCE fdt32_t)CPU_TO_FDT32(x);
++}
++
++static inline uint64_t fdt64_to_cpu(fdt64_t x)
++{
++	return (FDT_FORCE uint64_t)CPU_TO_FDT64(x);
++}
++static inline fdt64_t cpu_to_fdt64(uint64_t x)
++{
++	return (FDT_FORCE fdt64_t)CPU_TO_FDT64(x);
++}
++#undef CPU_TO_FDT64
++#undef CPU_TO_FDT32
++#undef CPU_TO_FDT16
++#undef EXTRACT_BYTE
++
++#ifdef __APPLE__
++#include <AvailabilityMacros.h>
++
++/* strnlen() is not available on Mac OS < 10.7 */
++# if !defined(MAC_OS_X_VERSION_10_7) || (MAC_OS_X_VERSION_MAX_ALLOWED < \
++                                         MAC_OS_X_VERSION_10_7)
++
++#define strnlen fdt_strnlen
++
++/*
++ * fdt_strnlen: returns the length of a string or max_count - which ever is
++ * smallest.
++ * Input 1 string: the string whose size is to be determined
++ * Input 2 max_count: the maximum value returned by this function
++ * Output: length of the string or max_count (the smallest of the two)
++ */
++static inline size_t fdt_strnlen(const char *string, size_t max_count)
++{
++    const char *p = memchr(string, 0, max_count);
++    return p ? p - string : max_count;
++}
++
++#endif /* !defined(MAC_OS_X_VERSION_10_7) || (MAC_OS_X_VERSION_MAX_ALLOWED <
++          MAC_OS_X_VERSION_10_7) */
++
++#endif /* __APPLE__ */
++
++#endif /* LIBFDT_ENV_H */
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0011-common-Add-essential-libc-functions.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0011-common-Add-essential-libc-functions.patch
new file mode 100644
index 0000000..871a178
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0011-common-Add-essential-libc-functions.patch
@@ -0,0 +1,101 @@
+From 0f2c7ca446063be6b193fbf870d38c0af19e15c5 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 28 Dec 2021 17:28:25 +0800
+Subject: [PATCH] common: Add essential libc functions
+
+The libfdt uses some of the libc functions, e.g. memcmp, memmove,
+strlen .etc. Add them in lib.c.
+
+The code is copied from TF-A (v2.5) [1] project, which is under the
+terms of BSD license. It is the same with boot-wrapper.
+
+[1]: https://git.trustedfirmware.org/TF-A/trusted-firmware-a.git
+
+Issue-Id: SCM-3814
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: If3b55b00afa8694c7522df989a41e0b38eda1d38
+---
+ common/lib.c | 71 +++++++++++++++++++++++++++++++++++++++++++++++++++-
+ 1 file changed, 70 insertions(+), 1 deletion(-)
+
+diff --git a/common/lib.c b/common/lib.c
+index fcf5f69..0be1c4a 100644
+--- a/common/lib.c
++++ b/common/lib.c
+@@ -32,4 +32,73 @@ void *memset(void *s, int c, size_t n)
+ 	return s;
+ }
+ 
+-/* TODO: memmove and memcmp could also be called */
++int memcmp(const void *s1, const void *s2, size_t len)
++{
++	const unsigned char *s = s1;
++	const unsigned char *d = s2;
++	unsigned char sc;
++	unsigned char dc;
++
++	while (len--) {
++		sc = *s++;
++		dc = *d++;
++		if (sc - dc)
++			return (sc - dc);
++	}
++
++	return 0;
++}
++
++void *memmove(void *dst, const void *src, size_t len)
++{
++	if ((size_t)dst - (size_t)src >= len) {
++		/* destination not in source data, so can safely use memcpy */
++		return memcpy(dst, src, len);
++	} else {
++		/* copy backwards... */
++		const char *end = dst;
++		const char *s = (const char *)src + len;
++		char *d = (char *)dst + len;
++		while (d != end)
++			*--d = *--s;
++	}
++	return dst;
++}
++
++void *memchr(const void *src, int c, size_t len)
++{
++	const unsigned char *s = src;
++
++	while (len--) {
++		if (*s == (unsigned char)c)
++			return (void *) s;
++		s++;
++	}
++
++	return NULL;
++}
++
++char *strrchr(const char *p, int ch)
++{
++	char *save;
++	char c;
++
++	c = ch;
++	for (save = NULL;; ++p) {
++		if (*p == c)
++			save = (char *)p;
++		if (*p == '\0')
++			return (save);
++	}
++	/* NOTREACHED */
++}
++
++size_t strlen(const char *s)
++{
++	const char *cursor = s;
++
++	while (*cursor)
++		cursor++;
++
++	return cursor - s;
++}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0012-Makefile-Add-the-libfdt-to-the-Makefile-system.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0012-Makefile-Add-the-libfdt-to-the-Makefile-system.patch
new file mode 100644
index 0000000..5917ef2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0012-Makefile-Add-the-libfdt-to-the-Makefile-system.patch
@@ -0,0 +1,61 @@
+From de5d2b6c200ae5dd8113751e58bf7cf5844eec5a Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 28 Dec 2021 17:42:48 +0800
+Subject: [PATCH] Makefile: Add the libfdt to the Makefile system
+
+Add the libfdt into Makefile system. The libfdt uses const value and
+thus gcc will enable the stack guard. The stack guard will fail the
+compile. Add -fno-stack-protector to fix it.
+
+Issue-Id: SCM-3814
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I472bc28cdc5cde3b22461a4b7d7a3752ae382b4b
+---
+ Makefile.am | 11 +++++++++--
+ 1 file changed, 9 insertions(+), 2 deletions(-)
+
+diff --git a/Makefile.am b/Makefile.am
+index 88a27de..5e8668a 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -36,6 +36,9 @@ PSCI_CPU_OFF	:= 0x84000002
+ COMMON_SRC	:= common/
+ COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o
+ 
++LIBFDT_SRC	:= common/libfdt/
++LIBFDT_OBJS	:= fdt.o fdt_ro.o fdt_rw.o
++
+ ARCH_OBJ	:= boot.o stack.o utils.o
+ 
+ if BOOTWRAPPER_32
+@@ -125,11 +128,12 @@ CHOSEN_NODE	:= chosen {						\
+ CPPFLAGS	+= $(INITRD_FLAGS)
+ CFLAGS		+= -I$(top_srcdir)/include/ -I$(top_srcdir)/$(ARCH_SRC)/include/
+ CFLAGS		+= -Wall -fomit-frame-pointer
++CFLAGS 		+= -fno-stack-protector
+ CFLAGS		+= -ffunction-sections -fdata-sections
+ CFLAGS		+= -fno-pic -fno-pie
+ LDFLAGS		+= --gc-sections
+ 
+-OBJ		:= $(addprefix $(ARCH_SRC),$(ARCH_OBJ)) $(addprefix $(COMMON_SRC),$(COMMON_OBJ))
++OBJ		:= $(addprefix $(ARCH_SRC),$(ARCH_OBJ)) $(addprefix $(COMMON_SRC),$(COMMON_OBJ)) $(addprefix $(LIBFDT_SRC),$(LIBFDT_OBJS))
+ 
+ # Don't lookup all prerequisites in $(top_srcdir), only the source files. When
+ # building outside the source tree $(ARCH_SRC) needs to be created.
+@@ -150,10 +154,13 @@ $(ARCH_SRC):
+ $(COMMON_SRC):
+ 	$(MKDIR_P) $@
+ 
++$(LIBFDT_SRC):
++	$(MKDIR_P) $@
++
+ %.o: %.S Makefile | $(ARCH_SRC)
+ 	$(CC) $(CPPFLAGS) -D__ASSEMBLY__ $(CFLAGS) $(DEFINES) -c -o $@ $<
+ 
+-%.o: %.c Makefile | $(COMMON_SRC)
++%.o: %.c Makefile | $(COMMON_SRC) $(LIBFDT_SRC)
+ 	$(CC) $(CPPFLAGS) $(CFLAGS) $(DEFINES) -c -o $@ $<
+ 
+ model.lds: $(LD_SCRIPT) Makefile
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0013-platform-Add-print_hex-func.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0013-platform-Add-print_hex-func.patch
new file mode 100644
index 0000000..136e18e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0013-platform-Add-print_hex-func.patch
@@ -0,0 +1,67 @@
+From 5b8cb5192dbd0332e027e8999c3afe4433983291 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Wed, 29 Dec 2021 10:50:21 +0800
+Subject: [PATCH] platform: Add print_hex func
+
+Refine the print functions, and add a new print_hex func to print hex
+numbers.
+
+Issue-Id: SCM-3814
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: Ic960345d9ef0b41d81d30c4a4dbd9c31139907c4
+---
+ common/platform.c | 33 +++++++++++++++++++++++++--------
+ 1 file changed, 25 insertions(+), 8 deletions(-)
+
+diff --git a/common/platform.c b/common/platform.c
+index d11f568..8269392 100644
+--- a/common/platform.c
++++ b/common/platform.c
+@@ -30,20 +30,37 @@
+ #define V2M_SYS(reg)	((void *)SYSREGS_BASE + V2M_SYS_##reg)
+ #endif
+ 
+-static void print_string(const char *str)
++static void print_char(const char c)
+ {
+ 	uint32_t flags;
++	do {
++		flags = raw_readl(PL011(UARTFR));
++	} while (flags & PL011_UARTFR_FIFO_FULL);
+ 
++	raw_writel(c, PL011(UARTDR));
++
++	do {
++		flags = raw_readl(PL011(UARTFR));
++	} while (flags & PL011_UARTFR_BUSY);
++}
++
++void print_string(const char *str)
++{
+ 	while (*str) {
+-		do
+-			flags = raw_readl(PL011(UARTFR));
+-		while (flags & PL011_UARTFR_FIFO_FULL);
++		print_char(*str++);
++	}
++}
+ 
+-		raw_writel(*str++, PL011(UARTDR));
++#define HEX_CHARS_PER_INT (2 * sizeof(int))
++
++void print_hex(unsigned int val)
++{
+ 
+-		do
+-			flags = raw_readl(PL011(UARTFR));
+-		while (flags & PL011_UARTFR_BUSY);
++	const char hex_chars[16] = "0123456789abcdef";
++	int i;
++	for (i = HEX_CHARS_PER_INT - 1; i >= 0; i--) {
++		int v = (val >> (4 * i)) & 0xf;
++		print_char(hex_chars[v]);
+ 	}
+ }
+ 
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0014-common-Add-mem-usage-to-memreserve.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0014-common-Add-mem-usage-to-memreserve.patch
new file mode 100644
index 0000000..ea51816
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0014-common-Add-mem-usage-to-memreserve.patch
@@ -0,0 +1,96 @@
+From b447242cd2457bec20d47fe6a8a5758d97a3bde3 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Wed, 19 Jan 2022 16:19:02 +0800
+Subject: [PATCH] common: Add mem usage to /memreserve/
+
+Set /memreserve/ to prevent next boot stages from overrding PSCI
+services with libfdt.
+
+Issue-Id: SCM-3815
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I2ea80cdf736a910fa2c3deb622e21d50f04be960
+---
+ Makefile.am          |  2 +-
+ common/boot.c        |  1 +
+ common/device_tree.c | 34 ++++++++++++++++++++++++++++++++++
+ include/boot.h       |  1 +
+ 4 files changed, 37 insertions(+), 1 deletion(-)
+ create mode 100644 common/device_tree.c
+
+diff --git a/Makefile.am b/Makefile.am
+index 5e8668a..734de92 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -34,7 +34,7 @@ endif
+ PSCI_CPU_OFF	:= 0x84000002
+ 
+ COMMON_SRC	:= common/
+-COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o
++COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o device_tree.o
+ 
+ LIBFDT_SRC	:= common/libfdt/
+ LIBFDT_OBJS	:= fdt.o fdt_ro.o fdt_rw.o
+diff --git a/common/boot.c b/common/boot.c
+index c74d34c..ee2bea0 100644
+--- a/common/boot.c
++++ b/common/boot.c
+@@ -63,6 +63,7 @@ void __noreturn first_spin(unsigned int cpu, unsigned long *mbox,
+ {
+ 	if (cpu == 0) {
+ 		init_platform();
++		dt_add_memreserve();
+ 
+ 		*mbox = (unsigned long)&entrypoint;
+ 		sevl();
+diff --git a/common/device_tree.c b/common/device_tree.c
+new file mode 100644
+index 0000000..4d0876c
+--- /dev/null
++++ b/common/device_tree.c
+@@ -0,0 +1,34 @@
++/*
++ * device_tree.c - Basic device tree node handler
++ *
++ * Copyright (C) 2021 ARM Limited. All rights reserved.
++ *
++ * Use of this source code is governed by a BSD-style license that can be
++ * found in the LICENSE.txt file.
++ */
++#include <libfdt.h>
++
++extern unsigned long dtb;
++extern char firmware_start[], firmware_end[];
++
++extern void print_string(const char *str);
++
++static void *blob;
++
++
++void dt_add_memreserve(void)
++{
++	int ret;
++
++	blob = (void*)&dtb;
++	print_string("Add /memreserve/\n\r");
++
++	fdt_open_into(blob, blob, fdt_totalsize(blob) +
++		      sizeof(struct fdt_reserve_entry));
++	ret = fdt_add_mem_rsv(blob, (uint64_t)firmware_start,
++			      (uint64_t)(firmware_end - firmware_start));
++
++	if(ret < 0) {
++		print_string("reserve mem add err\n\r");
++	}
++}
+diff --git a/include/boot.h b/include/boot.h
+index d75e013..c3e2ec1 100644
+--- a/include/boot.h
++++ b/include/boot.h
+@@ -16,4 +16,5 @@ void __noreturn spin(unsigned long *mbox, unsigned long invalid, int is_entry);
+ void __noreturn first_spin(unsigned int cpu, unsigned long *mbox,
+ 			   unsigned long invalid_addr);
+ 
++void dt_add_memreserve(void);
+ #endif
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0015-boot-Add-the-enable-keep-el-compile-option.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0015-boot-Add-the-enable-keep-el-compile-option.patch
new file mode 100644
index 0000000..0411ef0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0015-boot-Add-the-enable-keep-el-compile-option.patch
@@ -0,0 +1,102 @@
+From 8271c21bcff260295203214b7b8c87cdb8236453 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 4 Jan 2022 17:01:55 +0800
+Subject: [PATCH] boot: Add the --enable-keep-el compile option
+
+Add --enable-keep-el compile option to enable boot-wrapper booting next
+stage at EL2.
+The Armv8R AArch64 boots at EL2. If the next stage requires EL2 booting,
+the boot-wrapper should not drop to EL1.
+Currently, this option only works for Armv8R AArch64. Also, to work with
+Linux PSCI, this option will cause secondary cores booting at EL1.
+
+Issue-Id: SCM-3813
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I3ba9c87cf0b59d163ca433f74c9e3a46e5ca2c63
+---
+ Makefile.am         | 4 ++++
+ arch/aarch64/boot.S | 6 +++++-
+ common/psci.c       | 6 ++++++
+ configure.ac        | 5 +++++
+ 4 files changed, 20 insertions(+), 1 deletion(-)
+
+diff --git a/Makefile.am b/Makefile.am
+index 734de92..054becd 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -33,6 +33,10 @@ PSCI_CPU_ON	:= 0xc4000003
+ endif
+ PSCI_CPU_OFF	:= 0x84000002
+ 
++if KEEP_EL
++DEFINES		+= -DKEEP_EL
++endif
++
+ COMMON_SRC	:= common/
+ COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o device_tree.o
+ 
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 6dbd5cc..157c097 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -233,7 +233,11 @@ el2_init:
+ 	msr	cnthctl_el2, x0
+ 	isb
+ 
++#ifdef KEEP_EL
++	mov	w0, #SPSR_KERNEL
++#else
+ 	mov	w0, #SPSR_KERNEL_EL1
++#endif
+ 	ldr	x1, =spsr_to_elx
+ 	str	w0, [x1]
+ 	// fall through
+@@ -313,5 +317,5 @@ ASM_FUNC(jump_kernel)
+ 	.align 3
+ flag_keep_el:
+ 	.long 0
+-spsr_to_elx:
++ASM_DATA(spsr_to_elx)
+ 	.long 0
+diff --git a/common/psci.c b/common/psci.c
+index a0e8700..945780b 100644
+--- a/common/psci.c
++++ b/common/psci.c
+@@ -18,6 +18,8 @@
+ #error "No MPIDRs provided"
+ #endif
+ 
++extern unsigned int spsr_to_elx;
++
+ static unsigned long branch_table[NR_CPUS];
+ 
+ bakery_ticket_t branch_table_lock[NR_CPUS];
+@@ -44,6 +46,10 @@ static int psci_cpu_on(unsigned long target_mpidr, unsigned long address)
+ 	ret = psci_store_address(cpu, address);
+ 	bakery_unlock(branch_table_lock, this_cpu);
+ 
++#ifdef KEEP_EL
++	spsr_to_elx = SPSR_KERNEL_EL1;
++#endif
++
+ 	return ret;
+ }
+ 
+diff --git a/configure.ac b/configure.ac
+index 53e51be..0e07db3 100644
+--- a/configure.ac
++++ b/configure.ac
+@@ -25,6 +25,11 @@ AS_IF([test "x$BOOTWRAPPER_ES" = x32 -a "x$KERNEL_ES" != x32],
+ 	[AC_MSG_ERROR([a 32-bit boot-wrapper cannot launch a 64-bit kernel])]
+ )
+ 
++AC_ARG_ENABLE([keep-el],
++	AC_HELP_STRING([--enable-keep-el], [keep exception level when start kernel]),
++	[KEEP_EL=yes], [KEEP_EL=no])
++AM_CONDITIONAL([KEEP_EL], [test "x$KEEP_EL" = xyes])
++
+ # Allow a user to pass --with-kernel-dir
+ AC_ARG_WITH([kernel-dir],
+ 	AS_HELP_STRING([--with-kernel-dir], [specify the root Linux kernel build directory (required)]),
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0016-Makefile-Change-COUNTER_FREQ-to-100-MHz.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0016-Makefile-Change-COUNTER_FREQ-to-100-MHz.patch
new file mode 100644
index 0000000..a6b16e4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0016-Makefile-Change-COUNTER_FREQ-to-100-MHz.patch
@@ -0,0 +1,34 @@
+From dd3e3f414d0e6ed1643c2e2ccac676b7fc1dc7a9 Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Tue, 1 Feb 2022 11:28:46 +0000
+Subject: [PATCH] Makefile: Change COUNTER_FREQ to 100 MHz
+
+Older Arm Fast Models (AEM < RevC) had a base frequency of 24 MHz. but
+the RevC base models use 100 MHz. There is not a robust method of
+determining the configured base frequency at runtime, so update
+COUNTER_FREQ to be 100 MHz.
+
+Issue-Id: SCM-3871
+Upstream-Status: Pending
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: Ia9ad0f8ee488d1a887791f1fa1d8f3bf9c5887fd
+---
+ Makefile.am | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/Makefile.am b/Makefile.am
+index 40bc5d6..b48173c 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -13,7 +13,7 @@ SCRIPT_DIR	:= $(top_srcdir)/scripts
+ PHYS_OFFSET	:= $(shell perl -I $(SCRIPT_DIR) $(SCRIPT_DIR)/findmem.pl $(KERNEL_DTB))
+ UART_BASE	:= $(shell perl -I $(SCRIPT_DIR) $(SCRIPT_DIR)/findbase.pl $(KERNEL_DTB) 0 'arm,pl011')
+ SYSREGS_BASE	:= $(shell perl -I $(SCRIPT_DIR) $(SCRIPT_DIR)/findbase.pl $(KERNEL_DTB) 0 'arm,vexpress-sysreg' 2> /dev/null)
+-COUNTER_FREQ	:= 24000000
++COUNTER_FREQ	:= 100000000
+ 
+ CPU_IDS		:= $(shell perl -I $(SCRIPT_DIR) $(SCRIPT_DIR)/findcpuids.pl $(KERNEL_DTB))
+ NR_CPUS         := $(shell echo $(CPU_IDS) | tr ',' ' ' | wc -w)
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0017-PSCI-Apply-flush-cache-after-setting-branch_data.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0017-PSCI-Apply-flush-cache-after-setting-branch_data.patch
new file mode 100644
index 0000000..8d981f5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0017-PSCI-Apply-flush-cache-after-setting-branch_data.patch
@@ -0,0 +1,52 @@
+From 6923f2a0c59cf92ba5ad50ec1d658a357b4ba5d7 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 2 Nov 2021 10:48:39 +0800
+Subject: [PATCH] PSCI: Apply flush cache after setting branch_data
+
+For v8-R64, Hypervisor calls boot-wrapper's PSCI service using simple
+function call (instead of hvc).
+
+In this case, hypervisor's main core has enabled MPU and cache, but
+the secondary cores which are spinning have not enabled cache.
+That means if the main core set the branch_data to 1 to boot other
+cores, the secondary cores cannot see the change of branch_data and
+also cannot break the spin.
+
+Thus, the PSCI service in boot-wrapper needs a cache flush after
+setting branch_data in order to let other cores see the change.
+
+Issue-ID: SCM-3816
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: Ifc282091c54d8fb2ffdb8cfa7fd3ffc1f4be717e
+---
+ common/psci.c | 6 ++++++
+ 1 file changed, 6 insertions(+)
+
+diff --git a/common/psci.c b/common/psci.c
+index 945780b..6efc695 100644
+--- a/common/psci.c
++++ b/common/psci.c
+@@ -24,12 +24,18 @@ static unsigned long branch_table[NR_CPUS];
+ 
+ bakery_ticket_t branch_table_lock[NR_CPUS];
+ 
++static inline void flush_per_cpu_data(void *data)
++{
++	asm volatile ("dc cvac, %0" : : "r" (data));
++}
++
+ static int psci_store_address(unsigned int cpu, unsigned long address)
+ {
+ 	if (branch_table[cpu] != PSCI_ADDR_INVALID)
+ 		return PSCI_RET_ALREADY_ON;
+ 
+ 	branch_table[cpu] = address;
++	flush_per_cpu_data((void*)&(branch_table[cpu]));
+ 	return PSCI_RET_SUCCESS;
+ }
+ 
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0018-PSCI-Add-function-call-entry-point.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0018-PSCI-Add-function-call-entry-point.patch
new file mode 100644
index 0000000..97cd3cb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0018-PSCI-Add-function-call-entry-point.patch
@@ -0,0 +1,74 @@
+From ed46e83df2400b1b3f3364169aacf787bd91bd45 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 25 Jan 2022 14:56:36 +0800
+Subject: [PATCH] PSCI: Add function call entry point
+
+The max exception level of Armv8R AArch64 is EL2, which means it has no
+exclusive EL for firmware. That is, firmware and hypervisors have to share
+the EL2. Also, hypervisors cannot call firmware services via a 'smc'
+instruction. Thus, boot-wrapper has to provide a function entry point
+for Armv8R AArch64.
+
+Issue-Id: SCM-3816
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I06ec8e50298603155c6d8ae2330e71db2f111182
+---
+ common/psci.c | 24 ++++++++++++++++++++----
+ 1 file changed, 20 insertions(+), 4 deletions(-)
+
+diff --git a/common/psci.c b/common/psci.c
+index 6efc695..8fdefb5 100644
+--- a/common/psci.c
++++ b/common/psci.c
+@@ -20,6 +20,8 @@
+ 
+ extern unsigned int spsr_to_elx;
+ 
++unsigned long flag_from_smc_fn[NR_CPUS];
++
+ static unsigned long branch_table[NR_CPUS];
+ 
+ bakery_ticket_t branch_table_lock[NR_CPUS];
+@@ -49,12 +51,14 @@ static int psci_cpu_on(unsigned long target_mpidr, unsigned long address)
+ 		return PSCI_RET_INVALID_PARAMETERS;
+ 
+ 	bakery_lock(branch_table_lock, this_cpu);
+-	ret = psci_store_address(cpu, address);
+-	bakery_unlock(branch_table_lock, this_cpu);
+-
+ #ifdef KEEP_EL
+-	spsr_to_elx = SPSR_KERNEL_EL1;
++	if (!flag_from_smc_fn[this_cpu]) {
++		spsr_to_elx = SPSR_KERNEL_EL1;
++		flush_per_cpu_data((void*)&(spsr_to_elx));
++	}
+ #endif
++	ret = psci_store_address(cpu, address);
++	bakery_unlock(branch_table_lock, this_cpu);
+ 
+ 	return ret;
+ }
+@@ -90,6 +94,18 @@ long psci_call(unsigned long fid, unsigned long arg1, unsigned long arg2)
+ 	}
+ }
+ 
++long smc_fn_entry(unsigned long fid, unsigned long arg1, unsigned long arg2)
++{
++	long ret;
++	unsigned int this_cpu = this_cpu_logical_id();
++
++	flag_from_smc_fn[this_cpu] = 1;
++	ret = psci_call(fid, arg1, arg2);
++	flag_from_smc_fn[this_cpu] = 0;
++
++	return ret;
++}
++
+ void __noreturn psci_first_spin(unsigned int cpu)
+ {
+ 	if (cpu == MPIDR_INVALID)
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0019-lds-Rearrange-and-mark-the-sections.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0019-lds-Rearrange-and-mark-the-sections.patch
new file mode 100644
index 0000000..1f10209
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0019-lds-Rearrange-and-mark-the-sections.patch
@@ -0,0 +1,61 @@
+From 36b5fa3f4db49ac7aef42ff1d58a895226c7e96c Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Tue, 2 Nov 2021 15:10:28 +0800
+Subject: [PATCH] lds: Rearrange and mark the sections
+
+To make it possible for the next stage to protect sections with MPU,
+boot-wrapper needs to provide the text and data section information.
+By rearranging the .data .rodata and .vector sections, all sections
+can be split into 2 big sections:
+ - RO and Executable
+ - RW and Non-Executable
+Add firmware_data to mark the boundry, thus:
+firmware_start to firmware_data - 1 indicates RO and Executable section,
+firmware_data to firmware_end - 1 indicates RW and Non-Executable
+section.
+
+Also, the firmware_data and firmware_end should align with 64 bytes,
+since Armv8R AArch64 MPU requires it.
+
+Issue-ID: SCM-3816
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I55342aa7492f2c7b5c16ab9a6472c8cb45cff8fd
+---
+ model.lds.S | 7 ++++++-
+ 1 file changed, 6 insertions(+), 1 deletion(-)
+
+diff --git a/model.lds.S b/model.lds.S
+index ab98ddf..85451f9 100644
+--- a/model.lds.S
++++ b/model.lds.S
+@@ -63,12 +63,16 @@ SECTIONS
+ 	}
+ #endif
+ 
++#define FIRMWARE_ALIGN . = ALIGN(1 << 6)
+ 	.boot PHYS_OFFSET: {
+ 		PROVIDE(firmware_start = .);
+ 		*(.init)
+ 		*(.text*)
+-		*(.data* .rodata* .bss* COMMON)
+ 		*(.vectors)
++		*(.rodata*)
++		FIRMWARE_ALIGN;
++		PROVIDE(firmware_data = .);
++		*(.data* .bss* COMMON)
+ 		*(.stack)
+ 		PROVIDE(etext = .);
+ 	}
+@@ -77,6 +81,7 @@ SECTIONS
+ 		mbox = .;
+ 		QUAD(0x0)
+ 	}
++	FIRMWARE_ALIGN;
+ 	PROVIDE(firmware_end = .);
+ 
+ 	ASSERT(etext <= (PHYS_OFFSET + TEXT_LIMIT), ".text overflow!")
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0020-common-Provide-firmware-info-using-libfdt.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0020-common-Provide-firmware-info-using-libfdt.patch
new file mode 100644
index 0000000..cafcc09
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0020-common-Provide-firmware-info-using-libfdt.patch
@@ -0,0 +1,345 @@
+From 8bdbb64d13f14d40546b71dbcfee2b2a8ea002a5 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Wed, 29 Dec 2021 15:17:38 +0800
+Subject: [PATCH] common: Provide firmware info using libfdt
+
+Boot-wrapper uses libfdt to provide more info in device tree.
+We add a new node to include those new firmware relevant infomation.
+The new node defined as follows:
+  fw-shared-info {
+        compatible = "firmware,shared_info";
+
+        #address-cells = <0x02>;
+        #size-cells = <0x02>;
+
+        version = "1.0";
+        regions = <START_ADDR_HIGH START_ADDR_LOW SIZE_HIGH SIZE_LOW
+                   0x0 0x80000000 0x0 0x400000
+                   0x0 0x90000000 0x0 0x400000
+                   0x0 0xA0000000 0x0 0x400000>;
+        regions-permission = "RX", "R", "RWX", "RW";
+        regions-cache = "Cache", "NCache", "Cache", "Device"
+
+        function_entry = <ENTRY_ADDR_HIGH ENRTY_ADDR_LOW>;
+  };
+The node path is /fw-shared-info.
+For boot-wrapper, in real case, it will be:
+  fw-shared-info {
+        compatible = "firmware,shared_info";
+
+        #address-cells = <0x02>;
+        #size-cells = <0x02>;
+
+        version = "1.0";
+        regions = <0x0 firmware_start 0x0 firmware_code_size
+                   0x0 firmware_data 0x0 firmware_data_size>;
+        regions-permission = "RX", "RW";
+        regions-cache = "Cache", "Cache";
+
+        function_entry = <0x0 smc_fn_entry>;
+  };
+
+Issue-Id: SCM-3816
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: I6ebc59ce2bd3939b0fe066720d57821eaa1bed27
+---
+ common/device_tree.c | 271 ++++++++++++++++++++++++++++++++++++++++++-
+ 1 file changed, 270 insertions(+), 1 deletion(-)
+
+diff --git a/common/device_tree.c b/common/device_tree.c
+index 4d0876c..7f7befc 100644
+--- a/common/device_tree.c
++++ b/common/device_tree.c
+@@ -8,13 +8,225 @@
+  */
+ #include <libfdt.h>
+ 
++#define DEVICE_TREE_DEBUG 1
++
++#define FW_NODE_NAME	"/fw-shared-info"
++#define FW_COMPAT	"firmware,shared_info"
++#define FW_INFO_VER	"1.0"
++
++#ifdef BOOTWRAPPER_32
++#define CELL_NUM	1
++#define VAL_TYPE	uint32_t
++#else
++#define CELL_NUM	2
++#define VAL_TYPE	uint64_t
++#endif
++
++#define ALIGN(x)	(((x) + (FDT_TAGSIZE) - 1) & ~((FDT_TAGSIZE) - 1))
++
+ extern unsigned long dtb;
+-extern char firmware_start[], firmware_end[];
++extern char firmware_start[], firmware_data[], firmware_end[];
++
++extern long smc_fn_entry(unsigned long, unsigned long, unsigned long);
+ 
+ extern void print_string(const char *str);
++extern void print_hex(unsigned int val);
+ 
+ static void *blob;
+ 
++static char *realloc_node(char *fdt, const char *name)
++{
++	int delta;
++	int new_sz;
++	/* FDT_BEGIN_NODE, node name in off_struct and FDT_END_NODE */
++	delta = sizeof(struct fdt_node_header) + ALIGN(strlen(name) + 1)
++			+ FDT_TAGSIZE;
++	new_sz = fdt_totalsize(fdt) + delta;
++	fdt_open_into(fdt, fdt, new_sz);
++	return fdt;
++}
++
++static int create_node(const char *node_name)
++{
++	int node = 0;
++	char *p;
++
++	p = strrchr(node_name, '/');
++	if (!p) {
++		print_string("node name without '/'\r\n");
++		return -1;
++	}
++	*p = '\0';
++
++	blob = realloc_node(blob, p + 1);
++
++	if (p > node_name) {
++		node = fdt_path_offset(blob, node_name);
++		if (node < 0) {
++			print_string("no node name\r\n");
++			return -1;
++		}
++	}
++
++	node = fdt_add_subnode(blob, node, p + 1);
++	if (node < 0) {
++		print_string("add subnode err\r\n");
++		return -1;
++	}
++
++	return node;
++}
++
++static int dt_create_fw_node(void) {
++	int fw_node;
++
++	fw_node = fdt_path_offset(blob, FW_NODE_NAME);
++
++	if(fw_node < 0) {
++		fw_node = create_node(FW_NODE_NAME);
++	}
++
++	return fw_node;
++}
++
++static char *realloc_property(char *fdt, int nodeoffset, const char *name,
++			      int newlen)
++{
++	int delta = 0;
++	int oldlen = 0;
++	int new_sz;
++
++	if (!fdt_get_property(fdt, nodeoffset, name, &oldlen))
++		delta = sizeof(struct fdt_property) + strlen(name) + 1;
++
++	if (newlen > oldlen)
++		delta += ALIGN(newlen) - ALIGN(oldlen);
++
++	new_sz = fdt_totalsize(fdt) + delta;
++	fdt_open_into(fdt, fdt, new_sz);
++	return fdt;
++}
++
++static void dt_set_prop(int node, char *property, void *buf, int len)
++{
++	int err;
++
++	err = fdt_setprop(blob, node, property, buf, len);
++	if (err == -FDT_ERR_NOSPACE) {
++		blob = realloc_property(blob, node, property, len);
++		err = fdt_setprop(blob, node, property, buf, len);
++	}
++	if (err) {
++		print_string("fdt error\n\r");
++	}
++}
++
++static void dt_set_prop_u32(int node, char *property, uint32_t val)
++{
++	fdt32_t fdt_val = cpu_to_fdt32(val);
++	int len = sizeof(fdt32_t);
++
++	dt_set_prop(node, property, (void*)&fdt_val, len);
++}
++
++static void dt_set_prop_u64(int node, char *property, uint64_t val)
++{
++	fdt64_t fdt_val = cpu_to_fdt64(val);
++	int len = sizeof(fdt64_t);
++
++	dt_set_prop(node, property, (void*)&fdt_val, len);
++}
++
++/* This dt_set_prop_u32_array maybe unused according to the BOOTWRAPPER_32 */
++__attribute__((unused))
++static void dt_set_prop_u32_array(int node, char *property, uint32_t *vals,
++				       int size)
++{
++	fdt32_t *fdt_vals = (fdt32_t*)vals;
++	int len = sizeof(fdt32_t) * size;
++
++	for (int i = 0; i < size; i++) {
++		fdt_vals[i] = cpu_to_fdt32(vals[i]);
++	}
++
++	dt_set_prop(node, property, (void*)fdt_vals, len);
++}
++
++static void dt_set_prop_u64_array(int node, char *property, uint64_t *vals,
++				       int size)
++{
++	fdt64_t *fdt_vals = (fdt64_t*)vals;
++	int len = sizeof(fdt64_t) * size;
++
++	for (int i = 0; i < size; i++) {
++		fdt_vals[i] = cpu_to_fdt64(vals[i]);
++	}
++
++	dt_set_prop(node, property, (void*)fdt_vals, len);
++}
++
++#if DEVICE_TREE_DEBUG
++static void dt_dump_string(const void *s, int len)
++{
++	char *sub = (char*)s;
++	int sublen;
++	while(*sub && ((uint64_t)sub - (uint64_t)s) < len) {
++		sublen = strlen(sub) + 1;
++		print_string(sub);
++		print_string(" ");
++		sub += sublen;
++	}
++	print_string("\n\r");
++}
++
++static void dt_dump_fdt32_array(const void *vals, int len)
++{
++	fdt32_t *fdt_vals = (fdt32_t*)vals;
++	len = len / sizeof(fdt32_t);
++	for (int i = 0; i < len; i++) {
++		print_hex(fdt32_to_cpu(fdt_vals[i]));
++		print_string(" ");
++	}
++	print_string("\n\r");
++}
++
++static void dt_dump(int node, char *property, char type)
++{
++	const void *val;
++	int len;
++
++	val = fdt_getprop(blob, node, property, &len);
++	print_string(property);
++	print_string(": ");
++
++	if (type == 's') {
++		/* string type */
++		dt_dump_string(val, len);
++		return;
++	}
++
++	/* uint type */
++	dt_dump_fdt32_array(val, len);
++}
++
++void dt_dump_all(int node)
++{
++	if (node >= 0) {
++		print_string(FW_NODE_NAME" info:\r\n");
++		dt_dump(node, "compatible", 's');
++		dt_dump(node, "version", 's');
++		dt_dump(node, "function_entry", 'i');
++		dt_dump(node, "address-cells", 'i');
++		dt_dump(node, "size-cells", 'i');
++		dt_dump(node, "regions", 'i');
++		dt_dump(node, "regions-permission", 's');
++		dt_dump(node, "regions-cache", 's');
++		print_string("\r\n");
++	}
++}
++#else
++void dt_dump_all(int node) { (void*)node; return; }
++#endif
+ 
+ void dt_add_memreserve(void)
+ {
+@@ -32,3 +244,60 @@ void dt_add_memreserve(void)
+ 		print_string("reserve mem add err\n\r");
+ 	}
+ }
++
++void dt_fw_node_init(int enable)
++{
++	int fw_node;
++
++	VAL_TYPE regions[] = {
++		/* code region: start, end, ro, x, cachable */
++		(VAL_TYPE)firmware_start,
++		(VAL_TYPE)(firmware_data - firmware_start),
++		/* data region: start, end, rw, xn, cachable */
++		(VAL_TYPE)firmware_data,
++		(VAL_TYPE)(firmware_end - firmware_data),
++	};
++	int regions_num = sizeof(regions) / sizeof(VAL_TYPE);
++	char regions_permission[] = "RX\0RW";
++	char regions_cache[] = "Cache\0Cache";
++
++	if (!enable)
++		return;
++
++	print_string("Prepare "FW_NODE_NAME" node\n\r");
++
++	blob = (void*)&dtb;
++
++	if(fdt_path_offset(blob, "/psci") < 0) {
++		print_string("/psci node not found\n\r");
++		return;
++	}
++
++	fw_node = dt_create_fw_node();
++
++	if(fw_node < 0) {
++		print_string(FW_NODE_NAME" node create err\n\r");
++	}
++
++	dt_set_prop(fw_node, "compatible", FW_COMPAT, sizeof(FW_COMPAT));
++	dt_set_prop(fw_node, "version", FW_INFO_VER, sizeof(FW_INFO_VER));
++
++	dt_set_prop_u32(fw_node, "address-cells", CELL_NUM);
++	dt_set_prop_u32(fw_node, "size-cells", CELL_NUM);
++	dt_set_prop(fw_node, "regions-permission", regions_permission,
++		    sizeof(regions_permission));
++	dt_set_prop(fw_node, "regions-cache", regions_cache,
++		    sizeof(regions_cache));
++
++#ifdef BOOTWRAPPER_32
++	dt_set_prop_u32_array(fw_node, "regions", regions, regions_num);
++	dt_set_prop_u32(fw_node, "function_entry", (VAL_TYPE)smc_fn_entry);
++#else
++	dt_set_prop_u64_array(fw_node, "regions", regions, regions_num);
++	dt_set_prop_u64(fw_node, "function_entry", (VAL_TYPE)smc_fn_entry);
++#endif
++
++	fdt_pack(blob);
++
++	dt_dump_all(fw_node);
++}
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0021-boot-Enable-firmware-node-initialization.patch b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0021-boot-Enable-firmware-node-initialization.patch
new file mode 100644
index 0000000..943afde
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/boot-wrapper-aarch64/files/fvp-baser-aemv8r64/0021-boot-Enable-firmware-node-initialization.patch
@@ -0,0 +1,98 @@
+From 6dfc937d1ae54d2ae9f8c60ca29ba73ca14dc8c4 Mon Sep 17 00:00:00 2001
+From: Jaxson Han <jaxson.han@arm.com>
+Date: Wed, 29 Dec 2021 15:33:17 +0800
+Subject: [PATCH] boot: Enable firmware node initialization
+
+Enable the firmware node initialization, so that the next stage
+(hypervisor) could share the EL2 with firmware (boot-wrapper). The next
+stage (hypervisor) get the smccc entry point, code/data sections, the
+sections attrs and firmware node version and so on.
+It is worth noting that this EL2 sharing mechanism is only for Armv8R
+AArch64, thus add flag_v8r to record if the arch is Armv8R AArch64.
+Enable the firmware node initialization only if it is Armv8R AArch64.
+Also, we increase the stack size to 1024 to fix the stack overflow issue
+when using the libfdt.
+
+Add -fno-builtin options to CFLAGS to avoid the issue that the 'memset'
+in common/lib.c conflicts with builtin 'memset' function. GCC version
+>= 10 will have an incorrect compilation without -fno-builtin;
+
+Issue-Id: SCM-3816
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Jaxson Han <jaxson.han@arm.com>
+Change-Id: Ib274485a34d26215595fd0cd737be86610289817
+---
+ Makefile.am         | 4 ++--
+ arch/aarch64/boot.S | 6 ++++++
+ common/boot.c       | 4 ++++
+ 3 files changed, 12 insertions(+), 2 deletions(-)
+
+diff --git a/Makefile.am b/Makefile.am
+index 054becd..b01809c 100644
+--- a/Makefile.am
++++ b/Makefile.am
+@@ -23,7 +23,7 @@ DEFINES		+= -DCPU_IDS=$(CPU_IDS)
+ DEFINES		+= -DNR_CPUS=$(NR_CPUS)
+ DEFINES		+= $(if $(SYSREGS_BASE), -DSYSREGS_BASE=$(SYSREGS_BASE), )
+ DEFINES		+= -DUART_BASE=$(UART_BASE)
+-DEFINES		+= -DSTACK_SIZE=256
++DEFINES		+= -DSTACK_SIZE=1024
+ 
+ if KERNEL_32
+ DEFINES		+= -DKERNEL_32
+@@ -132,7 +132,7 @@ CHOSEN_NODE	:= chosen {						\
+ CPPFLAGS	+= $(INITRD_FLAGS)
+ CFLAGS		+= -I$(top_srcdir)/include/ -I$(top_srcdir)/$(ARCH_SRC)/include/
+ CFLAGS		+= -Wall -fomit-frame-pointer
+-CFLAGS 		+= -fno-stack-protector
++CFLAGS 		+= -fno-stack-protector -fno-builtin
+ CFLAGS		+= -ffunction-sections -fdata-sections
+ CFLAGS		+= -fno-pic -fno-pie
+ LDFLAGS		+= --gc-sections
+diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
+index 157c097..f310387 100644
+--- a/arch/aarch64/boot.S
++++ b/arch/aarch64/boot.S
+@@ -240,6 +240,10 @@ el2_init:
+ #endif
+ 	ldr	x1, =spsr_to_elx
+ 	str	w0, [x1]
++
++	mov	w0, #1
++	ldr	x1, =flag_v8r
++	str	w0, [x1]
+ 	// fall through
+ 
+ el_max_init:
+@@ -319,3 +323,5 @@ flag_keep_el:
+ 	.long 0
+ ASM_DATA(spsr_to_elx)
+ 	.long 0
++ASM_DATA(flag_v8r)
++	.long 0
+diff --git a/common/boot.c b/common/boot.c
+index ee2bea0..38b2dca 100644
+--- a/common/boot.c
++++ b/common/boot.c
+@@ -11,6 +11,9 @@
+ 
+ extern unsigned long entrypoint;
+ extern unsigned long dtb;
++extern unsigned int  flag_v8r;
++
++extern void dt_fw_node_init(int enable);
+ 
+ void init_platform(void);
+ 
+@@ -64,6 +67,7 @@ void __noreturn first_spin(unsigned int cpu, unsigned long *mbox,
+ 	if (cpu == 0) {
+ 		init_platform();
+ 		dt_add_memreserve();
++		dt_fw_node_init(flag_v8r == 1);
+ 
+ 		*mbox = (unsigned long)&entrypoint;
+ 		sevl();
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0001-feat-emulate-cntp-timer-register-accesses-using-cnth.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0001-feat-emulate-cntp-timer-register-accesses-using-cnth.patch
new file mode 100644
index 0000000..7094c8b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0001-feat-emulate-cntp-timer-register-accesses-using-cnth.patch
@@ -0,0 +1,252 @@
+From c8bd941579fb062359b683b184b851eea2ddb761 Mon Sep 17 00:00:00 2001
+From: Ben Horgan <ben.horgan@arm.com>
+Date: Fri, 4 Mar 2022 16:48:14 +0000
+Subject: [PATCH 1/5] feat: emulate cntp timer register accesses using cnthps
+
+Upstream-Status: Inappropriate [Experimental feature]
+Signed-off-by: Ben Horgan <ben.horgan@arm.com>
+Change-Id: I67508203273baf3bd8e6be2d99717028db945715
+---
+ Makefile                                |   3 +-
+ src/arch/aarch64/hypervisor/BUILD.gn    |   1 +
+ src/arch/aarch64/hypervisor/cpu.c       |  11 ++-
+ src/arch/aarch64/hypervisor/handler.c   |   6 ++
+ src/arch/aarch64/hypervisor/timer_el1.c | 104 ++++++++++++++++++++++++
+ src/arch/aarch64/hypervisor/timer_el1.h |  20 +++++
+ src/arch/aarch64/msr.h                  |   8 ++
+ 7 files changed, 150 insertions(+), 3 deletions(-)
+ create mode 100644 src/arch/aarch64/hypervisor/timer_el1.c
+ create mode 100644 src/arch/aarch64/hypervisor/timer_el1.h
+
+diff --git a/Makefile b/Makefile
+index c9fb16f..6371a8a 100644
+--- a/Makefile
++++ b/Makefile
+@@ -59,7 +59,8 @@ CHECKPATCH := $(CURDIR)/third_party/linux/scripts/checkpatch.pl \
+ # debug_el1.c : uses XMACROS, which checkpatch doesn't understand.
+ # perfmon.c : uses XMACROS, which checkpatch doesn't understand.
+ # feature_id.c : uses XMACROS, which checkpatch doesn't understand.
+-CHECKPATCH_IGNORE := "src/arch/aarch64/hypervisor/debug_el1.c\|src/arch/aarch64/hypervisor/perfmon.c\|src/arch/aarch64/hypervisor/feature_id.c"
++# timer_el1.c : uses XMACROS, which checkpatch doesn't understand.
++CHECKPATCH_IGNORE := "src/arch/aarch64/hypervisor/debug_el1.c\|src/arch/aarch64/hypervisor/perfmon.c\|src/arch/aarch64/hypervisor/feature_id.c\|src/arch/aarch64/hypervisor/timer_el1.c"
+ 
+ OUT ?= out/$(PROJECT)
+ OUT_DIR = out/$(PROJECT)
+diff --git a/src/arch/aarch64/hypervisor/BUILD.gn b/src/arch/aarch64/hypervisor/BUILD.gn
+index 6068d1e..de1a414 100644
+--- a/src/arch/aarch64/hypervisor/BUILD.gn
++++ b/src/arch/aarch64/hypervisor/BUILD.gn
+@@ -45,6 +45,7 @@ source_set("hypervisor") {
+     "handler.c",
+     "perfmon.c",
+     "psci_handler.c",
++    "timer_el1.c",
+     "vm.c",
+   ]
+ 
+diff --git a/src/arch/aarch64/hypervisor/cpu.c b/src/arch/aarch64/hypervisor/cpu.c
+index c6cebdd..cb41e6e 100644
+--- a/src/arch/aarch64/hypervisor/cpu.c
++++ b/src/arch/aarch64/hypervisor/cpu.c
+@@ -91,13 +91,20 @@ void arch_regs_reset(struct vcpu *vcpu)
+ 	if (is_primary) {
+ 		/*
+ 		 * cnthctl_el2 is redefined when VHE is enabled.
+-		 * EL1PCTEN, don't trap phys cnt access.
+-		 * EL1PCEN, don't trap phys timer access.
++		 * EL1PCTEN, don't trap phys cnt access. Except when in
++		 * secure world without vhe.
++		 * EL1PCEN, don't trap phys timer access. Except when in
++		 * secure world without vhe.
+ 		 */
+ 		if (has_vhe_support()) {
+ 			cnthctl |= (1U << 10) | (1U << 11);
+ 		} else {
++#if SECURE_WORLD == 1
++			cnthctl &= ~(1U << 0);
++			cnthctl &= ~(1U << 1);
++#else
+ 			cnthctl |= (1U << 0) | (1U << 1);
++#endif
+ 		}
+ 	}
+ 
+diff --git a/src/arch/aarch64/hypervisor/handler.c b/src/arch/aarch64/hypervisor/handler.c
+index cd64d68..c9068c5 100644
+--- a/src/arch/aarch64/hypervisor/handler.c
++++ b/src/arch/aarch64/hypervisor/handler.c
+@@ -34,6 +34,7 @@
+ #include "psci_handler.h"
+ #include "smc.h"
+ #include "sysregs.h"
++#include "timer_el1.h"
+ 
+ /**
+  * Hypervisor Fault Address Register Non-Secure.
+@@ -1276,6 +1277,11 @@ void handle_system_register_access(uintreg_t esr_el2)
+ 			inject_el1_unknown_exception(vcpu, esr_el2);
+ 			return;
+ 		}
++	} else if (timer_el1_is_register_access(esr_el2)) {
++		if (!timer_el1_process_access(vcpu, vm_id, esr_el2)) {
++			inject_el1_unknown_exception(vcpu, esr_el2);
++			return;
++		}
+ 	} else {
+ 		inject_el1_unknown_exception(vcpu, esr_el2);
+ 		return;
+diff --git a/src/arch/aarch64/hypervisor/timer_el1.c b/src/arch/aarch64/hypervisor/timer_el1.c
+new file mode 100644
+index 0000000..c30e554
+--- /dev/null
++++ b/src/arch/aarch64/hypervisor/timer_el1.c
+@@ -0,0 +1,104 @@
++/*
++ * Copyright 2022 The Hafnium Authors.
++ *
++ * Use of this source code is governed by a BSD-style
++ * license that can be found in the LICENSE file or at
++ * https://opensource.org/licenses/BSD-3-Clause.
++ */
++
++#include "timer_el1.h"
++
++#include "hf/dlog.h"
++
++#include "msr.h"
++#include "sysregs.h"
++
++/*
++ * Physical timer (CNTP) register encodings as defined in
++ * table D13-8 of the ARMv8 ARM (DDI0487F).
++ * TYPE, op0, op1, crn, crm, op2
++ * The register names are the concatenation of
++ * "CNTP_", TYPE and "_EL2".
++ */
++#define CNTP_REGISTERS          \
++	X(CTL,  3, 3, 14, 2, 1) \
++	X(CVAL, 3, 3, 14, 2, 2) \
++	X(TVAL, 3, 3, 14, 2, 0) \
++
++bool timer_el1_is_register_access(uintreg_t esr)
++{
++	uintreg_t sys_register = GET_ISS_SYSREG(esr);
++	bool is_timer_access;
++	switch (sys_register) {
++#define X(type, op0, op1, crn, crm, op2)                  \
++	case (GET_ISS_ENCODING(op0, op1, crn, crm, op2)): \
++		is_timer_access = true;                   \
++		break;
++			CNTP_REGISTERS
++#undef X
++	case (GET_ISS_ENCODING(3, 3, 14, 0, 1)):
++		is_timer_access = true;
++		break;
++	default:
++		is_timer_access = false;
++	}
++
++	return is_timer_access;
++}
++
++/* Accesses to CNTP timer emulated with CNTHPS */
++bool timer_el1_process_access(struct vcpu *vcpu, ffa_vm_id_t vm_id,
++			      uintreg_t esr)
++{
++	uintreg_t sys_register = GET_ISS_SYSREG(esr);
++	uintreg_t rt_register = GET_ISS_RT(esr);
++	uintreg_t value;
++
++	if (ISS_IS_READ(esr)) {
++		switch (sys_register) {
++#define X(type, op0, op1, crn, crm, op2)                           \
++		case (GET_ISS_ENCODING(op0, op1, crn, crm, op2)):  \
++			value = read_msr(MSR_CNTHPS_##type##_EL2); \
++			vcpu->regs.r[rt_register] = value;         \
++			break;
++				CNTP_REGISTERS
++#undef X
++		case (GET_ISS_ENCODING(3, 3, 14, 0, 1)):
++			value = read_msr(cntpct_el0);
++			vcpu->regs.r[rt_register] = value;
++			break;
++		default:
++			dlog_notice(
++				"Unsupported timer register "
++				"read: "
++				"op0=%d, op1=%d, crn=%d, crm=%d, op2=%d, "
++				"rt=%d.\n",
++				GET_ISS_OP0(esr), GET_ISS_OP1(esr),
++				GET_ISS_CRN(esr), GET_ISS_CRM(esr),
++				GET_ISS_OP2(esr), GET_ISS_RT(esr));
++			break;
++		}
++	} else {
++		value = vcpu->regs.r[rt_register];
++		switch (sys_register) {
++#define X(type, op0, op1, crn, crm, op2)                           \
++		case (GET_ISS_ENCODING(op0, op1, crn, crm, op2)):  \
++			write_msr(MSR_CNTHPS_##type##_EL2, value); \
++			break;
++				CNTP_REGISTERS
++#undef X
++		default:
++			dlog_notice(
++				"Unsupported timer register "
++				"write: "
++				"op0=%d, op1=%d, crn=%d, crm=%d, op2=%d, "
++				"rt=%d, value=%d.\n",
++				GET_ISS_OP0(esr), GET_ISS_OP1(esr),
++				GET_ISS_CRN(esr), GET_ISS_CRM(esr),
++				GET_ISS_OP2(esr), GET_ISS_RT(esr), value);
++			break;
++		}
++	}
++
++	return true;
++}
+diff --git a/src/arch/aarch64/hypervisor/timer_el1.h b/src/arch/aarch64/hypervisor/timer_el1.h
+new file mode 100644
+index 0000000..04a43b6
+--- /dev/null
++++ b/src/arch/aarch64/hypervisor/timer_el1.h
+@@ -0,0 +1,20 @@
++/*
++ * Copyright 2022 The Hafnium Authors.
++ *
++ * Use of this source code is governed by a BSD-style
++ * license that can be found in the LICENSE file or at
++ * https://opensource.org/licenses/BSD-3-Clause.
++ */
++
++#pragma once
++
++#include "hf/arch/types.h"
++
++#include "hf/cpu.h"
++
++#include "vmapi/hf/ffa.h"
++
++bool timer_el1_is_register_access(uintreg_t esr);
++
++bool timer_el1_process_access(struct vcpu *vcpu, ffa_vm_id_t vm_id,
++			      uintreg_t esr);
+diff --git a/src/arch/aarch64/msr.h b/src/arch/aarch64/msr.h
+index cd6778b..55e7833 100644
+--- a/src/arch/aarch64/msr.h
++++ b/src/arch/aarch64/msr.h
+@@ -126,3 +126,11 @@
+ #define MSR_ELR_EL12 S3_5_C4_C0_1
+ 
+ #endif
++
++/*
++ * Secure EL2 Physical timer (CNTHPS) register encodings as defined in
++ * table D13-8 of the ARMv8 ARM (DDI0487F).
++ */
++#define MSR_CNTHPS_CTL_EL2 S3_4_C14_C5_1
++#define MSR_CNTHPS_CVAL_EL2 S3_4_C14_C5_2
++#define MSR_CNTHPS_TVAL_EL2 S3_4_C14_C5_0
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0002-fix-ff-a-Use-FFA_INTERRUPT-to-signal-an-interrupted-.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0002-fix-ff-a-Use-FFA_INTERRUPT-to-signal-an-interrupted-.patch
new file mode 100644
index 0000000..2b57b23
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0002-fix-ff-a-Use-FFA_INTERRUPT-to-signal-an-interrupted-.patch
@@ -0,0 +1,152 @@
+From e6bcc390749f0560b3bc92507ecbaaabc7145200 Mon Sep 17 00:00:00 2001
+From: Lucian Paul-Trifu <lucian.paul-trifu@arm.com>
+Date: Wed, 10 Mar 2021 11:31:02 +0000
+Subject: [PATCH 2/5] fix(ff-a): Use FFA_INTERRUPT to signal an interrupted
+ FFA_MSG_WAIT
+
+Rather than FFA_ERROR(INTERRUPTED).
+
+Change-Id: I6b23a442714852b6183e0e46af6f0504ec0ee8f4
+Signed-off-by: Ben Horgan <ben.horgan@arm.com>
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ src/api.c                                                  | 2 +-
+ test/inc/test/vmapi/ffa.h                                  | 7 +++++++
+ test/vmapi/arch/aarch64/gicv3/services/common.c            | 3 +--
+ test/vmapi/arch/aarch64/gicv3/services/timer.c             | 2 +-
+ test/vmapi/el0_partitions/services/interruptible.c         | 3 +--
+ test/vmapi/el0_partitions/services/interruptible_echo.c    | 3 +--
+ test/vmapi/el0_partitions/services/receive_block.c         | 2 +-
+ .../primary_with_secondaries/services/interruptible.c      | 3 +--
+ .../primary_with_secondaries/services/receive_block.c      | 2 +-
+ 9 files changed, 15 insertions(+), 12 deletions(-)
+
+diff --git a/src/api.c b/src/api.c
+index b713b7c..00c4d44 100644
+--- a/src/api.c
++++ b/src/api.c
+@@ -1588,7 +1588,7 @@ struct ffa_value api_ffa_msg_recv(bool block, struct vcpu *current,
+ 	 * received. If a message is received the return value will be set at
+ 	 * that time to FFA_SUCCESS.
+ 	 */
+-	return_code = ffa_error(FFA_INTERRUPTED);
++	return_code = (struct ffa_value){.func = FFA_INTERRUPT_32};
+ 	if (api_ffa_msg_recv_block_interrupted(current)) {
+ 		goto out;
+ 	}
+diff --git a/test/inc/test/vmapi/ffa.h b/test/inc/test/vmapi/ffa.h
+index 8fc1223..f0f3e75 100644
+--- a/test/inc/test/vmapi/ffa.h
++++ b/test/inc/test/vmapi/ffa.h
+@@ -24,6 +24,13 @@
+ 		EXPECT_EQ(ffa_error_code(v), (ffa_error)); \
+ 	} while (0)
+ 
++#define EXPECT_FFA_INTERRUPT(value)                        \
++	do {                                               \
++		struct ffa_value v = (value);              \
++		EXPECT_EQ(v.func, FFA_INTERRUPT_32);       \
++	} while (0)
++
++
+ /*
+  * The bit 15 of the FF-A ID indicates whether the partition is executing
+  * in the normal world, in case it is a Virtual Machine (VM); or in the
+diff --git a/test/vmapi/arch/aarch64/gicv3/services/common.c b/test/vmapi/arch/aarch64/gicv3/services/common.c
+index 06df28d..4ada9e2 100644
+--- a/test/vmapi/arch/aarch64/gicv3/services/common.c
++++ b/test/vmapi/arch/aarch64/gicv3/services/common.c
+@@ -22,8 +22,7 @@ struct ffa_value mailbox_receive_retry(void)
+ 
+ 	do {
+ 		received = ffa_msg_wait();
+-	} while (received.func == FFA_ERROR_32 &&
+-		 received.arg2 == FFA_INTERRUPTED);
++	} while (received.func == FFA_INTERRUPT_32);
+ 
+ 	return received;
+ }
+diff --git a/test/vmapi/arch/aarch64/gicv3/services/timer.c b/test/vmapi/arch/aarch64/gicv3/services/timer.c
+index 156f160..d5d2816 100644
+--- a/test/vmapi/arch/aarch64/gicv3/services/timer.c
++++ b/test/vmapi/arch/aarch64/gicv3/services/timer.c
+@@ -104,7 +104,7 @@ TEST_SERVICE(timer)
+ 		} else if (receive) {
+ 			struct ffa_value res = ffa_msg_wait();
+ 
+-			EXPECT_FFA_ERROR(res, FFA_INTERRUPTED);
++			EXPECT_FFA_INTERRUPT(res);
+ 		} else {
+ 			/* Busy wait until the timer fires. */
+ 			while (!timer_fired) {
+diff --git a/test/vmapi/el0_partitions/services/interruptible.c b/test/vmapi/el0_partitions/services/interruptible.c
+index 0d00b16..4c9f099 100644
+--- a/test/vmapi/el0_partitions/services/interruptible.c
++++ b/test/vmapi/el0_partitions/services/interruptible.c
+@@ -50,8 +50,7 @@ static struct ffa_value mailbox_receive_retry()
+ 	do {
+ 		irq();
+ 		received = ffa_msg_wait();
+-	} while (received.func == FFA_ERROR_32 &&
+-		 ffa_error_code(received) == FFA_INTERRUPTED);
++	} while (received.func == FFA_INTERRUPT_32);
+ 
+ 	return received;
+ }
+diff --git a/test/vmapi/el0_partitions/services/interruptible_echo.c b/test/vmapi/el0_partitions/services/interruptible_echo.c
+index b618cf2..a857783 100644
+--- a/test/vmapi/el0_partitions/services/interruptible_echo.c
++++ b/test/vmapi/el0_partitions/services/interruptible_echo.c
+@@ -39,8 +39,7 @@ static struct ffa_value mailbox_receive_retry()
+ 	do {
+ 		irq();
+ 		received = ffa_msg_wait();
+-	} while (received.func == FFA_ERROR_32 &&
+-		 received.arg2 == FFA_INTERRUPTED);
++	} while (received.func == FFA_INTERRUPT_32);
+ 
+ 	return received;
+ }
+diff --git a/test/vmapi/el0_partitions/services/receive_block.c b/test/vmapi/el0_partitions/services/receive_block.c
+index 05a22f3..60da28b 100644
+--- a/test/vmapi/el0_partitions/services/receive_block.c
++++ b/test/vmapi/el0_partitions/services/receive_block.c
+@@ -27,7 +27,7 @@ TEST_SERVICE(receive_block)
+ 
+ 	for (i = 0; i < 10; ++i) {
+ 		struct ffa_value res = ffa_msg_wait();
+-		EXPECT_FFA_ERROR(res, FFA_INTERRUPTED);
++		EXPECT_FFA_INTERRUPT(res);
+ 	}
+ 
+ 	memcpy_s(SERVICE_SEND_BUFFER(), FFA_MSG_PAYLOAD_MAX, message,
+diff --git a/test/vmapi/primary_with_secondaries/services/interruptible.c b/test/vmapi/primary_with_secondaries/services/interruptible.c
+index cc1c1f9..005d1ff 100644
+--- a/test/vmapi/primary_with_secondaries/services/interruptible.c
++++ b/test/vmapi/primary_with_secondaries/services/interruptible.c
+@@ -46,8 +46,7 @@ struct ffa_value mailbox_receive_retry()
+ 
+ 	do {
+ 		received = ffa_msg_wait();
+-	} while (received.func == FFA_ERROR_32 &&
+-		 ffa_error_code(received) == FFA_INTERRUPTED);
++	} while (received.func == FFA_INTERRUPT_32);
+ 
+ 	return received;
+ }
+diff --git a/test/vmapi/primary_with_secondaries/services/receive_block.c b/test/vmapi/primary_with_secondaries/services/receive_block.c
+index edb4e3c..a6805ae 100644
+--- a/test/vmapi/primary_with_secondaries/services/receive_block.c
++++ b/test/vmapi/primary_with_secondaries/services/receive_block.c
+@@ -40,7 +40,7 @@ TEST_SERVICE(receive_block)
+ 
+ 	for (i = 0; i < 10; ++i) {
+ 		struct ffa_value res = ffa_msg_wait();
+-		EXPECT_FFA_ERROR(res, FFA_INTERRUPTED);
++		EXPECT_FFA_INTERRUPT(res);
+ 	}
+ 
+ 	memcpy_s(SERVICE_SEND_BUFFER(), FFA_MSG_PAYLOAD_MAX, message,
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0003-fix-ff-a-Add-FFA_SECONDARY_EP_REGISTER_64-to-list-of.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0003-fix-ff-a-Add-FFA_SECONDARY_EP_REGISTER_64-to-list-of.patch
new file mode 100644
index 0000000..8d2cc13
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0003-fix-ff-a-Add-FFA_SECONDARY_EP_REGISTER_64-to-list-of.patch
@@ -0,0 +1,28 @@
+From a6f466c2594b2f56d34fee72494fbd29ea9c7d21 Mon Sep 17 00:00:00 2001
+From: Ben Horgan <ben.horgan@arm.com>
+Date: Tue, 26 Apr 2022 12:59:42 +0000
+Subject: [PATCH 3/5] fix(ff-a): Add FFA_SECONDARY_EP_REGISTER_64 to list of
+ features
+
+Signed-off-by: Ben Horgan <ben.horgan@arm.com>
+Change-Id: Ic1344eb2c982c195210dc2c86aa6845f3e037077
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ src/api.c | 1 +
+ 1 file changed, 1 insertion(+)
+
+diff --git a/src/api.c b/src/api.c
+index 00c4d44..33a9b42 100644
+--- a/src/api.c
++++ b/src/api.c
+@@ -2021,6 +2021,7 @@ struct ffa_value api_ffa_features(uint32_t feature_function_id)
+ 	case FFA_MEM_PERM_SET_32:
+ 	case FFA_MEM_PERM_GET_64:
+ 	case FFA_MEM_PERM_SET_64:
++	case FFA_SECONDARY_EP_REGISTER_64:
+ #endif
+ 		return (struct ffa_value){.func = FFA_SUCCESS_32};
+ 
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0004-feat-emulate-interrupt-controller-register-access.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0004-feat-emulate-interrupt-controller-register-access.patch
new file mode 100644
index 0000000..95f1651
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0004-feat-emulate-interrupt-controller-register-access.patch
@@ -0,0 +1,159 @@
+From 380f2cf944dd5db36c168a11d31a46ad14cdcb6d Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Tue, 26 Apr 2022 14:43:58 +0100
+Subject: [PATCH 4/5] feat: emulate interrupt controller register access
+
+This emulates ICC_SGI1R_EL1 and ICC_IGRPEN1_EL1 register
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I0c11f034f3676067597461a183a341c809adcaa4
+Upstream-Status: Inappropriate [Experimental feature]
+---
+ src/arch/aarch64/hypervisor/handler.c |  5 ++
+ src/arch/aarch64/hypervisor/perfmon.c | 84 +++++++++++++++++++++++++++
+ src/arch/aarch64/hypervisor/perfmon.h |  5 ++
+ src/arch/aarch64/msr.h                |  3 +
+ 4 files changed, 97 insertions(+)
+
+diff --git a/src/arch/aarch64/hypervisor/handler.c b/src/arch/aarch64/hypervisor/handler.c
+index c9068c5..b9aa5d8 100644
+--- a/src/arch/aarch64/hypervisor/handler.c
++++ b/src/arch/aarch64/hypervisor/handler.c
+@@ -1282,6 +1282,11 @@ void handle_system_register_access(uintreg_t esr_el2)
+ 			inject_el1_unknown_exception(vcpu, esr_el2);
+ 			return;
+ 		}
++	} else if (intr_ctrl_is_register_access(esr_el2)) {
++		if (!intr_ctrl_el1_process_access(vcpu, vm_id, esr_el2)) {
++			inject_el1_unknown_exception(vcpu, esr_el2);
++			return;
++		}
+ 	} else {
+ 		inject_el1_unknown_exception(vcpu, esr_el2);
+ 		return;
+diff --git a/src/arch/aarch64/hypervisor/perfmon.c b/src/arch/aarch64/hypervisor/perfmon.c
+index f13b035..05e216c 100644
+--- a/src/arch/aarch64/hypervisor/perfmon.c
++++ b/src/arch/aarch64/hypervisor/perfmon.c
+@@ -116,6 +116,10 @@
+ 	X(PMEVTYPER30_EL0   , 3, 3, 14, 15, 6) \
+ 	X(PMCCFILTR_EL0     , 3, 3, 14, 15, 7)
+ 
++#define INTR_CTRL_REGISTERS                    \
++	X(ICC_IGRPEN1_EL1   , 3, 0, 12, 12, 7) \
++	X(ICC_SGI1R_EL1     , 3, 0, 12, 11, 5) \
++
+ /* clang-format on */
+ 
+ /**
+@@ -232,3 +236,83 @@ uintreg_t perfmon_get_pmccfiltr_el0_init_value(ffa_vm_id_t vm_id)
+ 
+ 	return 0;
+ }
++
++bool intr_ctrl_is_register_access(uintreg_t esr)
++{
++	uintreg_t op0 = GET_ISS_OP0(esr);
++	uintreg_t op1 = GET_ISS_OP1(esr);
++	uintreg_t crn = GET_ISS_CRN(esr);
++	uintreg_t crm = GET_ISS_CRM(esr);
++
++	if (op0 == 3 && op1 == 0 && crn == 12 && crm == 12) {
++		return true;
++	}
++
++	if (op0 == 3 && op1 == 0 && crn == 12 && crm == 11) {
++		return true;
++	}
++
++	return false;
++}
++
++bool intr_ctrl_el1_process_access(struct vcpu *vcpu, ffa_vm_id_t vm_id,
++				  uintreg_t esr)
++{
++	uintreg_t sys_register = GET_ISS_SYSREG(esr);
++	uintreg_t rt_register = GET_ISS_RT(esr);
++	uintreg_t value;
++
++	/* +1 because Rt can access register XZR */
++	CHECK(rt_register < NUM_GP_REGS + 1);
++
++	if (ISS_IS_READ(esr)) {
++		switch (sys_register) {
++#define X(reg_name, op0, op1, crn, crm, op2)              \
++	case (GET_ISS_ENCODING(op0, op1, crn, crm, op2)): \
++		value = read_msr(reg_name);               \
++		break;
++			INTR_CTRL_REGISTERS
++#undef X
++		default:
++			value = vcpu->regs.r[rt_register];
++			dlog_notice(
++				"Unsupported interrupt control register "
++				"read: "
++				"op0=%d, op1=%d, crn=%d, crm=%d, op2=%d, "
++				"rt=%d.\n",
++				GET_ISS_OP0(esr), GET_ISS_OP1(esr),
++				GET_ISS_CRN(esr), GET_ISS_CRM(esr),
++				GET_ISS_OP2(esr), GET_ISS_RT(esr));
++			break;
++		}
++		if (rt_register != RT_REG_XZR) {
++			vcpu->regs.r[rt_register] = value;
++		}
++	} else {
++		if (rt_register != RT_REG_XZR) {
++			value = vcpu->regs.r[rt_register];
++		} else {
++			value = 0;
++		}
++		switch (sys_register) {
++#define X(reg_name, op0, op1, crn, crm, op2)              \
++	case (GET_ISS_ENCODING(op0, op1, crn, crm, op2)): \
++		write_msr(reg_name, value);               \
++		break;
++			INTR_CTRL_REGISTERS
++#undef X
++		default:
++			dlog_notice(
++				"Unsupported interrupt control register "
++				"write: "
++				"op0=%d, op1=%d, crn=%d, crm=%d, op2=%d, "
++				"rt=%d.\n",
++				GET_ISS_OP0(esr), GET_ISS_OP1(esr),
++				GET_ISS_CRN(esr), GET_ISS_CRM(esr),
++				GET_ISS_OP2(esr), GET_ISS_RT(esr));
++			break;
++		}
++	}
++
++	return true;
++}
+diff --git a/src/arch/aarch64/hypervisor/perfmon.h b/src/arch/aarch64/hypervisor/perfmon.h
+index 81669ba..c90d45b 100644
+--- a/src/arch/aarch64/hypervisor/perfmon.h
++++ b/src/arch/aarch64/hypervisor/perfmon.h
+@@ -70,3 +70,8 @@ bool perfmon_process_access(struct vcpu *vcpu, ffa_vm_id_t vm_id,
+ 			    uintreg_t esr_el2);
+ 
+ uintreg_t perfmon_get_pmccfiltr_el0_init_value(ffa_vm_id_t vm_id);
++
++bool intr_ctrl_is_register_access(uintreg_t esr);
++
++bool intr_ctrl_el1_process_access(struct vcpu *vcpu, ffa_vm_id_t vm_id,
++				  uintreg_t esr);
+diff --git a/src/arch/aarch64/msr.h b/src/arch/aarch64/msr.h
+index 55e7833..82aa884 100644
+--- a/src/arch/aarch64/msr.h
++++ b/src/arch/aarch64/msr.h
+@@ -134,3 +134,6 @@
+ #define MSR_CNTHPS_CTL_EL2 S3_4_C14_C5_1
+ #define MSR_CNTHPS_CVAL_EL2 S3_4_C14_C5_2
+ #define MSR_CNTHPS_TVAL_EL2 S3_4_C14_C5_0
++
++#define ICC_IGRPEN1_EL1 S3_0_C12_C12_7
++#define ICC_SGI1R_EL1 S3_0_C12_C11_5
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0005-Revert-fix-ff-a-check-receiver-s-attributes-on-memor.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0005-Revert-fix-ff-a-check-receiver-s-attributes-on-memor.patch
new file mode 100644
index 0000000..e5f9489
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0005-Revert-fix-ff-a-check-receiver-s-attributes-on-memor.patch
@@ -0,0 +1,318 @@
+From 64d5628c8439e4649e9c1da9b9e02ebd5c7fb8cf Mon Sep 17 00:00:00 2001
+From: Ben Horgan <ben.horgan@arm.com>
+Date: Thu, 28 Apr 2022 15:53:31 +0000
+Subject: [PATCH 5/5] Revert "fix(ff-a): check receiver's attributes on memory
+ retrieve"
+
+This reverts commit a98603aa965e3ff3ca5383249213e2fd1a96d850.
+
+Change-Id: Ia71ce3ac52e9b2e85578372c24eb8d593b62435f
+Signed-off-by: Ben Horgan <ben.horgan@arm.com>
+---
+ src/ffa_memory.c                              | 76 ++++++-----------
+ test/vmapi/el0_partitions/memory_sharing.c    | 81 -------------------
+ .../primary_with_secondaries/memory_sharing.c | 81 -------------------
+ 3 files changed, 25 insertions(+), 213 deletions(-)
+
+diff --git a/src/ffa_memory.c b/src/ffa_memory.c
+index ab47929..2fcc386 100644
+--- a/src/ffa_memory.c
++++ b/src/ffa_memory.c
+@@ -1344,42 +1344,6 @@ static struct ffa_value ffa_memory_send_complete(
+ 	return ffa_mem_success(share_state->memory_region->handle);
+ }
+ 
+-/**
+- * Check that the memory attributes match Hafnium expectations:
+- * Normal Memory, Inner shareable, Write-Back Read-Allocate
+- * Write-Allocate Cacheable.
+- */
+-static struct ffa_value ffa_memory_attributes_validate(
+-	ffa_memory_access_permissions_t attributes)
+-{
+-	enum ffa_memory_type memory_type;
+-	enum ffa_memory_cacheability cacheability;
+-	enum ffa_memory_shareability shareability;
+-
+-	memory_type = ffa_get_memory_type_attr(attributes);
+-	if (memory_type != FFA_MEMORY_NORMAL_MEM) {
+-		dlog_verbose("Invalid memory type %#x, expected %#x.\n",
+-			     memory_type, FFA_MEMORY_NORMAL_MEM);
+-		return ffa_error(FFA_DENIED);
+-	}
+-
+-	cacheability = ffa_get_memory_cacheability_attr(attributes);
+-	if (cacheability != FFA_MEMORY_CACHE_WRITE_BACK) {
+-		dlog_verbose("Invalid cacheability %#x, expected %#x.\n",
+-			     cacheability, FFA_MEMORY_CACHE_WRITE_BACK);
+-		return ffa_error(FFA_DENIED);
+-	}
+-
+-	shareability = ffa_get_memory_shareability_attr(attributes);
+-	if (shareability != FFA_MEMORY_INNER_SHAREABLE) {
+-		dlog_verbose("Invalid shareability %#x, expected #%x.\n",
+-			     shareability, FFA_MEMORY_INNER_SHAREABLE);
+-		return ffa_error(FFA_DENIED);
+-	}
+-
+-	return (struct ffa_value){.func = FFA_SUCCESS_32};
+-}
+-
+ /**
+  * Check that the given `memory_region` represents a valid memory send request
+  * of the given `share_func` type, return the clear flag and permissions via the
+@@ -1400,7 +1364,10 @@ static struct ffa_value ffa_memory_send_validate(
+ 	uint32_t constituents_length;
+ 	enum ffa_data_access data_access;
+ 	enum ffa_instruction_access instruction_access;
+-	struct ffa_value ret;
++	ffa_memory_access_permissions_t attributes;
++	enum ffa_memory_type memory_type;
++	enum ffa_memory_cacheability memory_cacheability;
++	enum ffa_memory_shareability memory_shareability;
+ 
+ 	assert(permissions != NULL);
+ 
+@@ -1536,9 +1503,26 @@ static struct ffa_value ffa_memory_send_validate(
+ 	 * Normal Memory, Inner shareable, Write-Back Read-Allocate
+ 	 * Write-Allocate Cacheable.
+ 	 */
+-	ret = ffa_memory_attributes_validate(memory_region->attributes);
+-	if (ret.func != FFA_SUCCESS_32) {
+-		return ret;
++	attributes = memory_region->attributes;
++	memory_type = ffa_get_memory_type_attr(attributes);
++	if (memory_type != FFA_MEMORY_NORMAL_MEM) {
++		dlog_verbose("Invalid memory type %#x, expected %#x.\n",
++			     memory_type, FFA_MEMORY_NORMAL_MEM);
++		return ffa_error(FFA_INVALID_PARAMETERS);
++	}
++
++	memory_cacheability = ffa_get_memory_cacheability_attr(attributes);
++	if (memory_cacheability != FFA_MEMORY_CACHE_WRITE_BACK) {
++		dlog_verbose("Invalid cacheability %#x, expected %#x.\n",
++			     memory_cacheability, FFA_MEMORY_CACHE_WRITE_BACK);
++		return ffa_error(FFA_INVALID_PARAMETERS);
++	}
++
++	memory_shareability = ffa_get_memory_shareability_attr(attributes);
++	if (memory_shareability != FFA_MEMORY_INNER_SHAREABLE) {
++		dlog_verbose("Invalid shareability %#x, expected %#x.\n",
++			     memory_shareability, FFA_MEMORY_INNER_SHAREABLE);
++		return ffa_error(FFA_INVALID_PARAMETERS);
+ 	}
+ 
+ 	return (struct ffa_value){.func = FFA_SUCCESS_32};
+@@ -2376,6 +2360,7 @@ struct ffa_value ffa_memory_retrieve(struct vm_locked to_locked,
+ 	 * Check permissions from sender against permissions requested by
+ 	 * receiver.
+ 	 */
++	/* TODO: Check attributes too. */
+ 	sent_permissions =
+ 		memory_region->receivers[0].receiver_permissions.permissions;
+ 	sent_data_access = ffa_get_data_access_attr(sent_permissions);
+@@ -2453,17 +2438,6 @@ struct ffa_value ffa_memory_retrieve(struct vm_locked to_locked,
+ 		panic("Got unexpected FFA_INSTRUCTION_ACCESS_RESERVED. Should "
+ 		      "be checked before this point.");
+ 	}
+-
+-	/*
+-	 * Ensure receiver's attributes are compatible with how Hafnium maps
+-	 * memory: Normal Memory, Inner shareable, Write-Back Read-Allocate
+-	 * Write-Allocate Cacheable.
+-	 */
+-	ret = ffa_memory_attributes_validate(retrieve_request->attributes);
+-	if (ret.func != FFA_SUCCESS_32) {
+-		goto out;
+-	}
+-
+ 	memory_to_attributes = ffa_memory_permissions_to_mode(
+ 		permissions, share_state->sender_orig_mode);
+ 	ret = ffa_retrieve_check_update(
+diff --git a/test/vmapi/el0_partitions/memory_sharing.c b/test/vmapi/el0_partitions/memory_sharing.c
+index 3756d7d..c29f029 100644
+--- a/test/vmapi/el0_partitions/memory_sharing.c
++++ b/test/vmapi/el0_partitions/memory_sharing.c
+@@ -2160,87 +2160,6 @@ TEST(memory_sharing, ffa_validate_retrieve_req_mbz)
+ 	}
+ }
+ 
+-/**
+- * Memory can't be shared with arbitrary attributes because Hafnium maps pages
+- * with hardcoded values and doesn't support custom mappings.
+- */
+-TEST(memory_sharing, ffa_validate_retrieve_req_attributes)
+-{
+-	struct ffa_value ret;
+-	struct mailbox_buffers mb = set_up_mailbox();
+-	uint32_t msg_size;
+-	ffa_memory_handle_t handle;
+-
+-	struct ffa_value (*send_function[])(uint32_t, uint32_t) = {
+-		ffa_mem_share,
+-		ffa_mem_lend,
+-	};
+-
+-	struct ffa_memory_region_constituent constituents[] = {
+-		{.address = (uint64_t)pages, .page_count = 2},
+-		{.address = (uint64_t)pages + PAGE_SIZE * 3, .page_count = 1},
+-	};
+-
+-	SERVICE_SELECT(SERVICE_VM1, "ffa_memory_share_fail", mb.send);
+-
+-	struct {
+-		enum ffa_memory_type memory_type;
+-		enum ffa_memory_cacheability memory_cacheability;
+-		enum ffa_memory_shareability memory_shareability;
+-	} invalid_attributes[] = {
+-		/* Invalid memory type */
+-		{FFA_MEMORY_DEVICE_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_INNER_SHAREABLE},
+-		/* Invalid cacheability */
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_NON_CACHEABLE,
+-		 FFA_MEMORY_INNER_SHAREABLE},
+-		/* Invalid shareability */
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_SHARE_NON_SHAREABLE},
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_OUTER_SHAREABLE}};
+-
+-	for (uint32_t i = 0; i < ARRAY_SIZE(send_function); i++) {
+-		/* Prepare memory region, and set all flags */
+-		EXPECT_EQ(ffa_memory_region_init(
+-				  mb.send, HF_MAILBOX_SIZE, HF_PRIMARY_VM_ID,
+-				  SERVICE_VM1, constituents,
+-				  ARRAY_SIZE(constituents), 0, 0,
+-				  FFA_DATA_ACCESS_RW,
+-				  FFA_INSTRUCTION_ACCESS_NOT_SPECIFIED,
+-				  FFA_MEMORY_NORMAL_MEM,
+-				  FFA_MEMORY_CACHE_WRITE_BACK,
+-				  FFA_MEMORY_INNER_SHAREABLE, NULL, &msg_size),
+-			  0);
+-
+-		ret = send_function[i](msg_size, msg_size);
+-		EXPECT_EQ(ret.func, FFA_SUCCESS_32);
+-
+-		handle = ffa_mem_success_handle(ret);
+-
+-		for (uint32_t j = 0; j < ARRAY_SIZE(invalid_attributes); ++j) {
+-			msg_size = ffa_memory_retrieve_request_init(
+-				mb.send, handle, HF_PRIMARY_VM_ID, SERVICE_VM1,
+-				0, 0, FFA_DATA_ACCESS_RW,
+-				FFA_INSTRUCTION_ACCESS_NOT_SPECIFIED,
+-				invalid_attributes[j].memory_type,
+-				invalid_attributes[j].memory_cacheability,
+-				invalid_attributes[j].memory_shareability);
+-
+-			EXPECT_LE(msg_size, HF_MAILBOX_SIZE);
+-
+-			EXPECT_EQ(ffa_msg_send(HF_PRIMARY_VM_ID, SERVICE_VM1,
+-					       msg_size, 0)
+-					  .func,
+-				  FFA_SUCCESS_32);
+-
+-			ffa_run(SERVICE_VM1, 0);
+-		}
+-
+-		EXPECT_EQ(ffa_mem_reclaim(handle, 0).func, FFA_SUCCESS_32);
+-	}
+-}
+-
+ /**
+  * If memory is shared can't request zeroing of memory at both send and
+  * relinquish.
+diff --git a/test/vmapi/primary_with_secondaries/memory_sharing.c b/test/vmapi/primary_with_secondaries/memory_sharing.c
+index 6080709..4bcf252 100644
+--- a/test/vmapi/primary_with_secondaries/memory_sharing.c
++++ b/test/vmapi/primary_with_secondaries/memory_sharing.c
+@@ -2307,87 +2307,6 @@ TEST(memory_sharing, ffa_validate_retrieve_req_mbz)
+ 	}
+ }
+ 
+-/**
+- * Memory can't be shared with arbitrary attributes because Hafnium maps pages
+- * with hardcoded values and doesn't support custom mappings.
+- */
+-TEST(memory_sharing, ffa_validate_retrieve_req_attributes)
+-{
+-	struct ffa_value ret;
+-	struct mailbox_buffers mb = set_up_mailbox();
+-	uint32_t msg_size;
+-	ffa_memory_handle_t handle;
+-
+-	struct ffa_value (*send_function[])(uint32_t, uint32_t) = {
+-		ffa_mem_share,
+-		ffa_mem_lend,
+-	};
+-
+-	struct ffa_memory_region_constituent constituents[] = {
+-		{.address = (uint64_t)pages, .page_count = 2},
+-		{.address = (uint64_t)pages + PAGE_SIZE * 3, .page_count = 1},
+-	};
+-
+-	SERVICE_SELECT(SERVICE_VM1, "ffa_memory_share_fail_denied", mb.send);
+-
+-	struct {
+-		enum ffa_memory_type memory_type;
+-		enum ffa_memory_cacheability memory_cacheability;
+-		enum ffa_memory_shareability memory_shareability;
+-	} invalid_attributes[] = {
+-		/* Invalid memory type */
+-		{FFA_MEMORY_DEVICE_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_INNER_SHAREABLE},
+-		/* Invalid cacheability */
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_NON_CACHEABLE,
+-		 FFA_MEMORY_INNER_SHAREABLE},
+-		/* Invalid shareability */
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_SHARE_NON_SHAREABLE},
+-		{FFA_MEMORY_NORMAL_MEM, FFA_MEMORY_CACHE_WRITE_BACK,
+-		 FFA_MEMORY_OUTER_SHAREABLE}};
+-
+-	for (uint32_t i = 0; i < ARRAY_SIZE(send_function); i++) {
+-		/* Prepare memory region, and set all flags */
+-		EXPECT_EQ(ffa_memory_region_init(
+-				  mb.send, HF_MAILBOX_SIZE, HF_PRIMARY_VM_ID,
+-				  SERVICE_VM1, constituents,
+-				  ARRAY_SIZE(constituents), 0, 0,
+-				  FFA_DATA_ACCESS_RW,
+-				  FFA_INSTRUCTION_ACCESS_NOT_SPECIFIED,
+-				  FFA_MEMORY_NORMAL_MEM,
+-				  FFA_MEMORY_CACHE_WRITE_BACK,
+-				  FFA_MEMORY_INNER_SHAREABLE, NULL, &msg_size),
+-			  0);
+-
+-		ret = send_function[i](msg_size, msg_size);
+-		EXPECT_EQ(ret.func, FFA_SUCCESS_32);
+-
+-		handle = ffa_mem_success_handle(ret);
+-
+-		for (uint32_t j = 0; j < ARRAY_SIZE(invalid_attributes); ++j) {
+-			msg_size = ffa_memory_retrieve_request_init(
+-				mb.send, handle, HF_PRIMARY_VM_ID, SERVICE_VM1,
+-				0, 0, FFA_DATA_ACCESS_RW,
+-				FFA_INSTRUCTION_ACCESS_NOT_SPECIFIED,
+-				invalid_attributes[j].memory_type,
+-				invalid_attributes[j].memory_cacheability,
+-				invalid_attributes[j].memory_shareability);
+-
+-			EXPECT_LE(msg_size, HF_MAILBOX_SIZE);
+-
+-			EXPECT_EQ(ffa_msg_send(HF_PRIMARY_VM_ID, SERVICE_VM1,
+-					       msg_size, 0)
+-					  .func,
+-				  FFA_SUCCESS_32);
+-
+-			ffa_run(SERVICE_VM1, 0);
+-		}
+-
+-		EXPECT_EQ(ffa_mem_reclaim(handle, 0).func, FFA_SUCCESS_32);
+-	}
+-}
+-
+ /**
+  * If memory is shared can't request zeroing of memory at both send and
+  * relinquish.
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0006-tc-increase-heap-pages.patch b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0006-tc-increase-heap-pages.patch
new file mode 100644
index 0000000..671f6a5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/files/tc/0006-tc-increase-heap-pages.patch
@@ -0,0 +1,26 @@
+From e918cc5179241e1d35ba4b465b035b74b88e55d2 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 29 Apr 2022 20:07:50 +0100
+Subject: [PATCH] tc: increase heap pages
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ project/reference/BUILD.gn | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/project/reference/BUILD.gn b/project/reference/BUILD.gn
+index 5d84d13..4ea0890 100644
+--- a/project/reference/BUILD.gn
++++ b/project/reference/BUILD.gn
+@@ -233,7 +233,7 @@ aarch64_toolchains("secure_tc") {
+   gicd_base_address = "0x30000000"
+   gicr_base_address = "0x30080000"
+   gicr_frames = 8
+-  heap_pages = 60
++  heap_pages = 120
+   max_cpus = 8
+   max_vms = 16
+   branch_protection = "standard"
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium-tc.inc b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium-tc.inc
new file mode 100644
index 0000000..c8f77dc
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium-tc.inc
@@ -0,0 +1,24 @@
+# TC specific configuration
+
+COMPATIBLE_MACHINE = "(tc?)"
+HAFNIUM_PLATFORM = "secure_tc"
+
+SRCREV = "4eb7b54348234d7f4bbac64bac28f683d6574ba9"
+FILESEXTRAPATHS:prepend:tc := "${THISDIR}/files/tc:"
+
+SRC_URI:remove = " \
+    file://host-ld.patch \
+    file://native-dtc.patch"
+
+SRC_URI:append = " \
+        file://0001-feat-emulate-cntp-timer-register-accesses-using-cnth.patch \
+        file://0002-fix-ff-a-Use-FFA_INTERRUPT-to-signal-an-interrupted-.patch \
+        file://0003-fix-ff-a-Add-FFA_SECONDARY_EP_REGISTER_64-to-list-of.patch \
+        file://0004-feat-emulate-interrupt-controller-register-access.patch \
+        file://0005-Revert-fix-ff-a-check-receiver-s-attributes-on-memor.patch \
+        file://0006-tc-increase-heap-pages.patch \
+        "
+
+do_compile() {
+    PATH="${S}/prebuilts/linux-x64/clang/bin:$PATH" oe_runmake -C ${S}
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium_%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium_%.bbappend
new file mode 100644
index 0000000..bfc2c46
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/hafnium/hafnium_%.bbappend
@@ -0,0 +1,6 @@
+# Machine specific configurations
+
+MACHINE_HAFNIUM_REQUIRE ?= ""
+MACHINE_HAFNIUM_REQUIRE:tc = "hafnium-tc.inc"
+
+require ${MACHINE_HAFNIUM_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-image.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-image.bb
new file mode 100644
index 0000000..ad5ec95
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-image.bb
@@ -0,0 +1,13 @@
+SUMARY = "Corstone1000 platform Image"
+DESCRIPTION = "This is the main image which is the container of all the binaries \
+               generated for the Corstone1000 platform."
+LICENSE = "MIT"
+
+COMPATIBLE_MACHINE = "corstone1000"
+
+inherit image
+inherit wic_nopt
+
+PACKAGE_INSTALL = ""
+
+IMAGE_FSTYPES += "wic wic.nopt"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-initramfs-image.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-initramfs-image.bb
new file mode 100644
index 0000000..b778a00
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/corstone1000-initramfs-image.bb
@@ -0,0 +1,30 @@
+SUMARY = "Corstone1000 platform Initramfs Image"
+DESCRIPTION = "This is the main Linux image which includes an initramfs kernel/rootfs bundle."
+
+LICENSE = "MIT"
+
+COMPATIBLE_MACHINE = "corstone1000"
+
+IMAGE_FSTYPES = "${INITRAMFS_FSTYPES}"
+
+inherit core-image
+
+# By default all basic packages required for a bootable system are installed
+# by core-image . These packages are: packagegroup-core-boot and
+# packagegroup-base-extended
+
+inherit image-buildinfo
+
+IMAGE_FEATURES += "debug-tweaks"
+
+#package management is not supported in corstone1000
+IMAGE_FEATURES:remove = "package-management"
+
+# all optee packages
+IMAGE_INSTALL += "optee-client"
+
+# FF-A Debugfs driver
+IMAGE_INSTALL += "ffa-debugfs-mod"
+
+# psa-arch-tests linux userspace application
+IMAGE_INSTALL += "secure-partitions-psa-api-tests"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno.bb
new file mode 100644
index 0000000..80565af
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno.bb
@@ -0,0 +1,79 @@
+DESCRIPTION = "Firmware Image for Juno to be copied to the Configuration \
+microSD card"
+
+LICENSE = "BSD-3-Clause"
+SECTION = "firmware"
+LIC_FILES_CHKSUM = "file://${COMMON_LICENSE_DIR}/BSD-3-Clause;md5=550794465ba0ec5312d6919e203a55f9"
+
+INHIBIT_DEFAULT_DEPS = "1"
+DEPENDS = "trusted-firmware-a virtual/kernel virtual/control-processor-firmware"
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+COMPATIBLE_MACHINE = "juno"
+
+LINARO_RELEASE = "19.06"
+
+SRC_URI = "http://releases.linaro.org/members/arm/platforms/${LINARO_RELEASE}/juno-latest-oe-uboot.zip;subdir=${UNPACK_DIR} \
+    file://images-r0.txt \
+    file://images-r1.txt \
+    file://images-r2.txt \
+    file://uEnv.txt \
+"
+SRC_URI[md5sum] = "01b662b81fa409d55ff298238ad24003"
+SRC_URI[sha256sum] = "b8a3909bb3bc4350a8771b863193a3e33b358e2a727624a77c9ecf13516cec82"
+
+UNPACK_DIR = "juno-firmware-${LINARO_RELEASE}"
+
+inherit deploy nopackages
+
+do_configure[noexec] = "1"
+do_compile[noexec] = "1"
+
+# The ${D} is used as a temporary directory and we don't generate any
+# packages for this recipe.
+do_install() {
+    cp -a ${WORKDIR}/${UNPACK_DIR} ${D}
+    cp -f ${RECIPE_SYSROOT}/firmware/bl1-juno.bin \
+        ${D}/${UNPACK_DIR}/SOFTWARE/bl1.bin
+
+    cp -f ${RECIPE_SYSROOT}/firmware/fip-juno.bin \
+        ${D}/${UNPACK_DIR}/SOFTWARE/fip.bin
+
+    cp -f ${RECIPE_SYSROOT}/firmware/scp_romfw_bypass.bin \
+        ${D}/${UNPACK_DIR}/SOFTWARE/scp_bl1.bin
+
+    # u-boot environment file
+    cp -f ${WORKDIR}/uEnv.txt ${D}/${UNPACK_DIR}/SOFTWARE/
+
+    # Juno images list file
+    cp -f ${WORKDIR}/images-r0.txt ${D}/${UNPACK_DIR}/SITE1/HBI0262B/images.txt
+    cp -f ${WORKDIR}/images-r1.txt ${D}/${UNPACK_DIR}/SITE1/HBI0262C/images.txt
+    cp -f ${WORKDIR}/images-r2.txt ${D}/${UNPACK_DIR}/SITE1/HBI0262D/images.txt
+}
+
+do_deploy() {
+    # To avoid dependency loop between firmware-image-juno:do_install
+    # and virtual/kernel:do_deploy when INITRAMFS_IMAGE_BUNDLE = "1",
+    # we need to handle the kernel binaries copying in the do_deploy
+    # task.
+    for f in ${KERNEL_DEVICETREE}; do
+        install -m 755 -c ${DEPLOY_DIR_IMAGE}/$(basename $f) \
+            ${D}/${UNPACK_DIR}/SOFTWARE/.
+    done
+
+    if [ "${INITRAMFS_IMAGE_BUNDLE}" -eq 1 ]; then
+        cp -L -f ${DEPLOY_DIR_IMAGE}/Image-initramfs-juno.bin \
+            ${D}/${UNPACK_DIR}/SOFTWARE/Image
+    else
+        cp -L -f ${DEPLOY_DIR_IMAGE}/Image ${D}/${UNPACK_DIR}/SOFTWARE/
+    fi
+
+    # Compress the files
+    tar -C ${D}/${UNPACK_DIR} -zcvf ${WORKDIR}/${PN}.tar.gz ./
+
+    # Deploy the compressed archive to the deploy folder
+    install -D -p -m0644 ${WORKDIR}/${PN}.tar.gz ${DEPLOYDIR}/${PN}.tar.gz
+}
+do_deploy[depends] += "virtual/kernel:do_deploy"
+addtask deploy after do_install
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r0.txt b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r0.txt
new file mode 100644
index 0000000..3b36ed1
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r0.txt
@@ -0,0 +1,71 @@
+TITLE: Versatile Express Images Configuration File
+
+[IMAGES]
+TOTALIMAGES: 10                  ;Number of Images (Max: 32)
+
+NOR0UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR0ADDRESS: 0x00000000          ;Image Flash Address
+NOR0FILE: \SOFTWARE\fip.bin      ;Image File Name
+NOR0LOAD: 00000000               ;Image Load Address
+NOR0ENTRY: 00000000              ;Image Entry Point
+
+NOR1UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR1ADDRESS: 0x03EC0000          ;Image Flash Address
+NOR1FILE: \SOFTWARE\bl1.bin      ;Image File Name
+NOR1LOAD: 00000000               ;Image Load Address
+NOR1ENTRY: 00000000              ;Image Entry Point
+
+NOR2UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR2ADDRESS: 0x00500000          ;Image Flash Address
+NOR2FILE: \SOFTWARE\Image        ;Image File Name
+NOR2NAME: norkern                ;Rename kernel to norkern
+NOR2LOAD: 00000000               ;Image Load Address
+NOR2ENTRY: 00000000              ;Image Entry Point
+
+NOR3UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR3ADDRESS: 0x03000000          ;Image Flash Address
+NOR3FILE: \SOFTWARE\juno.dtb     ;Image File Name
+NOR3NAME: board.dtb              ;Specify target filename to preserve file extension
+NOR3LOAD: 00000000               ;Image Load Address
+NOR3ENTRY: 00000000              ;Image Entry Point
+
+NOR4UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR4ADDRESS: 0x030C0000          ;Image Flash Address
+NOR4FILE: \SOFTWARE\hdlcdclk.dat ;Image File Name
+NOR4LOAD: 00000000               ;Image Load Address
+NOR4ENTRY: 00000000              ;Image Entry Point
+
+NOR5UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR5ADDRESS: 0x03E40000          ;Image Flash Address
+NOR5FILE: \SOFTWARE\scp_bl1.bin  ;Image File Name
+NOR5LOAD: 00000000               ;Image Load Address
+NOR5ENTRY: 00000000              ;Image Entry Point
+
+NOR6UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR6ADDRESS: 0x0BF00000          ;Image Flash Address
+NOR6FILE: \SOFTWARE\startup.nsh  ;Image File Name
+NOR6NAME: startup.nsh
+NOR6LOAD: 00000000               ;Image Load Address
+NOR6ENTRY: 00000000              ;Image Entry Point
+
+NOR7UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR7ADDRESS: 0x0BFC0000          ;Image Flash Address
+NOR7FILE: \SOFTWARE\blank.img    ;Image File Name
+NOR7NAME: BOOTENV
+NOR7LOAD: 00000000               ;Image Load Address
+NOR7ENTRY: 00000000              ;Image Entry Point
+
+NOR8UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR8ADDRESS: 0x03100000          ;Image Flash Address
+NOR8FILE: \SOFTWARE\selftest     ;Image File Name
+NOR8LOAD: 00000000               ;Image Load Address
+NOR8ENTRY: 00000000              ;Image Entry Point
+
+NOR9UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR9ADDRESS: 0x03180000          ;Image Flash Address
+NOR9NAME: uEnv.txt
+NOR9FILE: \SOFTWARE\uEnv.txt     ;Image File Name
+NOR9LOAD: 00000000               ;Image Load Address
+NOR9ENTRY: 00000000              ;Image Entry Point
+
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r1.txt b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r1.txt
new file mode 100644
index 0000000..5db13af
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r1.txt
@@ -0,0 +1,71 @@
+TITLE: Versatile Express Images Configuration File
+
+[IMAGES]
+TOTALIMAGES: 10                  ;Number of Images (Max: 32)
+
+NOR0UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR0ADDRESS: 0x00000000          ;Image Flash Address
+NOR0FILE: \SOFTWARE\fip.bin      ;Image File Name
+NOR0LOAD: 00000000               ;Image Load Address
+NOR0ENTRY: 00000000              ;Image Entry Point
+
+NOR1UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR1ADDRESS: 0x03EC0000          ;Image Flash Address
+NOR1FILE: \SOFTWARE\bl1.bin      ;Image File Name
+NOR1LOAD: 00000000               ;Image Load Address
+NOR1ENTRY: 00000000              ;Image Entry Point
+
+NOR2UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR2ADDRESS: 0x00500000          ;Image Flash Address
+NOR2FILE: \SOFTWARE\Image        ;Image File Name
+NOR2NAME: norkern                ;Rename kernel to norkern
+NOR2LOAD: 00000000               ;Image Load Address
+NOR2ENTRY: 00000000              ;Image Entry Point
+
+NOR3UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR3ADDRESS: 0x03000000          ;Image Flash Address
+NOR3FILE: \SOFTWARE\juno-r1.dtb  ;Image File Name
+NOR3NAME: board.dtb              ;Specify target filename to preserve file extension
+NOR3LOAD: 00000000               ;Image Load Address
+NOR3ENTRY: 00000000              ;Image Entry Point
+
+NOR4UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR4ADDRESS: 0x030C0000          ;Image Flash Address
+NOR4FILE: \SOFTWARE\hdlcdclk.dat ;Image File Name
+NOR4LOAD: 00000000               ;Image Load Address
+NOR4ENTRY: 00000000              ;Image Entry Point
+
+NOR5UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR5ADDRESS: 0x03E40000          ;Image Flash Address
+NOR5FILE: \SOFTWARE\scp_bl1.bin  ;Image File Name
+NOR5LOAD: 00000000               ;Image Load Address
+NOR5ENTRY: 00000000              ;Image Entry Point
+
+NOR6UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR6ADDRESS: 0x0BF00000          ;Image Flash Address
+NOR6FILE: \SOFTWARE\startup.nsh  ;Image File Name
+NOR6NAME: startup.nsh
+NOR6LOAD: 00000000               ;Image Load Address
+NOR6ENTRY: 00000000              ;Image Entry Point
+
+NOR7UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR7ADDRESS: 0x0BFC0000          ;Image Flash Address
+NOR7FILE: \SOFTWARE\blank.img    ;Image File Name
+NOR7NAME: BOOTENV
+NOR7LOAD: 00000000               ;Image Load Address
+NOR7ENTRY: 00000000              ;Image Entry Point
+
+NOR8UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR8ADDRESS: 0x03100000          ;Image Flash Address
+NOR8FILE: \SOFTWARE\selftest     ;Image File Name
+NOR8LOAD: 00000000               ;Image Load Address
+NOR8ENTRY: 00000000              ;Image Entry Point
+
+NOR9UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR9ADDRESS: 0x03180000          ;Image Flash Address
+NOR9NAME: uEnv.txt
+NOR9FILE: \SOFTWARE\uEnv.txt     ;Image File Name
+NOR9LOAD: 00000000               ;Image Load Address
+NOR9ENTRY: 00000000              ;Image Entry Point
+
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r2.txt b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r2.txt
new file mode 100644
index 0000000..7c499bf
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/images-r2.txt
@@ -0,0 +1,71 @@
+TITLE: Versatile Express Images Configuration File
+
+[IMAGES]
+TOTALIMAGES: 10                  ;Number of Images (Max: 32)
+
+NOR0UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR0ADDRESS: 0x00000000          ;Image Flash Address
+NOR0FILE: \SOFTWARE\fip.bin      ;Image File Name
+NOR0LOAD: 00000000               ;Image Load Address
+NOR0ENTRY: 00000000              ;Image Entry Point
+
+NOR1UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR1ADDRESS: 0x03EC0000          ;Image Flash Address
+NOR1FILE: \SOFTWARE\bl1.bin      ;Image File Name
+NOR1LOAD: 00000000               ;Image Load Address
+NOR1ENTRY: 00000000              ;Image Entry Point
+
+NOR2UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR2ADDRESS: 0x00500000          ;Image Flash Address
+NOR2FILE: \SOFTWARE\Image        ;Image File Name
+NOR2NAME: norkern                ;Rename kernel to norkern
+NOR2LOAD: 00000000               ;Image Load Address
+NOR2ENTRY: 00000000              ;Image Entry Point
+
+NOR3UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR3ADDRESS: 0x03000000          ;Image Flash Address
+NOR3FILE: \SOFTWARE\juno-r2.dtb  ;Image File Name
+NOR3NAME: board.dtb              ;Specify target filename to preserve file extension
+NOR3LOAD: 00000000               ;Image Load Address
+NOR3ENTRY: 00000000              ;Image Entry Point
+
+NOR4UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR4ADDRESS: 0x030C0000          ;Image Flash Address
+NOR4FILE: \SOFTWARE\hdlcdclk.dat ;Image File Name
+NOR4LOAD: 00000000               ;Image Load Address
+NOR4ENTRY: 00000000              ;Image Entry Point
+
+NOR5UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR5ADDRESS: 0x03E40000          ;Image Flash Address
+NOR5FILE: \SOFTWARE\scp_bl1.bin  ;Image File Name
+NOR5LOAD: 00000000               ;Image Load Address
+NOR5ENTRY: 00000000              ;Image Entry Point
+
+NOR6UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR6ADDRESS: 0x0BF00000          ;Image Flash Address
+NOR6FILE: \SOFTWARE\startup.nsh  ;Image File Name
+NOR6NAME: startup.nsh
+NOR6LOAD: 00000000               ;Image Load Address
+NOR6ENTRY: 00000000              ;Image Entry Point
+
+NOR7UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR7ADDRESS: 0x0BFC0000          ;Image Flash Address
+NOR7FILE: \SOFTWARE\blank.img    ;Image File Name
+NOR7NAME: BOOTENV
+NOR7LOAD: 00000000               ;Image Load Address
+NOR7ENTRY: 00000000              ;Image Entry Point
+
+NOR8UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR8ADDRESS: 0x03100000          ;Image Flash Address
+NOR8FILE: \SOFTWARE\selftest     ;Image File Name
+NOR8LOAD: 00000000               ;Image Load Address
+NOR8ENTRY: 00000000              ;Image Entry Point
+
+NOR9UPDATE: AUTO                 ;Image Update:NONE/AUTO/FORCE
+NOR9ADDRESS: 0x03180000          ;Image Flash Address
+NOR9NAME: uEnv.txt
+NOR9FILE: \SOFTWARE\uEnv.txt     ;Image File Name
+NOR9LOAD: 00000000               ;Image Load Address
+NOR9ENTRY: 00000000              ;Image Entry Point
+
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/uEnv.txt b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/uEnv.txt
new file mode 100644
index 0000000..77c02e3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/firmware-image-juno/uEnv.txt
@@ -0,0 +1,11 @@
+uenvcmd=run mybootcmd
+mybootcmd=echo Loading custom boot command; \
+echo Loading kernel; \
+afs load ${kernel_name} ${kernel_addr_r} ; \
+if test $? -eq 1; then echo Loading ${kernel_alt_name} instead of ${kernel_name}; afs load ${kernel_alt_name} ${kernel_addr_r}; fi; \
+echo Loading device tree; \
+afs load  ${fdtfile} ${fdt_addr_r}; \
+if test $? -eq 1; then echo Loading ${fdt_alt_name} instead of ${fdtfile}; \
+afs load ${fdt_alt_name} ${fdt_addr_r}; fi; fdt addr ${fdt_addr_r}; fdt resize; \
+booti ${kernel_addr_r} - ${fdt_addr_r};
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/n1sdp-board-firmware_2021.10.12.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/n1sdp-board-firmware_2021.10.12.bb
new file mode 100644
index 0000000..dda99e7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/n1sdp-board-firmware_2021.10.12.bb
@@ -0,0 +1,35 @@
+SUMMARY = "Board Firmware binaries for N1SDP"
+SECTION = "firmware"
+
+LICENSE = "STM-SLA0044-Rev5"
+LIC_FILES_CHKSUM = "file://LICENSES/STM.TXT;md5=cd18335eff80d0a690a650f0e6748baf"
+
+inherit deploy
+
+INHIBIT_DEFAULT_DEPS = "1"
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+COMPATIBLE_MACHINE = "n1sdp"
+
+SRC_URI = "git://git.gitlab.arm.com/arm-reference-solutions/board-firmware.git;protocol=https;branch=n1sdp"
+
+SRCREV = "e6cd91c7a9733e501bc3b57ff6f9eb2461ffee54"
+
+S = "${WORKDIR}/git"
+
+INSTALL_DIR = "/n1sdp-board-firmware_source"
+
+do_install() {
+    rm -rf ${S}/SOFTWARE
+    install -d ${D}${INSTALL_DIR}
+    cp -Rp --no-preserve=ownership ${S}/* ${D}${INSTALL_DIR}
+}
+
+FILES:${PN} = "${INSTALL_DIR}"
+SYSROOT_DIRS += "${INSTALL_DIR}"
+
+do_deploy() {
+    install -d ${DEPLOYDIR}${INSTALL_DIR}
+    cp -Rp --no-preserve=ownership ${S}/* ${DEPLOYDIR}${INSTALL_DIR}
+}
+addtask deploy after do_install before do_build
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/sdcard-image-n1sdp_0.1.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/sdcard-image-n1sdp_0.1.bb
new file mode 100644
index 0000000..3ed71c5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/sdcard-image-n1sdp_0.1.bb
@@ -0,0 +1,85 @@
+SUMMARY = "Firmware image recipe for generating SD-Card artifacts."
+
+inherit deploy nopackages
+
+DEPENDS = "trusted-firmware-a \
+           virtual/control-processor-firmware \
+           n1sdp-board-firmware"
+
+LICENSE = "MIT"
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+COMPATIBLE_MACHINE = "n1sdp"
+RM_WORK_EXCLUDE += "${PN}"
+do_configure[noexec] = "1"
+do_compile[noexec] = "1"
+do_install[noexec] = "1"
+
+FIRMWARE_DIR = "n1sdp-board-firmware_source"
+PRIMARY_DIR = "${WORKDIR}/n1sdp-board-firmware_primary"
+SECONDARY_DIR = "${WORKDIR}/n1sdp-board-firmware_secondary"
+
+SOC_BINARIES = "mcp_fw.bin scp_fw.bin mcp_rom.bin scp_rom.bin"
+
+prepare_package() {
+    cd ${WORKDIR}
+
+    # Master/Primary
+    cp -av ${RECIPE_SYSROOT}/${FIRMWARE_DIR}/* ${PRIMARY_DIR}
+    mkdir -p ${PRIMARY_DIR}/SOFTWARE/
+
+    # Copy FIP binary
+    cp -v ${RECIPE_SYSROOT}/firmware/fip.bin ${PRIMARY_DIR}/SOFTWARE/
+
+    # Copy SOC binaries
+    for f in ${SOC_BINARIES}; do
+        cp -v ${RECIPE_SYSROOT}/firmware/${f} ${PRIMARY_DIR}/SOFTWARE/
+    done
+
+    sed -i -e 's|^C2C_ENABLE.*|C2C_ENABLE: TRUE            ;C2C enable TRUE/FALSE|' \
+        ${PRIMARY_DIR}/MB/HBI0316A/io_v123f.txt
+    sed -i -e 's|^C2C_SIDE.*|C2C_SIDE: MASTER            ;C2C side SLAVE/MASTER|' \
+        ${PRIMARY_DIR}/MB/HBI0316A/io_v123f.txt
+    sed -i -e 's|.*SOCCON: 0x1170.*PLATFORM_CTRL.*|SOCCON: 0x1170 0x00000100   ;SoC SCC PLATFORM_CTRL|' \
+        ${PRIMARY_DIR}/MB/HBI0316A/io_v123f.txt
+
+    # Update load address for trusted boot
+    sed -i -e '/^IMAGE4ADDRESS:/ s|0x60200000|0x64200000|' ${PRIMARY_DIR}/MB/HBI0316A/images.txt
+    sed -i -e '/^IMAGE4UPDATE:/ s|FORCE   |SCP_AUTO|' ${PRIMARY_DIR}/MB/HBI0316A/images.txt
+    sed -i -e '/^IMAGE4FILE: \\SOFTWARE\\/s|uefi.bin|fip.bin |' ${PRIMARY_DIR}/MB/HBI0316A/images.txt
+
+    # Slave/Secondary
+    cp -av ${RECIPE_SYSROOT}/${FIRMWARE_DIR}/* ${SECONDARY_DIR}
+    mkdir -p ${SECONDARY_DIR}/SOFTWARE/
+
+    # Copy SOC binaries
+    for f in ${SOC_BINARIES}; do
+        cp -v ${RECIPE_SYSROOT}/firmware/${f} ${SECONDARY_DIR}/SOFTWARE/
+    done
+
+    sed -i -e 's|^C2C_ENABLE.*|C2C_ENABLE: TRUE            ;C2C enable TRUE/FALSE|' \
+        ${SECONDARY_DIR}/MB/HBI0316A/io_v123f.txt
+    sed -i -e 's|^C2C_SIDE.*|C2C_SIDE: SLAVE             ;C2C side SLAVE/MASTER|' \
+        ${SECONDARY_DIR}/MB/HBI0316A/io_v123f.txt
+    sed -i -e 's|.*SOCCON: 0x1170.*PLATFORM_CTRL.*|SOCCON: 0x1170 0x00000101   ;SoC SCC PLATFORM_CTRL|' \
+        ${SECONDARY_DIR}/MB/HBI0316A/io_v123f.txt
+    sed -i -e '/^TOTALIMAGES:/ s|5|4|' ${SECONDARY_DIR}/MB/HBI0316A/images.txt
+    sed -i -e 's|^IMAGE4|;&|' ${SECONDARY_DIR}/MB/HBI0316A/images.txt
+}
+
+do_deploy() {
+    # prepare Master & Slave packages
+    prepare_package
+
+    for dir in ${PRIMARY_DIR} ${SECONDARY_DIR}; do
+        dir_name=$(basename ${dir})
+        mkdir -p ${D}/${dir_name}
+        cp -av ${dir} ${D}
+
+        # Compress the files
+        tar -C ${D}/${dir_name} -zcvf ${DEPLOYDIR}/${dir_name}.tar.gz ./
+    done
+}
+do_deploy[dirs] += "${PRIMARY_DIR} ${SECONDARY_DIR}"
+do_deploy[cleandirs] += "${PRIMARY_DIR} ${SECONDARY_DIR}"
+do_deploy[umask] = "022"
+addtask deploy after do_prepare_recipe_sysroot
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/images/tc-artifacts-image.bb b/meta-arm/meta-arm-bsp/recipes-bsp/images/tc-artifacts-image.bb
new file mode 100644
index 0000000..ded7404
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/images/tc-artifacts-image.bb
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: Apache-2.0
+#
+# Copyright (c) 2020 Arm Limited
+#
+SUMMARY = "Total Compute Images"
+DESCRIPTION = "Build all the images required for Total Compute platform"
+LICENSE = "Apache-2.0"
+
+COMPATIBLE_MACHINE = "(tc?)"
+
+inherit nopackages
+
+# The last image to be built is trusted-firmware-a
+DEPENDS += " trusted-firmware-a"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0002-tc0-fix-mpmm-config.patch b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0002-tc0-fix-mpmm-config.patch
new file mode 100644
index 0000000..f2044a9
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0002-tc0-fix-mpmm-config.patch
@@ -0,0 +1,92 @@
+From 736bd8aeceefd474c15a97e4a4ec99f07ef9a82c Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 11 Feb 2022 18:28:43 +0000
+Subject: [PATCH 2/4] tc0: fix mpmm config
+
+Do not enable MPMM in standard features set.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I7b273a2055452e2e8cd78a0d932514a6f2947ec5
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ product/tc0/scp_ramfw/config_mpmm.c | 15 ---------------
+ 1 file changed, 15 deletions(-)
+
+diff --git a/product/tc0/scp_ramfw/config_mpmm.c b/product/tc0/scp_ramfw/config_mpmm.c
+index 3bfe99d3..13d866a5 100644
+--- a/product/tc0/scp_ramfw/config_mpmm.c
++++ b/product/tc0/scp_ramfw/config_mpmm.c
+@@ -27,7 +27,6 @@ enum core_pd_idx {
+     CORE7_IDX
+ };
+ 
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VARIANT_STD)
+ static struct mod_mpmm_pct_table k_pct[] = {
+     { .cores_online = 4,
+       .default_perf_limit = 1153 * 1000000UL,
+@@ -115,7 +114,6 @@ static struct mod_mpmm_pct_table m_pct[] = {
+                           },
+                         } },
+ };
+-#endif
+ 
+ static struct mod_mpmm_pct_table m_elp_pct[] = {
+     { .cores_online = 1,
+@@ -132,7 +130,6 @@ static struct mod_mpmm_pct_table m_elp_pct[] = {
+                         } },
+ };
+ 
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VARIANT_STD)
+ static const struct mod_mpmm_core_config k_core_config[] = {
+     [0] = {
+         .pd_id = FWK_ID_ELEMENT_INIT(FWK_MODULE_IDX_POWER_DOMAIN, CORE0_IDX),
+@@ -180,7 +177,6 @@ static const struct mod_mpmm_core_config m_core_config[] = {
+         .core_starts_online = false,
+         },
+ };
+-#endif
+ 
+ static const struct mod_mpmm_core_config m_elp_core_config[] = {
+     [0] = {
+@@ -191,7 +187,6 @@ static const struct mod_mpmm_core_config m_elp_core_config[] = {
+         },
+ };
+ 
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VARIANT_STD)
+ static const struct mod_mpmm_domain_config k_domain_conf[] = {
+     [0] = {
+         .perf_id = FWK_ID_ELEMENT_INIT(
+@@ -219,7 +214,6 @@ static const struct mod_mpmm_domain_config m_domain_conf[] = {
+     },
+     [1] = {0},
+ };
+-#endif
+ 
+ static const struct mod_mpmm_domain_config m_elp_domain_conf[] = {
+     [0] = {
+@@ -236,14 +230,6 @@ static const struct mod_mpmm_domain_config m_elp_domain_conf[] = {
+ };
+ 
+ static const struct fwk_element element_table[] = {
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VAR_EXPERIMENT_POWER)
+-    [0] = {
+-        .name = "MPMM_MATTERHORN_ELP_ARM_ELEM",
+-        .sub_element_count = 1,
+-        .data = m_elp_domain_conf,
+-    },
+-    [1] = { 0 },
+-#else
+     [0] = {
+         .name = "MPMM_KLEIN_ELEM",
+         .sub_element_count = 4,
+@@ -260,7 +246,6 @@ static const struct fwk_element element_table[] = {
+         .data = m_elp_domain_conf,
+     },
+     [3] = { 0 },
+-#endif
+ };
+ 
+ static const struct fwk_element *mpmm_get_element_table(fwk_id_t module_id)
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0003-tc0-rename-platform-variant-to-platform-feature-set.patch b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0003-tc0-rename-platform-variant-to-platform-feature-set.patch
new file mode 100644
index 0000000..87dfbfa
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0003-tc0-rename-platform-variant-to-platform-feature-set.patch
@@ -0,0 +1,203 @@
+From 50e63f11762348bcd95d809af248f620f03d9ce4 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 11 Feb 2022 18:16:54 +0000
+Subject: [PATCH 3/4] tc0: rename platform variant to platform feature set
+
+THe PLATFORM_VARIANT flag was added to differentiate the software
+features enabled in SCP firmware. But this flag misleads to a new
+variant of same platform. This commits renames PLATFORM_VARIANT to
+PLATFORM_FEATURE_SET
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I93c0bc3e11fe18192bb8246df851345bdc473974
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ product/tc0/doc/{variants.md => features.md} | 28 +++++++++-----------
+ product/tc0/scp_ramfw/CMakeLists.txt         | 26 +++---------------
+ product/tc0/scp_ramfw/Firmware.cmake         |  2 +-
+ product/tc0/scp_ramfw/config_scmi_perf.c     |  8 +++---
+ product/tc0/scp_romfw/CMakeLists.txt         |  6 ++---
+ product/tc0/scp_romfw/Firmware.cmake         |  2 +-
+ 6 files changed, 25 insertions(+), 47 deletions(-)
+ rename product/tc0/doc/{variants.md => features.md} (77%)
+
+diff --git a/product/tc0/doc/variants.md b/product/tc0/doc/features.md
+similarity index 77%
+rename from product/tc0/doc/variants.md
+rename to product/tc0/doc/features.md
+index fbf616db..3ef520e2 100644
+--- a/product/tc0/doc/variants.md
++++ b/product/tc0/doc/features.md
+@@ -1,4 +1,4 @@
+-# TC0 Platform Variants
++# TC0 Platform Features
+ 
+ Copyright (c) 2022, Arm Limited. All rights reserved.
+ 
+@@ -7,30 +7,27 @@ Copyright (c) 2022, Arm Limited. All rights reserved.
+ 
+ Documentation for TC0 platform can be found at [1].
+ 
++### Standard
++
++The standard build provides all the features described in [1].
++For this default features, it's not required to provide any extra parameters in
++the build commands.
++
++### MPMM/Power/Performance (Experimental)
++
+ For the purpose of experimenting some of the software features that have been
+-introduced in SCP-firmware a new variant of TC0 has been created.
+-The variant(s) can be chosen at build time by adding:
++introduced in SCP-firmware of TC0. This can be enabled at build time, by adding:
+ 
+ ```sh
+ 
+ make -f Makefile.cmake \
+     PRODUCT=tc0 \
+     MODE=<debug,release> \
+-    PLATFORM_VARIANT=<0,1>
++    EXTRA_CONFIG_ARGS+=-DSCP_PLATFORM_FEATURE_SET=1
+ 
+ ```
+ 
+-
+-### Variant 0 (Standard build)
+-
+-The standard build provides all the features described in [1].
+-For this default variant, it's not required to provide any extra parameters in
+-the build commands.
+-
+-
+-### Variant 1 (Power/Performance testing)
+-
+-This variant adds support for the following software features:
++This adds support for the following software features:
+ - Traffic Cop
+ - MPMM (Maximum Power Mitigation Mechanism)
+ - Thermal Management
+@@ -63,7 +60,6 @@ Once built, the features above will act as:
+ 
+ ## Limitations
+ 
+-- The "variant" option is available only with the CMake build.
+ - The Thermal functionality is limited at this time cause the constant
+   temperature being sampled.
+ 
+diff --git a/product/tc0/scp_ramfw/CMakeLists.txt b/product/tc0/scp_ramfw/CMakeLists.txt
+index 96310320..ce3178ee 100644
+--- a/product/tc0/scp_ramfw/CMakeLists.txt
++++ b/product/tc0/scp_ramfw/CMakeLists.txt
+@@ -11,25 +11,13 @@
+ 
+ add_executable(tc0-bl2)
+ 
++set(SCP_PLATFORM_FEATURE_SET ${SCP_PLATFORM_FEATURE_SET_INIT} CACHE STRING "1")
+ 
+-# SCP_PLATFORM_VARIANT options:
+-# - 'TC0_VARIANT_STD' for TC0 standard build
+-# - 'TC0_VAR_EXPERIMENT_POWER' for TC0 with power/performance plugins used for
+-#   evaluation purposes
+-
+-
+-target_compile_definitions(tc0-bl2 PUBLIC -DTC0_VARIANT_STD=0)
+-target_compile_definitions(tc0-bl2 PUBLIC -DTC0_VAR_EXPERIMENT_POWER=1)
+-
+-
+-set(SCP_PLATFORM_VARIANT ${SCP_PLATFORM_VARIANT_INIT} CACHE STRING "1")
+-
+-
+-if (SCP_PLATFORM_VARIANT STREQUAL "1")
+-    message(NOTICE "SCP_PLATFORM_VARIANT set to EXPERIMENT_POWER (tc0-bl2)\n")
++if (SCP_PLATFORM_FEATURE_SET STREQUAL "1")
++    message(NOTICE "TC0 platform features MPMM/POWER/PERFORMANCE is experimental (tc0-bl2)\n")
+ 
+     target_compile_definitions(tc0-bl2
+-        PUBLIC -DPLATFORM_VARIANT=TC0_VAR_EXPERIMENT_POWER)
++	PUBLIC -DTC0_FEATURES_MPMM_POWER_PERF)
+ 
+     set(SCP_ENABLE_PLUGIN_HANDLER TRUE PARENT_SCOPE)
+     set(SCP_ENABLE_FAST_CHANNELS TRUE PARENT_SCOPE)
+@@ -56,12 +44,6 @@ if (SCP_PLATFORM_VARIANT STREQUAL "1")
+     list(PREPEND SCP_MODULE_PATHS
+         "${CMAKE_CURRENT_LIST_DIR}/../module/tc0_power_model")
+     target_sources(tc0-bl2 PRIVATE "config_tc0_power_model.c")
+-
+-else()
+-    message(NOTICE "SCP_PLATFORM_VARIANT set to STANDARD (tc0-bl2)\n")
+-
+-    target_compile_definitions(tc0-bl2
+-        PUBLIC -DPLATFORM_VARIANT=TC0_VARIANT_STD)
+ endif()
+ 
+ 
+diff --git a/product/tc0/scp_ramfw/Firmware.cmake b/product/tc0/scp_ramfw/Firmware.cmake
+index 11d8eaab..4a555296 100644
+--- a/product/tc0/scp_ramfw/Firmware.cmake
++++ b/product/tc0/scp_ramfw/Firmware.cmake
+@@ -27,7 +27,7 @@ set(SCP_ENABLE_FAST_CHANNELS_INIT FALSE)
+ 
+ set(SCP_ENABLE_PLUGIN_HANDLER_INIT FALSE)
+ 
+-set(SCP_PLATFORM_VARIANT_INIT 0)
++set(SCP_PLATFORM_FEATURE_SET_INIT 0)
+ 
+ set(SCP_ARCHITECTURE "armv7-m")
+ 
+diff --git a/product/tc0/scp_ramfw/config_scmi_perf.c b/product/tc0/scp_ramfw/config_scmi_perf.c
+index a4a47b3a..3e91939a 100644
+--- a/product/tc0/scp_ramfw/config_scmi_perf.c
++++ b/product/tc0/scp_ramfw/config_scmi_perf.c
+@@ -129,7 +129,7 @@ static const struct mod_scmi_perf_domain_config domains[] = {
+     },
+ };
+ 
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VAR_EXPERIMENT_POWER)
++#ifdef TC0_FEATURES_MPMM_POWER_PERF
+ static const struct mod_scmi_plugin_config plugins_table[] = {
+     [0] = {
+         .id = FWK_ID_MODULE_INIT(FWK_MODULE_IDX_TRAFFIC_COP),
+@@ -156,9 +156,9 @@ const struct fwk_module_config config_scmi_perf = {
+ #else
+         .fast_channels_alarm_id = FWK_ID_NONE_INIT,
+ #endif
+-#if defined(PLATFORM_VARIANT) && (PLATFORM_VARIANT == TC0_VAR_EXPERIMENT_POWER)
+-        .plugins = plugins_table,
+-        .plugins_count = FWK_ARRAY_SIZE(plugins_table),
++#ifdef TC0_FEATURES_MPMM_POWER_PERF
++	.plugins = plugins_table,
++	.plugins_count = FWK_ARRAY_SIZE(plugins_table),
+ #endif
+     })
+ };
+diff --git a/product/tc0/scp_romfw/CMakeLists.txt b/product/tc0/scp_romfw/CMakeLists.txt
+index f9f40ad3..09cd2f5d 100644
+--- a/product/tc0/scp_romfw/CMakeLists.txt
++++ b/product/tc0/scp_romfw/CMakeLists.txt
+@@ -48,6 +48,6 @@ target_include_directories(tc0-bl1
+     PUBLIC $<TARGET_PROPERTY:cmsis::core-m,INTERFACE_INCLUDE_DIRECTORIES>)
+ 
+ cmake_dependent_option(
+-    SCP_PLATFORM_VARIANT "Choose platform software variant?"
+-    "${SCP_PLATFORM_VARIANT_INIT}" "DEFINED SCP_PLATFORM_VARIANT_INIT"
+-    "${SCP_PLATFORM_VARIANT}")
++    SCP_PLATFORM_FEATURE_SET "Choose platform software features?"
++    "${SCP_PLATFORM_FEATURE_SET_INIT}" "DEFINED SCP_PLATFORM_FEATURE_SET_INIT"
++    "${SCP_PLATFORM_FEATURE_SET}")
+diff --git a/product/tc0/scp_romfw/Firmware.cmake b/product/tc0/scp_romfw/Firmware.cmake
+index ab4468be..e1360159 100644
+--- a/product/tc0/scp_romfw/Firmware.cmake
++++ b/product/tc0/scp_romfw/Firmware.cmake
+@@ -21,7 +21,7 @@ set(SCP_ENABLE_NOTIFICATIONS_INIT TRUE)
+ 
+ set(SCP_ENABLE_IPO_INIT FALSE)
+ 
+-set(SCP_PLATFORM_VARIANT_INIT 0)
++set(SCP_PLATFORM_FEATURE_SET_INIT 0)
+ 
+ set(SCP_ARCHITECTURE "armv7-m")
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0004-tc0-support-platform-feature-set-options-in-firmware.patch b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0004-tc0-support-platform-feature-set-options-in-firmware.patch
new file mode 100644
index 0000000..aa83332
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/files/tc/0004-tc0-support-platform-feature-set-options-in-firmware.patch
@@ -0,0 +1,114 @@
+From 3e737dd47b228bdeffb06e39bffec7a4a436b244 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Wed, 9 Feb 2022 16:02:10 +0000
+Subject: [PATCH 4/4] tc0: support platform feature set options in firmware.mk
+
+Support existing platform feature set options that is in cmake to
+firmware.mk. Two feature set for TC0 are
+0. Standard
+1. MPMM/Power/Performance (Experimental)
+
+Build option to select the feature set is using:
+make PRODUCT=tc0 MODE=<debug,release> SCP_PLATFORM_FEATURE_SET=<0,1>
+
+The default value is set to 0 (Standard).
+Refer product/tc0/doc/features.md for more details.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I4028686a8f8461e0e2c29e15d5e52eb1d37ca60a
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ product/tc0/scp_ramfw/firmware.mk | 41 +++++++++++++++++++++++++++++--
+ product/tc0/scp_romfw/firmware.mk | 12 +++++++++
+ 2 files changed, 51 insertions(+), 2 deletions(-)
+
+diff --git a/product/tc0/scp_ramfw/firmware.mk b/product/tc0/scp_ramfw/firmware.mk
+index ec6e6679..d7515f5b 100644
+--- a/product/tc0/scp_ramfw/firmware.mk
++++ b/product/tc0/scp_ramfw/firmware.mk
+@@ -9,8 +9,24 @@ BS_FIRMWARE_CPU := cortex-m3
+ BS_FIRMWARE_HAS_NOTIFICATION := yes
+ BS_FIRMWARE_HAS_RESOURCE_PERMISSIONS := yes
+ BS_FIRMWARE_USE_NEWLIB_NANO_SPECS := yes
+-BS_FIRMWARE_HAS_FAST_CHANNELS := no
+-BS_FIRMWARE_HAS_PERF_PLUGIN_HANDLER := no
++
++DEFAULT_SCP_PLATFORM_FEATURE_SET := 0
++
++export SCP_PLATFORM_FEATURE_SET ?= $(DEFAULT_SCP_PLATFORM_FEATURE_SET)
++ifneq ($(filter-out 0 1, $(SCP_PLATFORM_FEATURE_SET)),)
++    $(error "Invalid for SCP_PLATFORM_FEATURE_SET parameter. Valid options are \
++      0 or 1. Aborting...")
++endif
++
++ifeq ($(SCP_PLATFORM_FEATURE_SET),0)
++    BS_FIRMWARE_HAS_PERF_PLUGIN_HANDLER := no
++    BS_FIRMWARE_HAS_FAST_CHANNELS := no
++else
++    DEFINES += TC0_FEATURES_MPMM_POWER_PERF
++    BS_FIRMWARE_HAS_PERF_PLUGIN_HANDLER := yes
++    BS_FIRMWARE_HAS_FAST_CHANNELS := yes
++    $(info "TC0 platform features POWER/PERFORMANCE is experimental")
++endif
+ 
+ BS_FIRMWARE_MODULES := \
+     armv7m_mpu \
+@@ -44,6 +60,16 @@ ifeq ($(BS_FIRMWARE_HAS_RESOURCE_PERMISSIONS),yes)
+     BS_FIRMWARE_MODULES += resource_perms
+ endif
+ 
++ifeq ($(SCP_PLATFORM_FEATURE_SET),1)
++BS_FIRMWARE_MODULES += \
++        traffic_cop \
++        mpmm \
++        sensor \
++        reg_sensor \
++        thermal_mgmt \
++        tc0_power_model
++endif
++
+ BS_FIRMWARE_SOURCES := \
+     config_system_power.c \
+     config_armv7m_mpu.c \
+@@ -75,4 +101,15 @@ ifeq ($(BS_FIRMWARE_HAS_RESOURCE_PERMISSIONS),yes)
+     BS_FIRMWARE_SOURCES += config_resource_perms.c
+ endif
+ 
++ifeq ($(SCP_PLATFORM_FEATURE_SET),1)
++    BS_FIRMWARE_SOURCES += \
++        config_traffic_cop.c \
++        config_mpmm.c \
++        config_sensor.c \
++        config_reg_sensor.c \
++        config_thermal_mgmt.c \
++        config_tc0_power_model.c
++endif
++
++
+ include $(BS_DIR)/firmware.mk
+diff --git a/product/tc0/scp_romfw/firmware.mk b/product/tc0/scp_romfw/firmware.mk
+index 9977712f..0012b9fa 100644
+--- a/product/tc0/scp_romfw/firmware.mk
++++ b/product/tc0/scp_romfw/firmware.mk
+@@ -9,6 +9,18 @@ BS_FIRMWARE_CPU := cortex-m3
+ BS_FIRMWARE_HAS_NOTIFICATION := yes
+ BS_FIRMWARE_USE_NEWLIB_NANO_SPECS := yes
+ 
++DEFAULT_SCP_PLATFORM_FEATURE_SET := 0
++
++export SCP_PLATFORM_FEATURE_SET ?= $(DEFAULT_SCP_PLATFORM_FEATURE_SET)
++ifneq ($(filter-out 0 1, $(SCP_PLATFORM_FEATURE_SET)),)
++    $(error "Invalid for SCP_PLATFORM_FEATURE_SET parameter. Valid options are \
++      0 or 1. Aborting...")
++endif
++
++ifeq ($(SCP_PLATFORM_FEATURE_SET),1)
++    $(info "TC0 platform features POWER/PERFORMANCE is experimental")
++endif
++
+ BS_FIRMWARE_MODULE_HEADERS_ONLY := \
+     power_domain \
+     timer
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-juno.inc b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-juno.inc
new file mode 100644
index 0000000..ea2face
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-juno.inc
@@ -0,0 +1,16 @@
+# juno specific SCP configuration
+
+COMPATIBLE_MACHINE = "juno"
+
+SCP_PLATFORM = "juno"
+FW_TARGETS = "scp"
+FW_INSTALL:append = " romfw_bypass"
+
+do_install:append() {
+    for TYPE in ${FW_INSTALL}; do
+        if [ "$TYPE" = "romfw_bypass" ]; then
+            install -D "${B}/${TYPE}/${FW_TARGETS}/bin/${SCP_PLATFORM}-bl1-bypass.bin" "${D}/firmware/${FW}_${TYPE}.bin"
+            install -D "${B}/${TYPE}/${FW_TARGETS}/bin/${SCP_PLATFORM}-bl1-bypass" "${D}/firmware/${FW}_${TYPE}.elf"
+        fi
+    done
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-n1sdp.inc b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-n1sdp.inc
new file mode 100644
index 0000000..e66469c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-n1sdp.inc
@@ -0,0 +1,33 @@
+# N1SDP specific SCP configurations and build instructions
+
+SCP_PLATFORM  = "n1sdp"
+SCP_LOG_LEVEL = "INFO"
+
+# master branch at n1sdp: Introduce trusted board boot
+SRCREV  = "3e4c34ceccc1c960eb3a4adaa922f2a0c6b36be3"
+PV .= "+git${SRCPV}"
+
+COMPATIBLE_MACHINE:n1sdp = "n1sdp"
+
+DEPENDS += "fiptool-native"
+DEPENDS += "trusted-firmware-a"
+
+do_install:append() {
+   fiptool \
+       create \
+       --scp-fw "${D}/firmware/scp_ramfw.bin" \
+       --blob uuid=cfacc2c4-15e8-4668-82be-430a38fad705,file="${RECIPE_SYSROOT}/firmware/bl1.bin" \
+       "scp_fw.bin"
+
+   # This UUID is FIP_UUID_MCP_BL2 in SCP-Firmware.
+   fiptool \
+       create \
+       --blob uuid=54464222-a4cf-4bf8-b1b6-cee7dade539e,file="${D}/firmware/mcp_ramfw.bin" \
+       "mcp_fw.bin"
+
+   install "scp_fw.bin" "${D}/firmware/scp_fw.bin"
+   install "mcp_fw.bin" "${D}/firmware/mcp_fw.bin"
+
+   ln -sf "scp_romfw.bin" "${D}/firmware/scp_rom.bin"
+   ln -sf "mcp_romfw.bin" "${D}/firmware/mcp_rom.bin"
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-sgi575.inc b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-sgi575.inc
new file mode 100644
index 0000000..e1b0a85
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-sgi575.inc
@@ -0,0 +1,6 @@
+# SGI575 specific SCP configurations and build instructions
+
+SCP_PLATFORM  = "sgi575"
+SCP_LOG_LEVEL = "INFO"
+
+COMPATIBLE_MACHINE:sgi575 = "sgi575"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-tc.inc b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-tc.inc
new file mode 100644
index 0000000..a6a005c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware-tc.inc
@@ -0,0 +1,14 @@
+# TC specific SCP configuration
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/tc:"
+SRC_URI:append:tc = " \
+    file://0002-tc0-fix-mpmm-config.patch \
+    file://0003-tc0-rename-platform-variant-to-platform-feature-set.patch \
+    file://0004-tc0-support-platform-feature-set-options-in-firmware.patch \
+    "
+
+COMPATIBLE_MACHINE = "(tc?)"
+
+SCP_PLATFORM:tc0 = "tc0"
+SCP_PLATFORM:tc1 = "tc1"
+FW_TARGETS = "scp"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware_2.10.%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware_2.10.%.bbappend
new file mode 100644
index 0000000..bb1a48c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/scp-firmware/scp-firmware_2.10.%.bbappend
@@ -0,0 +1,10 @@
+# Include machine specific SCP configurations
+
+MACHINE_SCP_REQUIRE ?= ""
+
+MACHINE_SCP_REQUIRE:juno = "scp-firmware-juno.inc"
+MACHINE_SCP_REQUIRE:n1sdp = "scp-firmware-n1sdp.inc"
+MACHINE_SCP_REQUIRE:sgi575 = "scp-firmware-sgi575.inc"
+MACHINE_SCP_REQUIRE:tc = "scp-firmware-tc.inc"
+
+require ${MACHINE_SCP_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/corstone1000/0001-Fix-FF-A-version-in-SPMC-manifest.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/corstone1000/0001-Fix-FF-A-version-in-SPMC-manifest.patch
new file mode 100644
index 0000000..016de8d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/corstone1000/0001-Fix-FF-A-version-in-SPMC-manifest.patch
@@ -0,0 +1,34 @@
+Upstream-Status: Inappropriate
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+From a31aee0988ef64724ec5866f10709f51f8cb3237 Mon Sep 17 00:00:00 2001
+From: emeara01 <emekcan.aras@arm.com>
+Date: Wed, 11 May 2022 14:37:06 +0100
+Subject: [PATCH] Fix FF-A version in SPMC manifest
+
+OPTEE does not support FF-A version 1.1 in SPMC at the moment.
+This commit corrects the FF-A version in corstone1000_spmc_manifest.dts.
+This patch will not be upstreamed and will be dropped once
+OPTEE version is updated for Corstone1000.
+
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+---
+ .../corstone1000/common/fdts/corstone1000_spmc_manifest.dts     | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/plat/arm/board/corstone1000/common/fdts/corstone1000_spmc_manifest.dts b/plat/arm/board/corstone1000/common/fdts/corstone1000_spmc_manifest.dts
+index 8e49ab83f..5baa1b115 100644
+--- a/plat/arm/board/corstone1000/common/fdts/corstone1000_spmc_manifest.dts
++++ b/plat/arm/board/corstone1000/common/fdts/corstone1000_spmc_manifest.dts
+@@ -20,7 +20,7 @@
+ 	attribute {
+ 		spmc_id = <0x8000>;
+ 		maj_ver = <0x1>;
+-		min_ver = <0x1>;
++		min_ver = <0x0>;
+ 		exec_state = <0x0>;
+ 		load_address = <0x0 0x2002000>;
+ 		entrypoint = <0x0 0x2002000>;
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/n1sdp/bl_size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/n1sdp/bl_size.patch
new file mode 100644
index 0000000..a5b3019
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/n1sdp/bl_size.patch
@@ -0,0 +1,40 @@
+From 80b1efa92486a87f9e82dbf665ef612291148de8 Mon Sep 17 00:00:00 2001
+From: Adam Johnston <adam.johnston@arm.com>
+Date: Tue, 14 Jun 2022 11:19:30 +0000
+Subject: [PATCH] arm-bsp/trusted-firmware-a: N1SDP trusted boot
+
+Increase max size of BL2 on N1SDP by 4KB to enable trusted boot
+Decrease max size of BL1 on N1SDP by 8KB so BL1/BL2 fits above BL31 progbits
+
+Signed-off-by: Adam Johnston <adam.johnston@arm.com>
+Upstream-Status: Pending [Flagged to upstream]
+
+---
+ plat/arm/board/n1sdp/include/platform_def.h | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/plat/arm/board/n1sdp/include/platform_def.h b/plat/arm/board/n1sdp/include/platform_def.h
+index c9b81bafa..7468a31ed 100644
+--- a/plat/arm/board/n1sdp/include/platform_def.h
++++ b/plat/arm/board/n1sdp/include/platform_def.h
+@@ -91,7 +91,7 @@
+  * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
+  * plus a little space for growth.
+  */
+-#define PLAT_ARM_MAX_BL1_RW_SIZE	0xE000
++#define PLAT_ARM_MAX_BL1_RW_SIZE	0xC000
+ 
+ /*
+  * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
+@@ -110,7 +110,7 @@
+  * little space for growth.
+  */
+ #if TRUSTED_BOARD_BOOT
+-# define PLAT_ARM_MAX_BL2_SIZE		0x20000
++# define PLAT_ARM_MAX_BL2_SIZE		0x21000
+ #else
+ # define PLAT_ARM_MAX_BL2_SIZE		0x14000
+ #endif
+-- 
+2.35.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0001-plat-tc-Increase-maximum-BL2-size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0001-plat-tc-Increase-maximum-BL2-size.patch
new file mode 100644
index 0000000..74ab361
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0001-plat-tc-Increase-maximum-BL2-size.patch
@@ -0,0 +1,43 @@
+From 008cfc6457c239466ca62610d59aaf1a78f6b2f6 Mon Sep 17 00:00:00 2001
+From: Tudor Cretu <tudor.cretu@arm.com>
+Date: Fri, 21 May 2021 14:56:37 +0000
+Subject: [PATCH 1/7] plat: tc: Increase maximum BL2 size.
+
+BL2 size gets increased due to the firmware update changes.
+Increase the MAX_BL2_SIZE by 8Kb.
+
+Signed-off-by: Tudor Cretu <tudor.cretu@arm.com>
+Change-Id: I1cb28b0eb7f834426873ff9f4c40bd496413806f
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ plat/arm/board/tc/include/platform_def.h | 6 +++---
+ 1 file changed, 3 insertions(+), 3 deletions(-)
+
+diff --git a/plat/arm/board/tc/include/platform_def.h b/plat/arm/board/tc/include/platform_def.h
+index 745d91cab..cd77773aa 100644
+--- a/plat/arm/board/tc/include/platform_def.h
++++ b/plat/arm/board/tc/include/platform_def.h
+@@ -120,9 +120,9 @@
+  * little space for growth.
+  */
+ #if TRUSTED_BOARD_BOOT
+-# define PLAT_ARM_MAX_BL2_SIZE		0x20000
++# define PLAT_ARM_MAX_BL2_SIZE		0x25000
+ #else
+-# define PLAT_ARM_MAX_BL2_SIZE		0x14000
++# define PLAT_ARM_MAX_BL2_SIZE		0x19000
+ #endif
+ 
+ /*
+@@ -130,7 +130,7 @@
+  * calculated using the current BL31 PROGBITS debug size plus the sizes of
+  * BL2 and BL1-RW
+  */
+-#define PLAT_ARM_MAX_BL31_SIZE		0x3F000
++#define PLAT_ARM_MAX_BL31_SIZE		0x4F000
+ 
+ /*
+  * Size of cacheable stacks
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0002-Makefile-add-trusty_sp_fw_config-build-option.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0002-Makefile-add-trusty_sp_fw_config-build-option.patch
new file mode 100644
index 0000000..75cabdd
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0002-Makefile-add-trusty_sp_fw_config-build-option.patch
@@ -0,0 +1,46 @@
+From 2f8b0cc6be3787717247d1c02a45181a5ac6f125 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Mon, 11 Apr 2022 14:36:54 +0100
+Subject: [PATCH 2/7] Makefile: add trusty_sp_fw_config build option
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Ief90ae9113d32265ee2200f35f3e517b7b9a4bea
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ Makefile                            | 4 ++++
+ docs/plat/arm/arm-build-options.rst | 4 ++++
+ 2 files changed, 8 insertions(+)
+
+diff --git a/Makefile b/Makefile
+index 3941f8698..a20d647a2 100644
+--- a/Makefile
++++ b/Makefile
+@@ -531,6 +531,10 @@ ifneq (${SPD},none)
+             DTC_CPPFLAGS	+=	-DOPTEE_SP_FW_CONFIG
+         endif
+ 
++        ifeq ($(findstring trusty_sp,$(ARM_SPMC_MANIFEST_DTS)),trusty_sp)
++            DTC_CPPFLAGS	+=	-DTRUSTY_SP_FW_CONFIG
++        endif
++
+         ifeq ($(TS_SP_FW_CONFIG),1)
+             DTC_CPPFLAGS	+=	-DTS_SP_FW_CONFIG
+         endif
+diff --git a/docs/plat/arm/arm-build-options.rst b/docs/plat/arm/arm-build-options.rst
+index 339ebbe33..3c9a41fb8 100644
+--- a/docs/plat/arm/arm-build-options.rst
++++ b/docs/plat/arm/arm-build-options.rst
+@@ -107,6 +107,10 @@ Arm Platform Build Options
+    device tree. This flag is defined only when ``ARM_SPMC_MANIFEST_DTS`` manifest
+    file name contains pattern optee_sp.
+ 
++-  ``TRUSTY_SP_FW_CONFIG``: DTC build flag to include Trusty as SP in
++   tb_fw_config device tree. This flag is defined only when
++   ``ARM_SPMC_MANIFEST_DTS`` manifest file name contains pattern trusty_sp.
++
+ -  ``TS_SP_FW_CONFIG``: DTC build flag to include Trusted Services (Crypto and
+    internal-trusted-storage) as SP in tb_fw_config device tree.
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0003-fix-plat-arm-increase-sp-max-image-size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0003-fix-plat-arm-increase-sp-max-image-size.patch
new file mode 100644
index 0000000..6807191
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0003-fix-plat-arm-increase-sp-max-image-size.patch
@@ -0,0 +1,30 @@
+From 0060b1a4fbe3bc9992f59a2d4cb986821f7bcf13 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Mon, 11 Apr 2022 18:31:01 +0100
+Subject: [PATCH 3/7] fix(plat/arm): increase sp max image size
+
+Increase ARM_SP_MAX_SIZE to support Trusty image.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I9ef9e755769445aee998062a7fba508fad50b33e
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ include/plat/arm/common/fconf_arm_sp_getter.h | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/include/plat/arm/common/fconf_arm_sp_getter.h b/include/plat/arm/common/fconf_arm_sp_getter.h
+index aa628dfd3..3ed953d1c 100644
+--- a/include/plat/arm/common/fconf_arm_sp_getter.h
++++ b/include/plat/arm/common/fconf_arm_sp_getter.h
+@@ -13,7 +13,7 @@
+ /* arm_sp getter */
+ #define arm__sp_getter(prop)	arm_sp.prop
+ 
+-#define ARM_SP_MAX_SIZE		U(0xb0000)
++#define ARM_SP_MAX_SIZE		U(0x2000000)
+ #define ARM_SP_OWNER_NAME_LEN	U(8)
+ 
+ struct arm_sp_t {
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0004-fix-plat-tc-increase-tc_tzc_dram1_size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0004-fix-plat-tc-increase-tc_tzc_dram1_size.patch
new file mode 100644
index 0000000..aec8be0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0004-fix-plat-tc-increase-tc_tzc_dram1_size.patch
@@ -0,0 +1,69 @@
+From 000e19d360a5ad9abd7d823af86a364bac2afc58 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Mon, 11 Apr 2022 17:38:17 +0100
+Subject: [PATCH 4/7] fix(plat/tc): increase tc_tzc_dram1_size
+
+Increase TC_TZC_DRAM1_SIZE for Trusty image and its memory size.
+Update OP-TEE reserved memory range in DTS
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Iad433c3c155f28860b15bde2398df653487189dd
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ fdts/tc.dts                              |  4 ++--
+ plat/arm/board/tc/include/platform_def.h | 10 ++++++----
+ 2 files changed, 8 insertions(+), 6 deletions(-)
+
+diff --git a/fdts/tc.dts b/fdts/tc.dts
+index 20992294b..af64504a4 100644
+--- a/fdts/tc.dts
++++ b/fdts/tc.dts
+@@ -213,8 +213,8 @@
+ 			linux,cma-default;
+ 		};
+ 
+-		optee@0xfce00000 {
+-			reg = <0x00000000 0xfce00000 0 0x00200000>;
++		optee@0xf8e00000 {
++			reg = <0x00000000 0xf8e00000 0 0x00200000>;
+ 			no-map;
+ 		};
+ 	};
+diff --git a/plat/arm/board/tc/include/platform_def.h b/plat/arm/board/tc/include/platform_def.h
+index cd77773aa..35d8fd24e 100644
+--- a/plat/arm/board/tc/include/platform_def.h
++++ b/plat/arm/board/tc/include/platform_def.h
+@@ -31,7 +31,7 @@
+  */
+ #define TC_TZC_DRAM1_BASE		(ARM_AP_TZC_DRAM1_BASE -	\
+ 					 TC_TZC_DRAM1_SIZE)
+-#define TC_TZC_DRAM1_SIZE		UL(0x02000000)	/* 32 MB */
++#define TC_TZC_DRAM1_SIZE		UL(0x06000000)	/* 96 MB */
+ #define TC_TZC_DRAM1_END		(TC_TZC_DRAM1_BASE +		\
+ 					 TC_TZC_DRAM1_SIZE - 1)
+ 
+@@ -68,7 +68,9 @@
+  * max size of BL32 image.
+  */
+ #if defined(SPD_spmd)
+-#define PLAT_ARM_SPMC_BASE		TC_TZC_DRAM1_BASE
++#define TC_EL2SPMC_LOAD_ADDR		(TC_TZC_DRAM1_BASE + 0x04000000)
++
++#define PLAT_ARM_SPMC_BASE		TC_EL2SPMC_LOAD_ADDR
+ #define PLAT_ARM_SPMC_SIZE		UL(0x200000)  /* 2 MB */
+ #endif
+ 
+@@ -259,8 +261,8 @@
+ 		(TZC_REGION_ACCESS_RDWR(TZC_NSAID_DEFAULT))
+ 
+ /*
+- * The first region below, TC_TZC_DRAM1_BASE (0xfd000000) to
+- * ARM_SCP_TZC_DRAM1_END (0xffffffff) will mark the last 48 MB of DRAM as
++ * The first region below, TC_TZC_DRAM1_BASE (0xf9000000) to
++ * ARM_SCP_TZC_DRAM1_END (0xffffffff) will mark the last 112 MB of DRAM as
+  * secure. The second and third regions gives non secure access to rest of DRAM.
+  */
+ #define TC_TZC_REGIONS_DEF	\
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0005-feat-plat-tc-add-spmc-manifest-with-trusty-sp.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0005-feat-plat-tc-add-spmc-manifest-with-trusty-sp.patch
new file mode 100644
index 0000000..0b34683
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0005-feat-plat-tc-add-spmc-manifest-with-trusty-sp.patch
@@ -0,0 +1,169 @@
+From a04466ceb81a04c5179e8064837c34a89c2b11bd Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Mon, 11 Apr 2022 14:43:15 +0100
+Subject: [PATCH 5/7] feat(plat/tc): add spmc manifest with trusty sp
+
+Add SPMC manifest with Trusty SP. Define Trusty's load address,
+vcpu count, memory size.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: If4363580a478776d233f7f391a30e1cb345453c2
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ .../tc/fdts/tc_spmc_trusty_sp_manifest.dts    | 120 ++++++++++++++++++
+ plat/arm/board/tc/fdts/tc_tb_fw_config.dts    |   7 +-
+ 2 files changed, 126 insertions(+), 1 deletion(-)
+ create mode 100644 plat/arm/board/tc/fdts/tc_spmc_trusty_sp_manifest.dts
+
+diff --git a/plat/arm/board/tc/fdts/tc_spmc_trusty_sp_manifest.dts b/plat/arm/board/tc/fdts/tc_spmc_trusty_sp_manifest.dts
+new file mode 100644
+index 000000000..e2ea7b811
+--- /dev/null
++++ b/plat/arm/board/tc/fdts/tc_spmc_trusty_sp_manifest.dts
+@@ -0,0 +1,120 @@
++/*
++ * Copyright (c) 2022, Arm Limited. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++/dts-v1/;
++
++/ {
++	compatible = "arm,ffa-core-manifest-1.0";
++	#address-cells = <2>;
++	#size-cells = <1>;
++
++	attribute {
++		spmc_id = <0x8000>;
++		maj_ver = <0x1>;
++		min_ver = <0x1>;
++		exec_state = <0x0>;
++		load_address = <0x0 0xfd000000>;
++		entrypoint = <0x0 0xfd000000>;
++		binary_size = <0x80000>;
++	};
++
++	hypervisor {
++		compatible = "hafnium,hafnium";
++		vm1 {
++			is_ffa_partition;
++			debug_name = "trusty";
++			load_address = <0xf901f000>;
++			vcpu_count = <8>;
++			mem_size = <0x3f00000>; /* 64MB TZC DRAM - 1MB align */
++		};
++#ifdef TS_SP_FW_CONFIG
++		vm2 {
++			is_ffa_partition;
++			debug_name = "internal-trusted-storage";
++			load_address = <0xfee00000>;
++			vcpu_count = <1>;
++			mem_size = <2097152>; /* 2MB TZC DRAM */
++		};
++		vm3 {
++			is_ffa_partition;
++			debug_name = "crypto";
++			load_address = <0xfec00000>;
++			vcpu_count = <1>;
++			mem_size = <2097152>; /* 2MB TZC DRAM */
++		};
++#endif
++	};
++
++	cpus {
++		#address-cells = <0x2>;
++		#size-cells = <0x0>;
++
++		CPU0:cpu@0 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x0>;
++			enable-method = "psci";
++		};
++
++		/*
++		 * SPMC (Hafnium) requires secondary cpu nodes are declared in
++		 * descending order
++		 */
++		CPU7:cpu@700 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x700>;
++			enable-method = "psci";
++		};
++
++		CPU6:cpu@600 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x600>;
++			enable-method = "psci";
++		};
++
++		CPU5:cpu@500 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x500>;
++			enable-method = "psci";
++		};
++
++		CPU4:cpu@400 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x400>;
++			enable-method = "psci";
++		};
++
++		CPU3:cpu@300 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x300>;
++			enable-method = "psci";
++		};
++
++		CPU2:cpu@200 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x200>;
++			enable-method = "psci";
++		};
++
++		CPU1:cpu@100 {
++			device_type = "cpu";
++			compatible = "arm,armv8";
++			reg = <0x0 0x100>;
++			enable-method = "psci";
++		};
++	};
++
++	/* 96MB of TC_TZC_DRAM1_BASE */
++	memory@f9000000 {
++		device_type = "memory";
++		reg = <0x0 0xf9000000 0x6000000>;
++	};
++};
+diff --git a/plat/arm/board/tc/fdts/tc_tb_fw_config.dts b/plat/arm/board/tc/fdts/tc_tb_fw_config.dts
+index 4c6ccef25..a72772fb3 100644
+--- a/plat/arm/board/tc/fdts/tc_tb_fw_config.dts
++++ b/plat/arm/board/tc/fdts/tc_tb_fw_config.dts
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -47,6 +47,11 @@
+ 		       uuid = "486178e0-e7f8-11e3-bc5e-0002a5d5c51b";
+ 		       load-address = <0xfd280000>;
+ 		};
++#elif TRUSTY_SP_FW_CONFIG
++		trusty {
++		       uuid = "40ee25f0-a2bc-304c-8c4c-a173c57d8af1";
++		       load-address = <0xf901f000>;
++		};
+ #else
+ 		cactus-primary {
+ 			uuid = "b4b5671e-4a90-4fe1-b81f-fb13dae1dacb";
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0006-feat-plat-tc-update-dts-with-trusty-compatible-strin.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0006-feat-plat-tc-update-dts-with-trusty-compatible-strin.patch
new file mode 100644
index 0000000..e2bfb2c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0006-feat-plat-tc-update-dts-with-trusty-compatible-strin.patch
@@ -0,0 +1,50 @@
+From 96151af7eed28d63fdaa1ac6de2d14a9c71f1d4a Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Wed, 30 Mar 2022 12:14:49 +0000
+Subject: [PATCH 6/7] feat(plat/tc): update dts with trusty compatible string
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Ic6661df479e114bf3f464165c14df5fa02dc0139
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ fdts/tc.dts | 26 ++++++++++++++++++++++++++
+ 1 file changed, 26 insertions(+)
+
+diff --git a/fdts/tc.dts b/fdts/tc.dts
+index af64504a4..dc86958bf 100644
+--- a/fdts/tc.dts
++++ b/fdts/tc.dts
+@@ -555,4 +555,30 @@
+ 		compatible = "arm,trace-buffer-extension";
+ 		interrupts = <1 2 4>;
+ 	};
++
++	trusty {
++		#size-cells = <0x02>;
++		#address-cells = <0x02>;
++		ranges = <0x00>;
++		compatible = "android,trusty-v1";
++
++		virtio {
++			compatible = "android,trusty-virtio-v1";
++		};
++
++		test {
++			compatible = "android,trusty-test-v1";
++		};
++
++		log {
++			compatible = "android,trusty-log-v1";
++		};
++
++		irq {
++			ipi-range = <0x08 0x0f 0x08>;
++			interrupt-ranges = <0x00 0x0f 0x00 0x10 0x1f 0x01 0x20 0x3f 0x02>;
++			interrupt-templates = <0x01 0x00 0x8001 0x01 0x01 0x04 0x8001 0x01 0x00 0x04>;
++			compatible = "android,trusty-irq-v1";
++		};
++	};
+ };
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0007-fix-plat-tc-disable-smmu.patch b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0007-fix-plat-tc-disable-smmu.patch
new file mode 100644
index 0000000..91f5a9e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/0007-fix-plat-tc-disable-smmu.patch
@@ -0,0 +1,64 @@
+From 1a051bef6c63f6871edd8d87e969460f073820a7 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Wed, 27 Apr 2022 18:15:47 +0100
+Subject: [PATCH 7/7] fix(plat/tc): disable smmu
+
+Reserve static shared-dma-pool below 4GB. This removes dependency on
+SMMU driver. As there are stability issues in SMMU driver, it is
+disabled. This change is temporary and will be reverted upon proper
+fix and testing.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I6b1b4c2a0acdf62df8c26007c7ca596774e13710
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ fdts/tc.dts | 16 +++-------------
+ 1 file changed, 3 insertions(+), 13 deletions(-)
+
+diff --git a/fdts/tc.dts b/fdts/tc.dts
+index dc86958bf..fbae3e3e8 100644
+--- a/fdts/tc.dts
++++ b/fdts/tc.dts
+@@ -209,12 +209,12 @@
+ 		linux,cma {
+ 			compatible = "shared-dma-pool";
+ 			reusable;
+-			size = <0x0 0x8000000>;
++			reg = <0x0 0xf1000000 0x0 0x8000000>;
+ 			linux,cma-default;
+ 		};
+ 
+-		optee@0xf8e00000 {
+-			reg = <0x00000000 0xf8e00000 0 0x00200000>;
++		optee@0xf0e00000 {
++			reg = <0x0 0xf0e00000 0 0x00200000>;
+ 			no-map;
+ 		};
+ 	};
+@@ -460,13 +460,6 @@
+ 		>;
+ 	};
+ 
+-	smmu: smmu@2ce00000 {
+-		#iommu-cells = <1>;
+-		compatible = "arm,smmu-v3";
+-		reg = <0x0 0x2ce00000 0x0 0x20000>;
+-		status = "okay";
+-	};
+-
+ 	dp0: display@2cc00000 {
+ 		#address-cells = <1>;
+ 		#size-cells = <0>;
+@@ -476,9 +469,6 @@
+ 		interrupt-names = "DPU";
+ 		clocks = <&scmi_clk 0>;
+ 		clock-names = "aclk";
+-		iommus = <&smmu 0>, <&smmu 1>, <&smmu 2>, <&smmu 3>,
+-			<&smmu 4>, <&smmu 5>, <&smmu 6>, <&smmu 7>,
+-			<&smmu 8>, <&smmu 9>;
+ 		pl0: pipeline@0 {
+ 			reg = <0>;
+ 			clocks = <&scmi_clk 1>;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/generate_metadata.py b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/generate_metadata.py
new file mode 100644
index 0000000..f3670ce
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/files/tc/generate_metadata.py
@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+import argparse
+import uuid
+import zlib
+
+def main(metadata_file, img_type_uuids, location_uuids, img_uuids):
+    def add_field_to_metadata(value):
+        # Write the integer values to file in little endian representation
+        with open(metadata_file, "ab") as fp:
+            fp.write(value.to_bytes(4, byteorder='little'))
+
+    def add_uuid_to_metadata(uuid_str):
+        # Validate UUID string and write to file in little endian representation
+        uuid_val = uuid.UUID(uuid_str)
+        with open(metadata_file, "ab") as fp:
+            fp.write(uuid_val.bytes_le)
+
+    # Fill metadata preamble
+    add_field_to_metadata(1) #version=1
+    add_field_to_metadata(0) #active_index=0
+    add_field_to_metadata(0) #previous_active_index=0
+
+    for img_type_uuid, location_uuid in zip(img_type_uuids, location_uuids):
+        # Fill metadata image entry
+        add_uuid_to_metadata(img_type_uuid) # img_type_uuid
+        add_uuid_to_metadata(location_uuid) # location_uuid
+
+        for img_uuid in img_uuids:
+            # Fill metadata bank image info
+            add_uuid_to_metadata(img_uuid) # image unique bank_uuid
+            add_field_to_metadata(1)       # accepted=1
+            add_field_to_metadata(0)       # reserved (MBZ)
+
+    # Prepend CRC32
+    with open(metadata_file, 'rb+') as fp:
+        content = fp.read()
+        crc = zlib.crc32(content)
+        fp.seek(0)
+        fp.write(crc.to_bytes(4, byteorder='little') + content)
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--metadata_file', required=True,
+                        help='Output binary file to store the metadata')
+    parser.add_argument('--img_type_uuids', type=str, nargs='+', required=True,
+                        help='A list of UUIDs identifying the image types')
+    parser.add_argument('--location_uuids', type=str, nargs='+', required=True,
+                        help='A list of UUIDs of the storage volumes where the images are located. '
+                             'Must have the same length as img_type_uuids.')
+    parser.add_argument('--img_uuids', type=str, nargs='+', required=True,
+                        help='A list UUIDs of the images in a firmware bank')
+
+    args = parser.parse_args()
+
+    if len(args.img_type_uuids) != len(args.location_uuids):
+        parser.print_help()
+        raise argparse.ArgumentError(None, 'Arguments img_type_uuids and location_uuids must have the same length.')
+
+    main(args.metadata_file, args.img_type_uuids, args.location_uuids, args.img_uuids)
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/tf-a-tests_2.7.0.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/tf-a-tests_2.7.0.bbappend
new file mode 100644
index 0000000..ff22ff1
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/tf-a-tests_2.7.0.bbappend
@@ -0,0 +1,3 @@
+# Machine specific TFAs
+
+COMPATIBLE_MACHINE:corstone1000 = "corstone1000"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone1000.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone1000.inc
new file mode 100644
index 0000000..341c8a2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone1000.inc
@@ -0,0 +1,42 @@
+# Corstone1000 64-bit machines specific TFA support
+
+COMPATIBLE_MACHINE = "(corstone1000)"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/corstone1000:"
+
+SRC_URI:append = " \ 
+	file://0001-Fix-FF-A-version-in-SPMC-manifest.patch \
+	"
+
+TFA_DEBUG = "1"
+TFA_UBOOT ?= "1"
+TFA_MBEDTLS = "1"
+TFA_BUILD_TARGET = "bl2 bl31 fip"
+
+# Enabling Secure-EL1 Payload Dispatcher (SPD)
+TFA_SPD = "spmd"
+# Cortex-A35 supports Armv8.0-A (no S-EL2 execution state).
+# So, the SPD SPMC component should run at the S-EL1 execution state
+TFA_SPMD_SPM_AT_SEL2 = "0"
+
+# BL2 loads BL32 (optee). So, optee needs to be built first:
+DEPENDS += "optee-os"
+
+EXTRA_OEMAKE:append = " \
+                        ARCH=aarch64 \
+                        TARGET_PLATFORM=${TFA_TARGET_PLATFORM} \
+                        ENABLE_STACK_PROTECTOR=strong \
+                        ENABLE_PIE=1 \
+                        BL2_AT_EL3=1 \
+                        CREATE_KEYS=1 \
+                        GENERATE_COT=1 \
+                        TRUSTED_BOARD_BOOT=1 \
+                        COT=tbbr \
+                        ARM_ROTPK_LOCATION=devel_rsa  \
+                        ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem \
+                        BL32=${RECIPE_SYSROOT}/lib/firmware/tee-pager_v2.bin \
+                        LOG_LEVEL=50 \
+                        "
+
+# trigger TF-M build so TF-A binaries get signed
+do_deploy[depends]+= "virtual/trusted-firmware-m:do_prepare_recipe_sysroot"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone500.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone500.inc
new file mode 100644
index 0000000..acd9e3d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-corstone500.inc
@@ -0,0 +1,17 @@
+# Corstone-500 specific TFA support
+
+COMPATIBLE_MACHINE = "corstone500"
+TFA_PLATFORM = "a5ds"
+TFA_DEBUG = "1"
+TFA_UBOOT = "1"
+TFA_BUILD_TARGET = "all fip"
+TFA_INSTALL_TARGET = "bl1.bin fip.bin"
+
+EXTRA_OEMAKE:append = " \
+                    ARCH=aarch32 \
+                    FVP_HW_CONFIG_DTS=fdts/a5ds.dts \
+                    ARM_ARCH_MAJOR=7 \
+                    AARCH32_SP=sp_min \
+                    ARM_CORTEX_A5=yes \
+                    ARM_XLAT_TABLES_LIB_V1=1 \
+                    "
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp-arm32.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp-arm32.inc
new file mode 100644
index 0000000..fdaadb9
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp-arm32.inc
@@ -0,0 +1,12 @@
+# Armv7-A FVP specific TFA parameters
+
+COMPATIBLE_MACHINE = "fvp-base-arm32"
+TFA_PLATFORM = "fvp"
+TFA_UBOOT = "1"
+TFA_BUILD_TARGET = "dtbs bl1 bl32 fip"
+
+EXTRA_OEMAKE:append = " \
+    ARCH=aarch32 \
+    AARCH32_SP=sp_min \
+    "
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp.inc
new file mode 100644
index 0000000..43340cd
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-fvp.inc
@@ -0,0 +1,12 @@
+# FVP specific TFA parameters
+
+#
+# Armv8-A Base Platform FVP
+#
+
+COMPATIBLE_MACHINE = "fvp-base"
+TFA_PLATFORM = "fvp"
+TFA_DEBUG = "1"
+TFA_MBEDTLS = "1"
+TFA_UBOOT = "1"
+TFA_BUILD_TARGET = "bl1 bl2 bl31 dtbs fip"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-juno.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-juno.inc
new file mode 100644
index 0000000..3ddd8cb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-juno.inc
@@ -0,0 +1,13 @@
+# Juno specific TFA support
+
+COMPATIBLE_MACHINE = "juno"
+TFA_PLATFORM = "juno"
+TFA_DEBUG = "1"
+TFA_MBEDTLS = "1"
+TFA_UBOOT ?= "1"
+TFA_BUILD_TARGET = "bl1 bl2 bl31 dtbs fip"
+
+# Juno needs the System Control Processor Firmware
+DEPENDS += "virtual/control-processor-firmware"
+
+EXTRA_OEMAKE:append = " SCP_BL2=${RECIPE_SYSROOT}/firmware/scp_ramfw.bin"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-n1sdp.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-n1sdp.inc
new file mode 100644
index 0000000..f8a0b8d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-n1sdp.inc
@@ -0,0 +1,24 @@
+# N1SDP specific TFA support
+
+COMPATIBLE_MACHINE = "n1sdp"
+TFA_PLATFORM       = "n1sdp"
+TFA_BUILD_TARGET   = "all fip"
+TFA_INSTALL_TARGET = "bl1 bl2 bl31 n1sdp-multi-chip n1sdp-single-chip n1sdp_fw_config n1sdp_tb_fw_config fip"
+TFA_DEBUG          = "1"
+TFA_MBEDTLS        = "1"
+TFA_UBOOT          = "0"
+TFA_UEFI           = "1"
+
+SRC_URI:append = " file://bl_size.patch"
+
+TFA_ROT_KEY= "plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem"
+
+EXTRA_OEMAKE:append = "\
+                    TRUSTED_BOARD_BOOT=1 \
+                    GENERATE_COT=1 \
+                    CREATE_KEYS=1 \
+                    ENABLE_PIE=0 \
+                    ARM_ROTPK_LOCATION="devel_rsa" \
+                    ROT_KEY="${TFA_ROT_KEY}" \
+                    BL33=${RECIPE_SYSROOT}/firmware/uefi.bin \
+                    "
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-sgi575.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-sgi575.inc
new file mode 100644
index 0000000..20904b3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-sgi575.inc
@@ -0,0 +1,13 @@
+# SGI575 specific TFA support
+
+COMPATIBLE_MACHINE = "sgi575"
+TFA_PLATFORM       = "sgi575"
+TFA_BUILD_TARGET   = "all fip"
+TFA_INSTALL_TARGET = "bl1 fip"
+TFA_DEBUG          = "1"
+TFA_MBEDTLS        = "1"
+TFA_UBOOT          = "0"
+TFA_UEFI           = "1"
+
+EXTRA_OEMAKE += "TRUSTED_BOARD_BOOT=1 GENERATE_COT=1 ARM_ROTPK_LOCATION=devel_rsa \
+                     ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-tc.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-tc.inc
new file mode 100644
index 0000000..8cb53de
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a-tc.inc
@@ -0,0 +1,140 @@
+# TC0 specific TFA configuration
+
+DEPENDS += "scp-firmware util-linux-native gptfdisk-native"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/tc:"
+SRC_URI:append = " \
+    file://0001-plat-tc-Increase-maximum-BL2-size.patch \
+    file://0002-Makefile-add-trusty_sp_fw_config-build-option.patch \
+    file://0003-fix-plat-arm-increase-sp-max-image-size.patch \
+    file://0004-fix-plat-tc-increase-tc_tzc_dram1_size.patch \
+    file://0005-feat-plat-tc-add-spmc-manifest-with-trusty-sp.patch \
+    file://0006-feat-plat-tc-update-dts-with-trusty-compatible-strin.patch \
+    file://0007-fix-plat-tc-disable-smmu.patch \
+    file://generate_metadata.py \
+    "
+
+COMPATIBLE_MACHINE = "(tc?)"
+
+TFA_PLATFORM = "tc"
+TFA_BUILD_TARGET = "all fip"
+TFA_UBOOT = "1"
+TFA_INSTALL_TARGET = "bl1 fip"
+TFA_MBEDTLS = "1"
+TFA_DEBUG = "1"
+
+TFA_SPD = "spmd"
+TFA_SPMD_SPM_AT_SEL2 = "1"
+
+TFA_TARGET_PLATFORM:tc0 = "0"
+TFA_TARGET_PLATFORM:tc1 = "1"
+
+EXTRA_OEMAKE += "TARGET_PLATFORM=${TFA_TARGET_PLATFORM}"
+
+# Set optee as SP. Set spmc manifest and sp layout file to optee
+DEPENDS += "optee-os"
+
+TFA_SP_LAYOUT_FILE = "${RECIPE_SYSROOT}/lib/firmware/sp_layout.json"
+TFA_ARM_SPMC_MANIFEST_DTS = "plat/arm/board/tc/fdts/tc_spmc_optee_sp_manifest.dts"
+
+EXTRA_OEMAKE += "SCP_BL2=${RECIPE_SYSROOT}/firmware/scp_ramfw.bin"
+EXTRA_OEMAKE += "TRUSTED_BOARD_BOOT=1 GENERATE_COT=1 ARM_ROTPK_LOCATION=devel_rsa \
+                     ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem"
+EXTRA_OEMAKE += "PSA_FWU_SUPPORT=1 ARM_GPT_SUPPORT=1"
+
+do_generate_gpt() {
+    gpt_image="${BUILD_DIR}/fip_gpt.bin"
+    fip_bin="${BUILD_DIR}/fip.bin"
+    # the FIP partition type is not standardized, so generate one
+    fip_type_uuid=`uuidgen --sha1 --namespace @dns --name "fip_type_uuid"`
+    # metadata partition type UUID, specified by the document:
+    # Platform Security Firmware Update for the A-profile Arm Architecture
+    # version: 1.0BET0
+    metadata_type_uuid="8a7a84a0-8387-40f6-ab41-a8b9a5a60d23"
+    location_uuid=`uuidgen`
+    FIP_A_uuid=`uuidgen`
+    FIP_B_uuid=`uuidgen`
+
+    # maximum FIP size 4MB. This is the current size of the FIP rounded up to an integer number of MB.
+    fip_max_size=4194304
+    fip_bin_size=$(stat -c %s $fip_bin)
+    if [ $fip_max_size -lt $fip_bin_size ]; then
+        bberror "FIP binary ($fip_bin_size bytes) is larger than the GPT partition ($fip_max_size bytes)"
+    fi
+
+    # maximum metadata size 512B. This is the current size of the metadata rounded up to an integer number of sectors.
+    metadata_max_size=512
+    metadata_file="${BUILD_DIR}/metadata.bin"
+    python3 ${WORKDIR}/generate_metadata.py --metadata_file $metadata_file \
+                                            --img_type_uuids $fip_type_uuid \
+                                            --location_uuids $location_uuid \
+                                            --img_uuids $FIP_A_uuid $FIP_B_uuid
+
+    # create GPT image. The GPT contains 2 FIP partitions: FIP_A and FIP_B, and 2 metadata partitions: FWU-Metadata and Bkup-FWU-Metadata.
+    # the GPT layout is the following:
+    # -----------------------
+    # Protective MBR
+    # -----------------------
+    # Primary GPT Header
+    # -----------------------
+    # FIP_A
+    # -----------------------
+    # FIP_B
+    # -----------------------
+    # FWU-Metadata
+    # -----------------------
+    # Bkup-FWU-Metadata
+    # -----------------------
+    # Secondary GPT Header
+    # -----------------------
+
+    sector_size=512
+    gpt_header_size=33 # valid only for 512-byte sectors
+    num_sectors_fip=`expr $fip_max_size / $sector_size`
+    num_sectors_metadata=`expr $metadata_max_size / $sector_size`
+    start_sector_1=`expr 1 + $gpt_header_size` # size of MBR is 1 sector
+    start_sector_2=`expr $start_sector_1 + $num_sectors_fip`
+    start_sector_3=`expr $start_sector_2 + $num_sectors_fip`
+    start_sector_4=`expr $start_sector_3 + $num_sectors_metadata`
+    num_sectors_gpt=`expr $start_sector_4 + $num_sectors_metadata + $gpt_header_size`
+    gpt_size=`expr $num_sectors_gpt \* $sector_size`
+
+    # create raw image
+    dd if=/dev/zero of=$gpt_image bs=$gpt_size count=1
+
+    # create the GPT layout
+    sgdisk $gpt_image \
+           --set-alignment 1 \
+           --disk-guid $location_uuid \
+           \
+           --new 1:$start_sector_1:+$num_sectors_fip \
+           --change-name 1:FIP_A \
+           --typecode 1:$fip_type_uuid \
+           --partition-guid 1:$FIP_A_uuid \
+           \
+           --new 2:$start_sector_2:+$num_sectors_fip \
+           --change-name 2:FIP_B \
+           --typecode 2:$fip_type_uuid \
+           --partition-guid 2:$FIP_B_uuid \
+           \
+           --new 3:$start_sector_3:+$num_sectors_metadata \
+           --change-name 3:FWU-Metadata \
+           --typecode 3:$metadata_type_uuid \
+           \
+           --new 4:$start_sector_4:+$num_sectors_metadata \
+           --change-name 4:Bkup-FWU-Metadata \
+           --typecode 4:$metadata_type_uuid
+
+    # populate the GPT partitions
+    dd if=$fip_bin of=$gpt_image bs=$sector_size seek=$start_sector_1 count=$num_sectors_fip conv=notrunc
+    dd if=$fip_bin of=$gpt_image bs=$sector_size seek=$start_sector_2 count=$num_sectors_fip conv=notrunc
+    dd if=$metadata_file of=$gpt_image bs=$sector_size seek=$start_sector_3 count=$num_sectors_metadata conv=notrunc
+    dd if=$metadata_file of=$gpt_image bs=$sector_size seek=$start_sector_4 count=$num_sectors_metadata conv=notrunc
+}
+
+addtask do_generate_gpt after do_compile before do_install
+
+do_install:append() {
+    install -m 0644 ${BUILD_DIR}/fip_gpt.bin ${D}/firmware/fip_gpt-tc.bin
+    ln -sf fip_gpt-tc.bin ${D}/firmware/fip_gpt.bin
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a_2.7.%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a_2.7.%.bbappend
new file mode 100644
index 0000000..09ed3f7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-a/trusted-firmware-a_2.7.%.bbappend
@@ -0,0 +1,15 @@
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/:"
+
+# Machine specific TFAs
+
+MACHINE_TFA_REQUIRE ?= ""
+MACHINE_TFA_REQUIRE:corstone500 = "trusted-firmware-a-corstone500.inc"
+MACHINE_TFA_REQUIRE:corstone1000 = "trusted-firmware-a-corstone1000.inc"
+MACHINE_TFA_REQUIRE:fvp-base = "trusted-firmware-a-fvp.inc"
+MACHINE_TFA_REQUIRE:fvp-base-arm32 = "trusted-firmware-a-fvp-arm32.inc"
+MACHINE_TFA_REQUIRE:juno = "trusted-firmware-a-juno.inc"
+MACHINE_TFA_REQUIRE:n1sdp = "trusted-firmware-a-n1sdp.inc"
+MACHINE_TFA_REQUIRE:sgi575 = "trusted-firmware-a-sgi575.inc"
+MACHINE_TFA_REQUIRE:tc = "trusted-firmware-a-tc.inc"
+
+require ${MACHINE_TFA_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-corstone1000.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-corstone1000.inc
new file mode 100644
index 0000000..eb400e5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-corstone1000.inc
@@ -0,0 +1,49 @@
+# Corstone1000 machines specific TFM support
+
+COMPATIBLE_MACHINE = "(corstone1000)"
+
+TFM_PLATFORM = "arm/corstone1000"
+
+TFM_DEBUG = "1"
+
+## Default is the MPS3 board
+TFM_PLATFORM_IS_FVP ?= "FALSE"
+EXTRA_OECMAKE += "-DPLATFORM_IS_FVP=${TFM_PLATFORM_IS_FVP}"
+EXTRA_OECMAKE += "-DCC312_LEGACY_DRIVER_API_ENABLED=OFF"
+
+# libmetal
+LICENSE += "& BSD-3-Clause"
+LIC_FILES_CHKSUM += "file://../libmetal/LICENSE.md;md5=fe0b8a4beea8f0813b606d15a3df3d3c"
+SRC_URI += "git://github.com/OpenAMP/libmetal.git;protocol=https;branch=main;name=libmetal;destsuffix=git/libmetal"
+SRCREV_libmetal = "f252f0e007fbfb8b3a52b1d5901250ddac96baad"
+EXTRA_OECMAKE += "-DLIBMETAL_SRC_PATH=${WORKDIR}/git/libmetal -DLIBMETAL_BIN_PATH=${B}/libmetal-build"
+
+# OpenAMP
+LICENSE += "& BSD-2-Clause & BSD-3-Clause"
+LIC_FILES_CHKSUM += "file://../openamp/LICENSE.md;md5=a8d8cf662ef6bf9936a1e1413585ecbf"
+SRC_URI += "git://github.com/OpenAMP/open-amp.git;protocol=https;branch=main;name=openamp;destsuffix=git/openamp"
+SRCREV_openamp = "347397decaa43372fc4d00f965640ebde042966d"
+EXTRA_OECMAKE += "-DLIBOPENAMP_SRC_PATH=${WORKDIR}/git/openamp -DLIBOPENAMP_BIN_PATH=${B}/libopenamp-build"
+
+DEPENDS += "trusted-firmware-a"
+
+# adding host images signing support
+require trusted-firmware-m-sign-host-images.inc
+
+do_install() {
+  install -D -p -m 0644 ${B}/install/outputs/tfm_s_signed.bin ${D}/firmware/tfm_s_signed.bin
+  install -D -p -m 0644 ${B}/install/outputs/bl2_signed.bin ${D}/firmware/bl2_signed.bin
+  install -D -p -m 0644 ${B}/install/outputs/bl1.bin ${D}/firmware/bl1.bin
+
+  #
+  # Signing TF-A BL2 and the FIP image
+  #
+
+  sign_host_image ${TFA_BL2_BINARY} ${RECIPE_SYSROOT}/firmware ${TFA_BL2_RE_IMAGE_LOAD_ADDRESS} ${TFA_BL2_RE_SIGN_BIN_SIZE}
+
+  fiptool update \
+      --tb-fw ${D}/firmware/signed_${TFA_BL2_BINARY} \
+      ${RECIPE_SYSROOT}/firmware/${TFA_FIP_BINARY}
+
+  sign_host_image ${TFA_FIP_BINARY} ${RECIPE_SYSROOT}/firmware ${TFA_FIP_RE_IMAGE_LOAD_ADDRESS} ${TFA_FIP_RE_SIGN_BIN_SIZE}
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-sign-host-images.inc b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-sign-host-images.inc
new file mode 100644
index 0000000..49af356
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m-sign-host-images.inc
@@ -0,0 +1,50 @@
+# Signing host images using TF-M tools
+
+DEPENDS += "python3-imgtool-native fiptool-native"
+
+#
+# sign_host_image
+#
+# Description:
+#
+# A generic function that signs a host image
+# using MCUBOOT format
+#
+# Arguments:
+#
+# $1 ... host binary to sign
+# $2 ... host binary path
+# $3 ... load address of the given binary
+# $4 ... signed binary size
+#
+# Note: The signed binary is copied to ${D}/firmware
+#
+sign_host_image() {
+
+    host_binary_filename="`basename -s .bin ${1}`"
+    host_binary_layout="${host_binary_filename}_ns"
+
+    cat << EOF > ${B}/${host_binary_layout}
+enum image_attributes {
+    RE_IMAGE_LOAD_ADDRESS = ${3},
+    RE_SIGN_BIN_SIZE = ${4},
+};
+EOF
+
+    host_binary="${2}/`basename ${1}`"
+    host_binary_signed="${D}/firmware/signed_`basename ${1}`"
+
+    ${PYTHON} ${S}/bl2/ext/mcuboot/scripts/wrapper/wrapper.py \
+            -v ${RE_LAYOUT_WRAPPER_VERSION} \
+            --layout ${B}/${host_binary_layout} \
+            -k  ${TFM_SIGN_PRIVATE_KEY} \
+            --public-key-format full \
+            --align 1 \
+            --pad \
+            --pad-header \
+            -H ${RE_IMAGE_OFFSET} \
+            -s auto \
+            ${host_binary} \
+            ${host_binary_signed}
+
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m_1.6.%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m_1.6.%.bbappend
new file mode 100644
index 0000000..da70bc7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/trusted-firmware-m/trusted-firmware-m_1.6.%.bbappend
@@ -0,0 +1,6 @@
+# Machine specific configurations
+
+MACHINE_TFM_REQUIRE ?= ""
+MACHINE_TFM_REQUIRE:corstone1000 = "trusted-firmware-m-corstone1000.inc"
+
+require ${MACHINE_TFM_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0001-cmd-load-add-load-command-for-memory-mapped.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0001-cmd-load-add-load-command-for-memory-mapped.patch
new file mode 100644
index 0000000..d0ae964
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0001-cmd-load-add-load-command-for-memory-mapped.patch
@@ -0,0 +1,185 @@
+From c047b15fba9da36616bc9a346d9fe4d76e7ca4b2 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Thu, 24 Jun 2021 09:25:00 +0100
+Subject: [PATCH 01/27] cmd: load: add load command for memory mapped
+
+cp.b is used a lot as a way to load binaries to memory and execute
+them, however we may need to integrate this with the efi subsystem to
+set it up as a bootdev.
+
+So, introduce a loadm command that will be consistent with the other
+loadX commands and will call the efi API's.
+
+ex: loadm $kernel_addr $kernel_addr_r $kernel_size
+
+with this a kernel with CONFIG_EFI_STUB enabled will be loaded and
+then subsequently booted with bootefi command.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ README                           |  1 +
+ cmd/Kconfig                      |  6 ++++
+ cmd/bootefi.c                    | 12 ++++++++
+ cmd/load.c                       | 48 ++++++++++++++++++++++++++++++++
+ include/efi_loader.h             |  2 ++
+ lib/efi_loader/efi_device_path.c |  9 ++++++
+ 6 files changed, 78 insertions(+)
+
+diff --git a/README b/README
+index f51f392111f9..049bcd108980 100644
+--- a/README
++++ b/README
+@@ -2760,6 +2760,7 @@ rarpboot- boot image via network using RARP/TFTP protocol
+ diskboot- boot from IDE devicebootd   - boot default, i.e., run 'bootcmd'
+ loads	- load S-Record file over serial line
+ loadb	- load binary file over serial line (kermit mode)
++loadm   - load binary blob from source address to destination address
+ md	- memory display
+ mm	- memory modify (auto-incrementing)
+ nm	- memory modify (constant address)
+diff --git a/cmd/Kconfig b/cmd/Kconfig
+index 5e25e45fd288..ff50102a89c7 100644
+--- a/cmd/Kconfig
++++ b/cmd/Kconfig
+@@ -1076,6 +1076,12 @@ config CMD_LOADB
+ 	help
+ 	  Load a binary file over serial line.
+ 
++config CMD_LOADM
++	bool "loadm"
++	default y
++	help
++	  Load a binary over memory mapped.
++
+ config CMD_LOADS
+ 	bool "loads"
+ 	default y
+diff --git a/cmd/bootefi.c b/cmd/bootefi.c
+index 53d9f0e0dcca..8d492ea9e70c 100644
+--- a/cmd/bootefi.c
++++ b/cmd/bootefi.c
+@@ -34,6 +34,18 @@ static struct efi_device_path *bootefi_device_path;
+ static void *image_addr;
+ static size_t image_size;
+ 
++/**
++ * efi_get_image_parameters() - return image parameters
++ *
++ * @img_addr:		address of loaded image in memory
++ * @img_size:		size of loaded image
++ */
++void efi_get_image_parameters(void **img_addr, size_t *img_size)
++{
++	*img_addr = image_addr;
++	*img_size = image_size;
++}
++
+ /**
+  * efi_clear_bootdev() - clear boot device
+  */
+diff --git a/cmd/load.c b/cmd/load.c
+index 7e4a552d90ef..1224a7f85bb3 100644
+--- a/cmd/load.c
++++ b/cmd/load.c
+@@ -1063,6 +1063,44 @@ static ulong load_serial_ymodem(ulong offset, int mode)
+ 
+ #endif
+ 
++#if defined(CONFIG_CMD_LOADM)
++static int do_load_memory_bin(struct cmd_tbl *cmdtp, int flag, int argc,
++			      char *const argv[])
++{
++	ulong	addr, dest, size;
++	void	*src, *dst;
++
++	if (argc != 4)
++		return CMD_RET_USAGE;
++
++	addr = simple_strtoul(argv[1], NULL, 16);
++
++	dest = simple_strtoul(argv[2], NULL, 16);
++
++	size = simple_strtoul(argv[3], NULL, 16);
++
++	if (!size) {
++		printf("loadm: can not load zero bytes\n");
++		return 1;
++	}
++
++	src = map_sysmem(addr, size);
++	dst = map_sysmem(dest, size);
++
++	memcpy(dst, src, size);
++
++	unmap_sysmem(src);
++	unmap_sysmem(dst);
++
++	if (IS_ENABLED(CONFIG_CMD_BOOTEFI))
++		efi_set_bootdev("Mem", "", "", map_sysmem(dest, 0), size);
++
++	printf("loaded bin to memory: size: %lu\n", size);
++
++	return 0;
++}
++#endif
++
+ /* -------------------------------------------------------------------- */
+ 
+ #if defined(CONFIG_CMD_LOADS)
+@@ -1137,3 +1175,13 @@ U_BOOT_CMD(
+ );
+ 
+ #endif	/* CONFIG_CMD_LOADB */
++
++#if defined(CONFIG_CMD_LOADM)
++U_BOOT_CMD(
++	loadm, 4, 0,	do_load_memory_bin,
++	"load binary blob from source address to destination address",
++	"[src_addr] [dst_addr] [size]\n"
++	"     - load a binary blob from one memory location to other"
++	" from src_addr to dst_addr by size bytes"
++);
++#endif /* CONFIG_CMD_LOADM */
+diff --git a/include/efi_loader.h b/include/efi_loader.h
+index af36639ec6a7..126db279dd3e 100644
+--- a/include/efi_loader.h
++++ b/include/efi_loader.h
+@@ -585,6 +585,8 @@ efi_status_t efi_load_pe(struct efi_loaded_image_obj *handle,
+ void efi_save_gd(void);
+ /* Call this to relocate the runtime section to an address space */
+ void efi_runtime_relocate(ulong offset, struct efi_mem_desc *map);
++/* Call this to get image parameters */
++void efi_get_image_parameters(void **img_addr, size_t *img_size);
+ /* Add a new object to the object list. */
+ void efi_add_handle(efi_handle_t obj);
+ /* Create handle */
+diff --git a/lib/efi_loader/efi_device_path.c b/lib/efi_loader/efi_device_path.c
+index 0542aaae16c7..d8dc59b2c95c 100644
+--- a/lib/efi_loader/efi_device_path.c
++++ b/lib/efi_loader/efi_device_path.c
+@@ -1138,6 +1138,8 @@ efi_status_t efi_dp_from_name(const char *dev, const char *devnr,
+ {
+ 	struct blk_desc *desc = NULL;
+ 	struct disk_partition fs_partition;
++	size_t image_size;
++	void *image_addr;
+ 	int part = 0;
+ 	char *filename;
+ 	char *s;
+@@ -1153,6 +1155,13 @@ efi_status_t efi_dp_from_name(const char *dev, const char *devnr,
+ 	} else if (!strcmp(dev, "Uart")) {
+ 		if (device)
+ 			*device = efi_dp_from_uart();
++	} else if (!strcmp(dev, "Mem")) {
++		efi_get_image_parameters(&image_addr, &image_size);
++
++		if (device)
++			*device = efi_dp_from_mem(EFI_RESERVED_MEMORY_TYPE,
++						  (uintptr_t)image_addr,
++						  image_size);
+ 	} else {
+ 		part = blk_get_device_part_str(dev, devnr, &desc, &fs_partition,
+ 					       1);
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0002-arm-add-support-to-corstone1000-platform.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0002-arm-add-support-to-corstone1000-platform.patch
new file mode 100644
index 0000000..e0caaa4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0002-arm-add-support-to-corstone1000-platform.patch
@@ -0,0 +1,677 @@
+From 5cc889db3279ef4944ab64e36db3dbab1bf9ffa5 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Tue, 15 Feb 2022 09:44:10 +0000
+Subject: [PATCH 02/27] arm: add support to corstone1000 platform
+
+Corstone1000 is a platform from arm, which includes pre
+verified Corstone SSE710 sub-system that combines Cortex-A and
+Cortex-M processors [0].
+
+This code adds the support for the Cortex-A35 implementation
+at host side, it contains also the necessary bits to support
+the Corstone 1000 FVP (Fixed Virtual Platform) [1] and also the
+FPGA MPS3 board implementation of this platform. [2]
+
+0: https://documentation-service.arm.com/static/619e02b1f45f0b1fbf3a8f16
+1: https://developer.arm.com/tools-and-software/open-source-software/arm-platforms-software/arm-ecosystem-fvps
+2: https://documentation-service.arm.com/static/61f3f4d7fa8173727a1b71bf
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ arch/arm/Kconfig                         |   8 ++
+ arch/arm/dts/Makefile                    |   3 +
+ arch/arm/dts/corstone1000-fvp.dts        |  23 +++
+ arch/arm/dts/corstone1000-mps3.dts       |  32 +++++
+ arch/arm/dts/corstone1000.dtsi           | 169 +++++++++++++++++++++++
+ board/armltd/corstone1000/Kconfig        |  12 ++
+ board/armltd/corstone1000/MAINTAINERS    |   7 +
+ board/armltd/corstone1000/Makefile       |   7 +
+ board/armltd/corstone1000/corstone1000.c | 121 ++++++++++++++++
+ configs/corstone1000_defconfig           |  80 +++++++++++
+ include/configs/corstone1000.h           |  86 ++++++++++++
+ 11 files changed, 548 insertions(+)
+ create mode 100644 arch/arm/dts/corstone1000-fvp.dts
+ create mode 100644 arch/arm/dts/corstone1000-mps3.dts
+ create mode 100644 arch/arm/dts/corstone1000.dtsi
+ create mode 100644 board/armltd/corstone1000/Kconfig
+ create mode 100644 board/armltd/corstone1000/MAINTAINERS
+ create mode 100644 board/armltd/corstone1000/Makefile
+ create mode 100644 board/armltd/corstone1000/corstone1000.c
+ create mode 100644 configs/corstone1000_defconfig
+ create mode 100644 include/configs/corstone1000.h
+
+diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
+index 4567c183fb84..d64051b533a7 100644
+--- a/arch/arm/Kconfig
++++ b/arch/arm/Kconfig
+@@ -1266,6 +1266,12 @@ config TARGET_VEXPRESS64_JUNO
+ 	select USB
+ 	imply OF_HAS_PRIOR_STAGE
+ 
++config TARGET_CORSTONE1000
++	bool "Support Corstone1000 Platform"
++	select ARM64
++	select PL01X_SERIAL
++	select DM
++
+ config TARGET_TOTAL_COMPUTE
+ 	bool "Support Total Compute Platform"
+ 	select ARM64
+@@ -2198,6 +2204,8 @@ source "arch/arm/mach-nexell/Kconfig"
+ 
+ source "board/armltd/total_compute/Kconfig"
+ 
++source "board/armltd/corstone1000/Kconfig"
++
+ source "board/bosch/shc/Kconfig"
+ source "board/bosch/guardian/Kconfig"
+ source "board/Marvell/octeontx/Kconfig"
+diff --git a/arch/arm/dts/Makefile b/arch/arm/dts/Makefile
+index 644ba961a223..7de25d09c9fe 100644
+--- a/arch/arm/dts/Makefile
++++ b/arch/arm/dts/Makefile
+@@ -1215,6 +1215,9 @@ dtb-$(CONFIG_TARGET_EA_LPC3250DEVKITV2) += lpc3250-ea3250.dtb
+ 
+ dtb-$(CONFIG_ARCH_QEMU) += qemu-arm.dtb qemu-arm64.dtb
+ 
++dtb-$(CONFIG_TARGET_CORSTONE1000) += corstone1000-mps3.dtb \
++				     corstone1000-fvp.dtb
++
+ include $(srctree)/scripts/Makefile.dts
+ 
+ targets += $(dtb-y)
+diff --git a/arch/arm/dts/corstone1000-fvp.dts b/arch/arm/dts/corstone1000-fvp.dts
+new file mode 100644
+index 000000000000..1fcc137a493c
+--- /dev/null
++++ b/arch/arm/dts/corstone1000-fvp.dts
+@@ -0,0 +1,23 @@
++// SPDX-License-Identifier: GPL-2.0 or MIT
++/*
++ * Copyright (c) 2022, Arm Limited. All rights reserved.
++ * Copyright (c) 2022, Linaro Limited. All rights reserved.
++ *
++ */
++
++/dts-v1/;
++
++#include "corstone1000.dtsi"
++
++/ {
++	model = "ARM Corstone1000 FVP (Fixed Virtual Platform)";
++	compatible = "arm,corstone1000-fvp";
++
++	smsc: ethernet@4010000 {
++		compatible = "smsc,lan91c111";
++		reg = <0x40100000 0x10000>;
++		phy-mode = "mii";
++		interrupts = <GIC_SPI 116 IRQ_TYPE_LEVEL_HIGH>;
++		reg-io-width = <2>;
++	};
++};
+diff --git a/arch/arm/dts/corstone1000-mps3.dts b/arch/arm/dts/corstone1000-mps3.dts
+new file mode 100644
+index 000000000000..e3146747c2d9
+--- /dev/null
++++ b/arch/arm/dts/corstone1000-mps3.dts
+@@ -0,0 +1,32 @@
++// SPDX-License-Identifier: GPL-2.0 or MIT
++/*
++ * Copyright (c) 2022, Arm Limited. All rights reserved.
++ * Copyright (c) 2022, Linaro Limited. All rights reserved.
++ *
++ */
++
++/dts-v1/;
++
++#include "corstone1000.dtsi"
++
++/ {
++	model = "ARM Corstone1000 FPGA MPS3 board";
++	compatible = "arm,corstone1000-mps3";
++
++	smsc: ethernet@4010000 {
++		compatible = "smsc,lan9220", "smsc,lan9115";
++		reg = <0x40100000 0x10000>;
++		phy-mode = "mii";
++		interrupts = <GIC_SPI 116 IRQ_TYPE_LEVEL_HIGH>;
++		reg-io-width = <2>;
++		smsc,irq-push-pull;
++	};
++
++	usb_host: usb@40200000 {
++		compatible = "nxp,usb-isp1763";
++		reg = <0x40200000 0x100000>;
++		interrupts = <GIC_SPI 114 IRQ_TYPE_LEVEL_HIGH>;
++		bus-width = <16>;
++		dr_mode = "host";
++	};
++};
+diff --git a/arch/arm/dts/corstone1000.dtsi b/arch/arm/dts/corstone1000.dtsi
+new file mode 100644
+index 000000000000..d0194aa893f2
+--- /dev/null
++++ b/arch/arm/dts/corstone1000.dtsi
+@@ -0,0 +1,169 @@
++// SPDX-License-Identifier: GPL-2.0 or MIT
++/*
++ * Copyright (c) 2022, Arm Limited. All rights reserved.
++ * Copyright (c) 2022, Linaro Limited. All rights reserved.
++ *
++ */
++
++#include <dt-bindings/interrupt-controller/arm-gic.h>
++
++/ {
++	interrupt-parent = <&gic>;
++	#address-cells = <1>;
++	#size-cells = <1>;
++
++	aliases {
++		serial0 = &uart0;
++		serial1 = &uart1;
++	};
++
++	chosen {
++		stdout-path = "serial0:115200n8";
++	};
++
++	cpus {
++		#address-cells = <1>;
++		#size-cells = <0>;
++
++		cpu: cpu@0 {
++			device_type = "cpu";
++			compatible = "arm,cortex-a35";
++			reg = <0>;
++			next-level-cache = <&L2_0>;
++		};
++	};
++
++	memory@88200000 {
++		device_type = "memory";
++		reg = <0x88200000 0x77e00000>;
++	};
++
++	gic: interrupt-controller@1c000000 {
++		compatible = "arm,gic-400";
++		#interrupt-cells = <3>;
++		#address-cells = <0>;
++		interrupt-controller;
++		reg =	<0x1c010000 0x1000>,
++			<0x1c02f000 0x2000>,
++			<0x1c04f000 0x1000>,
++			<0x1c06f000 0x2000>;
++		interrupts = <GIC_PPI 9 (GIC_CPU_MASK_SIMPLE(1) |
++			      IRQ_TYPE_LEVEL_LOW)>;
++	};
++
++	L2_0: l2-cache0 {
++		compatible = "cache";
++		cache-level = <2>;
++		cache-size = <0x80000>;
++		cache-line-size = <64>;
++		cache-sets = <1024>;
++	};
++
++	refclk100mhz: refclk100mhz {
++		compatible = "fixed-clock";
++		#clock-cells = <0>;
++		clock-frequency = <100000000>;
++		clock-output-names = "apb_pclk";
++	};
++
++	smbclk: refclk24mhzx2 {
++		/* Reference 24MHz clock x 2 */
++		compatible = "fixed-clock";
++		#clock-cells = <0>;
++		clock-frequency = <48000000>;
++		clock-output-names = "smclk";
++	};
++
++	timer {
++		compatible = "arm,armv8-timer";
++		interrupts =	<GIC_PPI 13 (GIC_CPU_MASK_SIMPLE(1) |
++				 IRQ_TYPE_LEVEL_LOW)>,
++				<GIC_PPI 14 (GIC_CPU_MASK_SIMPLE(1) |
++				 IRQ_TYPE_LEVEL_LOW)>,
++				<GIC_PPI 11 (GIC_CPU_MASK_SIMPLE(1) |
++				 IRQ_TYPE_LEVEL_LOW)>,
++				<GIC_PPI 10 (GIC_CPU_MASK_SIMPLE(1) |
++				 IRQ_TYPE_LEVEL_LOW)>;
++	};
++
++	uartclk: uartclk {
++		/* UART clock - 50MHz */
++		compatible = "fixed-clock";
++		#clock-cells = <0>;
++		clock-frequency = <50000000>;
++		clock-output-names = "uartclk";
++	};
++
++	psci {
++		compatible = "arm,psci-1.0", "arm,psci-0.2";
++		method = "smc";
++	};
++
++	soc {
++		compatible = "simple-bus";
++		#address-cells = <1>;
++		#size-cells = <1>;
++		interrupt-parent = <&gic>;
++		ranges;
++
++		timer@1a220000 {
++			compatible = "arm,armv7-timer-mem";
++			reg = <0x1a220000 0x1000>;
++			#address-cells = <1>;
++			#size-cells = <1>;
++			clock-frequency = <50000000>;
++			ranges;
++
++			frame@1a230000 {
++				frame-number = <0>;
++				interrupts = <GIC_SPI 2 IRQ_TYPE_LEVEL_HIGH>;
++				reg = <0x1a230000 0x1000>;
++			};
++		};
++
++		uart0: serial@1a510000 {
++			compatible = "arm,pl011", "arm,primecell";
++			reg = <0x1a510000 0x1000>;
++			interrupts = <GIC_SPI 19 IRQ_TYPE_LEVEL_HIGH>;
++			clocks = <&uartclk>, <&refclk100mhz>;
++			clock-names = "uartclk", "apb_pclk";
++		};
++
++		uart1: serial@1a520000 {
++			compatible = "arm,pl011", "arm,primecell";
++			reg = <0x1a520000 0x1000>;
++			interrupts = <GIC_SPI 20 IRQ_TYPE_LEVEL_HIGH>;
++			clocks = <&uartclk>, <&refclk100mhz>;
++			clock-names = "uartclk", "apb_pclk";
++		};
++
++		mhu_hse1: mailbox@1b820000 {
++			compatible = "arm,mhuv2-tx", "arm,primecell";
++			reg = <0x1b820000 0x1000>;
++			clocks = <&refclk100mhz>;
++			clock-names = "apb_pclk";
++			interrupts = <GIC_SPI 45 IRQ_TYPE_LEVEL_HIGH>;
++			#mbox-cells = <2>;
++			arm,mhuv2-protocols = <0 0>;
++			secure-status = "okay";     /* secure-world-only */
++			status = "disabled";
++		};
++
++		mhu_seh1: mailbox@1b830000 {
++			compatible = "arm,mhuv2-rx", "arm,primecell";
++			reg = <0x1b830000 0x1000>;
++			clocks = <&refclk100mhz>;
++			clock-names = "apb_pclk";
++			interrupts = <GIC_SPI 46 IRQ_TYPE_LEVEL_HIGH>;
++			#mbox-cells = <2>;
++			arm,mhuv2-protocols = <0 0>;
++			secure-status = "okay";     /* secure-world-only */
++			status = "disabled";
++		};
++	};
++
++	arm_ffa: arm_ffa {
++		compatible = "arm,ffa";
++		method = "smc";
++	};
++};
+diff --git a/board/armltd/corstone1000/Kconfig b/board/armltd/corstone1000/Kconfig
+new file mode 100644
+index 000000000000..709674d4cf7d
+--- /dev/null
++++ b/board/armltd/corstone1000/Kconfig
+@@ -0,0 +1,12 @@
++if TARGET_CORSTONE1000
++
++config SYS_BOARD
++	default "corstone1000"
++
++config SYS_VENDOR
++	default "armltd"
++
++config SYS_CONFIG_NAME
++	default "corstone1000"
++
++endif
+diff --git a/board/armltd/corstone1000/MAINTAINERS b/board/armltd/corstone1000/MAINTAINERS
+new file mode 100644
+index 000000000000..8c905686de76
+--- /dev/null
++++ b/board/armltd/corstone1000/MAINTAINERS
+@@ -0,0 +1,7 @@
++CORSTONE1000 BOARD
++M:	Rui Miguel Silva <rui.silva@linaro.org>
++M:	Vishnu Banavath <vishnu.banavath@arm.com>
++S:	Maintained
++F:	board/armltd/corstone1000/
++F:	include/configs/corstone1000.h
++F:	configs/corstone1000_defconfig
+diff --git a/board/armltd/corstone1000/Makefile b/board/armltd/corstone1000/Makefile
+new file mode 100644
+index 000000000000..77a82c28929b
+--- /dev/null
++++ b/board/armltd/corstone1000/Makefile
+@@ -0,0 +1,7 @@
++# SPDX-License-Identifier: GPL-2.0+
++#
++# (C) Copyright 2022 Arm Limited
++# (C) Copyright 2022 Linaro
++# Rui Miguel Silva <rui.silva@linaro.org>
++
++obj-y	:= corstone1000.o
+diff --git a/board/armltd/corstone1000/corstone1000.c b/board/armltd/corstone1000/corstone1000.c
+new file mode 100644
+index 000000000000..eff1739f0b02
+--- /dev/null
++++ b/board/armltd/corstone1000/corstone1000.c
+@@ -0,0 +1,121 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2022 ARM Limited
++ * (C) Copyright 2022 Linaro
++ * Rui Miguel Silva <rui.silva@linaro.org>
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <dm/platform_data/serial_pl01x.h>
++#include <asm/armv8/mmu.h>
++#include <asm/global_data.h>
++
++
++static const struct pl01x_serial_plat serial_plat = {
++	.base = V2M_UART0,
++	.type = TYPE_PL011,
++	.clock = CONFIG_PL011_CLOCK,
++};
++
++U_BOOT_DRVINFO(corstone1000_serials) = {
++	.name = "serial_pl01x",
++	.plat = &serial_plat,
++};
++
++static struct mm_region corstone1000_mem_map[] = {
++	{
++		/* CVM */
++		.virt = 0x02000000UL,
++		.phys = 0x02000000UL,
++		.size = 0x02000000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
++			 PTE_BLOCK_INNER_SHARE
++	}, {
++		/* QSPI */
++		.virt = 0x08000000UL,
++		.phys = 0x08000000UL,
++		.size = 0x08000000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
++			 PTE_BLOCK_INNER_SHARE
++	}, {
++		/* Host Peripherals */
++		.virt = 0x1A000000UL,
++		.phys = 0x1A000000UL,
++		.size = 0x26000000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++			 PTE_BLOCK_NON_SHARE |
++			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
++        }, {
++                /* USB */
++                .virt = 0x40200000UL,
++                .phys = 0x40200000UL,
++                .size = 0x00100000UL,
++                .attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++                         PTE_BLOCK_NON_SHARE |
++                         PTE_BLOCK_PXN | PTE_BLOCK_UXN
++	}, {
++                 /* ethernet */
++                .virt = 0x40100000UL,
++                .phys = 0x40100000UL,
++                .size = 0x00100000UL,
++                .attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++                         PTE_BLOCK_NON_SHARE |
++                         PTE_BLOCK_PXN | PTE_BLOCK_UXN
++	}, {
++		/* OCVM */
++		.virt = 0x80000000UL,
++		.phys = 0x80000000UL,
++		.size = 0x80000000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
++			 PTE_BLOCK_INNER_SHARE
++	}, {
++		/* List terminator */
++		0,
++	}
++};
++
++struct mm_region *mem_map = corstone1000_mem_map;
++
++int board_init(void)
++{
++	return 0;
++}
++
++int dram_init(void)
++{
++	gd->ram_size = PHYS_SDRAM_1_SIZE;
++
++	return 0;
++}
++
++int dram_init_banksize(void)
++{
++	gd->bd->bi_dram[0].start = PHYS_SDRAM_1;
++	gd->bd->bi_dram[0].size = PHYS_SDRAM_1_SIZE;
++
++	return 0;
++}
++
++/*
++ * Board specific ethernet initialization routine.
++ * */
++int board_eth_init(struct bd_info *bis)
++{
++	int rc = 0;
++
++#ifndef CONFIG_DM_ETH
++#ifdef CONFIG_SMC91111
++	rc = smc91111_initialize(0, CONFIG_SMC91111_BASE);
++#endif
++#ifdef CONFIG_SMC911X
++	rc = smc911x_initialize(0, CONFIG_SMC911X_BASE);
++#endif
++#endif
++
++	return rc;
++}
++
++void reset_cpu(ulong addr)
++{
++}
+diff --git a/configs/corstone1000_defconfig b/configs/corstone1000_defconfig
+new file mode 100644
+index 000000000000..02f931b0d469
+--- /dev/null
++++ b/configs/corstone1000_defconfig
+@@ -0,0 +1,80 @@
++CONFIG_ARM=y
++CONFIG_TARGET_CORSTONE1000=y
++CONFIG_SYS_TEXT_BASE=0x80000000
++CONFIG_SYS_MALLOC_F_LEN=0x2000
++CONFIG_SYS_MALLOC_LEN=0x2000000
++CONFIG_SYS_LOAD_ADDR=0x82100000
++CONFIG_NR_DRAM_BANKS=1
++CONFIG_IDENT_STRING=" corstone1000 aarch64 "
++CONFIG_FIT=y
++CONFIG_BOOTDELAY=3
++CONFIG_USE_BOOTARGS=y
++CONFIG_BOOTARGS="console=ttyAMA0 loglevel=9 ip=dhcp earlyprintk"
++CONFIG_LOGLEVEL=7
++# CONFIG_DISPLAY_CPUINFO is not set
++# CONFIG_DISPLAY_BOARDINFO is not set
++CONFIG_HUSH_PARSER=y
++CONFIG_SYS_PROMPT="corstone1000# "
++# CONFIG_CMD_CONSOLE is not set
++CONFIG_CMD_BOOTZ=y
++CONFIG_CMD_BOOTM=y
++CONFIG_CMD_LOADM=y
++CONFIG_CMD_BOOTEFI=y
++CONFIG_EFI_LOADER=y
++CONFIG_EFI_PARTITION=y
++CONFIG_CMD_BOOTEFI_HELLO_COMPILE=y
++CONFIG_CMD_BOOTEFI_HELLO=y
++# CONFIG_CMD_XIMG is not set
++# CONFIG_CMD_ENV_EXISTS is not set
++CONFIG_CMD_NVEDIT_EFI=y
++# CONFIG_CMD_LOADS is not set
++CONFIG_CMD_USB=y
++CONFIG_CMD_ITEST=y
++# CONFIG_CMD_SETEXPR is not set
++# CONFIG_CMD_NFS is not set
++CONFIG_CMD_MII=y
++CONFIG_CMD_CACHE=y
++CONFIG_CMD_EFIDEBUG=y
++CONFIG_CMD_FAT=y
++CONFIG_OF_CONTROL=y
++CONFIG_REGMAP=y
++# CONFIG_MMC is not set
++CONFIG_DM_SERIAL=y
++CONFIG_USB=y
++CONFIG_DM_USB=y
++CONFIG_USB_STORAGE=y
++CONFIG_EFI_MM_COMM_TEE=y
++# CONFIG_OPTEE is not set
++# CONFIG_GENERATE_SMBIOS_TABLE is not set
++# CONFIG_HEXDUMP is not set
++CONFIG_EFI_RUNTIME_UPDATE_CAPSULE=y
++CONFIG_EFI_CAPSULE_ON_DISK=y
++# CONFIG_EFI_CAPSULE_ON_DISK_EARLY is not set
++# CONFIG_EFI_CAPSULE_AUTHENTICATE is not set
++CONFIG_EFI_HAVE_CAPSULE_SUPPORT=y
++CONFIG_EFI_CAPSULE_FIRMWARE_FIT=y
++CONFIG_EFI_CAPSULE_FIRMWARE_RAW=y
++CONFIG_EFI_SECURE_BOOT=y
++CONFIG_DM_RTC=y
++CONFIG_CMD_RTC=y
++CONFIG_EFI_GET_TIME=y
++CONFIG_EFI_SET_TIME=y
++CONFIG_RTC_EMULATION=y
++CONFIG_PSCI_RESET=y
++CONFIG_DISTRO_DEFAULTS=y
++CONFIG_CMD_DHCP=y
++CONFIG_SMC911X=y
++CONFIG_SMC911X_BASE=0x40100000
++CONFIG_DM_ETH=y
++CONFIG_PHY_SMSC=y
++CONFIG_CMD_BOOTEFI_SELFTEST=y
++CONFIG_CMD_TIME=y
++CONFIG_CMD_GETTIME=y
++CONFIG_NET_RANDOM_ETHADDR=y
++CONFIG_VERSION_VARIABLE=y
++CONFIG_PHYLIB=y
++CONFIG_PHY=y
++CONFIG_RAM=y
++CONFIG_ERRNO_STR=y
++CONFIG_CMD_EDITENV=y
++CONFIG_MISC=y
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+new file mode 100644
+index 000000000000..cf166f107efd
+--- /dev/null
++++ b/include/configs/corstone1000.h
+@@ -0,0 +1,86 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2022 ARM Limited
++ * (C) Copyright 2022 Linaro
++ * Rui Miguel Silva <rui.silva@linaro.org>
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ *
++ * Configuration for Corstone1000. Parts were derived from other ARM
++ * configurations.
++ */
++
++#ifndef __CORSTONE1000_H
++#define __CORSTONE1000_H
++
++#include <linux/sizes.h>
++
++#define CONFIG_SYS_INIT_SP_ADDR		(CONFIG_SYS_SDRAM_BASE + 0x03f00000)
++#define CONFIG_SKIP_LOWLEVEL_INIT
++
++#define CONFIG_SYS_HZ		1000
++
++#define V2M_SRAM0		0x02000000
++#define V2M_QSPI		0x08000000
++
++#define V2M_DEBUG		0x10000000
++#define V2M_BASE_PERIPH		0x1A000000
++
++#define V2M_BASE		0x80000000
++
++#define V2M_PERIPH_OFFSET(x)	(x << 16)
++
++#define V2M_SYSID		(V2M_BASE_PERIPH)
++#define V2M_SYSCTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(1))
++
++#define V2M_COUNTER_CTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(32))
++#define V2M_COUNTER_READ	(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(33))
++
++#define V2M_TIMER_CTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(34))
++#define V2M_TIMER_BASE0		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(35))
++
++#define V2M_UART0		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(81))
++#define V2M_UART1		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(82))
++
++#define CONFIG_PL011_CLOCK	50000000
++
++/* Physical Memory Map */
++#define PHYS_SDRAM_1		(V2M_BASE)
++#define PHYS_SDRAM_1_SIZE	0x80000000
++
++#define CONFIG_ENV_SECT_SIZE	SZ_64K
++
++#define CONFIG_SYS_SDRAM_BASE	PHYS_SDRAM_1
++
++/* Monitor Command Prompt */
++#define CONFIG_SYS_CBSIZE	512	/* Console I/O Buffer Size */
++#define CONFIG_SYS_MAXARGS	64	/* max command args */
++
++#define CONFIG_EXTRA_ENV_SETTINGS							\
++				"usb_pgood_delay=250\0"					\
++				"boot_bank_flag=0x08002000\0"				\
++				"kernel_addr_bank_0=0x083EE000\0"			\
++				"kernel_addr_bank_1=0x0936E000\0"			\
++				"retrieve_kernel_load_addr="				\
++					"if itest.l *${boot_bank_flag} == 0; then "	\
++					    "setenv kernel_addr $kernel_addr_bank_0;"	\
++					"else "						\
++					    "setenv kernel_addr $kernel_addr_bank_1;"	\
++					"fi;"						\
++					"\0"						\
++				"kernel_addr_r=0x88200000\0"				\
++				"fdt_high=0xffffffff\0"
++
++/*
++ * config_distro_bootcmd define the boot command to distro_bootcmd, but we here
++ * want to first try to load a kernel if exists, override that config then
++ */
++#undef CONFIG_BOOTCOMMAND
++
++#define CONFIG_BOOTCOMMAND								\
++				"run retrieve_kernel_load_addr;"			\
++				"echo Loading kernel from $kernel_addr to memory ... ;"	\
++				"loadm $kernel_addr $kernel_addr_r 0xc00000;"		\
++				"usb start; usb reset;"					\
++				"run distro_bootcmd;"					\
++				"bootefi $kernel_addr_r $fdtcontroladdr;"
++#endif
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0003-usb-common-move-urb-code-to-common.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0003-usb-common-move-urb-code-to-common.patch
new file mode 100644
index 0000000..9974915
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0003-usb-common-move-urb-code-to-common.patch
@@ -0,0 +1,497 @@
+From b8f4f76e135ff2061d0032292f5209ac911dba16 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Mon, 28 Jun 2021 23:20:55 +0100
+Subject: [PATCH 03/27] usb: common: move urb code to common
+
+Move urb code from musb only use to a more common scope, so other
+drivers in the future can use the handling of urb in usb.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ drivers/usb/common/Makefile                   |   2 +
+ drivers/usb/common/usb_urb.c                  | 160 ++++++++++++++++++
+ drivers/usb/host/r8a66597-hcd.c               |  30 +---
+ drivers/usb/musb-new/musb_core.c              |   2 +-
+ drivers/usb/musb-new/musb_host.c              |   2 +-
+ drivers/usb/musb-new/musb_host.h              |   2 +-
+ drivers/usb/musb-new/musb_uboot.c             |  38 +----
+ drivers/usb/musb-new/musb_uboot.h             |   2 +-
+ .../linux/usb/usb_urb_compat.h                |  46 ++++-
+ include/usb_defs.h                            |  32 ++++
+ 10 files changed, 240 insertions(+), 76 deletions(-)
+ create mode 100644 drivers/usb/common/usb_urb.c
+ rename drivers/usb/musb-new/usb-compat.h => include/linux/usb/usb_urb_compat.h (60%)
+
+diff --git a/drivers/usb/common/Makefile b/drivers/usb/common/Makefile
+index 3bedbf213f47..dc05cb0a5077 100644
+--- a/drivers/usb/common/Makefile
++++ b/drivers/usb/common/Makefile
+@@ -4,5 +4,7 @@
+ #
+ 
+ obj-$(CONFIG_$(SPL_)DM_USB) += common.o
++obj-$(CONFIG_USB_MUSB_HCD) += usb_urb.o
++obj-$(CONFIG_USB_MUSB_UDC) += usb_urb.o
+ obj-$(CONFIG_USB_EHCI_FSL) += fsl-dt-fixup.o fsl-errata.o
+ obj-$(CONFIG_USB_XHCI_FSL) += fsl-dt-fixup.o fsl-errata.o
+diff --git a/drivers/usb/common/usb_urb.c b/drivers/usb/common/usb_urb.c
+new file mode 100644
+index 000000000000..be3b6b9f32e8
+--- /dev/null
++++ b/drivers/usb/common/usb_urb.c
+@@ -0,0 +1,160 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Common code for usb urb handling, based on the musb-new code
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ */
++
++#include <dm/device.h>
++#include <dm/device_compat.h>
++#include <linux/usb/usb_urb_compat.h>
++
++#include <time.h>
++#include <usb.h>
++
++#if CONFIG_IS_ENABLED(DM_USB)
++struct usb_device *usb_dev_get_parent(struct usb_device *udev)
++{
++	struct udevice *parent = udev->dev->parent;
++
++	/*
++	 * When called from usb-uclass.c: usb_scan_device() udev->dev points
++	 * to the parent udevice, not the actual udevice belonging to the
++	 * udev as the device is not instantiated yet.
++	 *
++	 * If dev is an usb-bus, then we are called from usb_scan_device() for
++	 * an usb-device plugged directly into the root port, return NULL.
++	 */
++	if (device_get_uclass_id(udev->dev) == UCLASS_USB)
++		return NULL;
++
++	/*
++	 * If these 2 are not the same we are being called from
++	 * usb_scan_device() and udev itself is the parent.
++	 */
++	if (dev_get_parent_priv(udev->dev) != udev)
++		return udev;
++
++	/* We are being called normally, use the parent pointer */
++	if (device_get_uclass_id(parent) == UCLASS_USB_HUB)
++		return dev_get_parent_priv(parent);
++
++	return NULL;
++}
++#else
++struct usb_device *usb_dev_get_parent(struct usb_device *udev)
++{
++	return udev->parent;
++}
++#endif
++
++static void usb_urb_complete(struct urb *urb)
++{
++	urb->dev->status &= ~USB_ST_NOT_PROC;
++	urb->dev->act_len = urb->actual_length;
++
++	if (urb->status == -EINPROGRESS)
++		urb->status = 0;
++}
++
++void usb_urb_fill(struct urb *urb, struct usb_host_endpoint *hep,
++		  struct usb_device *dev, int endpoint_type,
++		  unsigned long pipe, void *buffer, int len,
++		  struct devrequest *setup, int interval)
++{
++	int epnum = usb_pipeendpoint(pipe);
++	int is_in = usb_pipein(pipe);
++	u16 maxpacketsize = is_in ? dev->epmaxpacketin[epnum] :
++					dev->epmaxpacketout[epnum];
++
++	memset(urb, 0, sizeof(struct urb));
++	memset(hep, 0, sizeof(struct usb_host_endpoint));
++	INIT_LIST_HEAD(&hep->urb_list);
++	INIT_LIST_HEAD(&urb->urb_list);
++	urb->ep = hep;
++	urb->complete = usb_urb_complete;
++	urb->status = -EINPROGRESS;
++	urb->dev = dev;
++	urb->pipe = pipe;
++	urb->transfer_buffer = buffer;
++	urb->transfer_dma = (unsigned long)buffer;
++	urb->transfer_buffer_length = len;
++	urb->setup_packet = (unsigned char *)setup;
++
++	urb->ep->desc.wMaxPacketSize = __cpu_to_le16(maxpacketsize);
++	urb->ep->desc.bmAttributes = endpoint_type;
++	urb->ep->desc.bEndpointAddress = ((is_in ? USB_DIR_IN : USB_DIR_OUT) |
++					  epnum);
++	urb->ep->desc.bInterval = interval;
++}
++
++int usb_urb_submit(struct usb_hcd *hcd, struct urb *urb)
++{
++	const struct usb_urb_ops *ops = hcd->urb_ops;
++	unsigned long timeout;
++	int ret;
++
++	if (!ops)
++		return -EINVAL;
++
++	ret = ops->urb_enqueue(hcd, urb, 0);
++	if (ret < 0) {
++		printf("Failed to enqueue URB to controller\n");
++		return ret;
++	}
++
++	timeout = get_timer(0) + USB_TIMEOUT_MS(urb->pipe);
++	do {
++		if (ctrlc())
++			return -EIO;
++		ops->isr(0, hcd);
++	} while (urb->status == -EINPROGRESS && get_timer(0) < timeout);
++
++	if (urb->status == -EINPROGRESS)
++		ops->urb_dequeue(hcd, urb, -ETIME);
++
++	return urb->status;
++}
++
++int usb_urb_submit_control(struct usb_hcd *hcd, struct urb *urb,
++			   struct usb_host_endpoint *hep,
++			   struct usb_device *dev, unsigned long pipe,
++			   void *buffer, int len, struct devrequest *setup,
++			   int interval, enum usb_device_speed speed)
++{
++	const struct usb_urb_ops *ops = hcd->urb_ops;
++
++	usb_urb_fill(urb, hep, dev, USB_ENDPOINT_XFER_CONTROL, pipe, buffer,
++		     len, setup, 0);
++
++	/* Fix speed for non hub-attached devices */
++	if (!usb_dev_get_parent(dev)) {
++		dev->speed = speed;
++		if (ops->hub_control)
++			return ops->hub_control(hcd, dev, pipe, buffer, len,
++						setup);
++	}
++
++	return usb_urb_submit(hcd, urb);
++}
++
++int usb_urb_submit_bulk(struct usb_hcd *hcd, struct urb *urb,
++			struct usb_host_endpoint *hep, struct usb_device *dev,
++			unsigned long pipe, void *buffer, int len)
++{
++	usb_urb_fill(urb, hep, dev, USB_ENDPOINT_XFER_BULK, pipe, buffer, len,
++		     NULL, 0);
++
++	return usb_urb_submit(hcd, urb);
++}
++
++int usb_urb_submit_irq(struct usb_hcd *hcd, struct urb *urb,
++		       struct usb_host_endpoint *hep, struct usb_device *dev,
++		       unsigned long pipe, void *buffer, int len, int interval)
++{
++	usb_urb_fill(urb, hep, dev, USB_ENDPOINT_XFER_INT, pipe, buffer, len,
++		     NULL, interval);
++
++	return usb_urb_submit(hcd, urb);
++}
+diff --git a/drivers/usb/host/r8a66597-hcd.c b/drivers/usb/host/r8a66597-hcd.c
+index f1fc93f3d403..3ccbc16da379 100644
+--- a/drivers/usb/host/r8a66597-hcd.c
++++ b/drivers/usb/host/r8a66597-hcd.c
+@@ -14,6 +14,7 @@
+ #include <dm/device_compat.h>
+ #include <linux/delay.h>
+ #include <linux/iopoll.h>
++#include <linux/usb/usb_urb_compat.h>
+ #include <power/regulator.h>
+ 
+ #include "r8a66597.h"
+@@ -24,35 +25,6 @@
+ #define R8A66597_DPRINT(...)
+ #endif
+ 
+-static inline struct usb_device *usb_dev_get_parent(struct usb_device *udev)
+-{
+-	struct udevice *parent = udev->dev->parent;
+-
+-	/*
+-	 * When called from usb-uclass.c: usb_scan_device() udev->dev points
+-	 * to the parent udevice, not the actual udevice belonging to the
+-	 * udev as the device is not instantiated yet.
+-	 *
+-	 * If dev is an usb-bus, then we are called from usb_scan_device() for
+-	 * an usb-device plugged directly into the root port, return NULL.
+-	 */
+-	if (device_get_uclass_id(udev->dev) == UCLASS_USB)
+-		return NULL;
+-
+-	/*
+-	 * If these 2 are not the same we are being called from
+-	 * usb_scan_device() and udev itself is the parent.
+-	 */
+-	if (dev_get_parent_priv(udev->dev) != udev)
+-		return udev;
+-
+-	/* We are being called normally, use the parent pointer */
+-	if (device_get_uclass_id(parent) == UCLASS_USB_HUB)
+-		return dev_get_parent_priv(parent);
+-
+-	return NULL;
+-}
+-
+ static void get_hub_data(struct usb_device *dev, u16 *hub_devnum, u16 *hubport)
+ {
+ 	struct usb_device *parent = usb_dev_get_parent(dev);
+diff --git a/drivers/usb/musb-new/musb_core.c b/drivers/usb/musb-new/musb_core.c
+index 18d9bc805f8a..fc7af7484e4c 100644
+--- a/drivers/usb/musb-new/musb_core.c
++++ b/drivers/usb/musb-new/musb_core.c
+@@ -89,9 +89,9 @@
+ #include <linux/usb/ch9.h>
+ #include <linux/usb/gadget.h>
+ #include <linux/usb/musb.h>
++#include <linux/usb/usb_urb_compat.h>
+ #include <asm/io.h>
+ #include "linux-compat.h"
+-#include "usb-compat.h"
+ #endif
+ 
+ #include "musb_core.h"
+diff --git a/drivers/usb/musb-new/musb_host.c b/drivers/usb/musb-new/musb_host.c
+index acb2d40f3b5a..e5905d90d66f 100644
+--- a/drivers/usb/musb-new/musb_host.c
++++ b/drivers/usb/musb-new/musb_host.c
+@@ -26,8 +26,8 @@
+ #include <dm/device_compat.h>
+ #include <usb.h>
+ #include <linux/bug.h>
++#include <linux/usb/usb_urb_compat.h>
+ #include "linux-compat.h"
+-#include "usb-compat.h"
+ #endif
+ 
+ #include "musb_core.h"
+diff --git a/drivers/usb/musb-new/musb_host.h b/drivers/usb/musb-new/musb_host.h
+index afc8fa35a738..5a604bdb0cf2 100644
+--- a/drivers/usb/musb-new/musb_host.h
++++ b/drivers/usb/musb-new/musb_host.h
+@@ -10,7 +10,7 @@
+ #ifndef _MUSB_HOST_H
+ #define _MUSB_HOST_H
+ #ifdef __UBOOT__
+-#include "usb-compat.h"
++#include <linux/usb/usb_urb_compat.h>
+ #endif
+ 
+ static inline struct usb_hcd *musb_to_hcd(struct musb *musb)
+diff --git a/drivers/usb/musb-new/musb_uboot.c b/drivers/usb/musb-new/musb_uboot.c
+index 61ff68def2fa..d186facc7e02 100644
+--- a/drivers/usb/musb-new/musb_uboot.c
++++ b/drivers/usb/musb-new/musb_uboot.c
+@@ -8,10 +8,10 @@
+ #include <linux/errno.h>
+ #include <linux/usb/ch9.h>
+ #include <linux/usb/gadget.h>
++#include <linux/usb/usb_urb_compat.h>
+ 
+ #include <usb.h>
+ #include "linux-compat.h"
+-#include "usb-compat.h"
+ #include "musb_core.h"
+ #include "musb_host.h"
+ #include "musb_gadget.h"
+@@ -453,39 +453,3 @@ struct musb *musb_register(struct musb_hdrc_platform_data *plat, void *bdata,
+ 
+ 	return *musbp;
+ }
+-
+-#if CONFIG_IS_ENABLED(DM_USB)
+-struct usb_device *usb_dev_get_parent(struct usb_device *udev)
+-{
+-	struct udevice *parent = udev->dev->parent;
+-
+-	/*
+-	 * When called from usb-uclass.c: usb_scan_device() udev->dev points
+-	 * to the parent udevice, not the actual udevice belonging to the
+-	 * udev as the device is not instantiated yet.
+-	 *
+-	 * If dev is an usb-bus, then we are called from usb_scan_device() for
+-	 * an usb-device plugged directly into the root port, return NULL.
+-	 */
+-	if (device_get_uclass_id(udev->dev) == UCLASS_USB)
+-		return NULL;
+-
+-	/*
+-	 * If these 2 are not the same we are being called from
+-	 * usb_scan_device() and udev itself is the parent.
+-	 */
+-	if (dev_get_parent_priv(udev->dev) != udev)
+-		return udev;
+-
+-	/* We are being called normally, use the parent pointer */
+-	if (device_get_uclass_id(parent) == UCLASS_USB_HUB)
+-		return dev_get_parent_priv(parent);
+-
+-	return NULL;
+-}
+-#else
+-struct usb_device *usb_dev_get_parent(struct usb_device *udev)
+-{
+-	return udev->parent;
+-}
+-#endif
+diff --git a/drivers/usb/musb-new/musb_uboot.h b/drivers/usb/musb-new/musb_uboot.h
+index 18282efccc9d..6b162f03b19e 100644
+--- a/drivers/usb/musb-new/musb_uboot.h
++++ b/drivers/usb/musb-new/musb_uboot.h
+@@ -8,8 +8,8 @@
+ #define __MUSB_UBOOT_H__
+ 
+ #include <usb.h>
++#include <linux/usb/usb_urb_compat.h>
+ #include "linux-compat.h"
+-#include "usb-compat.h"
+ #include "musb_core.h"
+ 
+ struct musb_host_data {
+diff --git a/drivers/usb/musb-new/usb-compat.h b/include/linux/usb/usb_urb_compat.h
+similarity index 60%
+rename from drivers/usb/musb-new/usb-compat.h
+rename to include/linux/usb/usb_urb_compat.h
+index df68c9220a7a..5ed96fa64e96 100644
+--- a/drivers/usb/musb-new/usb-compat.h
++++ b/include/linux/usb/usb_urb_compat.h
+@@ -1,16 +1,31 @@
+-#ifndef __USB_COMPAT_H__
+-#define __USB_COMPAT_H__
++#ifndef __USB_URB_COMPAT_H__
++#define __USB_URB_COMPAT_H__
+ 
+-#include "usb.h"
++#include <linux/compat.h>
++#include <usb.h>
+ 
+ struct udevice;
++struct urb;
++struct usb_hcd;
++
++
++struct usb_urb_ops {
++	int (*urb_enqueue)(struct usb_hcd *hcd, struct urb *urb,
++			   gfp_t mem_flags);
++	int (*urb_dequeue)(struct usb_hcd *hcd, struct urb *urb, int status);
++	int (*hub_control)(struct usb_hcd *hcd, struct usb_device *dev,
++			   unsigned long pipe, void *buffer, int len,
++			   struct devrequest *setup);
++	irqreturn_t (*isr)(int irq, void *priv);
++};
+ 
+ struct usb_hcd {
+ 	void *hcd_priv;
++	const struct usb_urb_ops *urb_ops;
+ };
+ 
+ struct usb_host_endpoint {
+-	struct usb_endpoint_descriptor		desc;
++	struct usb_endpoint_descriptor desc;
+ 	struct list_head urb_list;
+ 	void *hcpriv;
+ };
+@@ -23,8 +38,6 @@ struct usb_host_endpoint {
+ #define URB_SHORT_NOT_OK	0x0001	/* report short reads as errors */
+ #define URB_ZERO_PACKET		0x0040	/* Finish bulk OUT with short packet */
+ 
+-struct urb;
+-
+ typedef void (*usb_complete_t)(struct urb *);
+ 
+ struct urb {
+@@ -76,4 +89,25 @@ static inline int usb_hcd_unmap_urb_for_dma(struct usb_hcd *hcd,
+  */
+ struct usb_device *usb_dev_get_parent(struct usb_device *udev);
+ 
++int usb_urb_submit_control(struct usb_hcd *hcd, struct urb *urb,
++			   struct usb_host_endpoint *hep,
++			   struct usb_device *dev, unsigned long pipe,
++			   void *buffer, int len, struct devrequest *setup,
++			   int interval, enum usb_device_speed speed);
++
++int usb_urb_submit_bulk(struct usb_hcd *hcd, struct urb *urb,
++			struct usb_host_endpoint *hep, struct usb_device *dev,
++			unsigned long pipe, void *buffer, int len);
++
++int usb_urb_submit_irq(struct usb_hcd *hcd, struct urb *urb,
++		       struct usb_host_endpoint *hep, struct usb_device *dev,
++		       unsigned long pipe, void *buffer, int len, int interval);
++
++void usb_urb_fill(struct urb *urb, struct usb_host_endpoint *hep,
++		  struct usb_device *dev, int endpoint_type,
++		  unsigned long pipe, void *buffer, int len,
++		  struct devrequest *setup, int interval);
++
++int usb_urb_submit(struct usb_hcd *hcd, struct urb *urb);
++
+ #endif /* __USB_COMPAT_H__ */
+diff --git a/include/usb_defs.h b/include/usb_defs.h
+index 6dd2c997f9b3..ec00161710a5 100644
+--- a/include/usb_defs.h
++++ b/include/usb_defs.h
+@@ -81,6 +81,32 @@
+ #define EndpointOutRequest \
+ 	((USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_INTERFACE) << 8)
+ 
++/* class requests from the USB 2.0 hub spec, table 11-15 */
++#define HUB_CLASS_REQ(dir, type, request) ((((dir) | (type)) << 8) | (request))
++/* GetBusState and SetHubDescriptor are optional, omitted */
++#define ClearHubFeature		HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_HUB, \
++					      USB_REQ_CLEAR_FEATURE)
++#define ClearPortFeature	HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_PORT, \
++					      USB_REQ_CLEAR_FEATURE)
++#define GetHubDescriptor	HUB_CLASS_REQ(USB_DIR_IN, USB_RT_HUB, \
++					      USB_REQ_GET_DESCRIPTOR)
++#define GetHubStatus		HUB_CLASS_REQ(USB_DIR_IN, USB_RT_HUB, \
++					      USB_REQ_GET_STATUS)
++#define GetPortStatus		HUB_CLASS_REQ(USB_DIR_IN, USB_RT_PORT, \
++					      USB_REQ_GET_STATUS)
++#define SetHubFeature		HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_HUB, \
++					      USB_REQ_SET_FEATURE)
++#define SetPortFeature		HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_PORT, \
++					      USB_REQ_SET_FEATURE)
++#define ClearTTBuffer		HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_PORT, \
++					      HUB_CLEAR_TT_BUFFER)
++#define ResetTT			HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_PORT, \
++					      HUB_RESET_TT)
++#define GetTTState		HUB_CLASS_REQ(USB_DIR_IN, USB_RT_PORT, \
++					      HUB_GET_TT_STATE)
++#define StopTT			HUB_CLASS_REQ(USB_DIR_OUT, USB_RT_PORT, \
++					      HUB_STOP_TT)
++
+ /* Descriptor types */
+ #define USB_DT_DEVICE        0x01
+ #define USB_DT_CONFIG        0x02
+@@ -289,10 +315,16 @@
+ #define USB_SS_PORT_STAT_C_CONFIG_ERROR	0x0080
+ 
+ /* wHubCharacteristics (masks) */
++#define HUB_CHAR_COMMON_OCPM        0x0000 /* All ports Over-Current reporting */
++#define HUB_CHAR_INDV_PORT_LPSM     0x0001 /* per-port power control */
++#define HUB_CHAR_NO_LPSM            0x0002 /* no power switching */
+ #define HUB_CHAR_LPSM               0x0003
+ #define HUB_CHAR_COMPOUND           0x0004
++#define HUB_CHAR_INDV_PORT_OCPM     0x0008 /* per-port Over-current reporting */
++#define HUB_CHAR_NO_OCPM            0x0010 /* No Over-current Protection support */
+ #define HUB_CHAR_OCPM               0x0018
+ #define HUB_CHAR_TTTT               0x0060 /* TT Think Time mask */
++#define HUB_CHAR_PORTIND            0x0080 /* per-port indicators (LEDs) */
+ 
+ /*
+  * Hub Status & Hub Change bit masks
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0004-usb-add-isp1760-family-driver.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0004-usb-add-isp1760-family-driver.patch
new file mode 100644
index 0000000..5940f06
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0004-usb-add-isp1760-family-driver.patch
@@ -0,0 +1,3805 @@
+From 62a666a83766f5517e90464638455286bb33f433 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Mon, 28 Jun 2021 23:31:25 +0100
+Subject: [PATCH 04/27] usb: add isp1760 family driver
+
+ISP1760/61/63 are a family of usb controllers, blah, blah, more info
+here.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ Makefile                            |    1 +
+ drivers/usb/Kconfig                 |    2 +
+ drivers/usb/common/Makefile         |    1 +
+ drivers/usb/isp1760/Kconfig         |   12 +
+ drivers/usb/isp1760/Makefile        |    6 +
+ drivers/usb/isp1760/isp1760-core.c  |  378 ++++
+ drivers/usb/isp1760/isp1760-core.h  |   96 +
+ drivers/usb/isp1760/isp1760-hcd.c   | 2574 +++++++++++++++++++++++++++
+ drivers/usb/isp1760/isp1760-hcd.h   |   82 +
+ drivers/usb/isp1760/isp1760-if.c    |  127 ++
+ drivers/usb/isp1760/isp1760-regs.h  |  292 +++
+ drivers/usb/isp1760/isp1760-uboot.c |   76 +
+ drivers/usb/isp1760/isp1760-uboot.h |   27 +
+ 13 files changed, 3674 insertions(+)
+ create mode 100644 drivers/usb/isp1760/Kconfig
+ create mode 100644 drivers/usb/isp1760/Makefile
+ create mode 100644 drivers/usb/isp1760/isp1760-core.c
+ create mode 100644 drivers/usb/isp1760/isp1760-core.h
+ create mode 100644 drivers/usb/isp1760/isp1760-hcd.c
+ create mode 100644 drivers/usb/isp1760/isp1760-hcd.h
+ create mode 100644 drivers/usb/isp1760/isp1760-if.c
+ create mode 100644 drivers/usb/isp1760/isp1760-regs.h
+ create mode 100644 drivers/usb/isp1760/isp1760-uboot.c
+ create mode 100644 drivers/usb/isp1760/isp1760-uboot.h
+
+diff --git a/Makefile b/Makefile
+index ad83d60dc39d..2c857c1fe2cc 100644
+--- a/Makefile
++++ b/Makefile
+@@ -834,6 +834,7 @@ libs-y += drivers/usb/host/
+ libs-y += drivers/usb/mtu3/
+ libs-y += drivers/usb/musb/
+ libs-y += drivers/usb/musb-new/
++libs-y += drivers/usb/isp1760/
+ libs-y += drivers/usb/phy/
+ libs-y += drivers/usb/ulpi/
+ ifdef CONFIG_POST
+diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
+index ab1d061bd0d5..bbe07be02cab 100644
+--- a/drivers/usb/Kconfig
++++ b/drivers/usb/Kconfig
+@@ -78,6 +78,8 @@ source "drivers/usb/musb/Kconfig"
+ 
+ source "drivers/usb/musb-new/Kconfig"
+ 
++source "drivers/usb/isp1760/Kconfig"
++
+ source "drivers/usb/emul/Kconfig"
+ 
+ source "drivers/usb/phy/Kconfig"
+diff --git a/drivers/usb/common/Makefile b/drivers/usb/common/Makefile
+index dc05cb0a5077..f08b064d2493 100644
+--- a/drivers/usb/common/Makefile
++++ b/drivers/usb/common/Makefile
+@@ -4,6 +4,7 @@
+ #
+ 
+ obj-$(CONFIG_$(SPL_)DM_USB) += common.o
++obj-$(CONFIG_USB_ISP1760) += usb_urb.o
+ obj-$(CONFIG_USB_MUSB_HCD) += usb_urb.o
+ obj-$(CONFIG_USB_MUSB_UDC) += usb_urb.o
+ obj-$(CONFIG_USB_EHCI_FSL) += fsl-dt-fixup.o fsl-errata.o
+diff --git a/drivers/usb/isp1760/Kconfig b/drivers/usb/isp1760/Kconfig
+new file mode 100644
+index 000000000000..993d71e74cd2
+--- /dev/null
++++ b/drivers/usb/isp1760/Kconfig
+@@ -0,0 +1,12 @@
++# SPDX-License-Identifier: GPL-2.0
++
++config USB_ISP1760
++	tristate "NXP ISP 1760/1761/1763 support"
++	select DM_USB
++	select USB_HOST
++	help
++	  Say Y or M here if your system as an ISP1760/1761/1763 USB host
++	  controller.
++
++	  This USB controller is usually attached to a non-DMA-Master
++	  capable bus.
+diff --git a/drivers/usb/isp1760/Makefile b/drivers/usb/isp1760/Makefile
+new file mode 100644
+index 000000000000..2c809c01b118
+--- /dev/null
++++ b/drivers/usb/isp1760/Makefile
+@@ -0,0 +1,6 @@
++# SPDX-License-Identifier: GPL-2.0
++isp1760-y := isp1760-core.o isp1760-if.o isp1760-uboot.o isp1760-hcd.o
++
++#isp1760-hcd.o
++
++obj-$(CONFIG_USB_ISP1760) += isp1760.o
+diff --git a/drivers/usb/isp1760/isp1760-core.c b/drivers/usb/isp1760/isp1760-core.c
+new file mode 100644
+index 000000000000..3080595549c5
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-core.c
+@@ -0,0 +1,378 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ * This is based on linux kernel driver, original developed:
++ * Copyright 2014 Laurent Pinchart
++ * Copyright 2007 Sebastian Siewior
++ *
++ */
++
++#include <dm.h>
++#include <dm/device-internal.h>
++#include <dm/device_compat.h>
++#include <dm/devres.h>
++#include <linux/compat.h>
++#include <linux/delay.h>
++#include <linux/io.h>
++#include <linux/kernel.h>
++#include <regmap.h>
++#include <usb.h>
++
++#include "isp1760-core.h"
++#include "isp1760-hcd.h"
++#include "isp1760-regs.h"
++
++#define msleep(a) udelay(a * 1000)
++
++static int isp1760_init_core(struct isp1760_device *isp)
++{
++	struct isp1760_hcd *hcd = &isp->hcd;
++
++	/*
++	 * Reset the host controller, including the CPU interface
++	 * configuration.
++	 */
++	isp1760_field_set(hcd->fields, SW_RESET_RESET_ALL);
++	msleep(100);
++
++	/* Setup HW Mode Control: This assumes a level active-low interrupt */
++	if ((isp->devflags & ISP1760_FLAG_ANALOG_OC) && hcd->is_isp1763)
++		return -EINVAL;
++
++	if (isp->devflags & ISP1760_FLAG_BUS_WIDTH_16)
++		isp1760_field_clear(hcd->fields, HW_DATA_BUS_WIDTH);
++	if (isp->devflags & ISP1760_FLAG_BUS_WIDTH_8)
++		isp1760_field_set(hcd->fields, HW_DATA_BUS_WIDTH);
++	if (isp->devflags & ISP1760_FLAG_ANALOG_OC)
++		isp1760_field_set(hcd->fields, HW_ANA_DIGI_OC);
++	if (isp->devflags & ISP1760_FLAG_DACK_POL_HIGH)
++		isp1760_field_set(hcd->fields, HW_DACK_POL_HIGH);
++	if (isp->devflags & ISP1760_FLAG_DREQ_POL_HIGH)
++		isp1760_field_set(hcd->fields, HW_DREQ_POL_HIGH);
++	if (isp->devflags & ISP1760_FLAG_INTR_POL_HIGH)
++		isp1760_field_set(hcd->fields, HW_INTR_HIGH_ACT);
++	if (isp->devflags & ISP1760_FLAG_INTR_EDGE_TRIG)
++		isp1760_field_set(hcd->fields, HW_INTR_EDGE_TRIG);
++
++	/*
++	 * The ISP1761 has a dedicated DC IRQ line but supports sharing the HC
++	 * IRQ line for both the host and device controllers. Hardcode IRQ
++	 * sharing for now and disable the DC interrupts globally to avoid
++	 * spurious interrupts during HCD registration.
++	 */
++	if (isp->devflags & ISP1760_FLAG_ISP1761) {
++		isp1760_reg_write(hcd->regs, ISP176x_DC_MODE, 0);
++		isp1760_field_set(hcd->fields, HW_COMN_IRQ);
++	}
++
++	/*
++	 * PORT 1 Control register of the ISP1760 is the OTG control register
++	 * on ISP1761.
++	 *
++	 * TODO: Really support OTG. For now we configure port 1 in device mode
++	 */
++	if (((isp->devflags & ISP1760_FLAG_ISP1761) ||
++	     (isp->devflags & ISP1760_FLAG_ISP1763)) &&
++	    (isp->devflags & ISP1760_FLAG_PERIPHERAL_EN)) {
++		isp1760_field_set(hcd->fields, HW_DM_PULLDOWN);
++		isp1760_field_set(hcd->fields, HW_DP_PULLDOWN);
++		isp1760_field_set(hcd->fields, HW_OTG_DISABLE);
++	} else {
++		isp1760_field_set(hcd->fields, HW_SW_SEL_HC_DC);
++		isp1760_field_set(hcd->fields, HW_VBUS_DRV);
++		isp1760_field_set(hcd->fields, HW_SEL_CP_EXT);
++	}
++
++	printf( "%s bus width: %u, oc: %s\n",
++		 hcd->is_isp1763 ? "isp1763" : "isp1760",
++		 isp->devflags & ISP1760_FLAG_BUS_WIDTH_8 ? 8 :
++		 isp->devflags & ISP1760_FLAG_BUS_WIDTH_16 ? 16 : 32,
++		 hcd->is_isp1763 ? "not available" :
++		 isp->devflags & ISP1760_FLAG_ANALOG_OC ? "analog" : "digital");
++
++	return 0;
++}
++
++void isp1760_set_pullup(struct isp1760_device *isp, bool enable)
++{
++	struct isp1760_hcd *hcd = &isp->hcd;
++
++	if (enable)
++		isp1760_field_set(hcd->fields, HW_DP_PULLUP);
++	else
++		isp1760_field_set(hcd->fields, HW_DP_PULLUP_CLEAR);
++}
++
++/*
++ * ISP1760/61:
++ *
++ * 60kb divided in:
++ * - 32 blocks @ 256  bytes
++ * - 20 blocks @ 1024 bytes
++ * -  4 blocks @ 8192 bytes
++ */
++static const struct isp1760_memory_layout isp176x_memory_conf = {
++	.blocks[0]		= 32,
++	.blocks_size[0]		= 256,
++	.blocks[1]		= 20,
++	.blocks_size[1]		= 1024,
++	.blocks[2]		= 4,
++	.blocks_size[2]		= 8192,
++
++	.slot_num		= 32,
++	.payload_blocks		= 32 + 20 + 4,
++	.payload_area_size	= 0xf000,
++};
++
++/*
++ * ISP1763:
++ *
++ * 20kb divided in:
++ * - 8 blocks @ 256  bytes
++ * - 2 blocks @ 1024 bytes
++ * - 4 blocks @ 4096 bytes
++ */
++static const struct isp1760_memory_layout isp1763_memory_conf = {
++	.blocks[0]		= 8,
++	.blocks_size[0]		= 256,
++	.blocks[1]		= 2,
++	.blocks_size[1]		= 1024,
++	.blocks[2]		= 4,
++	.blocks_size[2]		= 4096,
++
++	.slot_num		= 16,
++	.payload_blocks		= 8 + 2 + 4,
++	.payload_area_size	= 0x5000,
++};
++
++static const struct regmap_config isp1760_hc_regmap_conf = {
++	.width = REGMAP_SIZE_16,
++};
++
++static const struct reg_field isp1760_hc_reg_fields[] = {
++	[HCS_PPC]		= REG_FIELD(ISP176x_HC_HCSPARAMS, 4, 4),
++	[HCS_N_PORTS]		= REG_FIELD(ISP176x_HC_HCSPARAMS, 0, 3),
++	[HCC_ISOC_CACHE]	= REG_FIELD(ISP176x_HC_HCCPARAMS, 7, 7),
++	[HCC_ISOC_THRES]	= REG_FIELD(ISP176x_HC_HCCPARAMS, 4, 6),
++	[CMD_LRESET]		= REG_FIELD(ISP176x_HC_USBCMD, 7, 7),
++	[CMD_RESET]		= REG_FIELD(ISP176x_HC_USBCMD, 1, 1),
++	[CMD_RUN]		= REG_FIELD(ISP176x_HC_USBCMD, 0, 0),
++	[STS_PCD]		= REG_FIELD(ISP176x_HC_USBSTS, 2, 2),
++	[HC_FRINDEX]		= REG_FIELD(ISP176x_HC_FRINDEX, 0, 13),
++	[FLAG_CF]		= REG_FIELD(ISP176x_HC_CONFIGFLAG, 0, 0),
++	[HC_ISO_PTD_DONEMAP]	= REG_FIELD(ISP176x_HC_ISO_PTD_DONEMAP, 0, 31),
++	[HC_ISO_PTD_SKIPMAP]	= REG_FIELD(ISP176x_HC_ISO_PTD_SKIPMAP, 0, 31),
++	[HC_ISO_PTD_LASTPTD]	= REG_FIELD(ISP176x_HC_ISO_PTD_LASTPTD, 0, 31),
++	[HC_INT_PTD_DONEMAP]	= REG_FIELD(ISP176x_HC_INT_PTD_DONEMAP, 0, 31),
++	[HC_INT_PTD_SKIPMAP]	= REG_FIELD(ISP176x_HC_INT_PTD_SKIPMAP, 0, 31),
++	[HC_INT_PTD_LASTPTD]	= REG_FIELD(ISP176x_HC_INT_PTD_LASTPTD, 0, 31),
++	[HC_ATL_PTD_DONEMAP]	= REG_FIELD(ISP176x_HC_ATL_PTD_DONEMAP, 0, 31),
++	[HC_ATL_PTD_SKIPMAP]	= REG_FIELD(ISP176x_HC_ATL_PTD_SKIPMAP, 0, 31),
++	[HC_ATL_PTD_LASTPTD]	= REG_FIELD(ISP176x_HC_ATL_PTD_LASTPTD, 0, 31),
++	[PORT_OWNER]		= REG_FIELD(ISP176x_HC_PORTSC1, 13, 13),
++	[PORT_POWER]		= REG_FIELD(ISP176x_HC_PORTSC1, 12, 12),
++	[PORT_LSTATUS]		= REG_FIELD(ISP176x_HC_PORTSC1, 10, 11),
++	[PORT_RESET]		= REG_FIELD(ISP176x_HC_PORTSC1, 8, 8),
++	[PORT_SUSPEND]		= REG_FIELD(ISP176x_HC_PORTSC1, 7, 7),
++	[PORT_RESUME]		= REG_FIELD(ISP176x_HC_PORTSC1, 6, 6),
++	[PORT_PE]		= REG_FIELD(ISP176x_HC_PORTSC1, 2, 2),
++	[PORT_CSC]		= REG_FIELD(ISP176x_HC_PORTSC1, 1, 1),
++	[PORT_CONNECT]		= REG_FIELD(ISP176x_HC_PORTSC1, 0, 0),
++	[ALL_ATX_RESET]		= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 31, 31),
++	[HW_ANA_DIGI_OC]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 15, 15),
++	[HW_COMN_IRQ]		= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 10, 10),
++	[HW_DATA_BUS_WIDTH]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 8, 8),
++	[HW_DACK_POL_HIGH]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 6, 6),
++	[HW_DREQ_POL_HIGH]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 5, 5),
++	[HW_INTR_HIGH_ACT]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 2, 2),
++	[HW_INTR_EDGE_TRIG]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 1, 1),
++	[HW_GLOBAL_INTR_EN]	= REG_FIELD(ISP176x_HC_HW_MODE_CTRL, 0, 0),
++	[HC_CHIP_REV]		= REG_FIELD(ISP176x_HC_CHIP_ID, 16, 31),
++	[HC_CHIP_ID_HIGH]	= REG_FIELD(ISP176x_HC_CHIP_ID, 8, 15),
++	[HC_CHIP_ID_LOW]	= REG_FIELD(ISP176x_HC_CHIP_ID, 0, 7),
++	[HC_SCRATCH]		= REG_FIELD(ISP176x_HC_SCRATCH, 0, 31),
++	[SW_RESET_RESET_ALL]	= REG_FIELD(ISP176x_HC_RESET, 0, 0),
++	[ISO_BUF_FILL]		= REG_FIELD(ISP176x_HC_BUFFER_STATUS, 2, 2),
++	[INT_BUF_FILL]		= REG_FIELD(ISP176x_HC_BUFFER_STATUS, 1, 1),
++	[ATL_BUF_FILL]		= REG_FIELD(ISP176x_HC_BUFFER_STATUS, 0, 0),
++	[MEM_BANK_SEL]		= REG_FIELD(ISP176x_HC_MEMORY, 16, 17),
++	[MEM_START_ADDR]	= REG_FIELD(ISP176x_HC_MEMORY, 0, 15),
++	[HC_INTERRUPT]		= REG_FIELD(ISP176x_HC_INTERRUPT, 0, 9),
++	[HC_ATL_IRQ_ENABLE]	= REG_FIELD(ISP176x_HC_INTERRUPT_ENABLE, 8, 8),
++	[HC_INT_IRQ_ENABLE]	= REG_FIELD(ISP176x_HC_INTERRUPT_ENABLE, 7, 7),
++	[HC_ISO_IRQ_MASK_OR]	= REG_FIELD(ISP176x_HC_ISO_IRQ_MASK_OR, 0, 31),
++	[HC_INT_IRQ_MASK_OR]	= REG_FIELD(ISP176x_HC_INT_IRQ_MASK_OR, 0, 31),
++	[HC_ATL_IRQ_MASK_OR]	= REG_FIELD(ISP176x_HC_ATL_IRQ_MASK_OR, 0, 31),
++	[HC_ISO_IRQ_MASK_AND]	= REG_FIELD(ISP176x_HC_ISO_IRQ_MASK_AND, 0, 31),
++	[HC_INT_IRQ_MASK_AND]	= REG_FIELD(ISP176x_HC_INT_IRQ_MASK_AND, 0, 31),
++	[HC_ATL_IRQ_MASK_AND]	= REG_FIELD(ISP176x_HC_ATL_IRQ_MASK_AND, 0, 31),
++	[HW_OTG_DISABLE]	= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 10, 10),
++	[HW_SW_SEL_HC_DC]	= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 7, 7),
++	[HW_VBUS_DRV]		= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 4, 4),
++	[HW_SEL_CP_EXT]		= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 3, 3),
++	[HW_DM_PULLDOWN]	= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 2, 2),
++	[HW_DP_PULLDOWN]	= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 1, 1),
++	[HW_DP_PULLUP]		= REG_FIELD(ISP176x_HC_OTG_CTRL_SET, 0, 0),
++	[HW_OTG_DISABLE_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 10, 10),
++	[HW_SW_SEL_HC_DC_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 7, 7),
++	[HW_VBUS_DRV_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 4, 4),
++	[HW_SEL_CP_EXT_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 3, 3),
++	[HW_DM_PULLDOWN_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 2, 2),
++	[HW_DP_PULLDOWN_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 1, 1),
++	[HW_DP_PULLUP_CLEAR]	= REG_FIELD(ISP176x_HC_OTG_CTRL_CLEAR, 0, 0),
++};
++
++static const struct regmap_config isp1763_hc_regmap_conf = {
++	.width = REGMAP_SIZE_16,
++};
++
++static const struct reg_field isp1763_hc_reg_fields[] = {
++	[CMD_LRESET]		= REG_FIELD(ISP1763_HC_USBCMD, 7, 7),
++	[CMD_RESET]		= REG_FIELD(ISP1763_HC_USBCMD, 1, 1),
++	[CMD_RUN]		= REG_FIELD(ISP1763_HC_USBCMD, 0, 0),
++	[STS_PCD]		= REG_FIELD(ISP1763_HC_USBSTS, 2, 2),
++	[HC_FRINDEX]		= REG_FIELD(ISP1763_HC_FRINDEX, 0, 13),
++	[FLAG_CF]		= REG_FIELD(ISP1763_HC_CONFIGFLAG, 0, 0),
++	[HC_ISO_PTD_DONEMAP]	= REG_FIELD(ISP1763_HC_ISO_PTD_DONEMAP, 0, 15),
++	[HC_ISO_PTD_SKIPMAP]	= REG_FIELD(ISP1763_HC_ISO_PTD_SKIPMAP, 0, 15),
++	[HC_ISO_PTD_LASTPTD]	= REG_FIELD(ISP1763_HC_ISO_PTD_LASTPTD, 0, 15),
++	[HC_INT_PTD_DONEMAP]	= REG_FIELD(ISP1763_HC_INT_PTD_DONEMAP, 0, 15),
++	[HC_INT_PTD_SKIPMAP]	= REG_FIELD(ISP1763_HC_INT_PTD_SKIPMAP, 0, 15),
++	[HC_INT_PTD_LASTPTD]	= REG_FIELD(ISP1763_HC_INT_PTD_LASTPTD, 0, 15),
++	[HC_ATL_PTD_DONEMAP]	= REG_FIELD(ISP1763_HC_ATL_PTD_DONEMAP, 0, 15),
++	[HC_ATL_PTD_SKIPMAP]	= REG_FIELD(ISP1763_HC_ATL_PTD_SKIPMAP, 0, 15),
++	[HC_ATL_PTD_LASTPTD]	= REG_FIELD(ISP1763_HC_ATL_PTD_LASTPTD, 0, 15),
++	[PORT_OWNER]		= REG_FIELD(ISP1763_HC_PORTSC1, 13, 13),
++	[PORT_POWER]		= REG_FIELD(ISP1763_HC_PORTSC1, 12, 12),
++	[PORT_LSTATUS]		= REG_FIELD(ISP1763_HC_PORTSC1, 10, 11),
++	[PORT_RESET]		= REG_FIELD(ISP1763_HC_PORTSC1, 8, 8),
++	[PORT_SUSPEND]		= REG_FIELD(ISP1763_HC_PORTSC1, 7, 7),
++	[PORT_RESUME]		= REG_FIELD(ISP1763_HC_PORTSC1, 6, 6),
++	[PORT_PE]		= REG_FIELD(ISP1763_HC_PORTSC1, 2, 2),
++	[PORT_CSC]		= REG_FIELD(ISP1763_HC_PORTSC1, 1, 1),
++	[PORT_CONNECT]		= REG_FIELD(ISP1763_HC_PORTSC1, 0, 0),
++	[HW_DATA_BUS_WIDTH]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 4, 4),
++	[HW_DACK_POL_HIGH]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 6, 6),
++	[HW_DREQ_POL_HIGH]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 5, 5),
++	[HW_INTF_LOCK]		= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 3, 3),
++	[HW_INTR_HIGH_ACT]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 2, 2),
++	[HW_INTR_EDGE_TRIG]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 1, 1),
++	[HW_GLOBAL_INTR_EN]	= REG_FIELD(ISP1763_HC_HW_MODE_CTRL, 0, 0),
++	[SW_RESET_RESET_ATX]	= REG_FIELD(ISP1763_HC_RESET, 3, 3),
++	[SW_RESET_RESET_ALL]	= REG_FIELD(ISP1763_HC_RESET, 0, 0),
++	[HC_CHIP_ID_HIGH]	= REG_FIELD(ISP1763_HC_CHIP_ID, 0, 15),
++	[HC_CHIP_ID_LOW]	= REG_FIELD(ISP1763_HC_CHIP_REV, 8, 15),
++	[HC_CHIP_REV]		= REG_FIELD(ISP1763_HC_CHIP_REV, 0, 7),
++	[HC_SCRATCH]		= REG_FIELD(ISP1763_HC_SCRATCH, 0, 15),
++	[ISO_BUF_FILL]		= REG_FIELD(ISP1763_HC_BUFFER_STATUS, 2, 2),
++	[INT_BUF_FILL]		= REG_FIELD(ISP1763_HC_BUFFER_STATUS, 1, 1),
++	[ATL_BUF_FILL]		= REG_FIELD(ISP1763_HC_BUFFER_STATUS, 0, 0),
++	[MEM_START_ADDR]	= REG_FIELD(ISP1763_HC_MEMORY, 0, 15),
++	[HC_DATA]		= REG_FIELD(ISP1763_HC_DATA, 0, 15),
++	[HC_INTERRUPT]		= REG_FIELD(ISP1763_HC_INTERRUPT, 0, 10),
++	[HC_ATL_IRQ_ENABLE]	= REG_FIELD(ISP1763_HC_INTERRUPT_ENABLE, 8, 8),
++	[HC_INT_IRQ_ENABLE]	= REG_FIELD(ISP1763_HC_INTERRUPT_ENABLE, 7, 7),
++	[HC_ISO_IRQ_MASK_OR]	= REG_FIELD(ISP1763_HC_ISO_IRQ_MASK_OR, 0, 15),
++	[HC_INT_IRQ_MASK_OR]	= REG_FIELD(ISP1763_HC_INT_IRQ_MASK_OR, 0, 15),
++	[HC_ATL_IRQ_MASK_OR]	= REG_FIELD(ISP1763_HC_ATL_IRQ_MASK_OR, 0, 15),
++	[HC_ISO_IRQ_MASK_AND]	= REG_FIELD(ISP1763_HC_ISO_IRQ_MASK_AND, 0, 15),
++	[HC_INT_IRQ_MASK_AND]	= REG_FIELD(ISP1763_HC_INT_IRQ_MASK_AND, 0, 15),
++	[HC_ATL_IRQ_MASK_AND]	= REG_FIELD(ISP1763_HC_ATL_IRQ_MASK_AND, 0, 15),
++	[HW_HC_2_DIS]		= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 15, 15),
++	[HW_OTG_DISABLE]	= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 10, 10),
++	[HW_SW_SEL_HC_DC]	= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 7, 7),
++	[HW_VBUS_DRV]		= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 4, 4),
++	[HW_SEL_CP_EXT]		= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 3, 3),
++	[HW_DM_PULLDOWN]	= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 2, 2),
++	[HW_DP_PULLDOWN]	= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 1, 1),
++	[HW_DP_PULLUP]		= REG_FIELD(ISP1763_HC_OTG_CTRL_SET, 0, 0),
++	[HW_HC_2_DIS_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 15, 15),
++	[HW_OTG_DISABLE_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 10, 10),
++	[HW_SW_SEL_HC_DC_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 7, 7),
++	[HW_VBUS_DRV_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 4, 4),
++	[HW_SEL_CP_EXT_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 3, 3),
++	[HW_DM_PULLDOWN_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 2, 2),
++	[HW_DP_PULLDOWN_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 1, 1),
++	[HW_DP_PULLUP_CLEAR]	= REG_FIELD(ISP1763_HC_OTG_CTRL_CLEAR, 0, 0),
++};
++
++int isp1760_register(struct isp1760_device *isp, struct resource *mem, int irq,
++		     unsigned long irqflags)
++{
++	const struct regmap_config *hc_regmap;
++	const struct reg_field *hc_reg_fields;
++	struct isp1760_hcd *hcd;
++	struct regmap_field *f;
++	unsigned int devflags;
++	struct udevice *dev;
++	int ret;
++	int i;
++
++	hcd = &isp->hcd;
++	devflags = isp->devflags;
++	dev = isp->dev;
++
++	hcd->is_isp1763 = !!(devflags & ISP1760_FLAG_ISP1763);
++
++	if (!hcd->is_isp1763 && (devflags & ISP1760_FLAG_BUS_WIDTH_8)) {
++		dev_err(dev, "isp1760/61 do not support data width 8\n");
++		return -EINVAL;
++	}
++
++	if (hcd->is_isp1763) {
++		hc_regmap = &isp1763_hc_regmap_conf;
++		hc_reg_fields = &isp1763_hc_reg_fields[0];
++	} else {
++		hc_regmap = &isp1760_hc_regmap_conf;
++		hc_reg_fields = &isp1760_hc_reg_fields[0];
++	}
++
++	hcd->base = devm_ioremap(dev, mem->start, resource_size(mem));
++	if (IS_ERR(hcd->base))
++		return PTR_ERR(hcd->base);
++
++	hcd->regs = devm_regmap_init(dev, NULL, NULL, hc_regmap);
++	if (IS_ERR(hcd->regs))
++		return PTR_ERR(hcd->regs);
++
++	for (i = 0; i < HC_FIELD_MAX; i++) {
++		f = devm_regmap_field_alloc(dev, hcd->regs, hc_reg_fields[i]);
++		if (IS_ERR(f))
++			return PTR_ERR(f);
++
++		hcd->fields[i] = f;
++	}
++
++	if (hcd->is_isp1763)
++		hcd->memory_layout = &isp1763_memory_conf;
++	else
++		hcd->memory_layout = &isp176x_memory_conf;
++
++	ret = isp1760_init_core(isp);
++	if (ret < 0)
++		return ret;
++
++	hcd->dev = dev;
++
++	ret = isp1760_hcd_register(hcd, mem, irq, irqflags, dev);
++	if (ret < 0)
++		return ret;
++
++	ret = isp1760_hcd_lowlevel_init(hcd);
++	if (ret < 0)
++		return ret;
++
++	dev_set_drvdata(dev, isp);
++
++	return 0;
++}
++
++void isp1760_unregister(struct isp1760_device *isp)
++{
++	isp1760_hcd_unregister(&isp->hcd);
++
++	return;
++}
+diff --git a/drivers/usb/isp1760/isp1760-core.h b/drivers/usb/isp1760/isp1760-core.h
+new file mode 100644
+index 000000000000..0a60e30b5fe7
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-core.h
+@@ -0,0 +1,96 @@
++/* SPDX-License-Identifier: GPL-2.0 */
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva
++ * Copyright 2014 Laurent Pinchart
++ * Copyright 2007 Sebastian Siewior
++ *
++ * Contacts:
++ *	Sebastian Siewior <bigeasy@linutronix.de>
++ *	Laurent Pinchart <laurent.pinchart@ideasonboard.com>
++ *	Rui Miguel Silva <rui.silva@linaro.org>
++ */
++
++#ifndef _ISP1760_CORE_H_
++#define _ISP1760_CORE_H_
++
++#include <linux/compat.h>
++#include <linux/ioport.h>
++#include <regmap.h>
++
++#include "isp1760-hcd.h"
++
++struct device;
++struct gpio_desc;
++
++/*
++ * Device flags that can vary from board to board.  All of these
++ * indicate the most "atypical" case, so that a devflags of 0 is
++ * a sane default configuration.
++ */
++#define ISP1760_FLAG_BUS_WIDTH_16	0x00000002 /* 16-bit data bus width */
++#define ISP1760_FLAG_PERIPHERAL_EN	0x00000004 /* Port 1 supports Peripheral mode*/
++#define ISP1760_FLAG_ANALOG_OC		0x00000008 /* Analog overcurrent */
++#define ISP1760_FLAG_DACK_POL_HIGH	0x00000010 /* DACK active high */
++#define ISP1760_FLAG_DREQ_POL_HIGH	0x00000020 /* DREQ active high */
++#define ISP1760_FLAG_ISP1761		0x00000040 /* Chip is ISP1761 */
++#define ISP1760_FLAG_INTR_POL_HIGH	0x00000080 /* Interrupt polarity active high */
++#define ISP1760_FLAG_INTR_EDGE_TRIG	0x00000100 /* Interrupt edge triggered */
++#define ISP1760_FLAG_ISP1763		0x00000200 /* Chip is ISP1763 */
++#define ISP1760_FLAG_BUS_WIDTH_8	0x00000400 /* 8-bit data bus width */
++
++struct isp1760_device {
++	struct udevice *dev;
++
++	unsigned int devflags;
++	struct gpio_desc *rst_gpio;
++
++	struct isp1760_hcd hcd;
++};
++
++int isp1760_register(struct isp1760_device *isp, struct resource *mem, int irq,
++		     unsigned long irqflags);
++void isp1760_unregister(struct isp1760_device *isp);
++
++void isp1760_set_pullup(struct isp1760_device *isp, bool enable);
++
++static inline u32 isp1760_field_read(struct regmap_field **fields, u32 field)
++{
++	unsigned int val;
++
++	regmap_field_read(fields[field], &val);
++
++	return val;
++}
++
++static inline void isp1760_field_write(struct regmap_field **fields, u32 field,
++				       u32 val)
++{
++	regmap_field_write(fields[field], val);
++}
++
++static inline void isp1760_field_set(struct regmap_field **fields, u32 field)
++{
++	isp1760_field_write(fields, field, 0xFFFFFFFF);
++}
++
++static inline void isp1760_field_clear(struct regmap_field **fields, u32 field)
++{
++	isp1760_field_write(fields, field, 0);
++}
++
++static inline u32 isp1760_reg_read(struct regmap *regs, u32 reg)
++{
++	unsigned int val;
++
++	regmap_read(regs, reg, &val);
++
++	return val;
++}
++
++static inline void isp1760_reg_write(struct regmap *regs, u32 reg, u32 val)
++{
++	regmap_write(regs, reg, val);
++}
++#endif
+diff --git a/drivers/usb/isp1760/isp1760-hcd.c b/drivers/usb/isp1760/isp1760-hcd.c
+new file mode 100644
+index 000000000000..b1d86dd69b94
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-hcd.c
+@@ -0,0 +1,2574 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ */
++
++#include <hexdump.h>
++#include <common.h>
++#include <asm/cache.h>
++#include <cpu_func.h>
++#include <dm.h>
++#include <dm/device-internal.h>
++#include <dm/device_compat.h>
++#include <linux/bug.h>
++#include <linux/kernel.h>
++#include <linux/list.h>
++#include <linux/usb/usb_urb_compat.h>
++#include <usb.h>
++#include <linux/io.h>
++#include <linux/iopoll.h>
++#include <asm/unaligned.h>
++
++#include "isp1760-core.h"
++#include "isp1760-hcd.h"
++#include "isp1760-regs.h"
++#include "isp1760-uboot.h"
++
++static struct kmem_cache *qtd_cachep;
++static struct kmem_cache *qh_cachep;
++static struct kmem_cache *urb_listitem_cachep;
++
++typedef void (packet_enqueue)(struct usb_hcd *hcd, struct isp1760_qh *qh,
++			      struct isp1760_qtd *qtd);
++
++static inline struct isp1760_hcd *hcd_to_priv(struct usb_hcd *hcd)
++{
++	return hcd->hcd_priv;
++}
++
++#define dw_to_le32(x)	(cpu_to_le32((__force u32)x))
++#define le32_to_dw(x)	((__force __dw)(le32_to_cpu(x)))
++
++/* urb state*/
++#define DELETE_URB		(0x0008)
++#define NO_TRANSFER_ACTIVE	(0xffffffff)
++
++/* Philips Proprietary Transfer Descriptor (PTD) */
++typedef __u32 __bitwise __dw;
++struct ptd {
++	__dw dw0;
++	__dw dw1;
++	__dw dw2;
++	__dw dw3;
++	__dw dw4;
++	__dw dw5;
++	__dw dw6;
++	__dw dw7;
++};
++
++struct ptd_le32 {
++	__le32 dw0;
++	__le32 dw1;
++	__le32 dw2;
++	__le32 dw3;
++	__le32 dw4;
++	__le32 dw5;
++	__le32 dw6;
++	__le32 dw7;
++};
++
++#define PTD_OFFSET		0x0400
++#define ISO_PTD_OFFSET		0x0400
++#define INT_PTD_OFFSET		0x0800
++#define ATL_PTD_OFFSET		0x0c00
++#define PAYLOAD_OFFSET		0x1000
++
++#define ISP_BANK_0		0x00
++#define ISP_BANK_1		0x01
++#define ISP_BANK_2		0x02
++#define ISP_BANK_3		0x03
++
++#define TO_DW(x)	((__force __dw)x)
++#define TO_U32(x)	((__force u32)x)
++
++ /* ATL */
++ /* DW0 */
++#define DW0_VALID_BIT			TO_DW(1)
++#define FROM_DW0_VALID(x)		(TO_U32(x) & 0x01)
++#define TO_DW0_LENGTH(x)		TO_DW((((u32)x) << 3))
++#define TO_DW0_MAXPACKET(x)		TO_DW((((u32)x) << 18))
++#define TO_DW0_MULTI(x)			TO_DW((((u32)x) << 29))
++#define TO_DW0_ENDPOINT(x)		TO_DW((((u32)x) << 31))
++/* DW1 */
++#define TO_DW1_DEVICE_ADDR(x)		TO_DW((((u32)x) << 3))
++#define TO_DW1_PID_TOKEN(x)		TO_DW((((u32)x) << 10))
++#define DW1_TRANS_BULK			TO_DW(((u32)2 << 12))
++#define DW1_TRANS_INT			TO_DW(((u32)3 << 12))
++#define DW1_TRANS_SPLIT			TO_DW(((u32)1 << 14))
++#define DW1_SE_USB_LOSPEED		TO_DW(((u32)2 << 16))
++#define TO_DW1_PORT_NUM(x)		TO_DW((((u32)x) << 18))
++#define TO_DW1_HUB_NUM(x)		TO_DW((((u32)x) << 25))
++/* DW2 */
++#define TO_DW2_DATA_START_ADDR(x)	TO_DW((((u32)x) << 8))
++#define TO_DW2_RL(x)			TO_DW(((x) << 25))
++#define FROM_DW2_RL(x)			((TO_U32(x) >> 25) & 0xf)
++/* DW3 */
++#define FROM_DW3_NRBYTESTRANSFERRED(x)		TO_U32((x) & 0x3fff)
++#define FROM_DW3_SCS_NRBYTESTRANSFERRED(x)	TO_U32((x) & 0x07ff)
++#define TO_DW3_NAKCOUNT(x)		TO_DW(((x) << 19))
++#define FROM_DW3_NAKCOUNT(x)		((TO_U32(x) >> 19) & 0xf)
++#define TO_DW3_CERR(x)			TO_DW(((x) << 23))
++#define FROM_DW3_CERR(x)		((TO_U32(x) >> 23) & 0x3)
++#define TO_DW3_DATA_TOGGLE(x)		TO_DW(((x) << 25))
++#define FROM_DW3_DATA_TOGGLE(x)		((TO_U32(x) >> 25) & 0x1)
++#define TO_DW3_PING(x)			TO_DW(((x) << 26))
++#define FROM_DW3_PING(x)		((TO_U32(x) >> 26) & 0x1)
++#define DW3_ERROR_BIT			TO_DW((1 << 28))
++#define DW3_BABBLE_BIT			TO_DW((1 << 29))
++#define DW3_HALT_BIT			TO_DW((1 << 30))
++#define DW3_ACTIVE_BIT			TO_DW((1 << 31))
++#define FROM_DW3_ACTIVE(x)		((TO_U32(x) >> 31) & 0x01)
++
++#define INT_UNDERRUN			(1 << 2)
++#define INT_BABBLE			(1 << 1)
++#define INT_EXACT			(1 << 0)
++
++#define SETUP_PID	(2)
++#define IN_PID		(1)
++#define OUT_PID		(0)
++
++/* Errata 1 */
++#define RL_COUNTER	(0)
++#define NAK_COUNTER	(0)
++#define ERR_COUNTER	(3)
++
++struct isp1760_qtd {
++	u8 packet_type;
++	void *data_buffer;
++	u32 payload_addr;
++
++	/* the rest is HCD-private */
++	struct list_head qtd_list;
++	struct urb *urb;
++	size_t length;
++	size_t actual_length;
++
++	/* QTD_ENQUEUED:	waiting for transfer (inactive) */
++	/* QTD_PAYLOAD_ALLOC:	chip mem has been allocated for payload */
++	/* QTD_XFER_STARTED:	valid ptd has been written to isp176x - only
++				interrupt handler may touch this qtd! */
++	/* QTD_XFER_COMPLETE:	payload has been transferred successfully */
++	/* QTD_RETIRE:		transfer error/abort qtd */
++#define QTD_ENQUEUED		0
++#define QTD_PAYLOAD_ALLOC	1
++#define QTD_XFER_STARTED	2
++#define QTD_XFER_COMPLETE	3
++#define QTD_RETIRE		4
++	u32 status;
++};
++
++/* Queue head, one for each active endpoint */
++struct isp1760_qh {
++	struct list_head qh_list;
++	struct list_head qtd_list;
++	int epnum;
++	u32 toggle;
++	u32 ping;
++	int slot;
++	int tt_buffer_dirty;	/* See USB2.0 spec section 11.17.5 */
++};
++
++struct urb_listitem {
++	struct list_head urb_list;
++	struct urb *urb;
++};
++
++static const u32 isp1763_hc_portsc1_fields[] = {
++	[PORT_OWNER]		= BIT(13),
++	[PORT_POWER]		= BIT(12),
++	[PORT_LSTATUS]		= BIT(10),
++	[PORT_RESET]		= BIT(8),
++	[PORT_SUSPEND]		= BIT(7),
++	[PORT_RESUME]		= BIT(6),
++	[PORT_PE]		= BIT(2),
++	[PORT_CSC]		= BIT(1),
++	[PORT_CONNECT]		= BIT(0),
++};
++
++static struct descriptor {
++	struct usb_device_descriptor device;
++	struct usb_config_descriptor config;
++	struct usb_interface_descriptor interface;
++	struct usb_endpoint_descriptor endpoint;
++} __packed rh_descriptor = {
++	{
++		/* usb 2.0 root hub device descriptor */
++		0x12,       /*  __u8  bLength; */
++		USB_DT_DEVICE, /* __u8 bDescriptorType; Device */
++		0x0002, /*  __le16 bcdUSB; v2.0 */
++
++		0x09,       /*  __u8  bDeviceClass; HUB_CLASSCODE */
++		0x00,       /*  __u8  bDeviceSubClass; */
++		0x00,       /*  __u8  bDeviceProtocol; [ usb 2.0 no TT ] */
++		0x40,       /*  __u8  bMaxPacketSize0; 64 Bytes */
++
++		0x6b1d,     /*  __le16 idVendor; Linux Foundation 0x1d6b */
++		0x0200,     /*  __le16 idProduct; device 0x0002 */
++		0x0001,     /*  __le16 bcdDevice */
++
++		0x03,       /*  __u8  iManufacturer; */
++		0x02,       /*  __u8  iProduct; */
++		0x01,       /*  __u8  iSerialNumber; */
++		0x01        /*  __u8  bNumConfigurations; */
++	}, {
++		/* one configuration */
++		0x09,       /*  __u8  bLength; */
++		USB_DT_CONFIG, /* __u8 bDescriptorType; Configuration */
++		0x1900,     /*  __le16 wTotalLength; */
++		0x01,       /*  __u8  bNumInterfaces; (1) */
++		0x01,       /*  __u8  bConfigurationValue; */
++		0x00,       /*  __u8  iConfiguration; */
++		0xc0,       /*  __u8  bmAttributes;
++					Bit 7: must be set,
++					6: Self-powered,
++					5: Remote wakeup,
++					4..0: resvd */
++		0x00,       /*  __u8  MaxPower; */
++	}, {
++		/* one interface */
++		0x09,       /*  __u8  if_bLength; */
++		USB_DT_INTERFACE, /* __u8 if_bDescriptorType; Interface */
++		0x00,       /*  __u8  if_bInterfaceNumber; */
++		0x00,       /*  __u8  if_bAlternateSetting; */
++		0x01,       /*  __u8  if_bNumEndpoints; */
++		0x09,       /*  __u8  if_bInterfaceClass; HUB_CLASSCODE */
++		0x00,       /*  __u8  if_bInterfaceSubClass; */
++		0x00,       /*  __u8  if_bInterfaceProtocol; [usb1.1 or single tt] */
++		0x00,       /*  __u8  if_iInterface; */
++	}, {
++		/* one endpoint (status change endpoint) */
++		0x07,       /*  __u8  ep_bLength; */
++		USB_DT_ENDPOINT, /* __u8 ep_bDescriptorType; Endpoint */
++		0x81,       /*  __u8  ep_bEndpointAddress; IN Endpoint 1 */
++		0x03,       /*  __u8  ep_bmAttributes; Interrupt */
++			/* __le16 ep_wMaxPacketSize; 1 + (MAX_ROOT_PORTS / 8)
++			* see hub.c:hub_configure() for details. */
++		(USB_MAXCHILDREN + 1 + 7) / 8, 0x00,
++		0x0c        /*  __u8  ep_bInterval; (256ms -- usb 2.0 spec) */
++	},
++};
++
++/*
++ * Access functions for isp176x registers regmap fields
++ */
++static u32 isp1760_hcd_read(struct usb_hcd *hcd, u32 field)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	return isp1760_field_read(priv->fields, field);
++}
++
++/*
++ * We need, in isp1763, to write directly the values to the portsc1
++ * register so it will make the other values to trigger.
++ */
++static void isp1760_hcd_portsc1_set_clear(struct isp1760_hcd *priv, u32 field,
++					  u32 val)
++{
++	u32 bit = isp1763_hc_portsc1_fields[field];
++	u32 port_status = readl(priv->base + ISP1763_HC_PORTSC1);
++
++	if (val)
++		writel(port_status | bit, priv->base + ISP1763_HC_PORTSC1);
++	else
++		writel(port_status & ~bit, priv->base + ISP1763_HC_PORTSC1);
++}
++
++static void isp1760_hcd_write(struct usb_hcd *hcd, u32 field, u32 val)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (unlikely(priv->is_isp1763 &&
++		     (field >= PORT_OWNER && field <= PORT_CONNECT)))
++		return isp1760_hcd_portsc1_set_clear(priv, field, val);
++
++	isp1760_field_write(priv->fields, field, val);
++}
++
++static void isp1760_hcd_set(struct usb_hcd *hcd, u32 field)
++{
++	isp1760_hcd_write(hcd, field, 0xFFFFFFFF);
++}
++
++static void isp1760_hcd_clear(struct usb_hcd *hcd, u32 field)
++{
++	isp1760_hcd_write(hcd, field, 0);
++}
++
++static int isp1760_hcd_set_and_wait(struct usb_hcd *hcd, u32 field,
++				    u32 timeout_us)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 val;
++
++	isp1760_hcd_set(hcd, field);
++
++	return regmap_field_read_poll_timeout(priv->fields[field], val,
++					      val, 10, timeout_us);
++}
++
++static int isp1760_hcd_set_and_wait_swap(struct usb_hcd *hcd, u32 field,
++					 u32 timeout_us)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 val;
++
++	isp1760_hcd_set(hcd, field);
++
++	return regmap_field_read_poll_timeout(priv->fields[field], val,
++					      !val, 10, timeout_us);
++}
++
++static int isp1760_hcd_clear_and_wait(struct usb_hcd *hcd, u32 field,
++				      u32 timeout_us)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 val;
++
++	isp1760_hcd_clear(hcd, field);
++
++	return regmap_field_read_poll_timeout(priv->fields[field], val,
++					      !val, 10, timeout_us);
++}
++
++static bool isp1760_hcd_is_set(struct usb_hcd *hcd, u32 field)
++{
++	return !!isp1760_hcd_read(hcd, field);
++}
++
++static bool isp1760_hcd_ppc_is_set(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (priv->is_isp1763)
++		return true;
++
++	return isp1760_hcd_is_set(hcd, HCS_PPC);
++}
++
++static u32 isp1760_hcd_n_ports(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (priv->is_isp1763)
++		return 1;
++
++	return isp1760_hcd_read(hcd, HCS_N_PORTS);
++}
++
++/*
++ * Access functions for isp176x memory (offset >= 0x0400).
++ *
++ * bank_reads8() reads memory locations prefetched by an earlier write to
++ * HC_MEMORY_REG (see isp176x datasheet). Unless you want to do fancy multi-
++ * bank optimizations, you should use the more generic mem_read() below.
++ *
++ * For access to ptd memory, use the specialized ptd_read() and ptd_write()
++ * below.
++ *
++ * These functions copy via MMIO data to/from the device. memcpy_{to|from}io()
++ * doesn't quite work because some people have to enforce 32-bit access
++ */
++static void bank_reads8(void __iomem *src_base, u32 src_offset, u32 bank_addr,
++							__u32 *dst, u32 bytes)
++{
++	__u32 __iomem *src;
++	u32 val;
++	__u8 *src_byteptr;
++	__u8 *dst_byteptr;
++
++	src = src_base + (bank_addr | src_offset);
++
++	if (src_offset < PAYLOAD_OFFSET) {
++		while (bytes >= 4) {
++			*dst = readl_relaxed(src);
++			bytes -= 4;
++			src++;
++			dst++;
++		}
++	} else {
++		while (bytes >= 4) {
++			*dst = __raw_readl(src);
++			bytes -= 4;
++			src++;
++			dst++;
++		}
++	}
++
++	if (!bytes)
++		return;
++
++	/* in case we have 3, 2 or 1 by left. The dst buffer may not be fully
++	 * allocated.
++	 */
++	if (src_offset < PAYLOAD_OFFSET)
++		val = readl_relaxed(src);
++	else
++		val = __raw_readl(src);
++
++	dst_byteptr = (void *) dst;
++	src_byteptr = (void *) &val;
++	while (bytes > 0) {
++		*dst_byteptr = *src_byteptr;
++		dst_byteptr++;
++		src_byteptr++;
++		bytes--;
++	}
++}
++
++static void isp1760_mem_read(struct usb_hcd *hcd, u32 src_offset, void *dst,
++			     u32 bytes)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	isp1760_hcd_write(hcd, MEM_BANK_SEL, ISP_BANK_0);
++	isp1760_hcd_write(hcd, MEM_START_ADDR, src_offset);
++	ndelay(100);
++
++	bank_reads8(priv->base, src_offset, ISP_BANK_0, dst, bytes);
++}
++
++/*
++ * ISP1763 does not have the banks direct host controller memory access,
++ * needs to use the HC_DATA register. Add data read/write according to this,
++ * and also adjust 16bit access.
++ */
++static void isp1763_mem_read(struct usb_hcd *hcd, u16 srcaddr,
++			     u16 *dstptr, u32 bytes)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++
++	/* Write the starting device address to the hcd memory register */
++	isp1760_reg_write(priv->regs, ISP1763_HC_MEMORY, srcaddr);
++	ndelay(100); /* Delay between consecutive access */
++
++	/* As long there are at least 16-bit to read ... */
++	while (bytes >= 2) {
++		*dstptr = __raw_readw(priv->base + ISP1763_HC_DATA);
++		bytes -= 2;
++		dstptr++;
++	}
++
++	/* If there are no more bytes to read, return */
++	if (bytes <= 0)
++		return;
++
++	*((u8 *)dstptr) = (u8)(readw(priv->base + ISP1763_HC_DATA) & 0xFF);
++}
++
++static void mem_read(struct usb_hcd *hcd, u32 src_offset, __u32 *dst,
++		     u32 bytes)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (!priv->is_isp1763)
++		return isp1760_mem_read(hcd, src_offset, (u16 *)dst, bytes);
++
++	isp1763_mem_read(hcd, (u16)src_offset, (u16 *)dst, bytes);
++}
++
++static void isp1760_mem_write(void __iomem *dst_base, u32 dst_offset,
++			      __u32 const *src, u32 bytes)
++{
++	__u32 __iomem *dst;
++
++	dst = dst_base + dst_offset;
++
++	if (dst_offset < PAYLOAD_OFFSET) {
++		while (bytes >= 4) {
++			writel_relaxed(*src, dst);
++			bytes -= 4;
++			src++;
++			dst++;
++		}
++	} else {
++		while (bytes >= 4) {
++			__raw_writel(*src, dst);
++			bytes -= 4;
++			src++;
++			dst++;
++		}
++	}
++
++	if (!bytes)
++		return;
++	/* in case we have 3, 2 or 1 bytes left. The buffer is allocated and the
++	 * extra bytes should not be read by the HW.
++	 */
++
++	if (dst_offset < PAYLOAD_OFFSET)
++		writel_relaxed(*src, dst);
++	else
++		__raw_writel(*src, dst);
++}
++
++static void isp1763_mem_write(struct usb_hcd *hcd, u16 dstaddr, u16 *src,
++			      u32 bytes)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	/* Write the starting device address to the hcd memory register */
++	isp1760_reg_write(priv->regs, ISP1763_HC_MEMORY, dstaddr);
++	ndelay(100); /* Delay between consecutive access */
++
++	while (bytes >= 2) {
++		/* Get and write the data; then adjust the data ptr and len */
++		__raw_writew(*src, priv->base + ISP1763_HC_DATA);
++		bytes -= 2;
++		src++;
++	}
++
++	/* If there are no more bytes to process, return */
++	if (bytes <= 0)
++		return;
++
++	/*
++	 * The only way to get here is if there is a single byte left,
++	 * get it and write it to the data reg;
++	 */
++	writew(*((u8 *)src), priv->base + ISP1763_HC_DATA);
++}
++
++static void mem_write(struct usb_hcd *hcd, u32 dst_offset, __u32 *src,
++		      u32 bytes)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (!priv->is_isp1763)
++		return isp1760_mem_write(priv->base, dst_offset, src, bytes);
++
++	isp1763_mem_write(hcd, dst_offset, (u16 *)src, bytes);
++}
++
++/*
++ * Read and write ptds. 'ptd_offset' should be one of ISO_PTD_OFFSET,
++ * INT_PTD_OFFSET, and ATL_PTD_OFFSET. 'slot' should be less than 32.
++ */
++static void isp1760_ptd_read(struct usb_hcd *hcd, u32 ptd_offset, u32 slot,
++			     struct ptd *ptd)
++{
++	u16 src_offset = ptd_offset + slot * sizeof(*ptd);
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	isp1760_hcd_write(hcd, MEM_BANK_SEL, ISP_BANK_0);
++	isp1760_hcd_write(hcd, MEM_START_ADDR, src_offset);
++	ndelay(90);
++
++	bank_reads8(priv->base, src_offset, ISP_BANK_0, (void *)ptd,
++		    sizeof(*ptd));
++}
++
++static void isp1763_ptd_read(struct usb_hcd *hcd, u32 ptd_offset, u32 slot,
++			     struct ptd *ptd)
++{
++	u16 src_offset = ptd_offset + slot * sizeof(*ptd);
++	struct ptd_le32 le32_ptd;
++
++	isp1763_mem_read(hcd, src_offset, (u16 *)&le32_ptd, sizeof(le32_ptd));
++	/* Normalize the data obtained */
++	ptd->dw0 = le32_to_dw(le32_ptd.dw0);
++	ptd->dw1 = le32_to_dw(le32_ptd.dw1);
++	ptd->dw2 = le32_to_dw(le32_ptd.dw2);
++	ptd->dw3 = le32_to_dw(le32_ptd.dw3);
++	ptd->dw4 = le32_to_dw(le32_ptd.dw4);
++	ptd->dw5 = le32_to_dw(le32_ptd.dw5);
++	ptd->dw6 = le32_to_dw(le32_ptd.dw6);
++	ptd->dw7 = le32_to_dw(le32_ptd.dw7);
++}
++
++static void ptd_read(struct usb_hcd *hcd, u32 ptd_offset, u32 slot,
++		     struct ptd *ptd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (!priv->is_isp1763)
++		return isp1760_ptd_read(hcd, ptd_offset, slot, ptd);
++
++	isp1763_ptd_read(hcd, ptd_offset, slot, ptd);
++}
++
++static void isp1763_ptd_write(struct usb_hcd *hcd, u32 ptd_offset, u32 slot,
++			      struct ptd *cpu_ptd)
++{
++	u16 dst_offset = ptd_offset + slot * sizeof(*cpu_ptd);
++	struct ptd_le32 ptd;
++
++	ptd.dw0 = dw_to_le32(cpu_ptd->dw0);
++	ptd.dw1 = dw_to_le32(cpu_ptd->dw1);
++	ptd.dw2 = dw_to_le32(cpu_ptd->dw2);
++	ptd.dw3 = dw_to_le32(cpu_ptd->dw3);
++	ptd.dw4 = dw_to_le32(cpu_ptd->dw4);
++	ptd.dw5 = dw_to_le32(cpu_ptd->dw5);
++	ptd.dw6 = dw_to_le32(cpu_ptd->dw6);
++	ptd.dw7 = dw_to_le32(cpu_ptd->dw7);
++
++	isp1763_mem_write(hcd, dst_offset,  (u16 *)&ptd.dw0,
++			  8 * sizeof(ptd.dw0));
++}
++
++static void isp1760_ptd_write(void __iomem *base, u32 ptd_offset, u32 slot,
++			      struct ptd *ptd)
++{
++	u32 dst_offset = ptd_offset + slot * sizeof(*ptd);
++
++	/*
++	 * Make sure dw0 gets written last (after other dw's and after payload)
++	 *  since it contains the enable bit
++	 */
++	isp1760_mem_write(base, dst_offset + sizeof(ptd->dw0),
++			  (__force u32 *)&ptd->dw1, 7 * sizeof(ptd->dw1));
++	wmb();
++	isp1760_mem_write(base, dst_offset, (__force u32 *)&ptd->dw0,
++			  sizeof(ptd->dw0));
++}
++
++static void ptd_write(struct usb_hcd *hcd, u32 ptd_offset, u32 slot,
++		      struct ptd *ptd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	if (!priv->is_isp1763)
++		return isp1760_ptd_write(priv->base, ptd_offset, slot, ptd);
++
++	isp1763_ptd_write(hcd, ptd_offset, slot, ptd);
++}
++
++/* memory management of the 60kb on the chip from 0x1000 to 0xffff */
++static void init_memory(struct isp1760_hcd *priv)
++{
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	int i, j, curr;
++	u32 payload_addr;
++
++	payload_addr = PAYLOAD_OFFSET;
++
++	for (i = 0, curr = 0; i < ARRAY_SIZE(mem->blocks); i++) {
++		for (j = 0; j < mem->blocks[i]; j++, curr++) {
++			priv->memory_pool[curr + j].start = payload_addr;
++			priv->memory_pool[curr + j].size = mem->blocks_size[i];
++			priv->memory_pool[curr + j].free = 1;
++			payload_addr += priv->memory_pool[curr + j].size;
++		}
++	}
++
++	WARN_ON(payload_addr - priv->memory_pool[0].start >
++		mem->payload_area_size);
++}
++
++static void alloc_mem(struct usb_hcd *hcd, struct isp1760_qtd *qtd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	int i;
++
++	WARN_ON(qtd->payload_addr);
++
++	if (!qtd->length)
++		return;
++
++	for (i = 0; i < mem->payload_blocks; i++) {
++		if (priv->memory_pool[i].size >= qtd->length &&
++				priv->memory_pool[i].free) {
++			priv->memory_pool[i].free = 0;
++			qtd->payload_addr = priv->memory_pool[i].start;
++			return;
++		}
++	}
++}
++
++static void free_mem(struct usb_hcd *hcd, struct isp1760_qtd *qtd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	int i;
++
++	if (!qtd->payload_addr)
++		return;
++
++	for (i = 0; i < mem->payload_blocks; i++) {
++		if (priv->memory_pool[i].start == qtd->payload_addr) {
++			WARN_ON(priv->memory_pool[i].free);
++			priv->memory_pool[i].free = 1;
++			qtd->payload_addr = 0;
++			return;
++		}
++	}
++
++	WARN_ON(1);
++	qtd->payload_addr = 0;
++}
++
++/* reset a non-running (STS_HALT == 1) controller */
++static int ehci_reset(struct usb_hcd *hcd)
++{
++	return isp1760_hcd_set_and_wait_swap(hcd, CMD_RESET, 250 * 1000);
++}
++
++static struct isp1760_qh *qh_alloc(gfp_t flags)
++{
++	struct isp1760_qh *qh;
++
++	qh = kmem_cache_alloc(qh_cachep, flags);
++	if (!qh)
++		return NULL;
++
++	memset(qh, '\0', qh_cachep->sz);
++	INIT_LIST_HEAD(&qh->qh_list);
++	INIT_LIST_HEAD(&qh->qtd_list);
++	qh->slot = -1;
++
++	return qh;
++}
++
++static void qh_free(struct isp1760_qh *qh)
++{
++	WARN_ON(!list_empty(&qh->qtd_list));
++	WARN_ON(qh->slot > -1);
++	kmem_cache_free(qh_cachep, qh);
++}
++
++/* one-time init, only for memory state */
++static int priv_init(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 isoc_cache;
++	u32 isoc_thres;
++	int i;
++
++	spin_lock_init(&priv->lock);
++
++	for (i = 0; i < QH_END; i++)
++		INIT_LIST_HEAD(&priv->qh_list[i]);
++
++	/*
++	 * hw default: 1K periodic list heads, one per frame.
++	 * periodic_size can shrink by USBCMD update if hcc_params allows.
++	 */
++	priv->periodic_size = DEFAULT_I_TDPS;
++
++	if (priv->is_isp1763) {
++		priv->i_thresh = 2;
++		return 0;
++	}
++
++	/* controllers may cache some of the periodic schedule ... */
++	isoc_cache = isp1760_hcd_read(hcd, HCC_ISOC_CACHE);
++	isoc_thres = isp1760_hcd_read(hcd, HCC_ISOC_THRES);
++
++	/* full frame cache */
++	if (isoc_cache)
++		priv->i_thresh = 8;
++	else /* N microframes cached */
++		priv->i_thresh = 2 + isoc_thres;
++
++	return 0;
++}
++
++static int isp1760_hc_setup(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 atx_reset;
++	int result;
++	u32 scratch;
++	u32 pattern;
++
++	if (priv->is_isp1763)
++		pattern = 0xcafe;
++	else
++		pattern = 0xdeadcafe;
++
++	isp1760_hcd_write(hcd, HC_SCRATCH, pattern);
++
++	/* Change bus pattern */
++	scratch = isp1760_hcd_read(hcd, HC_CHIP_ID_HIGH);
++	scratch = isp1760_hcd_read(hcd, HC_SCRATCH);
++	if (scratch != pattern) {
++		printf("Scratch test failed. 0x%08x\n", scratch);
++		return -ENODEV;
++	}
++
++	/*
++	 * The RESET_HC bit in the SW_RESET register is supposed to reset the
++	 * host controller without touching the CPU interface registers, but at
++	 * least on the ISP1761 it seems to behave as the RESET_ALL bit and
++	 * reset the whole device. We thus can't use it here, so let's reset
++	 * the host controller through the EHCI USB Command register. The device
++	 * has been reset in core code anyway, so this shouldn't matter.
++	 */
++	isp1760_hcd_clear(hcd, ISO_BUF_FILL);
++	isp1760_hcd_clear(hcd, INT_BUF_FILL);
++	isp1760_hcd_clear(hcd, ATL_BUF_FILL);
++
++	isp1760_hcd_set(hcd, HC_ATL_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_INT_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_ISO_PTD_SKIPMAP);
++
++	result = ehci_reset(hcd);
++	if (result)
++		return result;
++
++	/* Step 11 passed */
++
++	/* ATL reset */
++	if (priv->is_isp1763)
++		atx_reset = SW_RESET_RESET_ATX;
++	else
++		atx_reset = ALL_ATX_RESET;
++
++	isp1760_hcd_set(hcd, atx_reset);
++	mdelay(10);
++	isp1760_hcd_clear(hcd, atx_reset);
++
++	if (priv->is_isp1763) {
++		isp1760_hcd_set(hcd, HW_OTG_DISABLE);
++		isp1760_hcd_set(hcd, HW_SW_SEL_HC_DC_CLEAR);
++		isp1760_hcd_set(hcd, HW_HC_2_DIS_CLEAR);
++		isp1760_hcd_set(hcd, HW_DM_PULLDOWN);
++		isp1760_hcd_set(hcd, HW_DP_PULLDOWN);
++		mdelay(10);
++
++		isp1760_hcd_set(hcd, HW_INTF_LOCK);
++	}
++
++	isp1760_hcd_set(hcd, HC_INT_IRQ_ENABLE);
++	isp1760_hcd_set(hcd, HC_ATL_IRQ_ENABLE);
++
++	return priv_init(hcd);
++}
++
++static u32 base_to_chip(u32 base)
++{
++	return ((base - 0x400) >> 3);
++}
++
++static int last_qtd_of_urb(struct isp1760_qtd *qtd, struct isp1760_qh *qh)
++{
++	struct urb *urb;
++
++	if (list_is_last(&qtd->qtd_list, &qh->qtd_list))
++		return 1;
++
++	urb = qtd->urb;
++	qtd = list_entry(qtd->qtd_list.next, typeof(*qtd), qtd_list);
++
++	return (qtd->urb != urb);
++}
++
++/* magic numbers that can affect system performance */
++#define	EHCI_TUNE_CERR		3	/* 0-3 qtd retries; 0 == don't stop */
++#define	EHCI_TUNE_RL_HS		4	/* nak throttle; see 4.9 */
++#define	EHCI_TUNE_RL_TT		0
++#define	EHCI_TUNE_MULT_HS	1	/* 1-3 transactions/uframe; 4.10.3 */
++#define	EHCI_TUNE_MULT_TT	1
++#define	EHCI_TUNE_FLS		2	/* (small) 256 frame schedule */
++
++static void create_ptd_atl(struct isp1760_qh *qh, struct isp1760_qtd *qtd,
++			   struct ptd *ptd)
++{
++	u32 maxpacket;
++	u32 multi;
++	u32 rl = RL_COUNTER;
++	u32 nak = NAK_COUNTER;
++	u8 portnr;
++	u8 hubaddr;
++
++	memset(ptd, 0, sizeof(*ptd));
++
++	/* according to 3.6.2, max packet len can not be > 0x400 */
++	maxpacket = usb_maxpacket(qtd->urb->dev, qtd->urb->pipe);
++	multi =  1 + ((maxpacket >> 11) & 0x3);
++	maxpacket &= 0x7ff;
++
++	/* DW0 */
++	ptd->dw0 = DW0_VALID_BIT;
++	ptd->dw0 |= TO_DW0_LENGTH(qtd->length);
++	ptd->dw0 |= TO_DW0_MAXPACKET(maxpacket);
++	ptd->dw0 |= TO_DW0_ENDPOINT(usb_pipeendpoint(qtd->urb->pipe));
++
++	/* DW1 */
++	ptd->dw1 = TO_DW((usb_pipeendpoint(qtd->urb->pipe) >> 1));
++	ptd->dw1 |= TO_DW1_DEVICE_ADDR(usb_pipedevice(qtd->urb->pipe));
++	ptd->dw1 |= TO_DW1_PID_TOKEN(qtd->packet_type);
++
++	if (usb_pipebulk(qtd->urb->pipe))
++		ptd->dw1 |= DW1_TRANS_BULK;
++	else if  (usb_pipeint(qtd->urb->pipe))
++		ptd->dw1 |= DW1_TRANS_INT;
++
++	if (qtd->urb->dev->speed != USB_SPEED_HIGH) {
++		/* split transaction */
++
++		ptd->dw1 |= DW1_TRANS_SPLIT;
++		if (qtd->urb->dev->speed == USB_SPEED_LOW)
++			ptd->dw1 |= DW1_SE_USB_LOSPEED;
++
++		if (!qtd->urb->dev->dev->parent_priv_) {
++			portnr = qtd->urb->dev->portnr;
++			hubaddr = qtd->urb->dev->devnum;
++		} else {
++			usb_find_usb2_hub_address_port(qtd->urb->dev, &hubaddr,
++						       &portnr);
++		}
++
++		ptd->dw1 |= TO_DW1_PORT_NUM(portnr);
++		ptd->dw1 |= TO_DW1_HUB_NUM(hubaddr);
++
++		/* SE bit for Split INT transfers */
++		if (usb_pipeint(qtd->urb->pipe) &&
++				(qtd->urb->dev->speed == USB_SPEED_LOW))
++			ptd->dw1 |= DW1_SE_USB_LOSPEED;
++
++		rl = 0;
++		nak = 0;
++	} else {
++		ptd->dw0 |= TO_DW0_MULTI(multi);
++		if (usb_pipecontrol(qtd->urb->pipe) ||
++						usb_pipebulk(qtd->urb->pipe))
++			ptd->dw3 |= TO_DW3_PING(qh->ping);
++	}
++	/* DW2 */
++	ptd->dw2 = 0;
++	ptd->dw2 |= TO_DW2_DATA_START_ADDR(base_to_chip(qtd->payload_addr));
++	ptd->dw2 |= TO_DW2_RL(rl);
++
++	/* DW3 */
++	ptd->dw3 |= TO_DW3_NAKCOUNT(nak);
++	ptd->dw3 |= TO_DW3_DATA_TOGGLE(qh->toggle);
++
++	if (usb_pipecontrol(qtd->urb->pipe)) {
++		if (qtd->data_buffer == qtd->urb->setup_packet) {
++			ptd->dw3 &= ~TO_DW3_DATA_TOGGLE(1);
++		} else if (last_qtd_of_urb(qtd, qh)) {
++			ptd->dw3 |= TO_DW3_DATA_TOGGLE(1);
++		}
++	}
++
++	ptd->dw3 |= DW3_ACTIVE_BIT;
++	/* Cerr */
++	ptd->dw3 |= TO_DW3_CERR(ERR_COUNTER);
++}
++
++static void transform_add_int(struct isp1760_qh *qh, struct isp1760_qtd *qtd,
++			      struct ptd *ptd)
++{
++	struct usb_host_endpoint *hep = qtd->urb->ep;
++	struct usb_endpoint_descriptor *epd = &hep->desc;
++	u32 usof;
++	u32 period;
++
++	/*
++	 * Most of this is guessing. ISP1761 datasheet is quite unclear, and
++	 * the algorithm from the original Philips driver code, which was
++	 * pretty much used in this driver before as well, is quite horrendous
++	 * and, i believe, incorrect. The code below follows the datasheet and
++	 * USB2.0 spec as far as I can tell, and plug/unplug seems to be much
++	 * more reliable this way (fingers crossed...).
++	 */
++
++	if (qtd->urb->dev->speed == USB_SPEED_HIGH) {
++		/* urb->interval is in units of microframes (1/8 ms) */
++		period = epd->bInterval >> 3;
++
++		if (epd->bInterval > 4)
++			usof = 0x01; /* One bit set =>
++						interval 1 ms * uFrame-match */
++		else if (epd->bInterval > 2)
++			usof = 0x22; /* Two bits set => interval 1/2 ms */
++		else if (epd->bInterval > 1)
++			usof = 0x55; /* Four bits set => interval 1/4 ms */
++		else
++			usof = 0xff; /* All bits set => interval 1/8 ms */
++	} else {
++		/* urb->interval is in units of frames (1 ms) */
++		period = epd->bInterval;
++		usof = 0x0f;		/* Execute Start Split on any of the
++					   four first uFrames */
++
++		/*
++		 * First 8 bits in dw5 is uSCS and "specifies which uSOF the
++		 * complete split needs to be sent. Valid only for IN." Also,
++		 * "All bits can be set to one for every transfer." (p 82,
++		 * ISP1761 data sheet.) 0x1c is from Philips driver. Where did
++		 * that number come from? 0xff seems to work fine...
++		 */
++		/* ptd->dw5 = 0x1c; */
++		ptd->dw5 = TO_DW(0xff); /* Execute Complete Split on any uFrame */
++	}
++
++	period = period >> 1;/* Ensure equal or shorter period than requested */
++	period &= 0xf8; /* Mask off too large values and lowest unused 3 bits */
++
++	ptd->dw2 |= TO_DW(period);
++	ptd->dw4 = TO_DW(usof);
++}
++
++static void create_ptd_int(struct isp1760_qh *qh, struct isp1760_qtd *qtd,
++			   struct ptd *ptd)
++{
++	create_ptd_atl(qh, qtd, ptd);
++	transform_add_int(qh, qtd, ptd);
++}
++
++static void isp1760_urb_done(struct usb_hcd *hcd, struct urb *urb)
++{
++	if (usb_pipein(urb->pipe) && usb_pipetype(urb->pipe) != PIPE_CONTROL) {
++		void *ptr;
++		for (ptr = urb->transfer_buffer;
++		     ptr < urb->transfer_buffer + urb->transfer_buffer_length;
++		     ptr += PAGE_SIZE)
++			flush_dcache_range((unsigned  long)ptr,
++					   (unsigned long)ptr + PAGE_SIZE);
++	}
++
++	/* complete() can reenter this HCD */
++	usb_hcd_unlink_urb_from_ep(hcd, urb);
++	usb_hcd_giveback_urb(hcd, urb, urb->status);
++}
++
++static struct isp1760_qtd *qtd_alloc(gfp_t flags, struct urb *urb,
++				     u8 packet_type)
++{
++	struct isp1760_qtd *qtd;
++
++	qtd = kmem_cache_alloc(qtd_cachep, flags);
++	if (!qtd)
++		return NULL;
++
++	memset(qtd, '\0', sizeof(*qtd));
++	INIT_LIST_HEAD(&qtd->qtd_list);
++	qtd->urb = urb;
++	qtd->packet_type = packet_type;
++	qtd->status = QTD_ENQUEUED;
++	qtd->actual_length = 0;
++
++	return qtd;
++}
++
++static void qtd_free(struct isp1760_qtd *qtd)
++{
++	WARN_ON(qtd->payload_addr);
++	kmem_cache_free(qtd_cachep, qtd);
++}
++
++static void start_bus_transfer(struct usb_hcd *hcd, u32 ptd_offset, int slot,
++				struct isp1760_slotinfo *slots,
++				struct isp1760_qtd *qtd, struct isp1760_qh *qh,
++				struct ptd *ptd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	int skip_map;
++
++	WARN_ON((slot < 0) || (slot > mem->slot_num - 1));
++	WARN_ON(qtd->length && !qtd->payload_addr);
++	WARN_ON(slots[slot].qtd);
++	WARN_ON(slots[slot].qh);
++	WARN_ON(qtd->status != QTD_PAYLOAD_ALLOC);
++
++	if (priv->is_isp1763)
++		ndelay(100);
++
++	/* Make sure done map has not triggered from some unlinked transfer */
++	if (ptd_offset == ATL_PTD_OFFSET) {
++		skip_map = isp1760_hcd_read(hcd, HC_ATL_PTD_SKIPMAP);
++		isp1760_hcd_write(hcd, HC_ATL_PTD_SKIPMAP,
++				  skip_map | (1 << slot));
++		priv->atl_done_map |= isp1760_hcd_read(hcd, HC_ATL_PTD_DONEMAP);
++		priv->atl_done_map &= ~(1 << slot);
++	} else {
++		skip_map = isp1760_hcd_read(hcd, HC_INT_PTD_SKIPMAP);
++		isp1760_hcd_write(hcd, HC_INT_PTD_SKIPMAP,
++				  skip_map | (1 << slot));
++		priv->int_done_map |= isp1760_hcd_read(hcd, HC_INT_PTD_DONEMAP);
++		priv->int_done_map &= ~(1 << slot);
++	}
++
++	skip_map &= ~(1 << slot);
++	qh->slot = slot;
++	qtd->status = QTD_XFER_STARTED;
++	slots[slot].qtd = qtd;
++	slots[slot].qh = qh;
++
++	ptd_write(hcd, ptd_offset, slot, ptd);
++
++	if (ptd_offset == ATL_PTD_OFFSET)
++		isp1760_hcd_write(hcd, HC_ATL_PTD_SKIPMAP, skip_map);
++	else
++		isp1760_hcd_write(hcd, HC_INT_PTD_SKIPMAP, skip_map);
++}
++
++static int is_short_bulk(struct isp1760_qtd *qtd)
++{
++	return (usb_pipebulk(qtd->urb->pipe) &&
++					(qtd->actual_length < qtd->length));
++}
++
++static void collect_qtds(struct usb_hcd *hcd, struct isp1760_qh *qh,
++						struct list_head *urb_list)
++{
++	struct isp1760_qtd *qtd, *qtd_next;
++	struct urb_listitem *urb_listitem;
++	int last_qtd;
++
++	list_for_each_entry_safe(qtd, qtd_next, &qh->qtd_list, qtd_list) {
++		if (qtd->status < QTD_XFER_COMPLETE)
++			break;
++
++		last_qtd = last_qtd_of_urb(qtd, qh);
++
++		if ((!last_qtd) && (qtd->status == QTD_RETIRE))
++			qtd_next->status = QTD_RETIRE;
++
++		if (qtd->status == QTD_XFER_COMPLETE) {
++			if (qtd->actual_length) {
++				switch (qtd->packet_type) {
++				case IN_PID:
++					mem_read(hcd, qtd->payload_addr,
++						 qtd->data_buffer,
++						 qtd->actual_length);
++					fallthrough;
++				case OUT_PID:
++					qtd->urb->actual_length +=
++							qtd->actual_length;
++					fallthrough;
++				case SETUP_PID:
++					break;
++				}
++			}
++
++			if (is_short_bulk(qtd)) {
++				if (qtd->urb->transfer_flags & URB_SHORT_NOT_OK)
++					qtd->urb->status = -EREMOTEIO;
++				if (!last_qtd)
++					qtd_next->status = QTD_RETIRE;
++			}
++		}
++
++		if (qtd->payload_addr)
++			free_mem(hcd, qtd);
++
++		if (last_qtd) {
++			if ((qtd->status == QTD_RETIRE) &&
++					(qtd->urb->status == -EINPROGRESS))
++				qtd->urb->status = -EPIPE;
++			/* Defer calling of urb_done() since it releases lock */
++			urb_listitem = kmem_cache_alloc(urb_listitem_cachep,
++								GFP_ATOMIC);
++			if (unlikely(!urb_listitem))
++				break; /* Try again on next call */
++			urb_listitem->urb = qtd->urb;
++			list_add_tail(&urb_listitem->urb_list, urb_list);
++		}
++
++		list_del(&qtd->qtd_list);
++		qtd_free(qtd);
++	}
++}
++
++#define ENQUEUE_DEPTH	2
++static void enqueue_qtds(struct usb_hcd *hcd, struct isp1760_qh *qh)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	int slot_num = mem->slot_num;
++	int ptd_offset;
++	struct isp1760_slotinfo *slots;
++	int curr_slot, free_slot;
++	int n;
++	struct ptd ptd;
++	struct isp1760_qtd *qtd;
++
++	if (unlikely(list_empty(&qh->qtd_list)))
++		return;
++
++	/* Make sure this endpoint's TT buffer is clean before queueing ptds */
++	if (qh->tt_buffer_dirty)
++		return;
++
++	if (usb_pipeint(list_entry(qh->qtd_list.next, struct isp1760_qtd,
++				   qtd_list)->urb->pipe)) {
++		ptd_offset = INT_PTD_OFFSET;
++		slots = priv->int_slots;
++	} else {
++		ptd_offset = ATL_PTD_OFFSET;
++		slots = priv->atl_slots;
++	}
++
++	free_slot = -1;
++	for (curr_slot = 0; curr_slot < slot_num; curr_slot++) {
++		if ((free_slot == -1) && (slots[curr_slot].qtd == NULL))
++			free_slot = curr_slot;
++		if (slots[curr_slot].qh == qh)
++			break;
++	}
++
++	n = 0;
++	list_for_each_entry(qtd, &qh->qtd_list, qtd_list) {
++		if (qtd->status == QTD_ENQUEUED) {
++			WARN_ON(qtd->payload_addr);
++			alloc_mem(hcd, qtd);
++			if ((qtd->length) && (!qtd->payload_addr))
++				break;
++
++			if (qtd->length && (qtd->packet_type == SETUP_PID ||
++					    qtd->packet_type == OUT_PID)) {
++				mem_write(hcd, qtd->payload_addr,
++					  qtd->data_buffer, qtd->length);
++			}
++
++			qtd->status = QTD_PAYLOAD_ALLOC;
++		}
++
++		if (qtd->status == QTD_PAYLOAD_ALLOC) {
++/*
++			if ((curr_slot > 31) && (free_slot == -1))
++				printf("%s: No slot "
++					"available for transfer\n", __func__);
++*/
++			/* Start xfer for this endpoint if not already done */
++			if ((curr_slot > slot_num - 1) && (free_slot > -1)) {
++				if (usb_pipeint(qtd->urb->pipe))
++					create_ptd_int(qh, qtd, &ptd);
++				else
++					create_ptd_atl(qh, qtd, &ptd);
++
++				start_bus_transfer(hcd, ptd_offset, free_slot,
++							slots, qtd, qh, &ptd);
++				curr_slot = free_slot;
++			}
++
++			n++;
++			if (n >= ENQUEUE_DEPTH)
++				break;
++		}
++	}
++}
++
++static void schedule_ptds(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv;
++	struct isp1760_qh *qh, *qh_next;
++	struct list_head *ep_queue;
++	LIST_HEAD(urb_list);
++	struct urb_listitem *urb_listitem, *urb_listitem_next;
++	int i;
++
++	if (!hcd) {
++		WARN_ON(1);
++		return;
++	}
++
++	priv = hcd_to_priv(hcd);
++
++	/*
++	 * check finished/retired xfers, transfer payloads, call urb_done()
++	 */
++	for (i = 0; i < QH_END; i++) {
++		ep_queue = &priv->qh_list[i];
++		list_for_each_entry_safe(qh, qh_next, ep_queue, qh_list)
++			collect_qtds(hcd, qh, &urb_list);
++	}
++
++	list_for_each_entry_safe(urb_listitem, urb_listitem_next, &urb_list,
++				 urb_list) {
++		isp1760_urb_done(hcd, urb_listitem->urb);
++		kmem_cache_free(urb_listitem_cachep, urb_listitem);
++	}
++
++	/*
++	 * Schedule packets for transfer.
++	 *
++	 * According to USB2.0 specification:
++	 *
++	 * 1st prio: interrupt xfers, up to 80 % of bandwidth
++	 * 2nd prio: control xfers
++	 * 3rd prio: bulk xfers
++	 *
++	 * ... but let's use a simpler scheme here (mostly because ISP1761 doc
++	 * is very unclear on how to prioritize traffic):
++	 *
++	 * 1) Enqueue any queued control transfers, as long as payload chip mem
++	 *    and PTD ATL slots are available.
++	 * 2) Enqueue any queued INT transfers, as long as payload chip mem
++	 *    and PTD INT slots are available.
++	 * 3) Enqueue any queued bulk transfers, as long as payload chip mem
++	 *    and PTD ATL slots are available.
++	 *
++	 * Use double buffering (ENQUEUE_DEPTH==2) as a compromise between
++	 * conservation of chip mem and performance.
++	 *
++	 * I'm sure this scheme could be improved upon!
++	 */
++	for (i = 0; i < QH_END; i++) {
++		ep_queue = &priv->qh_list[i];
++		list_for_each_entry_safe(qh, qh_next, ep_queue, qh_list)
++			enqueue_qtds(hcd, qh);
++	}
++}
++
++#define PTD_STATE_QTD_DONE	1
++#define PTD_STATE_QTD_RELOAD	2
++#define PTD_STATE_URB_RETIRE	3
++
++static int check_int_transfer(struct usb_hcd *hcd, struct ptd *ptd,
++								struct urb *urb)
++{
++	u32 dw4;
++	int i;
++
++	dw4 = TO_U32(ptd->dw4);
++	dw4 >>= 8;
++
++	/* FIXME: ISP1761 datasheet does not say what to do with these. Do we
++	   need to handle these errors? Is it done in hardware? */
++	if (ptd->dw3 & DW3_HALT_BIT) {
++
++		urb->status = -EPROTO; /* Default unknown error */
++
++		for (i = 0; i < 8; i++) {
++			switch (dw4 & 0x7) {
++			case INT_UNDERRUN:
++				printf("underrun during uFrame %d\n", i);
++				urb->status = -ECOMM; /* Could not write data */
++				break;
++			case INT_EXACT:
++				printf("transaction error uFrame %d\n", i);
++				urb->status = -EPROTO; /* timeout, bad CRC, PID
++							  error etc. */
++				break;
++			case INT_BABBLE:
++				printf("babble error during uFrame %d\n", i);
++				urb->status = -EOVERFLOW;
++				break;
++			}
++			dw4 >>= 3;
++		}
++
++		return PTD_STATE_URB_RETIRE;
++	}
++
++	return PTD_STATE_QTD_DONE;
++}
++
++static int check_atl_transfer(struct usb_hcd *hcd, struct ptd *ptd,
++			      struct urb *urb)
++{
++	WARN_ON(!ptd);
++	if (ptd->dw3 & DW3_HALT_BIT) {
++		if (ptd->dw3 & DW3_BABBLE_BIT)
++			urb->status = -EOVERFLOW;
++		else if (FROM_DW3_CERR(ptd->dw3))
++			urb->status = -EPIPE;  /* Stall */
++		else
++			urb->status = -EPROTO; /* Unknown */
++
++		/* usefull debug
++		printf("%s: ptd error:\n"
++			"        dw0: %08x dw1: %08x dw2: %08x dw3: %08x\n"
++			"        dw4: %08x dw5: %08x dw6: %08x dw7: %08x\n",
++			__func__,
++			ptd->dw0, ptd->dw1, ptd->dw2, ptd->dw3,
++			ptd->dw4, ptd->dw5, ptd->dw6, ptd->dw7);
++		*/
++
++		return PTD_STATE_URB_RETIRE;
++	}
++
++	/* Transfer Error, *but* active and no HALT -> reload */
++	if ((ptd->dw3 & DW3_ERROR_BIT) && (ptd->dw3 & DW3_ACTIVE_BIT))
++		return PTD_STATE_QTD_RELOAD;
++
++	/*
++	* NAKs are handled in HW by the chip. Usually if the
++	* device is not able to send data fast enough.
++	* This happens mostly on slower hardware.
++	*/
++	if (!FROM_DW3_NAKCOUNT(ptd->dw3) && (ptd->dw3 & DW3_ACTIVE_BIT))
++		return PTD_STATE_QTD_RELOAD;
++
++	return PTD_STATE_QTD_DONE;
++}
++
++static void handle_done_ptds(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	struct isp1760_slotinfo *slots;
++	struct isp1760_qtd *qtd;
++	struct isp1760_qh *qh;
++	struct ptd ptd;
++	u32 ptd_offset;
++	int modified;
++	int skip_map;
++	int state;
++	int slot;
++
++	skip_map = isp1760_hcd_read(hcd, HC_INT_PTD_SKIPMAP);
++	priv->int_done_map &= ~skip_map;
++	skip_map = isp1760_hcd_read(hcd, HC_ATL_PTD_SKIPMAP);
++	priv->atl_done_map &= ~skip_map;
++
++	modified = priv->int_done_map || priv->atl_done_map;
++
++	while (priv->int_done_map || priv->atl_done_map) {
++		if (priv->int_done_map) {
++			/* INT ptd */
++			slot = __ffs(priv->int_done_map);
++			priv->int_done_map &= ~(1 << slot);
++			slots = priv->int_slots;
++			/* This should not trigger, and could be removed if
++			   noone have any problems with it triggering: */
++			if (!slots[slot].qh) {
++				WARN_ON(1);
++				continue;
++			}
++			ptd_offset = INT_PTD_OFFSET;
++			ptd_read(hcd, INT_PTD_OFFSET, slot, &ptd);
++			state = check_int_transfer(hcd, &ptd,
++						   slots[slot].qtd->urb);
++		} else {
++			/* ATL ptd */
++			slot = __ffs(priv->atl_done_map);
++			priv->atl_done_map &= ~(1 << slot);
++			slots = priv->atl_slots;
++			/* This should not trigger, and could be removed if
++			   noone have any problems with it triggering: */
++			if (!slots[slot].qh) {
++				WARN_ON(1);
++				continue;
++			}
++			ptd_offset = ATL_PTD_OFFSET;
++			ptd_read(hcd, ATL_PTD_OFFSET, slot, &ptd);
++			state = check_atl_transfer(hcd, &ptd,
++						   slots[slot].qtd->urb);
++		}
++
++		qtd = slots[slot].qtd;
++		slots[slot].qtd = NULL;
++		qh = slots[slot].qh;
++		slots[slot].qh = NULL;
++		qh->slot = -1;
++
++		WARN_ON(qtd->status != QTD_XFER_STARTED);
++
++		switch (state) {
++		case PTD_STATE_QTD_DONE:
++			if ((usb_pipeint(qtd->urb->pipe)) &&
++			    (qtd->urb->dev->speed != USB_SPEED_HIGH))
++				qtd->actual_length =
++				       FROM_DW3_SCS_NRBYTESTRANSFERRED(ptd.dw3);
++			else
++				qtd->actual_length =
++					FROM_DW3_NRBYTESTRANSFERRED(ptd.dw3);
++
++			qtd->status = QTD_XFER_COMPLETE;
++
++			if (list_is_last(&qtd->qtd_list, &qh->qtd_list) ||
++			    is_short_bulk(qtd))
++				qtd = NULL;
++			else
++				qtd = list_entry(qtd->qtd_list.next,
++							typeof(*qtd), qtd_list);
++
++			qh->toggle = FROM_DW3_DATA_TOGGLE(ptd.dw3);
++			qh->ping = FROM_DW3_PING(ptd.dw3);
++
++			break;
++
++		case PTD_STATE_QTD_RELOAD: /* QTD_RETRY, for atls only */
++			qtd->status = QTD_PAYLOAD_ALLOC;
++			ptd.dw0 |= DW0_VALID_BIT;
++			/* RL counter = ERR counter */
++			ptd.dw3 &= ~TO_DW3_NAKCOUNT(0xf);
++			ptd.dw3 |= TO_DW3_NAKCOUNT(FROM_DW2_RL(ptd.dw2));
++			ptd.dw3 &= ~TO_DW3_CERR(3);
++			ptd.dw3 |= TO_DW3_CERR(ERR_COUNTER);
++
++			qh->toggle = FROM_DW3_DATA_TOGGLE(ptd.dw3);
++			qh->ping = FROM_DW3_PING(ptd.dw3);
++			break;
++
++		case PTD_STATE_URB_RETIRE:
++			qtd->status = QTD_RETIRE;
++			qtd = NULL;
++			qh->toggle = 0;
++			qh->ping = 0;
++			break;
++
++		default:
++			WARN_ON(1);
++			continue;
++		}
++
++		if (qtd && (qtd->status == QTD_PAYLOAD_ALLOC)) {
++			if (slots == priv->int_slots) {
++				if (state == PTD_STATE_QTD_RELOAD)
++					dev_err(priv->dev,
++						"%s: PTD_STATE_QTD_RELOAD on "
++						"interrupt packet\n", __func__);
++				if (state != PTD_STATE_QTD_RELOAD)
++					create_ptd_int(qh, qtd, &ptd);
++			} else {
++				if (state != PTD_STATE_QTD_RELOAD)
++					create_ptd_atl(qh, qtd, &ptd);
++			}
++
++			start_bus_transfer(hcd, ptd_offset, slot, slots, qtd,
++				qh, &ptd);
++		}
++	}
++
++	if (modified)
++		schedule_ptds(hcd);
++}
++
++static irqreturn_t isp1760_irq(int irq, void *__hci)
++{
++	struct usb_hcd *hcd = __hci;
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	irqreturn_t irqret = IRQ_NONE;
++	u32 int_reg;
++	u32 imask;
++
++	imask = isp1760_hcd_read(hcd, HC_INTERRUPT);
++	if (unlikely(!imask))
++		return irqret;
++
++	int_reg = priv->is_isp1763 ? ISP1763_HC_INTERRUPT :
++		ISP176x_HC_INTERRUPT;
++	isp1760_reg_write(priv->regs, int_reg, imask);
++
++	priv->int_done_map |= isp1760_hcd_read(hcd, HC_INT_PTD_DONEMAP);
++	priv->atl_done_map |= isp1760_hcd_read(hcd, HC_ATL_PTD_DONEMAP);
++
++	handle_done_ptds(hcd);
++
++	irqret = IRQ_HANDLED;
++
++	return irqret;
++}
++
++static int isp1763_run(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	int retval;
++	u32 chipid_h;
++	u32 chipid_l;
++	u32 chip_rev;
++	u32 ptd_atl_int;
++	u32 ptd_iso;
++
++	chipid_h = isp1760_hcd_read(hcd, HC_CHIP_ID_HIGH);
++	chipid_l = isp1760_hcd_read(hcd, HC_CHIP_ID_LOW);
++	chip_rev = isp1760_hcd_read(hcd, HC_CHIP_REV);
++	printf("USB ISP %02x%02x HW rev. %d started\n", chipid_h,
++	       chipid_l, chip_rev);
++
++	isp1760_hcd_clear(hcd, ISO_BUF_FILL);
++	isp1760_hcd_clear(hcd, INT_BUF_FILL);
++	isp1760_hcd_clear(hcd, ATL_BUF_FILL);
++
++	isp1760_hcd_set(hcd, HC_ATL_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_INT_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_ISO_PTD_SKIPMAP);
++	ndelay(100);
++	isp1760_hcd_clear(hcd, HC_ATL_PTD_DONEMAP);
++	isp1760_hcd_clear(hcd, HC_INT_PTD_DONEMAP);
++	isp1760_hcd_clear(hcd, HC_ISO_PTD_DONEMAP);
++
++	isp1760_hcd_set(hcd, HW_OTG_DISABLE);
++	isp1760_reg_write(priv->regs, ISP1763_HC_OTG_CTRL_CLEAR, BIT(7));
++	isp1760_reg_write(priv->regs, ISP1763_HC_OTG_CTRL_CLEAR, BIT(15));
++	mdelay(10);
++
++	isp1760_hcd_set(hcd, HC_INT_IRQ_ENABLE);
++	isp1760_hcd_set(hcd, HC_ATL_IRQ_ENABLE);
++
++	isp1760_hcd_set(hcd, HW_GLOBAL_INTR_EN);
++
++	isp1760_hcd_clear(hcd, HC_ATL_IRQ_MASK_AND);
++	isp1760_hcd_clear(hcd, HC_INT_IRQ_MASK_AND);
++	isp1760_hcd_clear(hcd, HC_ISO_IRQ_MASK_AND);
++
++	isp1760_hcd_set(hcd, HC_ATL_IRQ_MASK_OR);
++	isp1760_hcd_set(hcd, HC_INT_IRQ_MASK_OR);
++	isp1760_hcd_set(hcd, HC_ISO_IRQ_MASK_OR);
++
++	ptd_atl_int = 0x8000;
++	ptd_iso = 0x0001;
++
++	isp1760_hcd_write(hcd, HC_ATL_PTD_LASTPTD, ptd_atl_int);
++	isp1760_hcd_write(hcd, HC_INT_PTD_LASTPTD, ptd_atl_int);
++	isp1760_hcd_write(hcd, HC_ISO_PTD_LASTPTD, ptd_iso);
++
++	isp1760_hcd_set(hcd, ATL_BUF_FILL);
++	isp1760_hcd_set(hcd, INT_BUF_FILL);
++
++	isp1760_hcd_clear(hcd, CMD_LRESET);
++	isp1760_hcd_clear(hcd, CMD_RESET);
++
++	retval = isp1760_hcd_set_and_wait(hcd, CMD_RUN, 250 * 1000);
++	if (retval)
++		return retval;
++
++	down_write(&ehci_cf_port_reset_rwsem);
++	retval = isp1760_hcd_set_and_wait(hcd, FLAG_CF, 250 * 1000);
++	up_write(&ehci_cf_port_reset_rwsem);
++	retval = 0;
++	if (retval)
++		return retval;
++
++	return 0;
++}
++
++static int isp1760_run(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	int retval;
++	u32 chipid_h;
++	u32 chipid_l;
++	u32 chip_rev;
++	u32 ptd_atl_int;
++	u32 ptd_iso;
++
++	/*
++	 * ISP1763 have some differences in the setup and order to enable
++	 * the ports, disable otg, setup buffers, and ATL, INT, ISO status.
++	 * So, just handle it a separate sequence.
++	 */
++	if (priv->is_isp1763)
++		return isp1763_run(hcd);
++
++	/* Set PTD interrupt AND & OR maps */
++	isp1760_hcd_clear(hcd, HC_ATL_IRQ_MASK_AND);
++	isp1760_hcd_clear(hcd, HC_INT_IRQ_MASK_AND);
++	isp1760_hcd_clear(hcd, HC_ISO_IRQ_MASK_AND);
++
++	isp1760_hcd_set(hcd, HC_ATL_IRQ_MASK_OR);
++	isp1760_hcd_set(hcd, HC_INT_IRQ_MASK_OR);
++	isp1760_hcd_set(hcd, HC_ISO_IRQ_MASK_OR);
++
++	/* step 23 passed */
++
++	isp1760_hcd_set(hcd, HW_GLOBAL_INTR_EN);
++
++	isp1760_hcd_clear(hcd, CMD_LRESET);
++	isp1760_hcd_clear(hcd, CMD_RESET);
++
++	retval = isp1760_hcd_set_and_wait(hcd, CMD_RUN, 250 * 1000);
++	if (retval)
++		return retval;
++
++	/*
++	 * XXX
++	 * Spec says to write FLAG_CF as last config action, priv code grabs
++	 * the semaphore while doing so.
++	 */
++	down_write(&ehci_cf_port_reset_rwsem);
++
++	retval = isp1760_hcd_set_and_wait(hcd, FLAG_CF, 250 * 1000);
++	up_write(&ehci_cf_port_reset_rwsem);
++	if (retval)
++		return retval;
++
++	chipid_h = isp1760_hcd_read(hcd, HC_CHIP_ID_HIGH);
++	chipid_l = isp1760_hcd_read(hcd, HC_CHIP_ID_LOW);
++	chip_rev = isp1760_hcd_read(hcd, HC_CHIP_REV);
++	dev_info(priv->dev, "USB ISP %02x%02x HW rev. %d started\n",
++		 chipid_h, chipid_l, chip_rev);
++
++	/* PTD Register Init Part 2, Step 28 */
++
++	/* Setup registers controlling PTD checking */
++	ptd_atl_int = 0x80000000;
++	ptd_iso = 0x00000001;
++
++	isp1760_hcd_write(hcd, HC_ATL_PTD_LASTPTD, ptd_atl_int);
++	isp1760_hcd_write(hcd, HC_INT_PTD_LASTPTD, ptd_atl_int);
++	isp1760_hcd_write(hcd, HC_ISO_PTD_LASTPTD, ptd_iso);
++
++	isp1760_hcd_set(hcd, HC_ATL_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_INT_PTD_SKIPMAP);
++	isp1760_hcd_set(hcd, HC_ISO_PTD_SKIPMAP);
++
++	isp1760_hcd_set(hcd, ATL_BUF_FILL);
++	isp1760_hcd_set(hcd, INT_BUF_FILL);
++
++	/* GRR this is run-once init(), being done every time the HC starts.
++	 * So long as they're part of class devices, we can't do it init()
++	 * since the class device isn't created that early.
++	 */
++	return 0;
++}
++
++static int qtd_fill(struct isp1760_qtd *qtd, void *databuffer, size_t len)
++{
++	qtd->data_buffer = databuffer;
++
++	qtd->length = len;
++
++	return qtd->length;
++}
++
++static void qtd_list_free(struct list_head *qtd_list)
++{
++	struct isp1760_qtd *qtd, *qtd_next;
++
++	list_for_each_entry_safe(qtd, qtd_next, qtd_list, qtd_list) {
++		list_del(&qtd->qtd_list);
++		qtd_free(qtd);
++	}
++}
++
++/*
++ * Packetize urb->transfer_buffer into list of packets of size wMaxPacketSize.
++ * Also calculate the PID type (SETUP/IN/OUT) for each packet.
++ */
++#define max_packet(wMaxPacketSize) ((wMaxPacketSize) & 0x07ff)
++static void packetize_urb(struct usb_hcd *hcd,
++		struct urb *urb, struct list_head *head, gfp_t flags)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++	struct isp1760_qtd *qtd;
++	void *buf;
++	int len, maxpacketsize;
++	u8 packet_type;
++
++	/*
++	 * URBs map to sequences of QTDs:  one logical transaction
++	 */
++
++	if (!urb->transfer_buffer && urb->transfer_buffer_length) {
++		/* XXX This looks like usb storage / SCSI bug */
++		dev_err(priv->dev, "buf is null, dma is %08lx len is %d\n",
++			(long unsigned)urb->transfer_dma,
++			urb->transfer_buffer_length);
++		WARN_ON(1);
++	}
++
++	if (usb_pipein(urb->pipe))
++		packet_type = IN_PID;
++	else
++		packet_type = OUT_PID;
++
++	if (usb_pipecontrol(urb->pipe)) {
++		qtd = qtd_alloc(flags, urb, SETUP_PID);
++		if (!qtd)
++			goto cleanup;
++		qtd_fill(qtd, urb->setup_packet, sizeof(struct usb_ctrlrequest));
++		list_add_tail(&qtd->qtd_list, head);
++
++		/* for zero length DATA stages, STATUS is always IN */
++		if (urb->transfer_buffer_length == 0)
++			packet_type = IN_PID;
++	}
++
++	maxpacketsize = max_packet(usb_maxpacket(urb->dev, urb->pipe));
++
++	/*
++	 * buffer gets wrapped in one or more qtds;
++	 * last one may be "short" (including zero len)
++	 * and may serve as a control status ack
++	 */
++	buf = urb->transfer_buffer;
++	len = urb->transfer_buffer_length;
++
++	for (;;) {
++		int this_qtd_len;
++
++		qtd = qtd_alloc(flags, urb, packet_type);
++		if (!qtd)
++			goto cleanup;
++
++		if (len > mem->blocks_size[ISP176x_BLOCK_NUM - 1])
++			len = mem->blocks_size[ISP176x_BLOCK_NUM - 1];
++
++		this_qtd_len = qtd_fill(qtd, buf, len);
++		list_add_tail(&qtd->qtd_list, head);
++
++		len -= this_qtd_len;
++		buf += this_qtd_len;
++
++		if (len <= 0)
++			break;
++	}
++
++	/*
++	 * control requests may need a terminating data "status" ack;
++	 * bulk ones may need a terminating short packet (zero length).
++	 */
++	if (urb->transfer_buffer_length != 0) {
++		int one_more = 0;
++
++		if (usb_pipecontrol(urb->pipe)) {
++			one_more = 1;
++			if (packet_type == IN_PID)
++				packet_type = OUT_PID;
++			else
++				packet_type = IN_PID;
++		} else if (usb_pipebulk(urb->pipe)
++			   && (urb->transfer_flags & URB_ZERO_PACKET)
++			   && !(urb->transfer_buffer_length %
++				maxpacketsize)) {
++			one_more = 1;
++		}
++		if (one_more) {
++			qtd = qtd_alloc(flags, urb, packet_type);
++			if (!qtd)
++				goto cleanup;
++
++			/* never any data in such packets */
++			qtd_fill(qtd, NULL, 0);
++			list_add_tail(&qtd->qtd_list, head);
++		}
++	}
++
++	return;
++
++cleanup:
++	qtd_list_free(head);
++}
++
++static int isp1760_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
++			       gfp_t mem_flags)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	struct isp1760_qh *qh = NULL;
++	struct list_head *ep_queue;
++	LIST_HEAD(new_qtds);
++	int qh_in_queue;
++	int retval;
++	int epnum;
++
++	switch (usb_pipetype(urb->pipe)) {
++	case PIPE_CONTROL:
++		ep_queue = &priv->qh_list[QH_CONTROL];
++		break;
++	case PIPE_BULK:
++		ep_queue = &priv->qh_list[QH_BULK];
++		break;
++	case PIPE_INTERRUPT:
++		ep_queue = &priv->qh_list[QH_INTERRUPT];
++		break;
++	case PIPE_ISOCHRONOUS:
++		printf("isochronous USB packets not yet supported\n");
++		return -EPIPE;
++	default:
++		printf("unknown pipe type\n");
++		return -EPIPE;
++	}
++
++	if (usb_pipein(urb->pipe))
++		urb->actual_length = 0;
++
++	packetize_urb(hcd, urb, &new_qtds, mem_flags);
++	if (list_empty(&new_qtds))
++		return -ENOMEM;
++
++	retval = usb_hcd_link_urb_to_ep(hcd, urb);
++	if (retval) {
++		qtd_list_free(&new_qtds);
++		goto out;
++	}
++
++	epnum = usb_pipeendpoint(urb->pipe);
++
++	qh_in_queue = 0;
++	list_for_each_entry(qh, ep_queue, qh_list) {
++		if (qh->epnum == epnum) {
++			qh_in_queue = 1;
++			break;
++		}
++	}
++
++	if (!qh_in_queue) {
++		qh = qh_alloc(GFP_ATOMIC);
++		if (!qh) {
++			retval = -ENOMEM;
++			usb_hcd_unlink_urb_from_ep(hcd, urb);
++			qtd_list_free(&new_qtds);
++			goto out;
++		}
++
++		qh->epnum = epnum;
++		list_add_tail(&qh->qh_list, ep_queue);
++		urb->ep->hcpriv = qh;
++	}
++
++	list_splice_tail(&new_qtds, &qh->qtd_list);
++	schedule_ptds(hcd);
++
++out:
++	return retval;
++}
++
++static void kill_transfer(struct usb_hcd *hcd, struct urb *urb,
++		struct isp1760_qh *qh)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	int skip_map;
++
++	WARN_ON(qh->slot == -1);
++
++	/* We need to forcefully reclaim the slot since some transfers never
++	   return, e.g. interrupt transfers and NAKed bulk transfers. */
++	if (usb_pipecontrol(urb->pipe) || usb_pipebulk(urb->pipe)) {
++		skip_map = isp1760_hcd_read(hcd, HC_ATL_PTD_SKIPMAP);
++		skip_map |= (1 << qh->slot);
++		isp1760_hcd_write(hcd, HC_ATL_PTD_SKIPMAP, skip_map);
++		ndelay(100);
++		priv->atl_slots[qh->slot].qh = NULL;
++		priv->atl_slots[qh->slot].qtd = NULL;
++	} else {
++		skip_map = isp1760_hcd_read(hcd, HC_INT_PTD_SKIPMAP);
++		skip_map |= (1 << qh->slot);
++		isp1760_hcd_write(hcd, HC_INT_PTD_SKIPMAP, skip_map);
++		priv->int_slots[qh->slot].qh = NULL;
++		priv->int_slots[qh->slot].qtd = NULL;
++	}
++
++	qh->slot = -1;
++}
++
++/*
++ * Retire the qtds beginning at 'qtd' and belonging all to the same urb, killing
++ * any active transfer belonging to the urb in the process.
++ */
++static void dequeue_urb_from_qtd(struct usb_hcd *hcd, struct isp1760_qh *qh,
++				 struct isp1760_qtd *qtd)
++{
++	struct urb *urb;
++	int urb_was_running;
++
++	urb = qtd->urb;
++	urb_was_running = 0;
++	list_for_each_entry_from(qtd, &qh->qtd_list, qtd_list) {
++		if (qtd->urb != urb)
++			break;
++
++		if (qtd->status >= QTD_XFER_STARTED)
++			urb_was_running = 1;
++		if (last_qtd_of_urb(qtd, qh) &&
++					(qtd->status >= QTD_XFER_COMPLETE))
++			urb_was_running = 0;
++
++		if (qtd->status == QTD_XFER_STARTED)
++			kill_transfer(hcd, urb, qh);
++		qtd->status = QTD_RETIRE;
++	}
++}
++
++int isp1760_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
++{
++	struct isp1760_qtd *qtd;
++	struct isp1760_qh *qh;
++	int retval = 0;
++
++	retval = usb_hcd_check_unlink_urb(hcd, urb, status);
++	if (retval)
++		goto out;
++
++	qh = urb->ep->hcpriv;
++	if (!qh) {
++		retval = -EINVAL;
++		goto out;
++	}
++
++	list_for_each_entry(qtd, &qh->qtd_list, qtd_list)
++		if (qtd->urb == urb) {
++			dequeue_urb_from_qtd(hcd, qh, qtd);
++			list_move(&qtd->qtd_list, &qh->qtd_list);
++			break;
++		}
++
++	urb->status = status;
++	schedule_ptds(hcd);
++
++out:
++	return retval;
++}
++
++static void isp1760_hub_descriptor(struct isp1760_hcd *priv,
++				   struct usb_hub_descriptor *desc)
++{
++	int ports;
++	u16 temp;
++
++	ports = isp1760_hcd_n_ports(priv->hcd);
++
++	desc->bDescriptorType = USB_DT_HUB;
++	/* priv 1.0, 2.3.9 says 20ms max */
++	desc->bPwrOn2PwrGood = 10;
++	desc->bHubContrCurrent = 0;
++
++	desc->bNbrPorts = ports;
++	temp = 1 + (ports / 8);
++	desc->bLength = 7 + 2 * temp;
++
++	/* ports removable, and usb 1.0 legacy PortPwrCtrlMask */
++	memset(&desc->u.hs.DeviceRemovable[0], 0, temp);
++	memset(&desc->u.hs.DeviceRemovable[temp], 0xff, temp);
++
++	/* per-port overcurrent reporting */
++	temp = HUB_CHAR_INDV_PORT_OCPM;
++	if (isp1760_hcd_ppc_is_set(priv->hcd))
++		/* per-port power control */
++		temp |= HUB_CHAR_INDV_PORT_LPSM;
++	else
++		/* no power switching */
++		temp |= HUB_CHAR_NO_LPSM;
++	desc->wHubCharacteristics = cpu_to_le16(temp);
++}
++
++#define	PORT_WAKE_BITS	(PORT_WKOC_E|PORT_WKDISC_E|PORT_WKCONN_E)
++
++static void check_reset_complete(struct usb_hcd *hcd, int index)
++{
++	if (!(isp1760_hcd_is_set(hcd, PORT_CONNECT)))
++		return;
++
++	/* if reset finished and it's still not enabled -- handoff */
++	if (!isp1760_hcd_is_set(hcd, PORT_PE)) {
++		printf("port %d full speed --> companion\n", index + 1);
++
++		isp1760_hcd_set(hcd, PORT_OWNER);
++
++		isp1760_hcd_clear(hcd, PORT_CSC);
++	} else {
++		printf("port %d high speed\n", index + 1);
++	}
++
++	return;
++}
++
++static int isp1760_hub_control(struct usb_hcd *hcd, struct usb_device *dev,
++			       unsigned long pipe, void *buffer, int length,
++			       struct devrequest *setup)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u16 typeReq, wValue, wIndex;
++	unsigned long flags;
++	char *buf = buffer;
++	void *src = NULL;
++	int src_len = 0;
++	int retval = 0;
++	u32 status;
++	int ports;
++
++	if (!setup)
++		return -EINVAL;
++
++	ports = isp1760_hcd_n_ports(hcd);
++
++	typeReq = setup->request | (setup->requesttype << 8);
++	wValue = le16_to_cpu(setup->value);
++	wIndex = le16_to_cpu(setup->index);
++
++	/*
++	 * FIXME:  support SetPortFeatures USB_PORT_FEAT_INDICATOR.
++	 * HCS_INDICATOR may say we can change LEDs to off/amber/green.
++	 * (track current state ourselves) ... blink for diagnostics,
++	 * power, "this is the one", etc.  EHCI spec supports this.
++	 */
++
++	switch (typeReq) {
++	case DeviceOutRequest | USB_REQ_SET_ADDRESS:
++		break;
++	case DeviceOutRequest | USB_REQ_SET_CONFIGURATION:
++		/* Nothing to do */
++		break;
++	case DeviceRequest | USB_REQ_GET_DESCRIPTOR:
++		switch (wValue & 0xff00) {
++		case USB_DT_DEVICE << 8:
++			src = &rh_descriptor.device;
++			src_len = 0x12;
++			break;
++		case USB_DT_CONFIG << 8:
++			src = &rh_descriptor.config;
++			src_len = 0x09;
++			break;
++		case USB_DT_STRING << 8:
++			switch (wValue & 0xff) {
++			case 0:	/* Language */
++				src = "\4\3\19\4";
++				src_len = 4;
++				break;
++			case 1:	/* Vendor String  */
++				src = "\16\3U\0-\0B\0o\0o\0t\0";
++				src_len = 14;
++				break;
++			case 2:	/* Product Name */
++				src = "\52\3I\0S\0P\0-\0 "
++					 "\0H\0o\0s\0t\0 "
++					 "\0C\0o\0n\0t\0r\0o\0l\0l\0e\0r\0";
++				src_len = 42;
++				break;
++			default:
++				goto error;
++			}
++			break;
++		}
++		break;
++	case ClearHubFeature:
++		switch (wValue) {
++		case C_HUB_LOCAL_POWER:
++		case C_HUB_OVER_CURRENT:
++			/* no hub-wide feature/status flags */
++			break;
++		default:
++			goto error;
++		}
++		break;
++	case ClearPortFeature:
++		if (!wIndex || wIndex > ports)
++			goto error;
++		wIndex--;
++
++		/*
++		 * Even if OWNER is set, so the port is owned by the
++		 * companion controller, hub_wq needs to be able to clear
++		 * the port-change status bits (especially
++		 * USB_PORT_STAT_C_CONNECTION).
++		 */
++
++		switch (wValue) {
++		case USB_PORT_FEAT_ENABLE:
++			isp1760_hcd_clear(hcd, PORT_PE);
++			break;
++		case USB_PORT_FEAT_C_ENABLE:
++			/* XXX error? */
++			break;
++		case USB_PORT_FEAT_SUSPEND:
++			if (isp1760_hcd_is_set(hcd, PORT_RESET))
++				goto error;
++
++			if (isp1760_hcd_is_set(hcd, PORT_SUSPEND)) {
++				if (!isp1760_hcd_is_set(hcd, PORT_PE))
++					goto error;
++				/* resume signaling for 20 msec */
++				isp1760_hcd_clear(hcd, PORT_CSC);
++				isp1760_hcd_set(hcd, PORT_RESUME);
++
++				priv->reset_done = get_timer(0) + 40;
++			}
++			break;
++		case USB_PORT_FEAT_C_SUSPEND:
++			/* we auto-clear this feature */
++			break;
++		case USB_PORT_FEAT_POWER:
++			if (isp1760_hcd_ppc_is_set(hcd))
++				isp1760_hcd_clear(hcd, PORT_POWER);
++			break;
++		case USB_PORT_FEAT_C_CONNECTION:
++			isp1760_hcd_set(hcd, PORT_CSC);
++			break;
++		case USB_PORT_FEAT_C_OVER_CURRENT:
++			/* XXX error ?*/
++			break;
++		case USB_PORT_FEAT_C_RESET:
++			/* GetPortStatus clears reset */
++			break;
++		default:
++			goto error;
++		}
++		isp1760_hcd_read(hcd, CMD_RUN);
++		break;
++	case GetHubDescriptor:
++		isp1760_hub_descriptor(priv, (struct usb_hub_descriptor *) buf);
++		break;
++	case GetHubStatus:
++		/* no hub-wide feature/status flags */
++		memset(buf, 0, 4);
++		break;
++	case GetPortStatus:
++		if (!wIndex || wIndex > ports)
++			goto error;
++		wIndex--;
++		status = 0;
++
++		/* wPortChange bits */
++		if (isp1760_hcd_is_set(hcd, PORT_CSC))
++			status |= USB_PORT_STAT_C_CONNECTION << 16;
++
++		/* whoever resumes must GetPortStatus to complete it!! */
++		if (isp1760_hcd_is_set(hcd, PORT_RESUME)) {
++			status |= USB_PORT_STAT_C_SUSPEND << 16;
++
++			if (!priv->reset_done) {
++				priv->reset_done = get_timer(0) + 20;
++			} else if (get_timer(0) > priv->reset_done) {
++				/* stop resume signaling */
++				isp1760_hcd_clear(hcd, PORT_CSC);
++
++				retval = isp1760_hcd_clear_and_wait(hcd,
++								    PORT_RESUME, 2000);
++				if (retval != 0) {
++					printf("port %d resume error %d\n",
++					       wIndex + 1, retval);
++					goto error;
++				}
++			}
++		}
++
++		/* whoever resets must GetPortStatus to complete it!! */
++		if (isp1760_hcd_is_set(hcd, PORT_RESET) &&
++		    get_timer(0) > priv->reset_done) {
++			status |= USB_PORT_STAT_C_RESET << 16;
++			priv->reset_done = 0;
++
++			/* force reset to complete */
++			/* REVISIT:  some hardware needs 550+ usec to clear
++			 * this bit; seems too long to spin routinely...
++			 */
++			retval = isp1760_hcd_clear_and_wait(hcd, PORT_RESET,
++							    750);
++			if (retval != 0) {
++				printf("port %d reset error %d\n", wIndex + 1,
++				       retval);
++				goto error;
++			}
++
++			/* see what we found out */
++			check_reset_complete(hcd, wIndex);
++		}
++		/*
++		 * Even if OWNER is set, there's no harm letting hub_wq
++		 * see the wPortStatus values (they should all be 0 except
++		 * for PORT_POWER anyway).
++		 */
++
++		if (isp1760_hcd_is_set(hcd, PORT_OWNER))
++			printf("PORT_OWNER is set\n");
++
++		if (isp1760_hcd_is_set(hcd, PORT_CONNECT)) {
++			status |= USB_PORT_STAT_CONNECTION;
++
++			/* status may be from integrated TT */
++			status |= USB_PORT_STAT_HIGH_SPEED;
++		}
++		if (isp1760_hcd_is_set(hcd, PORT_PE))
++			status |= USB_PORT_STAT_ENABLE;
++		if (isp1760_hcd_is_set(hcd, PORT_SUSPEND) &&
++		    isp1760_hcd_is_set(hcd, PORT_RESUME))
++			status |= USB_PORT_STAT_SUSPEND;
++		if (isp1760_hcd_is_set(hcd, PORT_RESET))
++			status |= USB_PORT_STAT_RESET;
++		if (isp1760_hcd_is_set(hcd, PORT_POWER))
++			status |= USB_PORT_STAT_POWER;
++
++		put_unaligned(cpu_to_le32(status), (__le32 *) buf);
++		break;
++	case SetHubFeature:
++		switch (wValue) {
++		case C_HUB_LOCAL_POWER:
++		case C_HUB_OVER_CURRENT:
++			/* no hub-wide feature/status flags */
++			break;
++		default:
++			goto error;
++		}
++		break;
++	case SetPortFeature:
++		wIndex &= 0xff;
++		if (!wIndex || wIndex > ports)
++			goto error;
++		wIndex--;
++
++		if (isp1760_hcd_is_set(hcd, PORT_OWNER))
++			break;
++
++		switch (wValue) {
++		case USB_PORT_FEAT_ENABLE:
++			isp1760_hcd_set(hcd, PORT_PE);
++			break;
++
++		case USB_PORT_FEAT_SUSPEND:
++			if (!isp1760_hcd_is_set(hcd, PORT_PE) ||
++			    isp1760_hcd_is_set(hcd, PORT_RESET))
++				goto error;
++
++			isp1760_hcd_set(hcd, PORT_SUSPEND);
++			break;
++		case USB_PORT_FEAT_POWER:
++			if (isp1760_hcd_ppc_is_set(hcd))
++				isp1760_hcd_set(hcd, PORT_POWER);
++			break;
++		case USB_PORT_FEAT_RESET:
++			if (isp1760_hcd_is_set(hcd, PORT_RESUME))
++				goto error;
++			/* line status bits may report this as low speed,
++			 * which can be fine if this root hub has a
++			 * transaction translator built in.
++			 */
++			if ((isp1760_hcd_is_set(hcd, PORT_CONNECT) &&
++			     !isp1760_hcd_is_set(hcd, PORT_PE)) &&
++			    (isp1760_hcd_read(hcd, PORT_LSTATUS) == 1)) {
++				isp1760_hcd_set(hcd, PORT_OWNER);
++			} else {
++				isp1760_hcd_set(hcd, PORT_RESET);
++				isp1760_hcd_clear(hcd, PORT_PE);
++
++				priv->reset_done = get_timer(0) + 50;
++			}
++			break;
++		default:
++			goto error;
++		}
++		break;
++
++	default:
++		printf("root: unknown request: 0x%0x\n", typeReq);
++		goto error;
++	}
++	spin_unlock_irqrestore(&priv->lock, flags);
++
++	if (src_len) {
++		length = min(src_len, length);
++
++		if (src != NULL && length > 0)
++			memcpy(buffer, src, length);
++		else
++			printf("zero copy USB descriptor\n");
++	}
++
++	dev->act_len = length;
++	dev->status = 0;
++
++	return 0;
++
++error:
++	/* "stall" on error */
++	dev->act_len = 0;
++	dev->status = USB_ST_STALLED;
++	return -EPIPE;
++}
++
++#ifndef __UBOOT__
++static int isp1760_hub_status_data(struct usb_hcd *hcd, char *buf)
++{
++	u32 status = 0;
++	int retval = 1;
++
++	/* init status to no-changes */
++	buf[0] = 0;
++
++	if (isp1760_hcd_is_set(hcd, PORT_OWNER) &&
++	    isp1760_hcd_is_set(hcd, PORT_CSC)) {
++		isp1760_hcd_clear(hcd, PORT_CSC);
++		goto done;
++	}
++
++done:
++	return status ? retval : 0;
++}
++
++static void isp1760_endpoint_disable(struct usb_hcd *hcd,
++		struct usb_host_endpoint *ep)
++{
++	struct isp1760_qh *qh, *qh_iter;
++	unsigned long spinflags;
++	int i;
++
++	qh = ep->hcpriv;
++	if (!qh)
++		return;
++
++	WARN_ON(!list_empty(&qh->qtd_list));
++
++	for (i = 0; i < QH_END; i++)
++		list_for_each_entry(qh_iter, &priv->qh_list[i], qh_list)
++			if (qh_iter == qh) {
++				list_del(&qh_iter->qh_list);
++				i = QH_END;
++				break;
++			}
++	qh_free(qh);
++	ep->hcpriv = NULL;
++
++	schedule_ptds(hcd);
++}
++
++static int isp1760_get_frame(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++	u32 fr;
++
++	fr = isp1760_hcd_read(hcd, HC_FRINDEX);
++	return (fr >> 3) % priv->periodic_size;
++}
++
++static void isp1760_stop(struct usb_hcd *hcd)
++{
++	struct isp1760_hcd *priv = hcd_to_priv(hcd);
++
++	msleep(20);
++
++	spin_lock_irq(&priv->lock);
++	ehci_reset(hcd);
++	/* Disable IRQ */
++	isp1760_hcd_clear(hcd, HW_GLOBAL_INTR_EN);
++	spin_unlock_irq(&priv->lock);
++
++	isp1760_hcd_clear(hcd, FLAG_CF);
++}
++
++static void isp1760_shutdown(struct usb_hcd *hcd)
++{
++	isp1760_stop(hcd);
++
++	isp1760_hcd_clear(hcd, HW_GLOBAL_INTR_EN);
++
++	isp1760_hcd_clear(hcd, CMD_RUN);
++}
++
++static void isp1760_clear_tt_buffer_complete(struct usb_hcd *hcd,
++						struct usb_host_endpoint *ep)
++{
++	struct isp1760_qh *qh = ep->hcpriv;
++	unsigned long spinflags;
++
++	if (!qh)
++		return;
++
++	qh->tt_buffer_dirty = 0;
++	schedule_ptds(hcd);
++}
++
++
++static const struct hc_driver isp1760_hc_driver = {
++	.description		= "isp1760-hcd",
++	.product_desc		= "NXP ISP1760 USB Host Controller",
++	.hcd_priv_size		= sizeof(struct isp1760_hcd *),
++	.irq			= isp1760_irq,
++	.flags			= HCD_MEMORY | HCD_USB2,
++	.reset			= isp1760_hc_setup,
++	.start			= isp1760_run,
++	.stop			= isp1760_stop,
++	.shutdown		= isp1760_shutdown,
++	.urb_enqueue		= isp1760_urb_enqueue,
++	.urb_dequeue		= isp1760_urb_dequeue,
++	.endpoint_disable	= isp1760_endpoint_disable,
++	.get_frame_number	= isp1760_get_frame,
++	.hub_status_data	= isp1760_hub_status_data,
++	.hub_control		= isp1760_hub_control,
++	.clear_tt_buffer_complete	= isp1760_clear_tt_buffer_complete,
++};
++#endif // __UBOOT__
++
++int __init isp1760_init_kmem_once(void)
++{
++	urb_listitem_cachep = kmem_cache_create("isp1760_urb_listitem",
++			sizeof(struct urb_listitem), 0, SLAB_TEMPORARY |
++			SLAB_MEM_SPREAD, NULL);
++
++	if (!urb_listitem_cachep)
++		return -ENOMEM;
++
++	qtd_cachep = kmem_cache_create("isp1760_qtd",
++			sizeof(struct isp1760_qtd), 0, SLAB_TEMPORARY |
++			SLAB_MEM_SPREAD, NULL);
++
++	if (!qtd_cachep)
++		return -ENOMEM;
++
++	qh_cachep = kmem_cache_create("isp1760_qh", sizeof(struct isp1760_qh),
++			0, SLAB_TEMPORARY | SLAB_MEM_SPREAD, NULL);
++
++	if (!qh_cachep) {
++		kmem_cache_destroy(qtd_cachep);
++		return -ENOMEM;
++	}
++
++	return 0;
++}
++
++void isp1760_deinit_kmem_cache(void)
++{
++	kmem_cache_destroy(qtd_cachep);
++	kmem_cache_destroy(qh_cachep);
++	kmem_cache_destroy(urb_listitem_cachep);
++}
++
++int isp1760_hcd_lowlevel_init(struct isp1760_hcd *priv)
++{
++	int ret;
++
++	ret = isp1760_hc_setup(priv->hcd);
++	if (ret < 0)
++		return ret;
++
++	ret = isp1760_run(priv->hcd);
++	if (ret < 0)
++		return ret;
++
++	return 0;
++}
++
++static const struct usb_urb_ops isp1760_urb_ops = {
++	.urb_enqueue = isp1760_urb_enqueue,
++	.urb_dequeue = isp1760_urb_dequeue,
++	.hub_control = isp1760_hub_control,
++	.isr = isp1760_irq,
++};
++
++int isp1760_hcd_register(struct isp1760_hcd *priv, struct resource *mem,
++			 int irq, unsigned long irqflags,
++			 struct udevice *dev)
++{
++	const struct isp1760_memory_layout *mem_layout = priv->memory_layout;
++	struct isp1760_host_data *host = dev_get_priv(dev);
++	struct usb_hcd *hcd = &host->hcd;
++	int ret;
++
++	priv->hcd = hcd;
++
++	hcd->hcd_priv = priv;
++
++	priv->hcd = hcd;
++
++	hcd->urb_ops = &isp1760_urb_ops;
++
++	priv->atl_slots = kcalloc(mem_layout->slot_num,
++				  sizeof(struct isp1760_slotinfo), GFP_KERNEL);
++	if (!priv->atl_slots)
++		return -ENOMEM;
++
++	priv->int_slots = kcalloc(mem_layout->slot_num,
++				  sizeof(struct isp1760_slotinfo), GFP_KERNEL);
++	if (!priv->int_slots) {
++		ret = -ENOMEM;
++		goto free_atl_slots;
++	}
++
++	host->host_speed = USB_SPEED_HIGH;
++
++	init_memory(priv);
++
++	return 0;
++
++free_atl_slots:
++	kfree(priv->atl_slots);
++
++	return ret;
++}
++
++void isp1760_hcd_unregister(struct isp1760_hcd *priv)
++{
++	struct isp1760_qh *qh, *qh_next;
++	int i;
++
++	for (i = 0; i < QH_END; i++)
++		list_for_each_entry_safe(qh, qh_next, &priv->qh_list[i],
++					 qh_list) {
++			qtd_list_free(&qh->qtd_list);
++			list_del(&qh->qh_list);
++			qh_free(qh);
++		}
++
++	kfree(priv->atl_slots);
++	kfree(priv->int_slots);
++}
+diff --git a/drivers/usb/isp1760/isp1760-hcd.h b/drivers/usb/isp1760/isp1760-hcd.h
+new file mode 100644
+index 000000000000..00f5ca8c1f75
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-hcd.h
+@@ -0,0 +1,82 @@
++/* SPDX-License-Identifier: GPL-2.0 */
++#ifndef _ISP1760_HCD_H_
++#define _ISP1760_HCD_H_
++
++#include <regmap.h>
++
++#include "isp1760-regs.h"
++
++struct isp1760_qh;
++struct isp1760_qtd;
++struct resource;
++struct usb_hcd;
++
++struct isp1760_slotinfo {
++	struct isp1760_qh *qh;
++	struct isp1760_qtd *qtd;
++	unsigned long timestamp;
++};
++
++/* chip memory management */
++#define ISP176x_BLOCK_MAX (32 + 20 + 4)
++#define ISP176x_BLOCK_NUM 3
++
++struct isp1760_memory_layout {
++	unsigned int blocks[ISP176x_BLOCK_NUM];
++	unsigned int blocks_size[ISP176x_BLOCK_NUM];
++
++	unsigned int slot_num;
++	unsigned int payload_blocks;
++	unsigned int payload_area_size;
++};
++
++struct isp1760_memory_chunk {
++	unsigned int start;
++	unsigned int size;
++	unsigned int free;
++};
++
++enum isp1760_queue_head_types {
++	QH_CONTROL,
++	QH_BULK,
++	QH_INTERRUPT,
++	QH_END
++};
++
++struct isp1760_hcd {
++	struct usb_hcd		*hcd;
++	struct udevice		*dev;
++
++	void __iomem		*base;
++
++	struct regmap		*regs;
++	struct regmap_field	*fields[HC_FIELD_MAX];
++
++	bool			is_isp1763;
++	const struct isp1760_memory_layout	*memory_layout;
++
++	spinlock_t		lock;
++	struct isp1760_slotinfo	*atl_slots;
++	int			atl_done_map;
++	struct isp1760_slotinfo	*int_slots;
++	int			int_done_map;
++	struct isp1760_memory_chunk memory_pool[ISP176x_BLOCK_MAX];
++	struct list_head	qh_list[QH_END];
++
++	/* periodic schedule support */
++#define	DEFAULT_I_TDPS		1024
++	unsigned		periodic_size;
++	unsigned		i_thresh;
++	unsigned long		reset_done;
++	unsigned long		next_statechange;
++};
++
++int isp1760_hcd_register(struct isp1760_hcd *priv, struct resource *mem,
++			 int irq, unsigned long irqflags, struct udevice *dev);
++void isp1760_hcd_unregister(struct isp1760_hcd *priv);
++int isp1760_hcd_lowlevel_init(struct isp1760_hcd *priv);
++
++int isp1760_init_kmem_once(void);
++void isp1760_deinit_kmem_cache(void);
++
++#endif /* _ISP1760_HCD_H_ */
+diff --git a/drivers/usb/isp1760/isp1760-if.c b/drivers/usb/isp1760/isp1760-if.c
+new file mode 100644
+index 000000000000..c610da6b23fb
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-if.c
+@@ -0,0 +1,127 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ * based on original code from:
++ * (c) 2007 Sebastian Siewior <bigeasy@linutronix.de>
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <dm/device-internal.h>
++#include <dm/device_compat.h>
++#include <dm/devres.h>
++#include <dm/lists.h>
++#include <linux/bug.h>
++#include <linux/io.h>
++#include <linux/kernel.h>
++#include <linux/usb/otg.h>
++#include <log.h>
++#include <usb.h>
++
++#include "isp1760-core.h"
++#include "isp1760-regs.h"
++#include "isp1760-uboot.h"
++
++
++static int isp1760_of_to_plat(struct udevice *dev)
++{
++	struct isp1760_device *isp = dev_get_plat(dev);
++	unsigned int devflags = 0;
++	u32 bus_width = 0;
++	ofnode dp;
++
++
++	if (!dev_has_ofnode(dev)) {
++		/* select isp1763 as the default device */
++		devflags = ISP1760_FLAG_ISP1763 | ISP1760_FLAG_BUS_WIDTH_16;
++		pr_err("isp1760: no platform data\n");
++		goto isp_setup;
++	}
++
++	dp = dev_ofnode(dev);
++
++	if (ofnode_device_is_compatible(dp, "nxp,usb-isp1761"))
++		devflags |= ISP1760_FLAG_ISP1761;
++
++	if (ofnode_device_is_compatible(dp, "nxp,usb-isp1763"))
++		devflags |= ISP1760_FLAG_ISP1763;
++
++	/*
++	* Some systems wire up only 8 of 16 data lines or
++	* 16 of the 32 data lines
++	*/
++	bus_width = ofnode_read_u32_default(dp, "bus-width", 16);
++	if (bus_width == 16)
++		devflags |= ISP1760_FLAG_BUS_WIDTH_16;
++	else if (bus_width == 8)
++		devflags |= ISP1760_FLAG_BUS_WIDTH_8;
++
++	if (usb_get_dr_mode(dev_ofnode(dev)) == USB_DR_MODE_PERIPHERAL)
++		devflags |= ISP1760_FLAG_PERIPHERAL_EN;
++
++	if (ofnode_read_bool(dp, "analog-oc"))
++		devflags |= ISP1760_FLAG_ANALOG_OC;
++
++	if (ofnode_read_bool(dp, "dack-polarity"))
++		devflags |= ISP1760_FLAG_DACK_POL_HIGH;
++
++	if (ofnode_read_bool(dp, "dreq-polarity"))
++		devflags |= ISP1760_FLAG_DREQ_POL_HIGH;
++
++isp_setup:
++	isp->devflags = devflags;
++	isp->dev = dev;
++
++	return 0;
++}
++
++static int isp1760_plat_probe(struct udevice *dev)
++{
++	struct isp1760_device *isp = dev_get_plat(dev);
++	struct resource mem_res;
++	struct resource irq_res;
++	int ret;
++
++	dev_read_resource(dev, 0, &mem_res);
++	dev_read_resource(dev, 1, &irq_res);
++
++	isp1760_init_kmem_once();
++
++	ret = isp1760_register(isp, &mem_res, irq_res.start, irq_res.flags);
++	if (ret < 0) {
++		isp1760_deinit_kmem_cache();
++		return ret;
++	}
++
++	return 0;
++}
++
++static int isp1760_plat_remove(struct udevice *dev)
++{
++	struct isp1760_device *isp = dev_get_plat(dev);
++
++	isp1760_deinit_kmem_cache();
++	isp1760_unregister(isp);
++
++	return 0;
++}
++
++static const struct udevice_id isp1760_ids[] = {
++	{ .compatible = "nxp,usb-isp1760", },
++	{ .compatible = "nxp,usb-isp1761", },
++	{ .compatible = "nxp,usb-isp1763", },
++	{ },
++};
++
++U_BOOT_DRIVER(isp1760) = {
++	.name		= "isp1760",
++	.id		= UCLASS_USB,
++	.of_match	= isp1760_ids,
++	.of_to_plat	= isp1760_of_to_plat,
++	.ops		= &isp1760_usb_ops,
++	.probe		= isp1760_plat_probe,
++	.remove		= isp1760_plat_remove,
++	.plat_auto	= sizeof(struct isp1760_device),
++	.priv_auto	= sizeof(struct isp1760_host_data),
++};
+diff --git a/drivers/usb/isp1760/isp1760-regs.h b/drivers/usb/isp1760/isp1760-regs.h
+new file mode 100644
+index 000000000000..94ea60c20b2a
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-regs.h
+@@ -0,0 +1,292 @@
++/* SPDX-License-Identifier: GPL-2.0 */
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva
++ * Copyright 2014 Laurent Pinchart
++ * Copyright 2007 Sebastian Siewior
++ *
++ * Contacts:
++ *     Sebastian Siewior <bigeasy@linutronix.de>
++ *     Laurent Pinchart <laurent.pinchart@ideasonboard.com>
++ *     Rui Miguel Silva <rui.silva@linaro.org>
++ */
++
++#ifndef _ISP176x_REGS_H_
++#define _ISP176x_REGS_H_
++
++/* -----------------------------------------------------------------------------
++ * Host Controller
++ */
++
++/* ISP1760/31 */
++/* EHCI capability registers */
++#define ISP176x_HC_VERSION		0x002
++#define ISP176x_HC_HCSPARAMS		0x004
++#define ISP176x_HC_HCCPARAMS		0x008
++
++/* EHCI operational registers */
++#define ISP176x_HC_USBCMD		0x020
++#define ISP176x_HC_USBSTS		0x024
++#define ISP176x_HC_FRINDEX		0x02c
++
++#define ISP176x_HC_CONFIGFLAG		0x060
++#define ISP176x_HC_PORTSC1		0x064
++
++#define ISP176x_HC_ISO_PTD_DONEMAP	0x130
++#define ISP176x_HC_ISO_PTD_SKIPMAP	0x134
++#define ISP176x_HC_ISO_PTD_LASTPTD	0x138
++#define ISP176x_HC_INT_PTD_DONEMAP	0x140
++#define ISP176x_HC_INT_PTD_SKIPMAP	0x144
++#define ISP176x_HC_INT_PTD_LASTPTD	0x148
++#define ISP176x_HC_ATL_PTD_DONEMAP	0x150
++#define ISP176x_HC_ATL_PTD_SKIPMAP	0x154
++#define ISP176x_HC_ATL_PTD_LASTPTD	0x158
++
++/* Configuration Register */
++#define ISP176x_HC_HW_MODE_CTRL		0x300
++#define ISP176x_HC_CHIP_ID		0x304
++#define ISP176x_HC_SCRATCH		0x308
++#define ISP176x_HC_RESET		0x30c
++#define ISP176x_HC_BUFFER_STATUS	0x334
++#define ISP176x_HC_MEMORY		0x33c
++
++/* Interrupt Register */
++#define ISP176x_HC_INTERRUPT		0x310
++#define ISP176x_HC_INTERRUPT_ENABLE	0x314
++#define ISP176x_HC_ISO_IRQ_MASK_OR	0x318
++#define ISP176x_HC_INT_IRQ_MASK_OR	0x31c
++#define ISP176x_HC_ATL_IRQ_MASK_OR	0x320
++#define ISP176x_HC_ISO_IRQ_MASK_AND	0x324
++#define ISP176x_HC_INT_IRQ_MASK_AND	0x328
++#define ISP176x_HC_ATL_IRQ_MASK_AND	0x32c
++
++#define ISP176x_HC_OTG_CTRL_SET		0x374
++#define ISP176x_HC_OTG_CTRL_CLEAR	0x376
++
++enum isp176x_host_controller_fields {
++	/* HC_PORTSC1 */
++	PORT_OWNER, PORT_POWER, PORT_LSTATUS, PORT_RESET, PORT_SUSPEND,
++	PORT_RESUME, PORT_PE, PORT_CSC, PORT_CONNECT,
++	/* HC_HCSPARAMS */
++	HCS_PPC, HCS_N_PORTS,
++	/* HC_HCCPARAMS */
++	HCC_ISOC_CACHE, HCC_ISOC_THRES,
++	/* HC_USBCMD */
++	CMD_LRESET, CMD_RESET, CMD_RUN,
++	/* HC_USBSTS */
++	STS_PCD,
++	/* HC_FRINDEX */
++	HC_FRINDEX,
++	/* HC_CONFIGFLAG */
++	FLAG_CF,
++	/* ISO/INT/ATL PTD */
++	HC_ISO_PTD_DONEMAP, HC_ISO_PTD_SKIPMAP, HC_ISO_PTD_LASTPTD,
++	HC_INT_PTD_DONEMAP, HC_INT_PTD_SKIPMAP, HC_INT_PTD_LASTPTD,
++	HC_ATL_PTD_DONEMAP, HC_ATL_PTD_SKIPMAP, HC_ATL_PTD_LASTPTD,
++	/* HC_HW_MODE_CTRL */
++	ALL_ATX_RESET, HW_ANA_DIGI_OC, HW_DEV_DMA, HW_COMN_IRQ, HW_COMN_DMA,
++	HW_DATA_BUS_WIDTH, HW_DACK_POL_HIGH, HW_DREQ_POL_HIGH, HW_INTR_HIGH_ACT,
++	HW_INTF_LOCK, HW_INTR_EDGE_TRIG, HW_GLOBAL_INTR_EN,
++	/* HC_CHIP_ID */
++	HC_CHIP_ID_HIGH, HC_CHIP_ID_LOW, HC_CHIP_REV,
++	/* HC_SCRATCH */
++	HC_SCRATCH,
++	/* HC_RESET */
++	SW_RESET_RESET_ATX, SW_RESET_RESET_HC, SW_RESET_RESET_ALL,
++	/* HC_BUFFER_STATUS */
++	ISO_BUF_FILL, INT_BUF_FILL, ATL_BUF_FILL,
++	/* HC_MEMORY */
++	MEM_BANK_SEL, MEM_START_ADDR,
++	/* HC_DATA */
++	HC_DATA,
++	/* HC_INTERRUPT */
++	HC_INTERRUPT,
++	/* HC_INTERRUPT_ENABLE */
++	HC_INT_IRQ_ENABLE, HC_ATL_IRQ_ENABLE,
++	/* INTERRUPT MASKS */
++	HC_ISO_IRQ_MASK_OR, HC_INT_IRQ_MASK_OR, HC_ATL_IRQ_MASK_OR,
++	HC_ISO_IRQ_MASK_AND, HC_INT_IRQ_MASK_AND, HC_ATL_IRQ_MASK_AND,
++	/* HW_OTG_CTRL_SET */
++	HW_OTG_DISABLE, HW_SW_SEL_HC_DC, HW_VBUS_DRV, HW_SEL_CP_EXT,
++	HW_DM_PULLDOWN, HW_DP_PULLDOWN, HW_DP_PULLUP, HW_HC_2_DIS,
++	/* HW_OTG_CTRL_CLR */
++	HW_OTG_DISABLE_CLEAR, HW_SW_SEL_HC_DC_CLEAR, HW_VBUS_DRV_CLEAR,
++	HW_SEL_CP_EXT_CLEAR, HW_DM_PULLDOWN_CLEAR, HW_DP_PULLDOWN_CLEAR,
++	HW_DP_PULLUP_CLEAR, HW_HC_2_DIS_CLEAR,
++	/* Last element */
++	HC_FIELD_MAX,
++};
++
++/* ISP1763 */
++/* EHCI operational registers */
++#define ISP1763_HC_USBCMD		0x8c
++#define ISP1763_HC_USBSTS		0x90
++#define ISP1763_HC_FRINDEX		0x98
++
++#define ISP1763_HC_CONFIGFLAG		0x9c
++#define ISP1763_HC_PORTSC1		0xa0
++
++#define ISP1763_HC_ISO_PTD_DONEMAP	0xa4
++#define ISP1763_HC_ISO_PTD_SKIPMAP	0xa6
++#define ISP1763_HC_ISO_PTD_LASTPTD	0xa8
++#define ISP1763_HC_INT_PTD_DONEMAP	0xaa
++#define ISP1763_HC_INT_PTD_SKIPMAP	0xac
++#define ISP1763_HC_INT_PTD_LASTPTD	0xae
++#define ISP1763_HC_ATL_PTD_DONEMAP	0xb0
++#define ISP1763_HC_ATL_PTD_SKIPMAP	0xb2
++#define ISP1763_HC_ATL_PTD_LASTPTD	0xb4
++
++/* Configuration Register */
++#define ISP1763_HC_HW_MODE_CTRL		0xb6
++#define ISP1763_HC_CHIP_REV		0x70
++#define ISP1763_HC_CHIP_ID		0x72
++#define ISP1763_HC_SCRATCH		0x78
++#define ISP1763_HC_RESET		0xb8
++#define ISP1763_HC_BUFFER_STATUS	0xba
++#define ISP1763_HC_MEMORY		0xc4
++#define ISP1763_HC_DATA			0xc6
++
++/* Interrupt Register */
++#define ISP1763_HC_INTERRUPT		0xd4
++#define ISP1763_HC_INTERRUPT_ENABLE	0xd6
++#define ISP1763_HC_ISO_IRQ_MASK_OR	0xd8
++#define ISP1763_HC_INT_IRQ_MASK_OR	0xda
++#define ISP1763_HC_ATL_IRQ_MASK_OR	0xdc
++#define ISP1763_HC_ISO_IRQ_MASK_AND	0xde
++#define ISP1763_HC_INT_IRQ_MASK_AND	0xe0
++#define ISP1763_HC_ATL_IRQ_MASK_AND	0xe2
++
++#define ISP1763_HC_OTG_CTRL_SET		0xe4
++#define ISP1763_HC_OTG_CTRL_CLEAR	0xe6
++
++/* -----------------------------------------------------------------------------
++ * Peripheral Controller
++ */
++
++#define DC_IEPTX(n)			(1 << (11 + 2 * (n)))
++#define DC_IEPRX(n)			(1 << (10 + 2 * (n)))
++#define DC_IEPRXTX(n)			(3 << (10 + 2 * (n)))
++
++#define ISP176x_DC_CDBGMOD_ACK		BIT(6)
++#define ISP176x_DC_DDBGMODIN_ACK	BIT(4)
++#define ISP176x_DC_DDBGMODOUT_ACK	BIT(2)
++
++#define ISP176x_DC_IEP0SETUP		BIT(8)
++#define ISP176x_DC_IEVBUS		BIT(7)
++#define ISP176x_DC_IEHS_STA		BIT(5)
++#define ISP176x_DC_IERESM		BIT(4)
++#define ISP176x_DC_IESUSP		BIT(3)
++#define ISP176x_DC_IEBRST		BIT(0)
++
++#define ISP176x_DC_ENDPTYP_ISOC		0x01
++#define ISP176x_DC_ENDPTYP_BULK		0x02
++#define ISP176x_DC_ENDPTYP_INTERRUPT	0x03
++
++/* Initialization Registers */
++#define ISP176x_DC_ADDRESS		0x0200
++#define ISP176x_DC_MODE			0x020c
++#define ISP176x_DC_INTCONF		0x0210
++#define ISP176x_DC_DEBUG		0x0212
++#define ISP176x_DC_INTENABLE		0x0214
++
++/* Data Flow Registers */
++#define ISP176x_DC_EPMAXPKTSZ		0x0204
++#define ISP176x_DC_EPTYPE		0x0208
++
++#define ISP176x_DC_BUFLEN		0x021c
++#define ISP176x_DC_BUFSTAT		0x021e
++#define ISP176x_DC_DATAPORT		0x0220
++
++#define ISP176x_DC_CTRLFUNC		0x0228
++#define ISP176x_DC_EPINDEX		0x022c
++
++/* DMA Registers */
++#define ISP176x_DC_DMACMD		0x0230
++#define ISP176x_DC_DMATXCOUNT		0x0234
++#define ISP176x_DC_DMACONF		0x0238
++#define ISP176x_DC_DMAHW		0x023c
++#define ISP176x_DC_DMAINTREASON		0x0250
++#define ISP176x_DC_DMAINTEN		0x0254
++#define ISP176x_DC_DMAEP		0x0258
++#define ISP176x_DC_DMABURSTCOUNT	0x0264
++
++/* General Registers */
++#define ISP176x_DC_INTERRUPT		0x0218
++#define ISP176x_DC_CHIPID		0x0270
++#define ISP176x_DC_FRAMENUM		0x0274
++#define ISP176x_DC_SCRATCH		0x0278
++#define ISP176x_DC_UNLOCKDEV		0x027c
++#define ISP176x_DC_INTPULSEWIDTH	0x0280
++#define ISP176x_DC_TESTMODE		0x0284
++
++enum isp176x_device_controller_fields {
++	/* DC_ADDRESS */
++	DC_DEVEN, DC_DEVADDR,
++	/* DC_MODE */
++	DC_VBUSSTAT, DC_SFRESET, DC_GLINTENA,
++	/* DC_INTCONF */
++	DC_CDBGMOD_ACK, DC_DDBGMODIN_ACK, DC_DDBGMODOUT_ACK, DC_INTPOL,
++	/* DC_INTENABLE */
++	DC_IEPRXTX_7, DC_IEPRXTX_6, DC_IEPRXTX_5, DC_IEPRXTX_4, DC_IEPRXTX_3,
++	DC_IEPRXTX_2, DC_IEPRXTX_1, DC_IEPRXTX_0,
++	DC_IEP0SETUP, DC_IEVBUS, DC_IEHS_STA, DC_IERESM, DC_IESUSP, DC_IEBRST,
++	/* DC_EPINDEX */
++	DC_EP0SETUP, DC_ENDPIDX, DC_EPDIR,
++	/* DC_CTRLFUNC */
++	DC_CLBUF, DC_VENDP, DC_DSEN, DC_STATUS, DC_STALL,
++	/* DC_BUFLEN */
++	DC_BUFLEN,
++	/* DC_EPMAXPKTSZ */
++	DC_FFOSZ,
++	/* DC_EPTYPE */
++	DC_EPENABLE, DC_ENDPTYP,
++	/* DC_FRAMENUM */
++	DC_FRAMENUM, DC_UFRAMENUM,
++	/* DC_CHIP_ID */
++	DC_CHIP_ID_HIGH, DC_CHIP_ID_LOW,
++	/* DC_SCRATCH */
++	DC_SCRATCH,
++	/* Last element */
++	DC_FIELD_MAX,
++};
++
++/* ISP1763 */
++/* Initialization Registers */
++#define ISP1763_DC_ADDRESS		0x00
++#define ISP1763_DC_MODE			0x0c
++#define ISP1763_DC_INTCONF		0x10
++#define ISP1763_DC_INTENABLE		0x14
++
++/* Data Flow Registers */
++#define ISP1763_DC_EPMAXPKTSZ		0x04
++#define ISP1763_DC_EPTYPE		0x08
++
++#define ISP1763_DC_BUFLEN		0x1c
++#define ISP1763_DC_BUFSTAT		0x1e
++#define ISP1763_DC_DATAPORT		0x20
++
++#define ISP1763_DC_CTRLFUNC		0x28
++#define ISP1763_DC_EPINDEX		0x2c
++
++/* DMA Registers */
++#define ISP1763_DC_DMACMD		0x30
++#define ISP1763_DC_DMATXCOUNT		0x34
++#define ISP1763_DC_DMACONF		0x38
++#define ISP1763_DC_DMAHW		0x3c
++#define ISP1763_DC_DMAINTREASON		0x50
++#define ISP1763_DC_DMAINTEN		0x54
++#define ISP1763_DC_DMAEP		0x58
++#define ISP1763_DC_DMABURSTCOUNT	0x64
++
++/* General Registers */
++#define ISP1763_DC_INTERRUPT		0x18
++#define ISP1763_DC_CHIPID_LOW		0x70
++#define ISP1763_DC_CHIPID_HIGH		0x72
++#define ISP1763_DC_FRAMENUM		0x74
++#define ISP1763_DC_SCRATCH		0x78
++#define ISP1763_DC_UNLOCKDEV		0x7c
++#define ISP1763_DC_INTPULSEWIDTH	0x80
++#define ISP1763_DC_TESTMODE		0x84
++
++#endif
+diff --git a/drivers/usb/isp1760/isp1760-uboot.c b/drivers/usb/isp1760/isp1760-uboot.c
+new file mode 100644
+index 000000000000..7635210fe2b4
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-uboot.c
+@@ -0,0 +1,76 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <dm/device-internal.h>
++#include <dm/device_compat.h>
++#include <dm/devres.h>
++#include <dm/lists.h>
++#include <linux/bug.h>
++#include <linux/io.h>
++#include <linux/kernel.h>
++#include <linux/usb/otg.h>
++#include <linux/usb/usb_urb_compat.h>
++#include <log.h>
++#include <usb.h>
++
++#include "isp1760-core.h"
++#include "isp1760-hcd.h"
++#include "isp1760-regs.h"
++#include "isp1760-uboot.h"
++
++static int isp1760_msg_submit_control(struct udevice *dev,
++				      struct usb_device *udev,
++				      unsigned long pipe, void *buffer,
++				      int length, struct devrequest *setup)
++{
++	struct isp1760_host_data *host = dev_get_priv(dev);
++
++	return usb_urb_submit_control(&host->hcd, &host->urb, &host->hep, udev,
++				      pipe, buffer, length, setup, 0,
++				      host->host_speed);
++}
++
++static int isp1760_msg_submit_bulk(struct udevice *dev, struct usb_device *udev,
++				   unsigned long pipe, void *buffer, int length)
++{
++	struct isp1760_host_data *host = dev_get_priv(dev);
++
++	return usb_urb_submit_bulk(&host->hcd, &host->urb, &host->hep, udev,
++				   pipe, buffer, length);
++}
++
++static int isp1760_msg_submit_irq(struct udevice *dev, struct usb_device *udev,
++				  unsigned long pipe, void *buffer, int length,
++				  int interval, bool nonblock)
++{
++	struct isp1760_host_data *host = dev_get_priv(dev);
++
++	return usb_urb_submit_irq(&host->hcd, &host->urb, &host->hep, udev,
++				  pipe, buffer, length, interval);
++}
++
++static int isp1760_get_max_xfer_size(struct udevice *dev, size_t *size)
++{
++	struct isp1760_host_data *host = dev_get_priv(dev);
++	struct isp1760_hcd *priv = host->hcd.hcd_priv;
++	const struct isp1760_memory_layout *mem = priv->memory_layout;
++
++	*size = mem->blocks_size[ISP176x_BLOCK_NUM - 1];
++
++	return 0;
++}
++
++
++struct dm_usb_ops isp1760_usb_ops = {
++	.control		= isp1760_msg_submit_control,
++	.bulk			= isp1760_msg_submit_bulk,
++	.interrupt		= isp1760_msg_submit_irq,
++	.get_max_xfer_size	= isp1760_get_max_xfer_size,
++};
+diff --git a/drivers/usb/isp1760/isp1760-uboot.h b/drivers/usb/isp1760/isp1760-uboot.h
+new file mode 100644
+index 000000000000..2486de6f9e27
+--- /dev/null
++++ b/drivers/usb/isp1760/isp1760-uboot.h
+@@ -0,0 +1,27 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Driver for the NXP ISP1760 chip
++ *
++ * Copyright 2021 Linaro, Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ */
++
++#ifndef __ISP1760_UBOOT_H__
++#define __ISP1760_UBOOT_H__
++
++#include <linux/usb/usb_urb_compat.h>
++#include <usb.h>
++
++#include "isp1760-core.h"
++
++struct isp1760_host_data {
++	struct isp1760_hcd *priv;
++	struct usb_hcd hcd;
++	enum usb_device_speed host_speed;
++	struct usb_host_endpoint hep;
++	struct urb urb;
++};
++
++extern struct dm_usb_ops isp1760_usb_ops;
++
++#endif
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0005-corstone1000-enable-isp1763-usb-controller.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0005-corstone1000-enable-isp1763-usb-controller.patch
new file mode 100644
index 0000000..2ce03b6
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0005-corstone1000-enable-isp1763-usb-controller.patch
@@ -0,0 +1,48 @@
+From b7fb62e512e00a5fdbb4321bb0b4109261518481 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Thu, 3 Mar 2022 16:52:02 +0000
+Subject: [PATCH 05/27] corstone1000: enable isp1763 usb controller
+
+MPS3 board have a ISP1763 usb controller, add the
+correspondent mmio area and enable it to be used for mass
+storage access for example.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ configs/corstone1000_defconfig | 1 +
+ include/configs/corstone1000.h | 6 ++++++
+ 2 files changed, 7 insertions(+)
+
+diff --git a/configs/corstone1000_defconfig b/configs/corstone1000_defconfig
+index 02f931b0d469..e573fe6fe6a2 100644
+--- a/configs/corstone1000_defconfig
++++ b/configs/corstone1000_defconfig
+@@ -42,6 +42,7 @@ CONFIG_REGMAP=y
+ CONFIG_DM_SERIAL=y
+ CONFIG_USB=y
+ CONFIG_DM_USB=y
++CONFIG_USB_ISP1760=y
+ CONFIG_USB_STORAGE=y
+ CONFIG_EFI_MM_COMM_TEE=y
+ # CONFIG_OPTEE is not set
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index cf166f107efd..8ba0effb0ab2 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -55,7 +55,13 @@
+ #define CONFIG_SYS_CBSIZE	512	/* Console I/O Buffer Size */
+ #define CONFIG_SYS_MAXARGS	64	/* max command args */
+ 
++#define BOOT_TARGET_DEVICES(func) \
++	func(USB, usb, 0)
++
++#include <config_distro_bootcmd.h>
++
+ #define CONFIG_EXTRA_ENV_SETTINGS							\
++				BOOTENV							\
+ 				"usb_pgood_delay=250\0"					\
+ 				"boot_bank_flag=0x08002000\0"				\
+ 				"kernel_addr_bank_0=0x083EE000\0"			\
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0006-arm_ffa-introducing-Arm-FF-A-low-level-driver.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0006-arm_ffa-introducing-Arm-FF-A-low-level-driver.patch
new file mode 100644
index 0000000..8cc848f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0006-arm_ffa-introducing-Arm-FF-A-low-level-driver.patch
@@ -0,0 +1,2610 @@
+From ede21dc1ca75132698cc81e88357c5789ccf67c7 Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Tue, 16 Nov 2021 12:34:52 +0000
+Subject: [PATCH 06/27] arm_ffa: introducing Arm FF-A low-level driver
+
+This driver implements Arm Firmware Framework for Armv8-A on u-boot
+
+The Firmware Framework for Arm A-profile processors (FF-A)
+describes interfaces (ABIs) that standardize communication
+between the Secure World and Normal World leveraging TrustZone
+technology.
+
+This driver is based on FF-A specification v1.0 and uses SMC32
+calling convention.
+
+FF-A specification:
+
+https://developer.arm.com/documentation/den0077/a/?lang=en
+
+The driver provides helper FF-A interfaces for user layers.
+These helper functions allow clients to pass data and select the
+FF-A function to use for the communication with secure world.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ MAINTAINERS                      |    8 +
+ arch/arm/cpu/armv8/smccc-call.S  |   27 +
+ arch/arm/lib/asm-offsets.c       |    6 +
+ common/board_r.c                 |    6 +
+ drivers/Kconfig                  |    2 +
+ drivers/Makefile                 |    1 +
+ drivers/arm-ffa/Kconfig          |   26 +
+ drivers/arm-ffa/Makefile         |    3 +
+ drivers/arm-ffa/arm-ffa-uclass.c |   67 ++
+ drivers/arm-ffa/arm_ffa_prv.h    |  199 ++++
+ drivers/arm-ffa/core.c           | 1484 ++++++++++++++++++++++++++++++
+ include/arm_ffa.h                |  191 ++++
+ include/arm_ffa_helper.h         |   45 +
+ include/dm/uclass-id.h           |    1 +
+ include/linux/arm-smccc.h        |   28 +-
+ lib/Kconfig                      |    1 +
+ lib/Makefile                     |    1 +
+ lib/arm-ffa/Kconfig              |    6 +
+ lib/arm-ffa/Makefile             |    8 +
+ lib/arm-ffa/arm_ffa_helper.c     |  188 ++++
+ lib/efi_loader/efi_boottime.c    |   17 +
+ 21 files changed, 2314 insertions(+), 1 deletion(-)
+ create mode 100644 drivers/arm-ffa/Kconfig
+ create mode 100644 drivers/arm-ffa/Makefile
+ create mode 100644 drivers/arm-ffa/arm-ffa-uclass.c
+ create mode 100644 drivers/arm-ffa/arm_ffa_prv.h
+ create mode 100644 drivers/arm-ffa/core.c
+ create mode 100644 include/arm_ffa.h
+ create mode 100644 include/arm_ffa_helper.h
+ create mode 100644 lib/arm-ffa/Kconfig
+ create mode 100644 lib/arm-ffa/Makefile
+ create mode 100644 lib/arm-ffa/arm_ffa_helper.c
+
+diff --git a/MAINTAINERS b/MAINTAINERS
+index 96582fc67777..14307e6da644 100644
+--- a/MAINTAINERS
++++ b/MAINTAINERS
+@@ -232,6 +232,14 @@ F:	board/CZ.NIC/
+ F:	configs/turris_*_defconfig
+ F:	include/configs/turris_*.h
+ 
++ARM FF-A
++M:	Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++S:	Maintained
++F:	drivers/arm-ffa/
++F:	include/arm_ffa.h
++F:	include/arm_ffa_helper.h
++F:	lib/arm-ffa/
++
+ ARM FREESCALE IMX
+ M:	Stefano Babic <sbabic@denx.de>
+ M:	Fabio Estevam <festevam@gmail.com>
+diff --git a/arch/arm/cpu/armv8/smccc-call.S b/arch/arm/cpu/armv8/smccc-call.S
+index dc92b28777c3..ffc39c9fefa2 100644
+--- a/arch/arm/cpu/armv8/smccc-call.S
++++ b/arch/arm/cpu/armv8/smccc-call.S
+@@ -1,6 +1,8 @@
+ /* SPDX-License-Identifier: GPL-2.0 */
+ /*
+  * Copyright (c) 2015, Linaro Limited
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+  */
+ #include <linux/linkage.h>
+ #include <linux/arm-smccc.h>
+@@ -45,3 +47,28 @@ ENDPROC(__arm_smccc_smc)
+ ENTRY(__arm_smccc_hvc)
+ 	SMCCC	hvc
+ ENDPROC(__arm_smccc_hvc)
++
++#if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++
++	.macro FFASMCCC instr
++	.cfi_startproc
++	\instr	#0
++	ldr	x9, [sp]
++	stp	x0, x1, [x9, #ARM_SMCCC_RES_X0_OFFS]
++	stp	x2, x3, [x9, #ARM_SMCCC_RES_X2_OFFS]
++	stp	x4, x5, [x9, #ARM_SMCCC_RES_X4_OFFS]
++	stp	x6, x7, [x9, #ARM_SMCCC_RES_X6_OFFS]
++	ret
++	.cfi_endproc
++	.endm
++
++/*
++ * void arm_ffa_smccc_smc(unsigned long a0, unsigned long a1, unsigned long a2,
++ *		  unsigned long a3, unsigned long a4, unsigned long a5,
++ *		  unsigned long a6, unsigned long a7, struct arm_smccc_res *res)
++ */
++ENTRY(__arm_ffa_smccc_smc)
++	FFASMCCC	smc
++ENDPROC(__arm_ffa_smccc_smc)
++
++#endif
+diff --git a/arch/arm/lib/asm-offsets.c b/arch/arm/lib/asm-offsets.c
+index 22fd541f9a28..45eca83a473c 100644
+--- a/arch/arm/lib/asm-offsets.c
++++ b/arch/arm/lib/asm-offsets.c
+@@ -9,6 +9,8 @@
+  * generate asm statements containing #defines,
+  * compile this file to assembler, and then extract the
+  * #defines from the assembly-language output.
++ *
++ * (C) Copyright 2021 ARM Limited
+  */
+ 
+ #include <common.h>
+@@ -115,6 +117,10 @@ int main(void)
+ #ifdef CONFIG_ARM_SMCCC
+ 	DEFINE(ARM_SMCCC_RES_X0_OFFS, offsetof(struct arm_smccc_res, a0));
+ 	DEFINE(ARM_SMCCC_RES_X2_OFFS, offsetof(struct arm_smccc_res, a2));
++#if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++	DEFINE(ARM_SMCCC_RES_X4_OFFS, offsetof(struct arm_smccc_res, a4));
++	DEFINE(ARM_SMCCC_RES_X6_OFFS, offsetof(struct arm_smccc_res, a6));
++#endif
+ 	DEFINE(ARM_SMCCC_QUIRK_ID_OFFS, offsetof(struct arm_smccc_quirk, id));
+ 	DEFINE(ARM_SMCCC_QUIRK_STATE_OFFS, offsetof(struct arm_smccc_quirk, state));
+ #endif
+diff --git a/common/board_r.c b/common/board_r.c
+index c24d9b4e220b..af20f38b104c 100644
+--- a/common/board_r.c
++++ b/common/board_r.c
+@@ -61,6 +61,9 @@
+ #include <wdt.h>
+ #include <asm-generic/gpio.h>
+ #include <efi_loader.h>
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++#include <arm_ffa_helper.h>
++#endif
+ 
+ DECLARE_GLOBAL_DATA_PTR;
+ 
+@@ -770,6 +773,9 @@ static init_fnc_t init_sequence_r[] = {
+ 	INIT_FUNC_WATCHDOG_RESET
+ 	initr_net,
+ #endif
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++	ffa_helper_init_device,
++#endif
+ #ifdef CONFIG_POST
+ 	initr_post,
+ #endif
+diff --git a/drivers/Kconfig b/drivers/Kconfig
+index b26ca8cf70c9..e83c23789d1b 100644
+--- a/drivers/Kconfig
++++ b/drivers/Kconfig
+@@ -6,6 +6,8 @@ source "drivers/core/Kconfig"
+ 
+ source "drivers/adc/Kconfig"
+ 
++source "drivers/arm-ffa/Kconfig"
++
+ source "drivers/ata/Kconfig"
+ 
+ source "drivers/axi/Kconfig"
+diff --git a/drivers/Makefile b/drivers/Makefile
+index 4e7cf284405a..6671d2a604ab 100644
+--- a/drivers/Makefile
++++ b/drivers/Makefile
+@@ -107,6 +107,7 @@ obj-y += iommu/
+ obj-y += smem/
+ obj-y += thermal/
+ obj-$(CONFIG_TEE) += tee/
++obj-$(CONFIG_ARM_FFA_TRANSPORT) += arm-ffa/
+ obj-y += axi/
+ obj-y += ufs/
+ obj-$(CONFIG_W1) += w1/
+diff --git a/drivers/arm-ffa/Kconfig b/drivers/arm-ffa/Kconfig
+new file mode 100644
+index 000000000000..d71444c1fa90
+--- /dev/null
++++ b/drivers/arm-ffa/Kconfig
+@@ -0,0 +1,26 @@
++# SPDX-License-Identifier: GPL-2.0
++
++config ARM_FFA_TRANSPORT
++	bool "Enable Arm Firmware Framework for Armv8-A driver"
++	depends on DM && ARM64
++	select ARM_SMCCC
++	select LIB_UUID
++	select ARM_FFA_TRANSPORT_HELPERS
++	select CMD_ARMFFA
++	help
++	  The Firmware Framework for Arm A-profile processors (FF-A)
++	  describes interfaces (ABIs) that standardize communication
++	  between the Secure World and Normal World leveraging TrustZone
++	  technology.
++
++	  This driver is based on FF-A specification v1.0 and uses SMC32
++	  calling convention.
++
++	  FF-A specification:
++
++	  https://developer.arm.com/documentation/den0077/a/?lang=en
++
++	  In u-boot FF-A design, the Secure World is considered as one
++	  entity to communicate with. FF-A communication is handled by
++	  one device and one instance. This device takes care of
++	  all the interactions between Normal world and Secure World.
+diff --git a/drivers/arm-ffa/Makefile b/drivers/arm-ffa/Makefile
+new file mode 100644
+index 000000000000..9fb5bea52299
+--- /dev/null
++++ b/drivers/arm-ffa/Makefile
+@@ -0,0 +1,3 @@
++# SPDX-License-Identifier: GPL-2.0+
++
++obj-y += arm-ffa-uclass.o core.o
+diff --git a/drivers/arm-ffa/arm-ffa-uclass.c b/drivers/arm-ffa/arm-ffa-uclass.c
+new file mode 100644
+index 000000000000..43f6066281fe
+--- /dev/null
++++ b/drivers/arm-ffa/arm-ffa-uclass.c
+@@ -0,0 +1,67 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <arm_ffa.h>
++#include <errno.h>
++#include <log.h>
++#include <asm/global_data.h>
++
++DECLARE_GLOBAL_DATA_PTR;
++
++UCLASS_DRIVER(ffa) = {
++	.name		= "ffa",
++	.id		= UCLASS_FFA,
++};
++
++/**
++ * ffa_get_invoke_func - performs a call to the FF-A driver dispatcher
++ * @func_id:	The FF-A function to be used
++ * @func_data:  Pointer to the FF-A function arguments
++ *				container structure. This also includes
++ *				pointers to the returned data needed by
++ *				clients.
++ *
++ * This runtime function passes the FF-A function ID and its arguments to
++ * the FF-A driver dispatcher.
++ * This function is called by the FF-A helper functions.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int __ffa_runtime ffa_get_invoke_func(u32 func_id, struct ffa_interface_data *func_data)
++{
++	if (!ffa_device_get_ops()->invoke_func)
++		return -EINVAL;
++
++	return ffa_device_get_ops()->invoke_func(func_id, func_data);
++}
++
++/**
++ * ffa_init_device - probes the arm_ffa device
++ *
++ * This boot time function makes sure the arm_ffa device is probed
++ * and ready for use.
++ * This function is called automatically at initcalls
++ * level (after u-boot relocation).
++ *
++ * Arm FF-A transport is implemented through a single u-boot
++ * device (arm_ffa). So, there is only one device belonging to UCLASS_FFA.
++ * All FF-A clients should use the arm_ffa device to use the FF-A
++ * transport.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int ffa_init_device(void)
++{
++	ffa_dbg("[%s]", __func__);
++
++	return ffa_get_device();
++}
+diff --git a/drivers/arm-ffa/arm_ffa_prv.h b/drivers/arm-ffa/arm_ffa_prv.h
+new file mode 100644
+index 000000000000..38ea4ba83efc
+--- /dev/null
++++ b/drivers/arm-ffa/arm_ffa_prv.h
+@@ -0,0 +1,199 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#ifndef __ARM_FFA_PRV_H
++#define __ARM_FFA_PRV_H
++
++#include <arm_ffa.h>
++#include <linux/bitfield.h>
++#include <linux/bitops.h>
++
++/*
++ * This header is private. It is exclusively used by the FF-A driver
++ */
++
++/* FF-A driver version definitions */
++
++#define MAJOR_VERSION_MASK		GENMASK(30, 16)
++#define MINOR_VERSION_MASK		GENMASK(15, 0)
++#define GET_FFA_MAJOR_VERSION(x)		\
++				((u16)(FIELD_GET(MAJOR_VERSION_MASK, (x))))
++#define GET_FFA_MINOR_VERSION(x)		\
++				((u16)(FIELD_GET(MINOR_VERSION_MASK, (x))))
++#define PACK_VERSION_INFO(major, minor)			\
++	(FIELD_PREP(MAJOR_VERSION_MASK, (major)) |	\
++	 FIELD_PREP(MINOR_VERSION_MASK, (minor)))
++
++#define FFA_MAJOR_VERSION		(1)
++#define FFA_MINOR_VERSION		(0)
++#define FFA_VERSION_1_0		\
++			PACK_VERSION_INFO(FFA_MAJOR_VERSION, FFA_MINOR_VERSION)
++
++/* Endpoint ID mask (u-boot endpoint ID) */
++
++#define GET_SELF_ENDPOINT_ID_MASK		GENMASK(15, 0)
++#define GET_SELF_ENDPOINT_ID(x)		\
++			((u16)(FIELD_GET(GET_SELF_ENDPOINT_ID_MASK, (x))))
++
++#define PREP_SELF_ENDPOINT_ID_MASK		GENMASK(31, 16)
++#define PREP_SELF_ENDPOINT_ID(x)		\
++			((u16)(FIELD_PREP(PREP_SELF_ENDPOINT_ID_MASK, (x))))
++
++/* Partition endpoint ID mask  (partition with which u-boot communicates with) */
++
++#define PREP_PART_ENDPOINT_ID_MASK		GENMASK(15, 0)
++#define PREP_PART_ENDPOINT_ID(x)		\
++			((u16)(FIELD_PREP(PREP_PART_ENDPOINT_ID_MASK, (x))))
++
++/* The FF-A SMC function prototype definition */
++
++typedef void (*invoke_ffa_fn_t)(unsigned long a0, unsigned long a1,
++			unsigned long a2, unsigned long a3, unsigned long a4,
++			unsigned long a5, unsigned long a6, unsigned long a7,
++			struct arm_smccc_res *res);
++
++/**
++ * enum ffa_conduit - Arm FF-A conduits supported by the Arm FF-A driver
++ * Currently only SMC32 is supported.
++ */
++enum ffa_conduit {
++	FFA_CONDUIT_SMC = 0,
++};
++
++/**
++ * FFA_DECLARE_ARGS - FF-A functions local variables
++ * @a0-a7:	local variables used to set registers x0-x7
++ * @res:	the structure hosting the FF-A function return data
++ *
++ * A helper macro for declaring local variables for the FF-A functions  arguments.
++ * The x0-x7 registers are used to exchange data with the secure world.
++ * But, only the bottom 32-bit of thes registers contains the data.
++ */
++#define FFA_DECLARE_ARGS \
++	unsigned long a0 = 0; \
++	unsigned long a1 = 0; \
++	unsigned long a2 = 0; \
++	unsigned long a3 = 0; \
++	unsigned long a4 = 0; \
++	unsigned long a5 = 0; \
++	unsigned long a6 = 0; \
++	unsigned long a7 = 0; \
++	struct arm_smccc_res res = {0}
++
++/* FF-A error codes */
++#define FFA_ERR_STAT_NOT_SUPPORTED				(-1)
++#define FFA_ERR_STAT_INVALID_PARAMETERS				(-2)
++#define FFA_ERR_STAT_NO_MEMORY				(-3)
++#define FFA_ERR_STAT_BUSY				(-4)
++#define FFA_ERR_STAT_INTERRUPTED				(-5)
++#define FFA_ERR_STAT_DENIED				(-6)
++#define FFA_ERR_STAT_RETRY				(-7)
++#define FFA_ERR_STAT_ABORTED				(-8)
++
++/**
++ * struct ffa_features_desc - FF-A functions features
++ * @func_id:	FF-A function
++ * @field1:	features read from register w2
++ * @field2:	features read from register w3
++ *
++ * Data structure describing the features of the  FF-A functions queried by
++ * FFA_FEATURES
++ */
++struct ffa_features_desc {
++	u32 func_id;
++	u32 field1;
++	u32 field2;
++};
++
++/**
++ * enum ffa_rxtx_buf_sizes - minimum sizes supported
++ * for the RX/TX buffers
++ */
++enum ffa_rxtx_buf_sizes {
++	RXTX_4K,
++	RXTX_64K,
++	RXTX_16K
++};
++
++/*
++ * Number of the FF-A interfaces features descriptors
++ * currently only FFA_RXTX_MAP descriptor is supported
++ */
++#define FFA_FEATURE_DESC_CNT (1)
++
++/**
++ * struct ffa_pdata - platform data for the arm_ffa device
++ * @conduit: The FF-A conduit used
++ *
++ * Platform data structure read from the device tree
++ */
++struct ffa_pdata {
++	enum ffa_conduit conduit;
++};
++
++/**
++ * struct ffa_rxtxpair - structure hosting the RX/TX buffers physical addresses
++ * @rxbuf:	physical address of the RX buffer
++ * @txbuf:	physical address of the TX buffer
++ *
++ * Data structure hosting the physical addresses of the mapped RX/TX buffers
++ * These physical address are used by the FF-A functions that use the RX/TX buffers
++ */
++struct ffa_rxtxpair {
++	u64 rxbuf; /* physical address */
++	u64 txbuf; /* physical address */
++};
++
++/**
++ * struct ffa_partition_desc - the secure partition descriptor
++ * @info:	partition information
++ * @UUID:	UUID
++ *
++ * Each partition has its descriptor containing the partitions information and the UUID
++ */
++struct ffa_partition_desc {
++	struct ffa_partition_info info;
++	union ffa_partition_uuid UUID;
++};
++
++/**
++ * struct ffa_partitions - descriptors for all secure partitions
++ * @count:	The number of partitions descriptors
++ * @descs	The partitions descriptors table
++ *
++ * This data structure contains the partitions descriptors table
++ */
++struct ffa_partitions {
++	u32 count;
++	struct ffa_partition_desc *descs; /* virtual address */
++};
++
++/**
++ * struct ffa_prvdata - the driver private data structure
++ *
++ * @dev:	The arm_ffa device under u-boot driver model
++ * @fwk_version:	FF-A framework version
++ * @id:	u-boot endpoint ID
++ * @partitions:	The partitions descriptors structure
++ * @pair:	The RX/TX buffers pair
++ * @conduit:	The selected conduit
++ * @invoke_ffa_fn:	The function executing the FF-A function
++ * @features:	Table of the FF-A functions having features
++ *
++ * The driver data structure hosting all resident data.
++ */
++struct ffa_prvdata {
++	struct udevice *dev;
++	u32 fwk_version;
++	u16 id;
++	struct ffa_partitions partitions;
++	struct ffa_rxtxpair pair;
++	enum ffa_conduit conduit;
++	invoke_ffa_fn_t invoke_ffa_fn;
++	struct ffa_features_desc features[FFA_FEATURE_DESC_CNT];
++};
++
++#endif
+diff --git a/drivers/arm-ffa/core.c b/drivers/arm-ffa/core.c
+new file mode 100644
+index 000000000000..98e2d2fa1767
+--- /dev/null
++++ b/drivers/arm-ffa/core.c
+@@ -0,0 +1,1484 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#include "arm_ffa_prv.h"
++#include <asm/global_data.h>
++#include <asm/io.h>
++#include <common.h>
++#include <dm.h>
++#include <fdtdec.h>
++#include <linux/errno.h>
++#include <linux/sizes.h>
++#include <log.h>
++#include <malloc.h>
++#include <mapmem.h>
++#include <string.h>
++
++DECLARE_GLOBAL_DATA_PTR;
++
++/**
++ * The device private data structure containing all the resident
++ * data read from secure world
++ */
++struct ffa_prvdata __ffa_runtime_data ffa_priv_data = {0};
++
++/*
++ * Driver functions
++ */
++
++/**
++ * ffa_get_device - probes the arm_ffa device
++ *
++ * This boot time function makes sure the arm_ffa device is probed
++ * and ready for use. This is done using uclass_get_device.
++ * The arm_ffa driver belongs to UCLASS_FFA.
++ * This function should be called before using the driver.
++ *
++ * Arm FF-A transport is implemented through a single u-boot
++ * device (arm_ffa). So, there is only one device belonging to UCLASS_FFA.
++ * All FF-A clients should use the arm_ffa device to use the FF-A
++ * transport.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int ffa_get_device(void)
++{
++	int ret;
++	int devnum = 0;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (ffa_priv_data.dev)
++		return FFA_ERR_STAT_SUCCESS;
++
++	/*
++	 * searching and probing the device
++	 */
++	ret = uclass_get_device(UCLASS_FFA, devnum, &ffa_priv_data.dev);
++	if (ret) {
++		ffa_err("can not find the device");
++		ffa_priv_data.dev = NULL;
++		return -ENODEV;
++	}
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_get_version - FFA_VERSION handler function
++ *
++ * This is the boot time function that implements FFA_VERSION FF-A function
++ * to get from the secure world the FF-A framework version
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_get_version(void)
++{
++	u16 major, minor;
++
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_VERSION;
++	a1 = FFA_VERSION_1_0;
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	if (res.a0 == FFA_ERR_STAT_NOT_SUPPORTED) {
++		ffa_err("A Firmware Framework implementation does not exist");
++		return -EOPNOTSUPP;
++	}
++
++	major = GET_FFA_MAJOR_VERSION(res.a0);
++	minor = GET_FFA_MINOR_VERSION(res.a0);
++
++	ffa_info("FF-A driver %d.%d\nFF-A framework %d.%d",
++		 FFA_MAJOR_VERSION, FFA_MINOR_VERSION, major, minor);
++
++	if ((major == FFA_MAJOR_VERSION && minor >= FFA_MINOR_VERSION)) {
++		ffa_info("Versions are compatible ");
++
++		ffa_priv_data.fwk_version = res.a0;
++
++		return FFA_ERR_STAT_SUCCESS;
++	}
++
++	ffa_info("Versions are incompatible ");
++	return -EPROTONOSUPPORT;
++}
++
++/**
++ * ffa_get_endpoint_id - FFA_ID_GET handler function
++ *
++ * This is the boot time function that implements FFA_ID_GET FF-A function
++ * to get from the secure world u-boot endpoint ID
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_get_endpoint_id(void)
++{
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_ID_GET;
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		if (((int)res.a2) == FFA_ERR_STAT_NOT_SUPPORTED) {
++			ffa_err("This function is not implemented at this FF-A instance");
++			return -EOPNOTSUPP;
++		}
++
++		ffa_err("Undefined error code (%d)", ((int)res.a2));
++		return -EINVAL;
++	}
++	case FFA_SUCCESS:
++	{
++		ffa_priv_data.id = GET_SELF_ENDPOINT_ID(res.a2);
++		ffa_info("endpoint ID is %u", ffa_priv_data.id);
++
++		return FFA_ERR_STAT_SUCCESS;
++	}
++	default:
++	{
++		ffa_err("Undefined response function (0x%lx)", res.a0);
++		return -EINVAL;
++	}
++	}
++}
++
++/**
++ * ffa_get_features_desc - returns the features descriptor of the specified
++ *						FF-A function
++ * @func_id:	the FF-A function which the features are to be retrieved
++ *
++ * This is a boot time function that searches the features descriptor of the
++ * specified FF-A function
++ *
++ * Return:
++ *
++ * When found, the address of the features descriptor is returned. Otherwise, NULL.
++ */
++static struct ffa_features_desc *ffa_get_features_desc(u32 func_id)
++{
++	u32 desc_idx;
++
++	/*
++	 * search for the descriptor of the selected FF-A interface
++	 */
++	for (desc_idx = 0; desc_idx < FFA_FEATURE_DESC_CNT ; desc_idx++)
++		if (ffa_priv_data.features[desc_idx].func_id == func_id)
++			return &ffa_priv_data.features[desc_idx];
++
++	return NULL;
++}
++
++/**
++ * ffa_get_rxtx_map_features - FFA_FEATURES handler function with FFA_RXTX_MAP
++ *							argument
++ *
++ * This is the boot time function that implements FFA_FEATURES FF-A function
++ * to retrieve the FFA_RXTX_MAP features
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_get_rxtx_map_features(void)
++{
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_FEATURES;
++	a1 = FFA_RXTX_MAP;
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		if (((int)res.a2) == FFA_ERR_STAT_NOT_SUPPORTED) {
++			ffa_err("FFA_RXTX_MAP is not implemented at this FF-A instance");
++			return -EOPNOTSUPP;
++		}
++
++		ffa_err("Undefined error code (%d)", ((int)res.a2));
++		return -EINVAL;
++	}
++	case FFA_SUCCESS:
++	{
++		u32 desc_idx;
++
++		/*
++		 * search for an empty descriptor
++		 */
++		for (desc_idx = 0; desc_idx < FFA_FEATURE_DESC_CNT ; desc_idx++)
++			if (!ffa_priv_data.features[desc_idx].func_id) {
++				/*
++				 * populate the descriptor with
++				 * the interface features data
++				 */
++				ffa_priv_data.features[desc_idx].func_id =
++					FFA_RXTX_MAP;
++				ffa_priv_data.features[desc_idx].field1 =
++					res.a2;
++
++				ffa_info("FFA_RXTX_MAP features data 0x%lx",
++					 res.a2);
++
++				return FFA_ERR_STAT_SUCCESS;
++			}
++
++		ffa_err("Cannot save FFA_RXTX_MAP features data. Descriptors table full");
++		return -ENOBUFS;
++	}
++	default:
++	{
++		ffa_err("Undefined response function (0x%lx)",
++			res.a0);
++		return -EINVAL;
++	}
++	}
++}
++
++/**
++ * ffa_get_rxtx_buffers_pages_cnt - reads from the features data descriptors
++ *						the minimum number of pages in each of the RX/TX
++ *						buffers
++ * @buf_4k_pages: Pointer to the minimum number of pages
++ *
++ * This is the boot time function that  returns the minimum number of pages
++ *  in each of the RX/TX buffers
++ *
++ * Return:
++ *
++ * buf_4k_pages points to the returned number of pages
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_get_rxtx_buffers_pages_cnt(size_t *buf_4k_pages)
++{
++	struct ffa_features_desc *desc = NULL;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!buf_4k_pages)
++		return -EINVAL;
++
++	desc = ffa_get_features_desc(FFA_RXTX_MAP);
++	if (!desc)
++		return -EINVAL;
++
++	ffa_dbg("FFA_RXTX_MAP descriptor found");
++
++	switch (desc->field1) {
++	case RXTX_4K:
++		*buf_4k_pages = 1;
++		break;
++	case RXTX_16K:
++		*buf_4k_pages = 4;
++		break;
++	case RXTX_64K:
++		*buf_4k_pages = 16;
++		break;
++	default:
++		ffa_err("RX/TX buffer size not supported");
++		return -EINVAL;
++	}
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_free_rxtx_buffers - frees the RX/TX buffers
++ * @buf_4k_pages: the minimum number of pages in each of the RX/TX
++ *			  buffers
++ *
++ * This is the boot time function used to free the RX/TX buffers
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_free_rxtx_buffers(size_t buf_4k_pages)
++{
++	efi_status_t free_rxbuf_ret, free_txbuf_ret;
++
++	ffa_info("Freeing RX/TX buffers");
++
++	free_rxbuf_ret = efi_free_pages(ffa_priv_data.pair.rxbuf, buf_4k_pages);
++	free_txbuf_ret = efi_free_pages(ffa_priv_data.pair.txbuf, buf_4k_pages);
++
++	if (free_rxbuf_ret != EFI_SUCCESS || free_txbuf_ret != EFI_SUCCESS) {
++		ffa_err("Failed to free RX/TX buffers (rx: %lu , tx: %lu)",
++			free_rxbuf_ret,
++			free_txbuf_ret);
++		return -EINVAL;
++	}
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_alloc_rxtx_buffers - allocates the RX/TX buffers
++ * @buf_4k_pages: the minimum number of pages in each of the RX/TX
++ *			  buffers
++ *
++ * This is the boot time function used by ffa_map_rxtx_buffers to allocate
++ * the RX/TX buffers before mapping them
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_alloc_rxtx_buffers(size_t buf_4k_pages)
++{
++	ffa_dbg("[%s]", __func__);
++
++#if CONFIG_IS_ENABLED(EFI_LOADER)
++
++	efi_status_t efi_ret;
++	void *virt_txbuf;
++	void *virt_rxbuf;
++
++	ffa_info("Using %lu 4KB page(s) for RX/TX buffers size",
++		 buf_4k_pages);
++
++	efi_ret = efi_allocate_pages(EFI_ALLOCATE_ANY_PAGES,
++				     EFI_BOOT_SERVICES_DATA,
++				     buf_4k_pages,
++				     &ffa_priv_data.pair.rxbuf);
++
++	if (efi_ret != EFI_SUCCESS) {
++		ffa_priv_data.pair.rxbuf = 0;
++		ffa_err("Failure to allocate RX buffer (EFI error: 0x%lx)",
++			efi_ret);
++
++		return -ENOBUFS;
++	}
++
++	ffa_info("RX buffer at phys 0x%llx",
++		 ffa_priv_data.pair.rxbuf);
++
++	/*
++	 * convert the RX buffer physical address to virtual address
++	 */
++	virt_rxbuf = (void *)map_sysmem((phys_addr_t)ffa_priv_data.pair.rxbuf, 0);
++
++	/*
++	 * make sure the buffer is clean before use
++	 */
++	memset(virt_rxbuf, 0, buf_4k_pages * SZ_4K);
++
++	unmap_sysmem(virt_rxbuf);
++
++	efi_ret = efi_allocate_pages(EFI_ALLOCATE_ANY_PAGES,
++				     EFI_RUNTIME_SERVICES_DATA,
++				     buf_4k_pages,
++				     &ffa_priv_data.pair.txbuf);
++
++	if (efi_ret != EFI_SUCCESS) {
++		ffa_dbg("FFA_RXTX_MAP: freeing RX buffer");
++		efi_free_pages(ffa_priv_data.pair.rxbuf, buf_4k_pages);
++		ffa_priv_data.pair.rxbuf = 0;
++		ffa_priv_data.pair.txbuf = 0;
++		ffa_err("Failure to allocate the TX buffer (EFI error: 0x%lx)"
++			, efi_ret);
++
++		return -ENOBUFS;
++	}
++
++	ffa_info("TX buffer at phys 0x%llx",
++		 ffa_priv_data.pair.txbuf);
++
++	/*
++	 * convert the TX buffer physical address to virtual address
++	 */
++	virt_txbuf = (void *)map_sysmem((phys_addr_t)ffa_priv_data.pair.txbuf, 0);
++
++	/*
++	 * make sure the buffer is clean before use
++	 */
++	memset(virt_txbuf, 0, buf_4k_pages * SZ_4K);
++
++	unmap_sysmem(virt_txbuf);
++
++	return FFA_ERR_STAT_SUCCESS;
++
++#else
++	return -ENOBUFS;
++#endif
++}
++
++/**
++ * ffa_map_rxtx_buffers - FFA_RXTX_MAP handler function
++ * @buf_4k_pages: the minimum number of pages in each of the RX/TX
++ *			  buffers
++ *
++ * This is the boot time function that implements FFA_RXTX_MAP FF-A function
++ * to map the RX/TX buffers
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_map_rxtx_buffers(size_t buf_4k_pages)
++{
++	int ret;
++
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	ret = ffa_alloc_rxtx_buffers(buf_4k_pages);
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	a0 = FFA_RXTX_MAP;
++	a1 = ffa_priv_data.pair.txbuf;
++	a2 = ffa_priv_data.pair.rxbuf;
++	a3 = buf_4k_pages;
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		switch (((int)res.a2)) {
++		case FFA_ERR_STAT_INVALID_PARAMETERS:
++			ffa_err("One or more fields in input parameters is incorrectly encoded");
++			ret = -EPERM;
++			break;
++		case FFA_ERR_STAT_NO_MEMORY:
++			ffa_err("Not enough memory");
++			ret = -ENOMEM;
++			break;
++		case FFA_ERR_STAT_DENIED:
++			ffa_err("Buffer pair already registered");
++			ret = -EACCES;
++			break;
++		case FFA_ERR_STAT_NOT_SUPPORTED:
++			ffa_err("This function is not implemented at this FF-A instance");
++			ret = -EOPNOTSUPP;
++			break;
++		default:
++			ffa_err("Undefined error (%d)",
++				((int)res.a2));
++			ret = -EINVAL;
++		}
++		break;
++	}
++	case FFA_SUCCESS:
++		ffa_info("RX/TX buffers mapped");
++		return FFA_ERR_STAT_SUCCESS;
++	default:
++		ffa_err("Undefined response function (0x%lx)",
++			res.a0);
++		ret = -EINVAL;
++	}
++
++	ffa_free_rxtx_buffers(buf_4k_pages);
++
++	return ret;
++}
++
++/**
++ * ffa_unmap_rxtx_buffers - FFA_RXTX_UNMAP handler function
++ *
++ * This is the boot time function that implements FFA_RXTX_UNMAP FF-A function
++ * to unmap the RX/TX buffers
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_unmap_rxtx_buffers(void)
++{
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_RXTX_UNMAP;
++	a1 = PREP_SELF_ENDPOINT_ID(ffa_priv_data.id);
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		if (((int)res.a2) == FFA_ERR_STAT_NOT_SUPPORTED)
++			panic("[FFA] FFA_RXTX_UNMAP is not implemented at this FF-A instance\n");
++		else if (((int)res.a2) == FFA_ERR_STAT_INVALID_PARAMETERS)
++			panic("[FFA] There is no buffer pair registered on behalf of the caller\n");
++		else
++			panic("[FFA] Undefined error (%d)\n", ((int)res.a2));
++	}
++	case FFA_SUCCESS:
++	{
++		size_t buf_4k_pages = 0;
++		int ret;
++
++		ret = ffa_get_rxtx_buffers_pages_cnt(&buf_4k_pages);
++		if (ret != FFA_ERR_STAT_SUCCESS)
++			panic("[FFA] RX/TX buffers unmapped but failure in getting pages count\n");
++
++		ret = ffa_free_rxtx_buffers(buf_4k_pages);
++		if (ret != FFA_ERR_STAT_SUCCESS)
++			panic("[FFA] RX/TX buffers unmapped but failure in freeing the memory\n");
++
++		ffa_info("RX/TX buffers unmapped and memory freed");
++
++		return FFA_ERR_STAT_SUCCESS;
++	}
++	default:
++		panic("[FFA] Undefined response function (0x%lx)", res.a0);
++	}
++}
++
++/**
++ * ffa_release_rx_buffer - FFA_RX_RELEASE handler function
++ *
++ * This is the boot time function that invokes FFA_RX_RELEASE FF-A function
++ * to release the ownership of the RX buffer
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_release_rx_buffer(void)
++{
++	FFA_DECLARE_ARGS;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_RX_RELEASE;
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		if (((int)res.a2) == FFA_ERR_STAT_NOT_SUPPORTED)
++			panic("[FFA] FFA_RX_RELEASE is not implemented at this FF-A instance\n");
++		else if (((int)res.a2) == FFA_ERR_STAT_DENIED)
++			panic("[FFA] Caller did not have ownership of the RX buffer\n");
++		else
++			panic("[FFA] Undefined error (%d)\n", ((int)res.a2));
++	}
++	case FFA_SUCCESS:
++		ffa_info("RX buffer released");
++		return FFA_ERR_STAT_SUCCESS;
++
++	default:
++		panic("[FFA] Undefined response function (0x%lx)\n", res.a0);
++	}
++}
++
++/**
++ * ffa_uuid_are_identical - checks whether two given UUIDs are identical
++ * @uuid1: first UUID
++ * @uuid2: second UUID
++ *
++ * This is a boot time function used by ffa_read_partitions_info to search
++ * for a UUID in the partitions descriptors table
++ *
++ * Return:
++ *
++ * 1 when UUIDs match. Otherwise, 0
++ */
++int ffa_uuid_are_identical(const union ffa_partition_uuid *uuid1,
++			   const union ffa_partition_uuid *uuid2)
++{
++	if (!uuid1 || !uuid2)
++		return 0;
++
++	return (!memcmp(uuid1, uuid2, sizeof(union ffa_partition_uuid)));
++}
++
++/**
++ * ffa_read_partitions_info - reads the data queried by FFA_PARTITION_INFO_GET
++ *							and saves it in the private structure
++ * @count: The number of partitions queried
++ * @part_uuid: Pointer to the partition(s) UUID
++ *
++ * This is the boot time function that reads the partitions information
++ * returned by the FFA_PARTITION_INFO_GET and saves it in the private
++ * data structure.
++ *
++ * Return:
++ *
++ * The private data structure is updated with the partition(s) information
++ * FFA_ERR_STAT_SUCCESS is returned on success. Otherwise, failure
++ */
++static int ffa_read_partitions_info(u32 count, union ffa_partition_uuid *part_uuid)
++{
++	ffa_dbg("[%s]", __func__);
++
++	if (!count) {
++		ffa_err("No partition detected");
++		return -ENODATA;
++	}
++
++	ffa_info("Reading partitions data from the RX buffer");
++
++#if CONFIG_IS_ENABLED(EFI_LOADER)
++
++	if (!part_uuid) {
++		/*
++		 * querying information of all partitions
++		 */
++		u64 data_pages;
++		u64 data_bytes;
++		efi_status_t efi_ret;
++		size_t buf_4k_pages = 0;
++		u32 desc_idx;
++		struct ffa_partition_info *parts_info;
++		int ret;
++
++		data_bytes = count * sizeof(struct ffa_partition_desc);
++		data_pages = efi_size_in_pages(data_bytes);
++
++		/*
++		 * get the RX buffer size in pages
++		 */
++		ret = ffa_get_rxtx_buffers_pages_cnt(&buf_4k_pages);
++		if (ret != FFA_ERR_STAT_SUCCESS) {
++			ffa_err("Can not get the RX buffer size (error %d)", ret);
++			return ret;
++		}
++
++		if (data_pages > buf_4k_pages) {
++			ffa_err("Partitions data size exceeds the RX buffer size:");
++			ffa_err("    Sizes in pages: data %llu , RX buffer %lu ",
++				data_pages,
++				buf_4k_pages);
++
++			return -ENOMEM;
++		}
++
++		efi_ret = efi_allocate_pages(EFI_ALLOCATE_ANY_PAGES,
++					     EFI_RUNTIME_SERVICES_DATA,
++					     data_pages,
++					     (u64 *)&ffa_priv_data.partitions.descs);
++
++		if (efi_ret != EFI_SUCCESS) {
++			ffa_priv_data.partitions.descs = NULL;
++
++			ffa_err("Cannot  allocate partitions data buffer (EFI error 0x%lx)",
++				efi_ret);
++
++			return -ENOBUFS;
++		}
++
++		/*
++		 * convert the descs buffer physical address
++		 * to virtual address
++		 * This virtual address should not be unmapped
++		 * descs is expected to be a virtual address
++		 */
++		ffa_priv_data.partitions.descs =
++			(struct ffa_partition_desc *)
++			map_sysmem((phys_addr_t)
++				   ffa_priv_data.partitions.descs, 0);
++
++		/*
++		 * make sure the buffer is clean before use
++		 */
++		memset(ffa_priv_data.partitions.descs, 0,
++		       data_pages * SZ_4K);
++
++		ffa_info("Copying %lld page(s) of partitions data from RX buffer",
++			 data_pages);
++
++		/*
++		 * convert the RX buffer physical address to
++		 * virtual address
++		 */
++		parts_info = (struct ffa_partition_info *)
++			map_sysmem((phys_addr_t)
++				   ffa_priv_data.pair.rxbuf, 0);
++
++		for (desc_idx = 0 ; desc_idx < count ; desc_idx++) {
++			ffa_priv_data.partitions.descs[desc_idx].info =
++				parts_info[desc_idx];
++
++			ffa_info("Partition ID %x : info cached",
++				 ffa_priv_data.partitions.descs[desc_idx].info.id);
++		}
++		unmap_sysmem(parts_info);
++
++		ffa_priv_data.partitions.count = count;
++
++		ffa_info("%d partition(s) found and cached", count);
++
++	} else {
++		u32 rx_desc_idx, cached_desc_idx;
++		struct ffa_partition_info *parts_info;
++		u8 desc_found;
++
++		/*
++		 * convert the RX buffer physical address to virtual address
++		 */
++		parts_info = (struct ffa_partition_info *)
++			map_sysmem((phys_addr_t)ffa_priv_data.pair.rxbuf, 0);
++
++		/*
++		 * search for the SP IDs read from the RX buffer
++		 * in the already cached SPs.
++		 * Update the UUID when ID found.
++		 */
++		for (rx_desc_idx = 0; rx_desc_idx < count ; rx_desc_idx++) {
++			desc_found = 0;
++
++			/*
++			 * search the current ID in the cached partitions
++			 */
++			for (cached_desc_idx = 0;
++			     cached_desc_idx < ffa_priv_data.partitions.count;
++			     cached_desc_idx++) {
++				/*
++				 * save the UUID
++				 */
++				if (ffa_priv_data.partitions.descs[cached_desc_idx].info.id ==
++				    parts_info[rx_desc_idx].id) {
++					ffa_priv_data.partitions.descs[cached_desc_idx].UUID =
++						*part_uuid;
++
++					desc_found = 1;
++					break;
++				}
++			}
++
++			if (!desc_found) {
++				unmap_sysmem(parts_info);
++				return -ENODATA;
++			}
++		}
++		unmap_sysmem(parts_info);
++	}
++#else
++#warning "arm_ffa: reading FFA_PARTITION_INFO_GET data not implemented"
++#endif
++
++	return  FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_query_partitions_info - invokes FFA_PARTITION_INFO_GET
++ *							and saves partitions data
++ * @part_uuid: Pointer to the partition(s) UUID
++ * @pcount: Pointer to the number of partitions variable filled when querying
++ *
++ * This is the boot time function that executes the FFA_PARTITION_INFO_GET
++ * to query the partitions data. Then, it calls ffa_read_partitions_info
++ * to save the data in the private data structure.
++ *
++ * After reading the data the RX buffer is released using ffa_release_rx_buffer
++ *
++ * Return:
++ *
++ * When part_uuid is NULL, all partitions data are retrieved from secure world
++ * When part_uuid is non NULL, data for partitions matching the given UUID are
++ * retrieved and the number of partitions is returned
++ * FFA_ERR_STAT_SUCCESS is returned on success. Otherwise, failure
++ */
++static int ffa_query_partitions_info(union ffa_partition_uuid *part_uuid,
++				     u32 *pcount)
++{
++	unsigned long a0 = 0;
++	union ffa_partition_uuid query_uuid = {0};
++	unsigned long a5 = 0;
++	unsigned long a6 = 0;
++	unsigned long a7 = 0;
++	struct arm_smccc_res res = {0};
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	a0 = FFA_PARTITION_INFO_GET;
++
++	/*
++	 * If a UUID is specified. Information for one or more
++	 * partitions in the system is queried. Otherwise, information
++	 * for all installed partitions is queried
++	 */
++
++	if (part_uuid) {
++		if (!pcount)
++			return -EINVAL;
++
++		query_uuid = *part_uuid;
++	}
++
++	ffa_priv_data.invoke_ffa_fn(a0, query_uuid.words.a1, query_uuid.words.a2,
++				    query_uuid.words.a3, query_uuid.words.a4,
++				    a5, a6, a7, &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		switch (((int)res.a2)) {
++		case FFA_ERR_STAT_INVALID_PARAMETERS:
++			ffa_err("Unrecognized UUID");
++			return -EPERM;
++		case FFA_ERR_STAT_NO_MEMORY:
++			ffa_err("Results cannot fit in RX buffer of the caller");
++			return -ENOMEM;
++		case FFA_ERR_STAT_DENIED:
++			ffa_err("Callee is not in a state to handle this request");
++			return -EACCES;
++		case FFA_ERR_STAT_NOT_SUPPORTED:
++			ffa_err("This function is not implemented at this FF-A instance");
++			return -EOPNOTSUPP;
++		case FFA_ERR_STAT_BUSY:
++			ffa_err("RX buffer of the caller is not free");
++			return -EBUSY;
++		default:
++			ffa_err("Undefined error (%d)", ((int)res.a2));
++			return -EINVAL;
++		}
++	}
++	case FFA_SUCCESS:
++	{
++		int ret;
++
++		/*
++		 * res.a2 contains the count of partition information descriptors
++		 * populated in the RX buffer
++		 */
++		if (res.a2) {
++			ret = ffa_read_partitions_info(res.a2, part_uuid);
++			if (ret)
++				ffa_err("Failed to read partition(s) data , error (%d)", ret);
++		}
++
++		/*
++		 * return the SP count
++		 */
++		if (part_uuid) {
++			if (!ret)
++				*pcount = res.a2;
++			else
++				*pcount = 0;
++		}
++		/*
++		 * After calling FFA_PARTITION_INFO_GET the buffer ownership
++		 * is assigned to the consumer (u-boot). So, we need to give
++		 * the ownership back to the secure world
++		 */
++		ret = ffa_release_rx_buffer();
++
++		if (!part_uuid && !res.a2) {
++			ffa_err("[FFA] no partition installed in the system");
++			return -ENODEV;
++		}
++
++		return ret;
++	}
++	default:
++		ffa_err("Undefined response function (0x%lx)", res.a0);
++		return  -EINVAL;
++	}
++}
++
++/**
++ * ffa_get_partitions_info - FFA_PARTITION_INFO_GET handler function
++ * @func_data: Pointer to the FF-A function arguments container structure.
++ * The passed arguments:
++ * Mode 1: When getting from the driver the number of
++ *	secure partitions:
++ *	@data0_size: UUID size
++ *	@data0: pointer to the UUID (little endian)
++ *	@data1_size: size of the number of partitions
++ *				variable
++ *	@data1: pointer to the number of partitions
++ *			 variable. The variable will be set
++ *			 by the driver
++ * Mode 2: When requesting the driver to return the
++ *	partitions information:
++ *	@data0_size: UUID size
++ *	@data0: pointer to the UUID (little endian)
++ *	@data1_size: size of the SPs information buffer
++ *	@data1: pointer to SPs information buffer
++ *		(allocated by the client).
++ *		The buffer will be filled by the driver
++ *
++ * This is the boot time function that queries the secure partition data from
++ * the private data structure. If not found, it invokes FFA_PARTITION_INFO_GET
++ * FF-A function to query the partition information from secure world.
++ *
++ * A client of the FF-A driver should know the UUID of the service it wants to
++ * access. It should use the UUID to request the FF-A driver to provide the
++ * partition(s) information of the service. The FF-A driver uses
++ * PARTITION_INFO_GET to obtain this information. This is implemented through
++ * ffa_get_partitions_info function.
++ * A new FFA_PARTITION_INFO_GET call is issued (first one performed through
++ * ffa_cache_partitions_info) allowing to retrieve the partition(s) information.
++ * They are not saved (already done). We only update the UUID in the cached area.
++ * This assumes that partitions data does not change in the secure world.
++ * Otherwise u-boot will have an outdated partition data. The benefit of caching
++ * the information in the FF-A driver is to accommodate discovery after
++ * ExitBootServices().
++ *
++ * When invoked through a client request, ffa_get_partitions_info should be
++ * called twice. First call is to get from the driver the number of secure
++ * partitions (SPs) associated to a particular UUID.
++ * Then, the caller (client) allocates the buffer to host the SPs data and
++ * issues a 2nd call. Then, the driver fills the SPs data in the pre-allocated
++ * buffer.
++ *
++ * To achieve the mechanism described above, ffa_get_partitions_info uses the
++ * following functions:
++ *		ffa_read_partitions_info
++ *		ffa_query_partitions_info
++ *
++ * Return:
++ *
++ * @data1: When pointing to the number of partitions variable, the number is
++ * set by the driver.
++ * When pointing to the partitions information buffer, the buffer will be
++ * filled by the driver.
++ *
++ * On success FFA_ERR_STAT_SUCCESS is returned. Otherwise, failure
++ */
++static int ffa_get_partitions_info(struct ffa_interface_data *func_data)
++{
++	/*
++	 * fill_data:
++	 * 0: return the SP count
++	 * 1: fill SP data and return it to the caller
++	 * -1: undefined mode
++	 */
++	int fill_data = -1;
++	u32 desc_idx, client_desc_idx;
++	union ffa_partition_uuid *part_uuid;
++	u32 client_desc_max_cnt;
++	u32 parts_found = 0;
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!func_data) {
++		ffa_err("No function data provided");
++		return -EINVAL;
++	}
++
++	if (!ffa_priv_data.partitions.count || !ffa_priv_data.partitions.descs)
++		panic("[FFA] No partition installed\n");
++
++	if (func_data->data0_size == sizeof(union ffa_partition_uuid) &&
++	    func_data->data0 &&
++	    func_data->data1_size == sizeof(u32) &&
++	    func_data->data1) {
++		/*
++		 * data0 (in): pointer to UUID
++		 * data1 (in): pointer to SP count
++		 * Out: SP count returned in the count variable pointed by data1
++		 */
++
++		fill_data = 0;
++
++		ffa_info("Preparing for checking partitions count");
++
++	} else if ((func_data->data0_size == sizeof(union ffa_partition_uuid)) &&
++		   func_data->data0 &&
++		   (func_data->data1_size >= sizeof(struct ffa_partition_info)) &&
++		   !(func_data->data1_size % sizeof(struct ffa_partition_info)) &&
++		   func_data->data1) {
++		/*
++		 * data0 (in): pointer to UUID
++		 * data1 (in): pointer to SPs descriptors buffer
++		 *             (created by the client)
++		 * Out: SPs descriptors returned in the buffer
++		 *      pointed by data1
++		 */
++
++		fill_data = 1;
++
++		client_desc_idx = 0;
++
++		/*
++		 * number of empty descriptors preallocated by the caller
++		 */
++		client_desc_max_cnt =
++			func_data->data1_size / sizeof(struct ffa_partition_info);
++
++		ffa_info("Preparing for filling partitions info");
++
++	} else {
++		ffa_err("Invalid function arguments provided");
++		return -EINVAL;
++	}
++
++	part_uuid = (union ffa_partition_uuid *)func_data->data0;
++
++	ffa_info("Searching partitions using the provided UUID");
++
++#ifdef DEBUG
++	{
++		u32 dbg_uuid_cnt;
++
++		ffa_dbg("UUID: [LSB]");
++
++		for (dbg_uuid_cnt = 0 ; dbg_uuid_cnt < UUID_SIZE ; dbg_uuid_cnt++)
++			ffa_dbg(" %02x", part_uuid->bytes[dbg_uuid_cnt]);
++	}
++#endif
++
++	/*
++	 * search in the cached partitions
++	 */
++	for (desc_idx = 0;
++	     desc_idx < ffa_priv_data.partitions.count;
++	     desc_idx++) {
++		if (ffa_uuid_are_identical(&ffa_priv_data.partitions.descs[desc_idx].UUID,
++					   part_uuid)) {
++			ffa_info("Partition ID %x matches the provided UUID",
++				 ffa_priv_data.partitions.descs[desc_idx].info.id);
++
++			parts_found++;
++
++			if (fill_data) {
++				/*
++				 * trying to fill the partition info in data1
++				 */
++
++				if (client_desc_idx < client_desc_max_cnt) {
++					((struct ffa_partition_info *)
++					 func_data->data1)[client_desc_idx++] =
++						ffa_priv_data.partitions.descs[desc_idx].info;
++					continue;
++				}
++
++				ffa_err("Failed to fill the current descriptor client buffer full");
++				return -ENOBUFS;
++			}
++		}
++	}
++
++	if (!parts_found) {
++		int ret;
++
++		ffa_info("No partition found. Querying framework ...");
++
++		ret = ffa_query_partitions_info(part_uuid, &parts_found);
++
++		if (ret == FFA_ERR_STAT_SUCCESS) {
++			if (!fill_data) {
++				*((u32 *)func_data->data1) = parts_found;
++
++				ffa_info("Number of partition(s) found matching the UUID: %d",
++					 parts_found);
++			} else {
++				/*
++				 * we want to read SPs info
++				 */
++
++				/*
++				 * If SPs data filled, retry searching SP info again
++				 */
++				if (parts_found)
++					ret = ffa_get_partitions_info(func_data);
++				else
++					ret = -ENODATA;
++			}
++		}
++
++		return ret;
++	}
++
++	/* partition(s) found */
++	if (!fill_data)
++		*((u32 *)func_data->data1) = parts_found;
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_cache_partitions_info - Queries and saves all secure partitions data
++ *
++ * This is a boot time function that invokes FFA_PARTITION_INFO_GET FF-A
++ * function to query from secure world all partitions information.
++ *
++ * The FFA_PARTITION_INFO_GET call is issued with nil UUID as an argument.
++ * All installed partitions information are returned. We cache them in the
++ * resident private data structure and we keep the UUID field empty
++ * (in FF-A 1.0 UUID is not provided by the partition descriptor)
++ *
++ * This function is called at the device probing level.
++ * ffa_cache_partitions_info uses ffa_query_partitions_info to get the data
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_cache_partitions_info(void)
++{
++	ffa_dbg("[%s]", __func__);
++	return ffa_query_partitions_info(NULL, NULL);
++}
++
++/**
++ * ffa_msg_send_direct_req - FFA_MSG_SEND_DIRECT_{REQ,RESP} handler function
++ * @func_data: Pointer to the FF-A function arguments container structure.
++ *					The passed arguments:
++ *						@data0_size: partition ID size
++ *						@data0: pointer to the partition ID
++ *						@data1_size: exchanged data size
++ *						@data1: pointer to the data buffer preallocated by
++ *								the client (in/out)
++ *
++ * This is the runtime function that implements FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ * FF-A functions.
++ *
++ * FFA_MSG_SEND_DIRECT_REQ is used to send the data to the secure partition.
++ * The response from the secure partition is handled by reading the
++ * FFA_MSG_SEND_DIRECT_RESP arguments.
++ *
++ * The maximum size of the data that can be exchanged is 20 bytes which is
++ * sizeof(struct ffa_send_direct_data) as defined by the FF-A specification 1.0
++ * in the section relevant to FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int __ffa_runtime ffa_msg_send_direct_req(struct ffa_interface_data
++						 *func_data)
++{
++	u16 dst_part_id;
++	unsigned long a0 = 0;
++	unsigned long a1 = 0;
++	unsigned long a2 = 0;
++	struct ffa_send_direct_data *msg;
++	struct arm_smccc_res res = {0};
++
++	ffa_dbg("[%s]", __func__);
++
++	if (!ffa_priv_data.invoke_ffa_fn)
++		panic("[FFA] no private data found\n");
++
++	if (!func_data)
++		return -EINVAL;
++
++	if (!ffa_priv_data.partitions.count || !ffa_priv_data.partitions.descs)
++		panic("[FFA] no partition installed\n");
++
++	if (func_data->data0_size != sizeof(u16) ||
++	    !func_data->data0 ||
++	    func_data->data1_size != FFA_MSG_SEND_DIRECT_MAX_SIZE ||
++	    !func_data->data1) {
++		ffa_err("Undefined interface parameters");
++		return -EINVAL;
++	}
++
++	dst_part_id = *((u16 *)func_data->data0);
++	msg = func_data->data1;
++
++	ffa_dbg("Sending data to partition ID 0x%x", dst_part_id);
++
++	a0 = FFA_MSG_SEND_DIRECT_REQ;
++
++	a1 = PREP_SELF_ENDPOINT_ID(ffa_priv_data.id) |
++		PREP_PART_ENDPOINT_ID(dst_part_id);
++
++	ffa_priv_data.invoke_ffa_fn(a0, a1, a2,
++				    msg->a3,
++				    msg->a4,
++				    msg->a5,
++				    msg->a6,
++				    msg->a7,
++				    &res);
++
++	while (res.a0 == FFA_INTERRUPT)
++		ffa_priv_data.invoke_ffa_fn(FFA_RUN, res.a1,
++					    0, 0, 0, 0, 0, 0,
++					    &res);
++
++	switch (res.a0) {
++	case FFA_ERROR:
++	{
++		switch (((int)res.a2)) {
++		case FFA_ERR_STAT_INVALID_PARAMETERS:
++			ffa_err("Invalid endpoint ID or non-zero reserved register");
++			return -EPERM;
++		case FFA_ERR_STAT_ABORTED:
++			panic("[FFA] Message target ran into unexpected error and has aborted\n");
++		case FFA_ERR_STAT_DENIED:
++			panic("[FFA] Callee is not in a state to handle this request\n");
++		case FFA_ERR_STAT_NOT_SUPPORTED:
++			panic("[FFA] This function is not implemented at this FF-A instance\n");
++		case FFA_ERR_STAT_BUSY:
++			panic("[FFA] Message target is busy\n");
++		default:
++			panic("[FFA] Undefined error (%d)\n", ((int)res.a2));
++		}
++	}
++	case FFA_SUCCESS:
++
++		ffa_dbg("Message sent with no response");
++		return FFA_ERR_STAT_SUCCESS;
++
++	case FFA_MSG_SEND_DIRECT_RESP:
++
++		ffa_dbg("Message sent with response");
++
++		/*
++		 * extract the 32-bit wide return data
++		 */
++		msg->a3 = (u32)res.a3;
++		msg->a4 = (u32)res.a4;
++		msg->a5 = (u32)res.a5;
++		msg->a6 = (u32)res.a6;
++		msg->a7 = (u32)res.a7;
++
++		return FFA_ERR_STAT_SUCCESS;
++
++	default:
++		panic("[FFA] Undefined response function (0x%lx)\n", res.a0);
++	}
++}
++
++/**
++ * invoke_ffa_drv_api - The driver dispatcher function
++ * @func_id:	The FF-A function to be used
++ * @func_data:  Pointer to the FF-A function arguments container
++ *					structure. This also includes pointers to the
++ *					returned data needed by clients.
++ * The dispatcher is a runtime function that selects the FF-A function handler
++ * based on the input FF-A function ID.
++ * The input arguments are passed to the handler function.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int __ffa_runtime invoke_ffa_drv_api(u32 func_id,
++				     struct ffa_interface_data *func_data)
++{
++	if (!ffa_priv_data.dev)
++		panic("[FFA] no device found\n");
++
++	switch (func_id) {
++	case FFA_PARTITION_INFO_GET:
++		return ffa_get_partitions_info(func_data);
++	case FFA_RXTX_UNMAP:
++		return ffa_unmap_rxtx_buffers();
++	case FFA_MSG_SEND_DIRECT_REQ:
++		return ffa_msg_send_direct_req(func_data);
++	default:
++
++		ffa_err("Undefined FF-A interface (%d)", func_id);
++
++		return -EINVAL;
++	}
++}
++
++/**
++ * ffa_init_private_data - Initialization of the private data
++ * @dev:	the arm_ffa device
++ *
++ * This boot time function reads data from the platform data structure
++ * and populates the private data structure
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_init_private_data(struct udevice *dev)
++{
++	struct ffa_pdata *pdata = dev_get_plat(dev);
++
++	ffa_priv_data.conduit = pdata->conduit;
++
++	if (ffa_priv_data.conduit == FFA_CONDUIT_SMC) {
++		ffa_priv_data.invoke_ffa_fn = arm_ffa_smccc_smc;
++	} else {
++		ffa_err("Undefined FF-A conduit (%d)", ffa_priv_data.conduit);
++		return -EINVAL;
++	}
++
++	ffa_info("Conduit is %s",
++		 ((ffa_priv_data.conduit == FFA_CONDUIT_SMC) ?
++		   "SMC" : "NOT SUPPORTED"));
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_probe - The driver probe function
++ * @dev:	the arm_ffa device
++ *
++ * Probing is done at boot time and triggered by the uclass device discovery.
++ * At probe level the following actions are done:
++ *	- initialization of the driver private data structure
++ *	- querying from secure world the FF-A framework version
++ *	- querying from secure world the u-boot endpoint ID
++ *	- querying from secure world the supported features of the specified FF-A calls
++ *	- mapping the RX/TX buffers
++ *	- querying from secure world all the partitions information
++ *
++ * All data queried from secure world is saved in the resident private data structure.
++ *
++ * The probe will fail if either FF-A framework is not detected or the
++ * FF-A requests are not behaving correctly. This ensures that the
++ * driver is not installed and its operations are not exported to the clients.
++ * However, once the driver is successfully probed and an FF-A anomaly is
++ * detected when clients invoke the driver operations, the driver cause
++ * u-boot to panic because the client would not know what to do in such conditions.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_probe(struct udevice *dev)
++{
++	int ret;
++	size_t buf_4k_pages = 0;
++
++	ffa_dbg("[%s]: initializing the FF-A driver", __func__);
++
++	ret = ffa_init_private_data(dev);
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_get_version();
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_get_endpoint_id();
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_get_rxtx_map_features();
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_get_rxtx_buffers_pages_cnt(&buf_4k_pages);
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_map_rxtx_buffers(buf_4k_pages);
++
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return ret;
++
++	ret = ffa_cache_partitions_info();
++
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		ffa_free_rxtx_buffers(buf_4k_pages);
++		return ret;
++	}
++
++	ffa_dbg("[%s]: initialization done", __func__);
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_of_to_plat - Reads the device tree node
++ * @dev:	the arm_ffa device
++ *
++ * This boot time function reads data from the device tree node and populates
++ * the platform data structure
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++static int ffa_of_to_plat(struct udevice *dev)
++{
++	struct ffa_pdata *pdata = dev_get_plat(dev);
++	const char *conduit;
++
++	ffa_dbg("[%s]", __func__);
++
++	conduit = fdt_getprop(gd->fdt_blob, dev_of_offset(dev), "method", NULL);
++
++	if (strcmp("smc", conduit)) {
++		ffa_err("Unsupported conduit");
++		return -EINVAL;
++	}
++
++	pdata->conduit = FFA_CONDUIT_SMC;
++
++	return FFA_ERR_STAT_SUCCESS;
++}
++
++/**
++ * ffa_drv_ops - The driver operations runtime structure
++ * @invoke_func:	The driver dispatcher
++ */
++struct ffa_ops __ffa_runtime_data ffa_drv_ops = {
++	.invoke_func = invoke_ffa_drv_api
++};
++
++/**
++ * ffa_device_get_ops - driver operations getter
++ *
++ * Return:
++ * This runtime function returns a pointer to the driver operations structure
++ */
++const struct ffa_ops * __ffa_runtime ffa_device_get_ops(void)
++{
++	return &ffa_drv_ops;
++}
++
++/**
++ * Defining the device tree compatible string
++ */
++
++static const struct udevice_id ffa_match_id[] = {
++	{"arm,ffa", 0},
++	{},
++};
++
++/**
++ * Declaring the arm_ffa driver under UCLASS_FFA
++ */
++
++U_BOOT_DRIVER(arm_ffa) = {
++	.name		= "arm_ffa",
++	.of_match	= ffa_match_id,
++	.id		= UCLASS_FFA,
++	.of_to_plat	= ffa_of_to_plat,
++	.probe		= ffa_probe,
++	.plat_auto	= sizeof(struct ffa_pdata),
++};
+diff --git a/include/arm_ffa.h b/include/arm_ffa.h
+new file mode 100644
+index 000000000000..313f46f74764
+--- /dev/null
++++ b/include/arm_ffa.h
+@@ -0,0 +1,191 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#ifndef __ARM_FFA_H
++#define __ARM_FFA_H
++
++#include <linux/arm-smccc.h>
++#include <linux/printk.h>
++
++/*
++ * This header is public. It can be used by clients to access
++ * data structures and definitions they need
++ */
++
++/*
++ * Macros for displaying logs
++ */
++
++#define ffa_dbg(fmt, ...)  pr_debug("[FFA] " fmt "\n", ##__VA_ARGS__)
++#define ffa_info(fmt, ...)  pr_info("[FFA] " fmt "\n", ##__VA_ARGS__)
++#define ffa_err(fmt, ...)  pr_err("[FFA] " fmt "\n", ##__VA_ARGS__)
++
++/*
++ * The driver operations success error code
++ */
++#define FFA_ERR_STAT_SUCCESS				(0)
++
++#if CONFIG_IS_ENABLED(EFI_LOADER)
++
++#include <efi_loader.h>
++
++/*
++ * __ffa_runtime_data and __ffa_runtime - controls whether data/code are
++ * available after calling the EFI ExitBootServices service.
++ * Data/code tagged with these keywords are resident (available at boot time and
++ * at runtime)
++ */
++
++#define __ffa_runtime_data __efi_runtime_data
++#define __ffa_runtime __efi_runtime
++
++#else
++
++#define __ffa_runtime_data
++#define __ffa_runtime
++
++#endif
++
++/*
++ * Definitions of the Arm FF-A interfaces supported by the Arm FF-A driver
++ */
++
++#define FFA_SMC(calling_convention, func_num)				\
++	ARM_SMCCC_CALL_VAL(ARM_SMCCC_FAST_CALL, (calling_convention),	\
++			   ARM_SMCCC_OWNER_STANDARD, (func_num))
++
++#define FFA_SMC_32(func_num)	FFA_SMC(ARM_SMCCC_SMC_32, (func_num))
++
++#define FFA_VERSION				FFA_SMC_32(0x63)
++#define FFA_ID_GET				FFA_SMC_32(0x69)
++#define FFA_FEATURES				FFA_SMC_32(0x64)
++#define FFA_PARTITION_INFO_GET				FFA_SMC_32(0x68)
++#define FFA_RXTX_MAP				FFA_SMC_32(0x66)
++#define FFA_RXTX_UNMAP				FFA_SMC_32(0x67)
++#define FFA_RX_RELEASE				FFA_SMC_32(0x65)
++#define FFA_MSG_SEND_DIRECT_REQ				FFA_SMC_32(0x6F)
++#define FFA_MSG_SEND_DIRECT_RESP				FFA_SMC_32(0x70)
++#define FFA_RUN				FFA_SMC_32(0x6D)
++#define FFA_ERROR				FFA_SMC_32(0x60)
++#define FFA_SUCCESS				FFA_SMC_32(0x61)
++#define FFA_INTERRUPT				FFA_SMC_32(0x62)
++
++/*
++ * struct ffa_partition_info - Partition information descriptor
++ * @id:	Partition ID
++ * @exec_ctxt:	Execution context count
++ * @properties:	Partition properties
++ *
++ * Data structure containing information about partitions instantiated in the system
++ * This structure is filled with the data queried by FFA_PARTITION_INFO_GET
++ */
++struct  __packed ffa_partition_info {
++	u16 id;
++	u16 exec_ctxt;
++/* partition supports receipt of direct requests */
++#define FFA_PARTITION_DIRECT_RECV	BIT(0)
++/* partition can send direct requests. */
++#define FFA_PARTITION_DIRECT_SEND	BIT(1)
++/* partition can send and receive indirect messages. */
++#define FFA_PARTITION_INDIRECT_MSG	BIT(2)
++	u32 properties;
++};
++
++/*
++ * struct ffa_send_direct_data - Data structure hosting the data
++ *                                       used by FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ * @a3-a7:	Data read/written from/to w3-w7 registers
++ *
++ * Data structure containing the data to be sent by FFA_MSG_SEND_DIRECT_REQ
++ * or read from FFA_MSG_SEND_DIRECT_RESP
++ */
++struct __packed ffa_send_direct_data {
++	u32 a3; /* w3 */
++	u32 a4; /* w4 */
++	u32 a5; /* w5 */
++	u32 a6; /* w6 */
++	u32 a7; /* w7 */
++};
++
++#define FFA_MSG_SEND_DIRECT_MAX_SIZE (sizeof(struct ffa_send_direct_data))
++
++/* UUID data size */
++#define UUID_SIZE (16)
++
++/*
++ * union ffa_partition_uuid - Data union hosting the UUID
++ *							transmitted by FFA_PARTITION_INFO_GET
++ * @words:	data structure giving 32-bit words access to the UUID data
++ * @bytes:	data structure giving byte access to the UUID data
++ *
++ * The structure holds little-endian UUID data.
++ */
++union ffa_partition_uuid {
++	struct __packed words {
++	u32 a1; /* w1 */
++	u32 a2; /* w2 */
++	u32 a3; /* w3 */
++	u32 a4; /* w4 */
++	} words;
++	u8 bytes[UUID_SIZE];
++};
++
++/**
++ * struct ffa_interface_data - generic FF-A interface data structure used to exchange
++ *							data between user layers and the driver
++ * @data0_size:	size of the first argument
++ * @data0:	pointer to the first argument
++ * @data1_size>:	size of the second argument
++ * @data1:	pointer to the second argument
++ *
++ * Using this structure user layers can pass various types of data with different sizes.
++ * The driver internal functions can detect the nature of this data, verfy compliance
++ * then execute the request when appropriate.
++ */
++struct ffa_interface_data {
++	u32 data0_size; /* size of the first argument */
++	void *data0; /* pointer to the first argument */
++	u32 data1_size; /* size of the second argument */
++	void *data1; /* pointer to the second argument */
++};
++
++/**
++ * struct ffa_ops - The driver operations structure
++ * @invoke_func:	function pointer to the invoke function
++ *
++ * The data structure providing all the operations supported by the driver.
++ * This structure is resident.
++ */
++struct ffa_ops {
++	/* the driver dispatcher */
++	int (*invoke_func)(u32 func_id, struct ffa_interface_data *func_data);
++};
++
++/**
++ * The device driver and the Uclass driver public functions
++ */
++
++/**
++ * ffa_get_invoke_func - performs a call to the FF-A driver dispatcher
++ */
++int __ffa_runtime ffa_get_invoke_func(u32 func_id,
++				      struct ffa_interface_data *func_data);
++
++/**
++ * ffa_device_get_ops - driver operations getter
++ */
++const struct ffa_ops * __ffa_runtime ffa_device_get_ops(void);
++
++/**
++ * ffa_get_device - probes the arm_ffa device
++ */
++int ffa_get_device(void);
++
++/**
++ * ffa_init_device - probes the arm_ffa device
++ */
++int ffa_init_device(void);
++#endif
+diff --git a/include/arm_ffa_helper.h b/include/arm_ffa_helper.h
+new file mode 100644
+index 000000000000..0e143e54511e
+--- /dev/null
++++ b/include/arm_ffa_helper.h
+@@ -0,0 +1,45 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#ifndef __ARM_FFA_HELPER_H
++#define __ARM_FFA_HELPER_H
++
++#include <arm_ffa.h>
++
++/*
++ * This header is public. Including this header provides all data structures
++ * and definitions needed by clients to use the FF-A transport driver
++ *
++ * It also provides helper functions allowing to pass data and invoke  FF-A functions
++ */
++
++/**
++ * ffa_helper_get_partitions_info - Wrapper function for FFA_PARTITION_INFO_GET
++ */
++int ffa_helper_get_partitions_info(struct ffa_interface_data *func_data);
++
++/**
++ * ffa_helper_unmap_rxtx_buffers - Wrapper function for FFA_RXTX_UNMAP
++ */
++int ffa_helper_unmap_rxtx_buffers(void);
++
++/**
++ * ffa_helper_msg_send_direct_req - Wrapper function for
++ *								FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ */
++int __ffa_runtime ffa_helper_msg_send_direct_req(struct ffa_interface_data
++						 *func_data);
++
++/**
++ * ffa_helper_init_device - Wrapper function for probing the arm_ffa device
++ */
++int ffa_helper_init_device(void);
++
++/**
++ * ffa_uuid_str_to_bin - Converts a big endian UUID string to a little endian buffer
++ */
++int ffa_uuid_str_to_bin(const char *uuid_str, unsigned char *uuid_bin);
++#endif
+diff --git a/include/dm/uclass-id.h b/include/dm/uclass-id.h
+index 0e26e1d13824..a1181b8f48e7 100644
+--- a/include/dm/uclass-id.h
++++ b/include/dm/uclass-id.h
+@@ -52,6 +52,7 @@ enum uclass_id {
+ 	UCLASS_EFI_MEDIA,	/* Devices provided by UEFI firmware */
+ 	UCLASS_ETH,		/* Ethernet device */
+ 	UCLASS_ETH_PHY,		/* Ethernet PHY device */
++	UCLASS_FFA,		/* Arm Firmware Framework for Armv8-A */
+ 	UCLASS_FIRMWARE,	/* Firmware */
+ 	UCLASS_FS_FIRMWARE_LOADER,		/* Generic loader */
+ 	UCLASS_GPIO,		/* Bank of general-purpose I/O pins */
+diff --git a/include/linux/arm-smccc.h b/include/linux/arm-smccc.h
+index 7f2be2339475..54980a130fdb 100644
+--- a/include/linux/arm-smccc.h
++++ b/include/linux/arm-smccc.h
+@@ -1,6 +1,8 @@
+ /* SPDX-License-Identifier: GPL-2.0 */
+ /*
+  * Copyright (c) 2015, Linaro Limited
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+  */
+ #ifndef __LINUX_ARM_SMCCC_H
+ #define __LINUX_ARM_SMCCC_H
+@@ -57,13 +59,17 @@
+ #include <linux/types.h>
+ /**
+  * struct arm_smccc_res - Result from SMC/HVC call
+- * @a0-a3 result values from registers 0 to 3
++ * @a0-a7 result values from registers 0 to 7
+  */
+ struct arm_smccc_res {
+ 	unsigned long a0;
+ 	unsigned long a1;
+ 	unsigned long a2;
+ 	unsigned long a3;
++	unsigned long a4;
++	unsigned long a5;
++	unsigned long a6;
++	unsigned long a7;
+ };
+ 
+ /**
+@@ -113,6 +119,26 @@ asmlinkage void __arm_smccc_hvc(unsigned long a0, unsigned long a1,
+ 			unsigned long a5, unsigned long a6, unsigned long a7,
+ 			struct arm_smccc_res *res, struct arm_smccc_quirk *quirk);
+ 
++#if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++/**
++ * __arm_ffa_smccc_smc() - make SMC calls used for FF-A transport
++ * @a0-a7: arguments passed in 64-bit registers x0 to x7
++ * @res: result values from 64-bit registers x0 to x7
++ *
++ * This function is used to make SMC calls following SMC32 Calling Convention.
++ * The content of the supplied parameters is copied to registers x0 to x7 prior
++ * to the SMC instruction. The SMC call return data is 32-bit data read from
++ * registers x0 tp x7.
++ */
++asmlinkage void __arm_ffa_smccc_smc(unsigned long a0, unsigned long a1,
++				    unsigned long a2, unsigned long a3, unsigned long a4,
++				    unsigned long a5, unsigned long a6, unsigned long a7,
++				    struct arm_smccc_res *res);
++
++#define arm_ffa_smccc_smc __arm_ffa_smccc_smc
++
++#endif
++
+ #define arm_smccc_smc(...) __arm_smccc_smc(__VA_ARGS__, NULL)
+ 
+ #define arm_smccc_smc_quirk(...) __arm_smccc_smc(__VA_ARGS__)
+diff --git a/lib/Kconfig b/lib/Kconfig
+index 3c6fa99b1a6a..473821b882e2 100644
+--- a/lib/Kconfig
++++ b/lib/Kconfig
+@@ -810,6 +810,7 @@ config SMBIOS_PARSER
+ source lib/efi/Kconfig
+ source lib/efi_loader/Kconfig
+ source lib/optee/Kconfig
++source lib/arm-ffa/Kconfig
+ 
+ config TEST_FDTDEC
+ 	bool "enable fdtdec test"
+diff --git a/lib/Makefile b/lib/Makefile
+index 11b03d1cbec8..8e6fad613067 100644
+--- a/lib/Makefile
++++ b/lib/Makefile
+@@ -9,6 +9,7 @@ obj-$(CONFIG_EFI) += efi/
+ obj-$(CONFIG_EFI_LOADER) += efi_driver/
+ obj-$(CONFIG_EFI_LOADER) += efi_loader/
+ obj-$(CONFIG_CMD_BOOTEFI_SELFTEST) += efi_selftest/
++obj-$(CONFIG_ARM_FFA_TRANSPORT_HELPERS) += arm-ffa/
+ obj-$(CONFIG_LZMA) += lzma/
+ obj-$(CONFIG_BZIP2) += bzip2/
+ obj-$(CONFIG_TIZEN) += tizen/
+diff --git a/lib/arm-ffa/Kconfig b/lib/arm-ffa/Kconfig
+new file mode 100644
+index 000000000000..79acbc5a8fe3
+--- /dev/null
++++ b/lib/arm-ffa/Kconfig
+@@ -0,0 +1,6 @@
++config ARM_FFA_TRANSPORT_HELPERS
++	bool "Enable interface helpers for Arm Firmware Framework for Armv8-A"
++	depends on ARM_FFA_TRANSPORT
++	help
++	  User layers call FF-A interfaces using helper functions which
++	  pass the data and the FF-A function ID to the low level driver
+diff --git a/lib/arm-ffa/Makefile b/lib/arm-ffa/Makefile
+new file mode 100644
+index 000000000000..c30c0f398126
+--- /dev/null
++++ b/lib/arm-ffa/Makefile
+@@ -0,0 +1,8 @@
++# SPDX-License-Identifier: GPL-2.0+
++#
++# (C) Copyright 2021 Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++#
++
++# This file only gets included when CONFIG_ARM_FFA_TRANSPORT_HELPERS is set
++
++obj-y += arm_ffa_helper.o
+diff --git a/lib/arm-ffa/arm_ffa_helper.c b/lib/arm-ffa/arm_ffa_helper.c
+new file mode 100644
+index 000000000000..623899d38044
+--- /dev/null
++++ b/lib/arm-ffa/arm_ffa_helper.c
+@@ -0,0 +1,188 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#include <common.h>
++#include <arm_ffa_helper.h>
++#include <uuid.h>
++
++/**
++ * ffa_helper_get_partitions_info - Wrapper function for FFA_PARTITION_INFO_GET
++ *
++ * @func_data: Pointer to the FF-A function arguments container
++ *				structure.
++ *				The passed arguments:
++ *					Mode 1: When getting from the driver the number of
++ *						secure partitions:
++ *						@data0_size: UUID size
++ *						@data0: pointer to the UUID (little endian)
++ *						@data1_size: size of the number of partitions
++ *									variable
++ *						@data1: pointer to the number of partitions
++ *								 variable. The variable will be set
++ *								 by the driver
++ *					Mode 2: When requesting the driver to return the
++ *						partitions information:
++ *						@data0_size: UUID size
++ *						@data0: pointer to the UUID (little endian)
++ *						@data1_size: size of the SPs information buffer
++ *						@data1: pointer to SPs information buffer
++ *								(allocated by the client).
++ *							The buffer will be filled by the driver
++ *
++ * This is the boot time function used by clients who wants to get from secure
++ * world the partition(s) information.
++ *
++ * A client of the FF-A driver should know the UUID of the service it wants to
++ * access. It should use the UUID to request the FF-A driver to provide the
++ * partition(s) information of the service. The client should use
++ * ffa_helper_get_partitions_info to pass the UUID information to the driver
++ * which uses PARTITION_INFO_GET to obtain the partition(s) information.
++ *
++ * ffa_helper_get_partitions_info should be called twice. First call is to get
++ * from the driver the number of secure partitions (SPs) associated to a
++ * particular UUID. Then, the caller (client) allocates the buffer to host the
++ * SPs data and issues a 2nd call. Then, the driver fills the SPs data in the
++ * pre-allocated buffer.
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int ffa_helper_get_partitions_info(struct ffa_interface_data *func_data)
++{
++	return ffa_get_invoke_func(FFA_PARTITION_INFO_GET, func_data);
++}
++
++/**
++ * ffa_helper_unmap_rxtx_buffers - Wrapper function for FFA_RXTX_UNMAP
++ *
++ * This is the boot time function that allows clients to unmap the RX/TX
++ * buffers
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int ffa_helper_unmap_rxtx_buffers(void)
++{
++	return ffa_get_invoke_func(FFA_RXTX_UNMAP, NULL);
++}
++
++/**
++ * ffa_helper_msg_send_direct_req - Wrapper function for
++ *								FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ * @func_data: Pointer to the FF-A function arguments container structure.
++ * The passed arguments:
++ *	@data0_size: partition ID size
++ *	@data0: pointer to the partition ID
++ *	@data1_size: exchanged data size
++ *	@data1: pointer to the data buffer preallocated by the client (in/out)
++ *
++ * This is the runtime function that allows clients to send data to the secure
++ * world partitions. The arm_ffa driver uses FFA_MSG_SEND_DIRECT_REQ to send the
++ * data to the secure partition. The response from the secure partition is
++ * handled internally by the driver using FFA_MSG_SEND_DIRECT_RESP and returned
++ * to ffa_helper_msg_send_direct_req through @func_data
++ *
++ * The maximum size of the data that can be exchanged is 20 bytes which is
++ * sizeof(struct ffa_send_direct_data) as defined by the FF-A specification 1.0
++ * in the section relevant to FFA_MSG_SEND_DIRECT_{REQ,RESP}
++ *
++ * The client should pre-allocate a buffer pointed by @data1 which the size
++ * is sizeof(struct ffa_send_direct_data)
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int __ffa_runtime ffa_helper_msg_send_direct_req(struct ffa_interface_data
++											 *func_data)
++{
++	return ffa_get_invoke_func(FFA_MSG_SEND_DIRECT_REQ, func_data);
++}
++
++/**
++ * ffa_helper_init_device - Wrapper function for probing the arm_ffa device
++ *
++ * This boot time function should be called to probe the arm_ffa device so
++ * it becomes ready for use.
++ * To achieve that, this function is called automatically at initcalls
++ * level (after u-boot relocation).
++ *
++ * Return:
++ *
++ * FFA_ERR_STAT_SUCCESS on success. Otherwise, failure
++ */
++int ffa_helper_init_device(void)
++{
++	return ffa_init_device();
++}
++
++/**
++ * ffa_uuid_str_to_bin - Converts a big endian UUID string to a little endian buffer
++ * @uuid_str:	UUID string in big endian format (36 bytes wide + '/0')
++ * @uuid_bin:	preallocated 16 bytes UUID buffer in little endian format
++ *
++ * UUID binary format used by the FF-A framework (16 bytes):
++ *
++ * [LSB] 4B-2B-2B-2B-6B (little endian data fields)
++ *
++ * UUID string is 36 length of characters (36 bytes):
++ *
++ * xxxxxxxx-xxxx-xxxx-xxxx-xxxxxxxxxxxx
++ * be                be      be       be      be
++ *
++ * where x is a hexadecimal character. Fields are separated by '-'s.
++ * When converting to a binary UUID, these endianness rules apply:
++ *     be: means the field in the string is considered a big endian hex number
++ *         and should be converted to little endian binary format
++ *
++ * Return:
++ *
++ *    uuid_bin filled with little endian UUID data
++ *    On success 0 is returned. Otherwise, failure code.
++ */
++int ffa_uuid_str_to_bin(const char *uuid_str, unsigned char *uuid_bin)
++{
++	u16 tmp16 = 0;
++	u32 tmp32 = 0;
++	u64 tmp64 = 0;
++
++	if (!uuid_str_valid(uuid_str) || !uuid_bin)
++		return -EINVAL;
++
++	/*
++	 * reverse bytes from big to little endian
++	 */
++	tmp32 = simple_strtoul(uuid_str, NULL, 16);
++	memcpy(uuid_bin, &tmp32, 4);
++
++	/*
++	 * reverse bytes from big to little endian
++	 */
++	tmp16 = simple_strtoul(uuid_str + 9, NULL, 16);
++	memcpy(uuid_bin + 4, &tmp16, 2);
++
++	/*
++	 * reverse bytes from big to little endian
++	 */
++	tmp16 = simple_strtoul(uuid_str + 14, NULL, 16);
++	memcpy(uuid_bin + 6, &tmp16, 2);
++
++	/*
++	 * reverse bytes from big to little endian
++	 */
++	tmp16 = simple_strtoul(uuid_str + 19, NULL, 16);
++	memcpy(uuid_bin + 8, &tmp16, 2);
++
++	/*
++	 * reverse bytes from big to little endian
++	 */
++	tmp64 = simple_strtoull(uuid_str + 24, NULL, 16);
++	memcpy(uuid_bin + 10, (char *)&tmp64, 6);
++
++	return 0;
++}
+diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
+index 5bcb8253edba..cffa2c69d621 100644
+--- a/lib/efi_loader/efi_boottime.c
++++ b/lib/efi_loader/efi_boottime.c
+@@ -23,6 +23,10 @@
+ #include <asm/setjmp.h>
+ #include <linux/libfdt_env.h>
+ 
++#if defined(CONFIG_ARM_FFA_TRANSPORT)
++#include <arm_ffa_helper.h>
++#endif
++
+ DECLARE_GLOBAL_DATA_PTR;
+ 
+ /* Task priority level */
+@@ -2114,6 +2118,10 @@ static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
+ 	struct efi_event *evt, *next_event;
+ 	efi_status_t ret = EFI_SUCCESS;
+ 
++#if defined(CONFIG_ARM_FFA_TRANSPORT)
++	int ffa_ret;
++#endif
++
+ 	EFI_ENTRY("%p, %zx", image_handle, map_key);
+ 
+ 	/* Check that the caller has read the current memory map */
+@@ -2174,6 +2182,15 @@ static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
+ 		dm_remove_devices_flags(DM_REMOVE_ACTIVE_ALL);
+ 	}
+ 
++#if defined(CONFIG_ARM_FFA_TRANSPORT)
++	/* unmap FF-A RX/TX buffers */
++	ffa_ret = ffa_helper_unmap_rxtx_buffers();
++	if (ffa_ret)
++		debug("[efi_boottime][ERROR]: can not unmap FF-A RX/TX buffers\n");
++	else
++		debug("[efi_boottime][INFO]: FF-A RX/TX buffers unmapped\n");
++#endif
++
+ 	/* Patch out unsupported runtime function */
+ 	efi_runtime_detach();
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0007-arm_ffa-introducing-armffa-command.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0007-arm_ffa-introducing-armffa-command.patch
new file mode 100644
index 0000000..83f0547
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0007-arm_ffa-introducing-armffa-command.patch
@@ -0,0 +1,342 @@
+From 541b2b51dc77832ab5845cab4762d29976838d36 Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Tue, 16 Nov 2021 12:36:27 +0000
+Subject: [PATCH 07/27] arm_ffa: introducing armffa command
+
+A new armffa command is provided as an example of how to use
+the FF-A helper functions to communicate with secure world.
+
+The armffa command allows to query secure partitions data from
+the secure world and exchanging messages with the partitions.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ MAINTAINERS  |   1 +
+ cmd/Kconfig  |  10 ++
+ cmd/Makefile |   2 +
+ cmd/armffa.c | 266 +++++++++++++++++++++++++++++++++++++++++++++++++++
+ 4 files changed, 279 insertions(+)
+ create mode 100644 cmd/armffa.c
+
+diff --git a/MAINTAINERS b/MAINTAINERS
+index 14307e6da644..f3fd559da54a 100644
+--- a/MAINTAINERS
++++ b/MAINTAINERS
+@@ -235,6 +235,7 @@ F:	include/configs/turris_*.h
+ ARM FF-A
+ M:	Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+ S:	Maintained
++F:	cmd/armffa.c
+ F:	drivers/arm-ffa/
+ F:	include/arm_ffa.h
+ F:	include/arm_ffa_helper.h
+diff --git a/cmd/Kconfig b/cmd/Kconfig
+index ff50102a89c7..ff124bf4bad0 100644
+--- a/cmd/Kconfig
++++ b/cmd/Kconfig
+@@ -813,6 +813,16 @@ endmenu
+ 
+ menu "Device access commands"
+ 
++config CMD_ARMFFA
++	bool "Arm FF-A test command"
++	depends on ARM_FFA_TRANSPORT
++	help
++	  Provides a test command for the Arm FF-A driver
++	  supported options:
++		- Listing the partition(s) info
++		- Sending a data pattern to the specified partition
++		- Displaying the arm_ffa device info
++
+ config CMD_ARMFLASH
+ 	#depends on FLASH_CFI_DRIVER
+ 	bool "armflash"
+diff --git a/cmd/Makefile b/cmd/Makefile
+index 166c652d9825..770b846c44e0 100644
+--- a/cmd/Makefile
++++ b/cmd/Makefile
+@@ -12,6 +12,8 @@ obj-y += panic.o
+ obj-y += version.o
+ 
+ # command
++
++obj-$(CONFIG_CMD_ARMFFA) += armffa.o
+ obj-$(CONFIG_CMD_ACPI) += acpi.o
+ obj-$(CONFIG_CMD_ADDRMAP) += addrmap.o
+ obj-$(CONFIG_CMD_AES) += aes.o
+diff --git a/cmd/armffa.c b/cmd/armffa.c
+new file mode 100644
+index 000000000000..71a6ebb656d1
+--- /dev/null
++++ b/cmd/armffa.c
+@@ -0,0 +1,266 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#include <arm_ffa_helper.h>
++#include <asm/io.h>
++#include <common.h>
++#include <command.h>
++#include <dm.h>
++#include <mapmem.h>
++#include <stdlib.h>
++
++/**
++ * do_ffa_get_singular_partition_info - implementation of the getpart subcommand
++ * @cmdtp:		Command Table
++ * @flag:		flags
++ * @argc:		number of arguments
++ * @argv:		arguments
++ *
++ * This function queries the secure partition information which the UUID is provided
++ * as an argument. The function uses the arm_ffa driver helper function
++ * to retrieve the data.
++ * The input UUID string is expected to be in big endian format.
++ *
++ * Return:
++ *
++ * CMD_RET_SUCCESS: on success, otherwise failure
++ */
++static int do_ffa_get_singular_partition_info(struct cmd_tbl *cmdtp, int flag, int argc,
++					      char *const argv[])
++{
++	struct ffa_interface_data func_data = {0};
++	u32 count = 0;
++	int ret;
++	union ffa_partition_uuid service_uuid = {0};
++	struct ffa_partition_info *parts_info;
++	u32 info_idx;
++
++	if (argc != 1)
++		return -EINVAL;
++
++	if (ffa_uuid_str_to_bin(argv[0], (unsigned char *)&service_uuid)) {
++		ffa_err("Invalid UUID");
++		return -EINVAL;
++	}
++
++	/*
++	 * get from the driver the count of the SPs matching the UUID
++	 */
++	func_data.data0_size = sizeof(service_uuid);
++	func_data.data0 = &service_uuid;
++	func_data.data1_size = sizeof(count);
++	func_data.data1 = &count;
++
++	ret = ffa_helper_get_partitions_info(&func_data);
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		ffa_err("Failure in querying partitions count (error code: %d)", ret);
++		return ret;
++	}
++
++	if (!count) {
++		ffa_info("No secure partition found");
++		return ret;
++	}
++
++	/*
++	 * pre-allocate a buffer to be filled by the driver
++	 * with	 ffa_partition_info structs
++	 */
++
++	parts_info = calloc(count, sizeof(struct ffa_partition_info));
++	if (!parts_info)
++		return -EINVAL;
++
++	ffa_info("Pre-allocating %d partition(s) info structures", count);
++
++	func_data.data1_size = count * sizeof(struct ffa_partition_info);
++	func_data.data1 = parts_info;
++
++	/*
++	 * ask the driver to fill the buffer with the SPs info
++	 */
++	ret = ffa_helper_get_partitions_info(&func_data);
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		ffa_err("Failure in querying partition(s) info (error code: %d)", ret);
++		free(parts_info);
++		return ret;
++	}
++
++	/*
++	 * SPs found , show the partition information
++	 */
++	for (info_idx = 0; info_idx < count ; info_idx++) {
++		ffa_info("Partition: id = 0x%x , exec_ctxt 0x%x , properties 0x%x",
++			 parts_info[info_idx].id,
++			 parts_info[info_idx].exec_ctxt,
++			 parts_info[info_idx].properties);
++	}
++
++	free(parts_info);
++
++	return 0;
++}
++
++/**
++ * do_ffa_msg_send_direct_req - implementation of the ping subcommand
++ * @cmdtp:		Command Table
++ * @flag:		flags
++ * @argc:		number of arguments
++ * @argv:		arguments
++ *
++ * This function sends data to the secure partition which the ID is provided
++ * as an argument. The function uses the arm_ffa driver helper function
++ * to send data.
++ *
++ * Return:
++ *
++ * CMD_RET_SUCCESS: on success, otherwise failure
++ */
++int  do_ffa_msg_send_direct_req(struct cmd_tbl *cmdtp, int flag, int argc,
++				char *const argv[])
++{
++	struct ffa_interface_data func_data = {0};
++	struct ffa_send_direct_data msg = {0};
++	u32 pattern = 0xaabbccd0;
++	u16 part_id;
++	int ret;
++
++	if (argc != 1)
++		return -EINVAL;
++
++	errno = 0;
++	part_id = strtoul(argv[0], NULL, 16);
++
++	if (errno) {
++		ffa_err("Invalid partition ID");
++		return -EINVAL;
++	}
++
++	/*
++	 * telling the driver which partition to use
++	 */
++	func_data.data0_size = sizeof(part_id);
++	func_data.data0 = &part_id;
++
++	/*
++	 * filling the message data
++	 */
++	msg.a3 = ++pattern;
++	msg.a4 = ++pattern;
++	msg.a5 = ++pattern;
++	msg.a6 = ++pattern;
++	msg.a7 = ++pattern;
++	func_data.data1_size = sizeof(msg);
++	func_data.data1 = &msg;
++
++	ret = ffa_helper_msg_send_direct_req(&func_data);
++	if (ret == FFA_ERR_STAT_SUCCESS) {
++		u8 cnt;
++
++		ffa_info("SP response:\n[LSB]");
++		for (cnt = 0;
++		     cnt < sizeof(struct ffa_send_direct_data) / sizeof(u32);
++		     cnt++)
++			ffa_info("0x%x", ((u32 *)&msg)[cnt]);
++	} else {
++		ffa_err("Sending direct request error (%d)", ret);
++	}
++
++	return ret;
++}
++
++/**
++ *do_ffa_dev_list - implementation of the devlist subcommand
++ * @cmdtp: [in]		Command Table
++ * @flag:		flags
++ * @argc:		number of arguments
++ * @argv:		arguments
++ *
++ * This function queries the devices belonging to the UCLASS_FFA
++ * class. Currently, one device is expected to show up: the arm_ffa device
++ *
++ * Return:
++ *
++ * CMD_RET_SUCCESS: on success, otherwise failure
++ */
++int do_ffa_dev_list(struct cmd_tbl *cmdtp, int flag, int argc, char *const argv[])
++{
++	struct udevice *dev = NULL;
++	int i, ret;
++
++	ffa_info("arm_ffa uclass entries:");
++
++	for (i = 0, ret = uclass_first_device(UCLASS_FFA, &dev);
++	     dev;
++	     ret = uclass_next_device(&dev), i++) {
++		if (ret)
++			break;
++
++		ffa_info("entry %d - instance %08x, ops %08x, plat %08x",
++			 i,
++			 (u32)map_to_sysmem(dev),
++			 (u32)map_to_sysmem(dev->driver->ops),
++			 (u32)map_to_sysmem(dev_get_plat(dev)));
++	}
++
++	return cmd_process_error(cmdtp, ret);
++}
++
++static struct cmd_tbl armffa_commands[] = {
++	U_BOOT_CMD_MKENT(getpart, 1, 1, do_ffa_get_singular_partition_info, "", ""),
++	U_BOOT_CMD_MKENT(ping, 1, 1, do_ffa_msg_send_direct_req, "", ""),
++	U_BOOT_CMD_MKENT(devlist, 0, 1, do_ffa_dev_list, "", ""),
++};
++
++/**
++ * do_armffa - the armffa command main function
++ * @cmdtp:	Command Table
++ * @flag:		flags
++ * @argc:		number of arguments
++ * @argv:		arguments
++ *
++ * This function identifies which armffa subcommand to run.
++ * Then, it makes sure the arm_ffa device is probed and
++ * ready for use.
++ * Then, it runs the subcommand.
++ *
++ * Return:
++ *
++ * CMD_RET_SUCCESS: on success, otherwise failure
++ */
++static int do_armffa(struct cmd_tbl *cmdtp, int flag, int argc, char *const argv[])
++{
++	struct cmd_tbl *armffa_cmd;
++	int ret;
++
++	if (argc < 2)
++		return CMD_RET_USAGE;
++
++	armffa_cmd = find_cmd_tbl(argv[1], armffa_commands, ARRAY_SIZE(armffa_commands));
++
++	argc -= 2;
++	argv += 2;
++
++	if (!armffa_cmd || argc > armffa_cmd->maxargs)
++		return CMD_RET_USAGE;
++
++	ret = ffa_helper_init_device();
++	if (ret != FFA_ERR_STAT_SUCCESS)
++		return cmd_process_error(cmdtp, ret);
++
++	ret = armffa_cmd->cmd(armffa_cmd, flag, argc, argv);
++
++	return cmd_process_error(armffa_cmd, ret);
++}
++
++U_BOOT_CMD(armffa, 4, 1, do_armffa,
++	   "Arm FF-A operations test command",
++	   "getpart <partition UUID>\n"
++	   "	 - lists the partition(s) info\n"
++	   "ping <partition ID>\n"
++	   "	 - sends a data pattern to the specified partition\n"
++	   "devlist\n"
++	   "	 - displays the arm_ffa device info\n");
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0008-arm_ffa-introducing-MM-communication-with-FF-A.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0008-arm_ffa-introducing-MM-communication-with-FF-A.patch
new file mode 100644
index 0000000..9b1383e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0008-arm_ffa-introducing-MM-communication-with-FF-A.patch
@@ -0,0 +1,383 @@
+From 2f09d4a2e87febd7365b9e18d669208ff2c35edc Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Wed, 13 Oct 2021 17:51:44 +0100
+Subject: [PATCH 08/27] arm_ffa: introducing MM communication with FF-A
+
+This commit allows to perform MM communication using FF-A transport.
+
+The MM SP (also called partition) can be StandAlonneMM or smm-gateway.
+Both partitions run in OP-TEE.
+
+When using the u-boot FF-A driver, StandAlonneMM and smm-gateway are
+supported.
+
+On EFI services such as GetVariable()/SetVariable(), the data
+is copied from the communication buffer to the MM shared buffer.
+
+Then, notifies the MM SP about data availability in the MM shared buffer.
+Communication with the MM SP is performed using FF-A transport.
+
+On such event, MM SP can read the data and updates the MM shared buffer
+with response data.
+
+The response data is copied back to the communication buffer.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/Kconfig            |  14 +-
+ lib/efi_loader/efi_variable_tee.c | 265 +++++++++++++++++++++++++++++-
+ 2 files changed, 273 insertions(+), 6 deletions(-)
+
+diff --git a/lib/efi_loader/Kconfig b/lib/efi_loader/Kconfig
+index e5e35fe51f65..6827b821545e 100644
+--- a/lib/efi_loader/Kconfig
++++ b/lib/efi_loader/Kconfig
+@@ -56,13 +56,23 @@ config EFI_VARIABLE_FILE_STORE
+ 	  stored as file /ubootefi.var on the EFI system partition.
+ 
+ config EFI_MM_COMM_TEE
+-	bool "UEFI variables storage service via OP-TEE"
+-	depends on OPTEE
++	bool "UEFI variables storage service via the trusted world"
++	depends on OPTEE || ARM_FFA_TRANSPORT
+ 	help
++	  The MM SP (also called partition) can be StandAlonneMM or smm-gateway.
++	  When using the u-boot OP-TEE driver, StandAlonneMM is supported.
++	  When using the u-boot FF-A  driver, StandAlonneMM and smm-gateway are supported.
++
+ 	  If OP-TEE is present and running StandAloneMM, dispatch all UEFI
+ 	  variable related operations to that. The application will verify,
+ 	  authenticate and store the variables on an RPMB.
+ 
++	  When ARM_FFA_TRANSPORT is used, dispatch all UEFI variable related
++	  operations to the MM SP running under Optee in the trusted world.
++	  A door bell mechanism is used to notify the SP when there is data in the shared
++	  MM buffer. The data is copied by u-boot to thea shared buffer before issuing
++	  the door bell event.
++
+ endchoice
+ 
+ config EFI_VARIABLES_PRESEED
+diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
+index dfef18435dfa..9cb8cfb9c779 100644
+--- a/lib/efi_loader/efi_variable_tee.c
++++ b/lib/efi_loader/efi_variable_tee.c
+@@ -15,6 +15,28 @@
+ #include <malloc.h>
+ #include <mm_communication.h>
+ 
++#if (IS_ENABLED(CONFIG_OPTEE))
++#define OPTEE_PAGE_SIZE BIT(12)
++#endif
++
++#if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++
++#include <arm_ffa_helper.h>
++#include <mapmem.h>
++
++/* MM return codes */
++#define MM_SUCCESS (0)
++
++#define ARM_SVC_ID_SP_EVENT_COMPLETE_AARCH64 (0xC4000061)
++#define ARM_SVC_ID_SP_EVENT_COMPLETE ARM_SVC_ID_SP_EVENT_COMPLETE_AARCH64
++
++/* MM_SP_UUID_DATA defined by the platform */
++union ffa_partition_uuid mm_sp_svc_uuid = {.bytes = {MM_SP_UUID_DATA}};
++
++static u16 __efi_runtime_data mm_sp_id;
++
++#endif
++
+ extern struct efi_var_file __efi_runtime_data *efi_var_buf;
+ static efi_uintn_t max_buffer_size;	/* comm + var + func + data */
+ static efi_uintn_t max_payload_size;	/* func + data */
+@@ -24,6 +46,7 @@ struct mm_connection {
+ 	u32 session;
+ };
+ 
++#if (IS_ENABLED(CONFIG_OPTEE))
+ /**
+  * get_connection() - Retrieve OP-TEE session for a specific UUID.
+  *
+@@ -143,16 +166,229 @@ static efi_status_t optee_mm_communicate(void *comm_buf, ulong dsize)
+ 
+ 	return ret;
+ }
++#endif
++
++#if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++
++/**
++ * ffa_notify_mm_sp() - Announce there is data in the shared buffer
++ *
++ * Notifies the MM partition in the trusted world that
++ * data is available in the shared buffer.
++ * This is a blocking call during which trusted world has exclusive access
++ * to the MM shared buffer.
++ *
++ * Return:
++ *
++ * 0 on success
++ */
++static int __efi_runtime ffa_notify_mm_sp(void)
++{
++	struct ffa_interface_data func_data = {0};
++	struct ffa_send_direct_data msg = {0};
++	int ret;
++	u32 sp_event_complete;
++	int sp_event_ret;
++
++	func_data.data0_size = sizeof(mm_sp_id);
++	func_data.data0 = &mm_sp_id;
++
++	msg.a3 = FFA_SHARED_MM_BUFFER_ADDR;
++	msg.a4 = FFA_SHARED_MM_BUFFER_SIZE;
++	func_data.data1_size = sizeof(msg);
++	func_data.data1 = &msg;
++
++	ret = ffa_helper_msg_send_direct_req(&func_data);
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		log_err("EFI: Failure to notify the MM SP , FF-A  error (%d)\n", ret);
++		return ret;
++	}
++
++	sp_event_complete = msg.a3;
++	sp_event_ret = (int)msg.a4;
++
++	if (sp_event_complete == ARM_SVC_ID_SP_EVENT_COMPLETE && sp_event_ret == MM_SUCCESS)
++		return 0;
++
++	log_err("EFI: Failure to notify the MM SP (0x%x , %d)\n",
++		sp_event_complete,
++		sp_event_ret);
++
++	return -EACCES;
++}
++
++/**
++ * ffa_discover_mm_sp_id() - Query the MM partition ID
++ *
++ * Use the FF-A driver to get the MM partition ID.
++ * If multiple partitions are found, use the first one
++ *
++ * Return:
++ *
++ * 0 on success
++ */
++static int __efi_runtime ffa_discover_mm_sp_id(void)
++{
++	struct ffa_interface_data func_data = {0};
++	u32 count = 0;
++	int ret;
++	struct ffa_partition_info *parts_info;
++
++	/*
++	 * get from the driver the count of the SPs matching the UUID
++	 */
++	func_data.data0_size = sizeof(mm_sp_svc_uuid);
++	func_data.data0 = &mm_sp_svc_uuid;
++	func_data.data1_size = sizeof(count);
++	func_data.data1 = &count;
++
++	ret = ffa_helper_get_partitions_info(&func_data);
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		log_err("EFI: Failure in querying partitions count (error code: %d)\n", ret);
++		return ret;
++	}
++
++	if (!count) {
++		log_info("EFI: No MM partition found\n");
++		return ret;
++	}
++
++	/*
++	 * pre-allocate a buffer to be filled by the driver
++	 * with	 ffa_partition_info structs
++	 */
++
++	parts_info = calloc(count, sizeof(struct ffa_partition_info));
++	if (!parts_info)
++		return -EINVAL;
++
++	log_info("EFI: Pre-allocating %d partition(s) info structures\n", count);
++
++	func_data.data1_size = count *
++		sizeof(struct ffa_partition_info);
++	func_data.data1 = parts_info;
++
++	/*
++	 * ask the driver to fill the
++	 * buffer with the SPs info
++	 */
++	ret = ffa_helper_get_partitions_info(&func_data);
++	if (ret != FFA_ERR_STAT_SUCCESS) {
++		log_err("EFI: Failure in querying partition(s) info (error code: %d)\n", ret);
++		free(parts_info);
++		return ret;
++	}
++
++	/*
++	 * MM SPs found , use the first one
++	 */
++
++	mm_sp_id = parts_info[0].id;
++
++	log_info("EFI: MM partition ID 0x%x\n", mm_sp_id);
++
++	free(parts_info);
++
++	return 0;
++}
+ 
+ /**
+- * mm_communicate() - Adjust the cmonnucation buffer to StandAlonneMM and send
++ * ffa_mm_communicate() - Exchange EFI services data with  the MM partition using FF-A
++ * @comm_buf:		locally allocated communication buffer used for for rx/tx
++ * @dsize:				communication buffer size
++ *
++ * Issues a door bell event to notify the MM partition (SP) running in OP-TEE
++ * that there is data to read from the shared buffer.
++ * Communication with the MM SP is performed using FF-A transport.
++ * On the event, MM SP can read the data from the buffer and
++ * update the MM shared buffer with response data.
++ * The response data is copied back to the communication buffer.
++ *
++ * Return:
++ *
++ * EFI status code
++ */
++static efi_status_t __efi_runtime ffa_mm_communicate(void *comm_buf, ulong comm_buf_size)
++{
++	ulong tx_data_size;
++	int ffa_ret;
++	struct efi_mm_communicate_header *mm_hdr;
++	void *virt_shared_buf;
++
++	if (!comm_buf)
++		return EFI_INVALID_PARAMETER;
++
++	/* Discover MM partition ID */
++	if (!mm_sp_id && ffa_discover_mm_sp_id()  != FFA_ERR_STAT_SUCCESS) {
++		log_err("EFI: Failure to discover MM partition ID\n");
++		return EFI_UNSUPPORTED;
++	}
++
++	mm_hdr = (struct efi_mm_communicate_header *)comm_buf;
++	tx_data_size = mm_hdr->message_len + sizeof(efi_guid_t) + sizeof(size_t);
++
++	if (comm_buf_size != tx_data_size || tx_data_size > FFA_SHARED_MM_BUFFER_SIZE)
++		return EFI_INVALID_PARAMETER;
++
++	/* Copy the data to the shared buffer */
++
++	virt_shared_buf = (void *)map_sysmem((phys_addr_t)FFA_SHARED_MM_BUFFER_ADDR, 0);
++	efi_memcpy_runtime(virt_shared_buf, comm_buf, tx_data_size);
++
++	/* Announce there is data in the shared buffer */
++
++	ffa_ret = ffa_notify_mm_sp();
++	if (ffa_ret)
++		unmap_sysmem(virt_shared_buf);
++
++	switch (ffa_ret) {
++	case 0:
++	{
++		ulong rx_data_size;
++		/* Copy the MM SP response from the shared buffer to the communication buffer */
++		rx_data_size = ((struct efi_mm_communicate_header *)virt_shared_buf)->message_len +
++			sizeof(efi_guid_t) +
++			sizeof(size_t);
++
++		if (rx_data_size > comm_buf_size) {
++			unmap_sysmem(virt_shared_buf);
++			return EFI_OUT_OF_RESOURCES;
++		}
++
++		efi_memcpy_runtime(comm_buf, virt_shared_buf, rx_data_size);
++		unmap_sysmem(virt_shared_buf);
++
++		return EFI_SUCCESS;
++	}
++	case -EINVAL:
++		return EFI_DEVICE_ERROR;
++	case -EPERM:
++		return EFI_INVALID_PARAMETER;
++	case -EACCES:
++		return EFI_ACCESS_DENIED;
++	case -EBUSY:
++		return EFI_OUT_OF_RESOURCES;
++	default:
++		return EFI_ACCESS_DENIED;
++	}
++}
++#endif
++
++/**
++ * mm_communicate() - Adjust the communication buffer to the MM SP and send
+  * it to OP-TEE
+  *
+- * @comm_buf:		locally allocted communcation buffer
++ * @comm_buf:		locally allocted communication buffer
+  * @dsize:		buffer size
++ *
++ * The MM SP (also called partition) can be StandAlonneMM or smm-gateway.
++ * The comm_buf format is the same for both partitions.
++ * When using the u-boot OP-TEE driver, StandAlonneMM is supported.
++ * When using the u-boot FF-A  driver, StandAlonneMM and smm-gateway are supported.
++ *
+  * Return:		status code
+  */
+-static efi_status_t mm_communicate(u8 *comm_buf, efi_uintn_t dsize)
++static efi_status_t __efi_runtime mm_communicate(u8 *comm_buf, efi_uintn_t dsize)
+ {
+ 	efi_status_t ret;
+ 	struct efi_mm_communicate_header *mm_hdr;
+@@ -162,7 +398,11 @@ static efi_status_t mm_communicate(u8 *comm_buf, efi_uintn_t dsize)
+ 	mm_hdr = (struct efi_mm_communicate_header *)comm_buf;
+ 	var_hdr = (struct smm_variable_communicate_header *)mm_hdr->data;
+ 
++	#if (IS_ENABLED(CONFIG_OPTEE))
+ 	ret = optee_mm_communicate(comm_buf, dsize);
++	#elif (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++	ret = ffa_mm_communicate(comm_buf, dsize);
++	#endif
+ 	if (ret != EFI_SUCCESS) {
+ 		log_err("%s failed!\n", __func__);
+ 		return ret;
+@@ -258,6 +498,23 @@ efi_status_t EFIAPI get_max_payload(efi_uintn_t *size)
+ 		goto out;
+ 	}
+ 	*size = var_payload->size;
++
++	#if (IS_ENABLED(CONFIG_OPTEE))
++	/*
++	 * Although the max payload is configurable on StMM, we only share a
++	 * single page from OP-TEE for the non-secure buffer used to communicate
++	 * with StMM. Since OP-TEE will reject to map anything bigger than that,
++	 * make sure we are in bounds.
++	 */
++	if (*size > OPTEE_PAGE_SIZE)
++		*size = OPTEE_PAGE_SIZE - MM_COMMUNICATE_HEADER_SIZE  -
++			MM_VARIABLE_COMMUNICATE_SIZE;
++	#elif (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
++		if (*size > FFA_SHARED_MM_BUFFER_SIZE)
++			*size = FFA_SHARED_MM_BUFFER_SIZE - MM_COMMUNICATE_HEADER_SIZE  -
++				MM_VARIABLE_COMMUNICATE_SIZE;
++	#endif
++
+ 	/*
+ 	 * There seems to be a bug in EDK2 miscalculating the boundaries and
+ 	 * size checks, so deduct 2 more bytes to fulfill this requirement. Fix
+@@ -697,7 +954,7 @@ void efi_variables_boot_exit_notify(void)
+ 		ret = EFI_NOT_FOUND;
+ 
+ 	if (ret != EFI_SUCCESS)
+-		log_err("Unable to notify StMM for ExitBootServices\n");
++		log_err("Unable to notify the MM partition for ExitBootServices\n");
+ 	free(comm_buf);
+ 
+ 	/*
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0009-arm_ffa-introducing-test-module-for-UCLASS_FFA.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0009-arm_ffa-introducing-test-module-for-UCLASS_FFA.patch
new file mode 100644
index 0000000..4fb317b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0009-arm_ffa-introducing-test-module-for-UCLASS_FFA.patch
@@ -0,0 +1,132 @@
+From c9a2c457648b732292482fae59a7fd61cefffd33 Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Tue, 16 Nov 2021 12:38:48 +0000
+Subject: [PATCH 09/27] arm_ffa: introducing test module for UCLASS_FFA
+
+This is the test module for the UCLASS_FFA class.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ MAINTAINERS      |  1 +
+ test/dm/Makefile |  1 +
+ test/dm/ffa.c    | 56 ++++++++++++++++++++++++++++++++++++++++++++++++
+ test/dm/ffa.h    | 19 ++++++++++++++++
+ 4 files changed, 77 insertions(+)
+ create mode 100644 test/dm/ffa.c
+ create mode 100644 test/dm/ffa.h
+
+diff --git a/MAINTAINERS b/MAINTAINERS
+index f3fd559da54a..6510f844fe09 100644
+--- a/MAINTAINERS
++++ b/MAINTAINERS
+@@ -240,6 +240,7 @@ F:	drivers/arm-ffa/
+ F:	include/arm_ffa.h
+ F:	include/arm_ffa_helper.h
+ F:	lib/arm-ffa/
++F:	test/dm/ffa.c
+ 
+ ARM FREESCALE IMX
+ M:	Stefano Babic <sbabic@denx.de>
+diff --git a/test/dm/Makefile b/test/dm/Makefile
+index d46552fbf320..ddac250cdff0 100644
+--- a/test/dm/Makefile
++++ b/test/dm/Makefile
+@@ -79,6 +79,7 @@ obj-$(CONFIG_POWER_DOMAIN) += power-domain.o
+ obj-$(CONFIG_ACPI_PMC) += pmc.o
+ obj-$(CONFIG_DM_PMIC) += pmic.o
+ obj-$(CONFIG_DM_PWM) += pwm.o
++obj-$(CONFIG_ARM_FFA_TRANSPORT) += ffa.o
+ obj-$(CONFIG_QFW) += qfw.o
+ obj-$(CONFIG_RAM) += ram.o
+ obj-y += regmap.o
+diff --git a/test/dm/ffa.c b/test/dm/ffa.c
+new file mode 100644
+index 000000000000..b937cea57b80
+--- /dev/null
++++ b/test/dm/ffa.c
+@@ -0,0 +1,56 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * Test for UCLASS_FFA  class
++ *
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <dm/test.h>
++#include <test/test.h>
++#include <test/ut.h>
++#include <arm_ffa_helper.h>
++#include "ffa.h"
++
++/* Basic test of 'armffa' command */
++static int dm_test_armffa_cmd(struct unit_test_state *uts)
++{
++	ut_assertok(ffa_helper_init_device());
++
++	ut_assertok(console_record_reset_enable());
++
++	/* armffa getpart <UUID> */
++	ut_assertok(run_command("armffa getpart " SE_PROXY_PARTITION_UUID, 0));
++	ut_assert_console_end();
++
++	/* armffa ping <ID> */
++	ut_assertok(run_command("armffa ping " SE_PROXY_PARTITION_ID, 0));
++	ut_assert_console_end();
++
++	/* armffa devlist */
++	ut_assertok(run_command("armffa devlist", 0));
++	ut_assert_console_end();
++
++	return 0;
++}
++
++DM_TEST(dm_test_armffa_cmd, UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT | UT_TESTF_CONSOLE_REC);
++
++static int  test_ffa_msg_send_direct_req(void)
++{
++	char *const argv[1] = {SE_PROXY_PARTITION_ID}; /* Corstone1000 SE Proxy ID */
++
++	return do_ffa_msg_send_direct_req(NULL, 0, 1, argv);
++}
++
++/* Basic test of the FFA uclass */
++static int dm_test_ffa_uclass(struct unit_test_state *uts)
++{
++	ut_assertok(ffa_init_device());
++	ut_assertok(test_ffa_msg_send_direct_req());
++	return 0;
++}
++
++DM_TEST(dm_test_ffa_uclass, UT_TESTF_SCAN_PDATA | UT_TESTF_SCAN_FDT);
+diff --git a/test/dm/ffa.h b/test/dm/ffa.h
+new file mode 100644
+index 000000000000..a0802bd6928a
+--- /dev/null
++++ b/test/dm/ffa.h
+@@ -0,0 +1,19 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2021 ARM Limited
++ * Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
++ */
++
++#ifndef __TEST_DM_FFA_H
++#define __TEST_DM_FFA_H
++
++#define SE_PROXY_PARTITION_ID "0x8002"
++#define SE_PROXY_PARTITION_UUID "46bb39d1-b4d9-45b5-88ff-040027dab249"
++
++/**
++ * do_ffa_msg_send_direct_req - implementation of the ping subcommand
++ */
++int  do_ffa_msg_send_direct_req(struct cmd_tbl *cmdtp, int flag, int argc,
++				char *const argv[]);
++
++#endif /*__TEST_DM_FFA_H */
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0010-arm_ffa-corstone1000-enable-FF-A-and-MM-support.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0010-arm_ffa-corstone1000-enable-FF-A-and-MM-support.patch
new file mode 100644
index 0000000..bc96fc4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0010-arm_ffa-corstone1000-enable-FF-A-and-MM-support.patch
@@ -0,0 +1,57 @@
+From ce6598d255113458fd5c9d19bb7469b721e37f6f Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Tue, 2 Nov 2021 16:44:39 +0000
+Subject: [PATCH 10/27] arm_ffa: corstone1000: enable FF-A and MM support
+
+This commit allows corstone1000 platform to perform
+MM communication between u-boot and the secure world
+using FF-A transport.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ configs/corstone1000_defconfig |  1 +
+ include/configs/corstone1000.h | 15 +++++++++++++++
+ 2 files changed, 16 insertions(+)
+
+diff --git a/configs/corstone1000_defconfig b/configs/corstone1000_defconfig
+index e573fe6fe6a2..b042d4e49419 100644
+--- a/configs/corstone1000_defconfig
++++ b/configs/corstone1000_defconfig
+@@ -44,6 +44,7 @@ CONFIG_USB=y
+ CONFIG_DM_USB=y
+ CONFIG_USB_ISP1760=y
+ CONFIG_USB_STORAGE=y
++CONFIG_ARM_FFA_TRANSPORT=y
+ CONFIG_EFI_MM_COMM_TEE=y
+ # CONFIG_OPTEE is not set
+ # CONFIG_GENERATE_SMBIOS_TABLE is not set
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index 8ba0effb0ab2..afc9ccfc192b 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -14,6 +14,21 @@
+ 
+ #include <linux/sizes.h>
+ 
++/* MM SP UUID binary data (little-endian format) */
++#define MM_SP_UUID_DATA	\
++	0xed, 0x32, 0xd5, 0x33,	\
++	0x99, 0xe6, 0x42, 0x09,	\
++	0x9c, 0xc0, 0x2d, 0x72,	\
++	0xcd, 0xd9, 0x98, 0xa7
++
++#define FFA_SHARED_MM_BUFFER_SIZE	SZ_4K /* 4 KB */
++
++/*
++ * shared buffer physical address used for communication between
++ * u-boot and the MM SP
++ */
++#define FFA_SHARED_MM_BUFFER_ADDR	(0x023F8000)
++
+ #define CONFIG_SYS_INIT_SP_ADDR		(CONFIG_SYS_SDRAM_BASE + 0x03f00000)
+ #define CONFIG_SKIP_LOWLEVEL_INIT
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0011-efi-corstone1000-introduce-EFI-capsule-update.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0011-efi-corstone1000-introduce-EFI-capsule-update.patch
new file mode 100644
index 0000000..cb24ec3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0011-efi-corstone1000-introduce-EFI-capsule-update.patch
@@ -0,0 +1,359 @@
+From e70d0128090158872847b82b82cdbcf0e2f13885 Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Thu, 11 Nov 2021 16:27:59 +0000
+Subject: [PATCH 11/27] efi: corstone1000: introduce EFI capsule update
+
+This commit provides capsule update feature for Corstone1000.
+
+This feature is available before and after ExitBootServices().
+
+A platform specific capsule buffer is allocated. This buffer
+is physically contiguous and allocated at the start of the DDR
+memory after u-boot relocation to the end of DDR.
+
+The capsule buffer is shared between u-boot and the secure world.
+On UpdateCapsule() , capsule data is copied to the buffer
+and a buffer ready event is generated using FF-A transport.
+
+On ExitBootServices() a kernel started event is sent to the
+SE Proxy FW update service. This event is generated on each boot.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ include/configs/corstone1000.h |  18 +++++
+ include/efi_loader.h           |   4 +-
+ lib/efi_loader/efi_boottime.c  |  47 ++++++++++++
+ lib/efi_loader/efi_capsule.c   | 135 ++++++++++++++++++++++++++++++++-
+ lib/efi_loader/efi_setup.c     |  15 ++++
+ 5 files changed, 215 insertions(+), 4 deletions(-)
+
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index afc9ccfc192b..a400cdef69d0 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -14,6 +14,24 @@
+ 
+ #include <linux/sizes.h>
+ 
++/* The SE Proxy partition ID*/
++#define CORSTONE1000_SEPROXY_PART_ID		(0x8002)
++
++/* Update service ID provided by  the SE Proxy secure partition*/
++#define CORSTONE1000_SEPROXY_UPDATE_SVC_ID	(0x4)
++
++/* Notification events used with SE Proxy update service */
++#define CORSTONE1000_BUFFER_READY_EVT		(0x1)
++#define CORSTONE1000_KERNEL_STARTED_EVT		(0x2)
++
++/* Size in 4KB pages of the EFI capsule buffer */
++#define CORSTONE1000_CAPSULE_BUFFER_SIZE	(8192) /* 32 MB */
++
++/* Capsule GUID */
++#define EFI_CORSTONE1000_CAPSULE_ID_GUID \
++	EFI_GUID(0x3a770ddc, 0x409b, 0x48b2, 0x81, 0x41, \
++		 0x93, 0xb7, 0xc6, 0x0b, 0x20, 0x9e)
++
+ /* MM SP UUID binary data (little-endian format) */
+ #define MM_SP_UUID_DATA	\
+ 	0xed, 0x32, 0xd5, 0x33,	\
+diff --git a/include/efi_loader.h b/include/efi_loader.h
+index 126db279dd3e..01b432e6184b 100644
+--- a/include/efi_loader.h
++++ b/include/efi_loader.h
+@@ -965,11 +965,11 @@ extern const struct efi_firmware_management_protocol efi_fmp_fit;
+ extern const struct efi_firmware_management_protocol efi_fmp_raw;
+ 
+ /* Capsule update */
+-efi_status_t EFIAPI efi_update_capsule(
++efi_status_t __efi_runtime EFIAPI efi_update_capsule(
+ 		struct efi_capsule_header **capsule_header_array,
+ 		efi_uintn_t capsule_count,
+ 		u64 scatter_gather_list);
+-efi_status_t EFIAPI efi_query_capsule_caps(
++efi_status_t __efi_runtime EFIAPI efi_query_capsule_caps(
+ 		struct efi_capsule_header **capsule_header_array,
+ 		efi_uintn_t capsule_count,
+ 		u64 *maximum_capsule_size,
+diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
+index cffa2c69d621..5c77a40c3ebe 100644
+--- a/lib/efi_loader/efi_boottime.c
++++ b/lib/efi_loader/efi_boottime.c
+@@ -2096,6 +2096,44 @@ static void efi_exit_caches(void)
+ #endif
+ }
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++/**
++ * efi_corstone1000_kernel_started_event - notifies SE Proxy FW update service
++ *
++ * This function notifies the SE Proxy update service that the kernel has already started
++ *
++ * Return:
++ *
++ * 0: on success, otherwise failure
++ */
++static int efi_corstone1000_kernel_started_event(void)
++{
++	struct ffa_interface_data func_data = {0};
++	struct ffa_send_direct_data msg = {0};
++	u16 part_id = CORSTONE1000_SEPROXY_PART_ID;
++
++	log_debug("[%s]\n", __func__);
++
++	/*
++	 * telling the driver which partition to use
++	 */
++	func_data.data0_size = sizeof(part_id);
++	func_data.data0 = &part_id;
++
++	/*
++	 * setting the kernel started  event arguments
++	 */
++	msg.a3 = CORSTONE1000_SEPROXY_UPDATE_SVC_ID;
++	msg.a5 = CORSTONE1000_KERNEL_STARTED_EVT;
++
++	func_data.data1_size = sizeof(msg);
++	func_data.data1 = &msg;
++
++	return ffa_helper_msg_send_direct_req(&func_data);
++}
++
++#endif
++
+ /**
+  * efi_exit_boot_services() - stop all boot services
+  * @image_handle: handle of the loaded image
+@@ -2209,6 +2247,15 @@ static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
+ 	/* Recalculate CRC32 */
+ 	efi_update_table_header_crc32(&systab.hdr);
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++	/* Notifying SE Proxy FW update service */
++	ffa_ret = efi_corstone1000_kernel_started_event();
++	if (ffa_ret)
++		debug("[efi_boottime][ERROR]: Failure to notify SE Proxy FW update service\n");
++	else
++		debug("[efi_boottime][INFO]: SE Proxy FW update service notified\n");
++#endif
++
+ 	/* Give the payload some time to boot */
+ 	efi_set_watchdog(0);
+ 	WATCHDOG_RESET();
+diff --git a/lib/efi_loader/efi_capsule.c b/lib/efi_loader/efi_capsule.c
+index f00440163d41..c100c1b95298 100644
+--- a/lib/efi_loader/efi_capsule.c
++++ b/lib/efi_loader/efi_capsule.c
+@@ -24,6 +24,14 @@
+ #include <crypto/pkcs7_parser.h>
+ #include <linux/err.h>
+ 
++#ifdef CONFIG_TARGET_CORSTONE1000
++#include <arm_ffa_helper.h>
++#include <cpu_func.h>
++
++void *__efi_runtime_data corstone1000_capsule_buf; /* capsule shared buffer virtual address */
++efi_guid_t corstone1000_capsule_guid = EFI_CORSTONE1000_CAPSULE_ID_GUID;
++#endif
++
+ DECLARE_GLOBAL_DATA_PTR;
+ 
+ const efi_guid_t efi_guid_capsule_report = EFI_CAPSULE_REPORT_GUID;
+@@ -509,6 +517,89 @@ static efi_status_t efi_capsule_update_firmware(
+ }
+ #endif /* CONFIG_EFI_CAPSULE_FIRMWARE_MANAGEMENT */
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++
++/**
++ * efi_corstone1000_alloc_capsule_shared_buf - allocate capsule shared buffer
++ * @capsule_image_size:	The capsule data (header + payload)
++ *
++ * This function allocates the physically contiguous buffer shared between u-boot
++ * and the secure world. On UpdateCapsule() capsule data is copied to the buffer
++ * and a door bell event is generated.
++ * The buffer is allocated at the start of the DDR memory after u-boot has been relocated
++ * to the end of DDR.
++ *
++ * Return:
++ *
++ * 0: on success, otherwise failure
++ */
++efi_status_t efi_corstone1000_alloc_capsule_shared_buf(void)
++{
++	efi_status_t efi_ret;
++	u64 ram_base = CONFIG_SYS_SDRAM_BASE;
++
++	log_debug("[%s]\n", __func__);
++
++	efi_ret = efi_allocate_pages(EFI_ALLOCATE_ADDRESS,
++				     EFI_RUNTIME_SERVICES_DATA,
++				     CORSTONE1000_CAPSULE_BUFFER_SIZE,
++				     &ram_base);
++
++	if (efi_ret != EFI_SUCCESS) {
++		corstone1000_capsule_buf = NULL;
++		log_err("EFI: Corstone1000: Allocating capsule shared buffer error (%d)\n"
++			, (int)efi_ret);
++		return efi_ret;
++	}
++
++	log_info("EFI: Corstone1000: Capsule shared buffer at 0x%x , size %d pages\n"
++		, (unsigned int)ram_base,
++		CORSTONE1000_CAPSULE_BUFFER_SIZE);
++
++	corstone1000_capsule_buf = (void *)map_sysmem((phys_addr_t)ram_base, 0);
++
++	return EFI_SUCCESS;
++}
++
++/**
++ * efi_corstone1000_buffer_ready_event - issue door bell event
++ * @capsule_image_size:	The capsule data (header + payload)
++ *
++ * This function notifies the SE Proxy update service that capsule data is available
++ * in the capsule shared buffer.
++ *
++ * Return:
++ *
++ * 0: on success, otherwise failure
++ */
++static int __efi_runtime efi_corstone1000_buffer_ready_event(u32 capsule_image_size)
++{
++	struct ffa_interface_data func_data = {0};
++	struct ffa_send_direct_data msg = {0};
++	u16 part_id = CORSTONE1000_SEPROXY_PART_ID;
++
++	log_debug("[%s]\n", __func__);
++
++	/*
++	 * telling the driver which partition to use
++	 */
++	func_data.data0_size = sizeof(part_id);
++	func_data.data0 = &part_id;
++
++	/*
++	 * setting the buffer ready event arguments
++	 */
++	msg.a3 = CORSTONE1000_SEPROXY_UPDATE_SVC_ID;
++	msg.a4 = capsule_image_size;
++	msg.a5 = CORSTONE1000_BUFFER_READY_EVT;
++
++	func_data.data1_size = sizeof(msg);
++	func_data.data1 = &msg;
++
++	return ffa_helper_msg_send_direct_req(&func_data);
++}
++#endif
++
+ /**
+  * efi_update_capsule() - process information from operating system
+  * @capsule_header_array:	Array of virtual address pointers
+@@ -522,7 +613,7 @@ static efi_status_t efi_capsule_update_firmware(
+  *
+  * Return:			status code
+  */
+-efi_status_t EFIAPI efi_update_capsule(
++efi_status_t __efi_runtime EFIAPI efi_update_capsule(
+ 		struct efi_capsule_header **capsule_header_array,
+ 		efi_uintn_t capsule_count,
+ 		u64 scatter_gather_list)
+@@ -539,6 +630,13 @@ efi_status_t EFIAPI efi_update_capsule(
+ 		goto out;
+ 	}
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++		if (capsule_count != 1 || !corstone1000_capsule_buf) {
++			ret = EFI_INVALID_PARAMETER;
++			goto out;
++		}
++#endif
++
+ 	ret = EFI_SUCCESS;
+ 	for (i = 0, capsule = *capsule_header_array; i < capsule_count;
+ 	     i++, capsule = *(++capsule_header_array)) {
+@@ -551,6 +649,39 @@ efi_status_t EFIAPI efi_update_capsule(
+ 
+ 		log_debug("Capsule[%d] (guid:%pUs)\n",
+ 			  i, &capsule->capsule_guid);
++
++#if CONFIG_IS_ENABLED(TARGET_CORSTONE1000)
++		if (guidcmp(&corstone1000_capsule_guid, &capsule->capsule_guid)) {
++			ret = EFI_INVALID_PARAMETER;
++			log_err("Corstone1000: Invalid capsule GUID\n");
++			goto out;
++		}
++
++		if (efi_size_in_pages(capsule->capsule_image_size) >
++		    CORSTONE1000_CAPSULE_BUFFER_SIZE) {
++			log_err("Corstone1000: Capsule data size exceeds the shared buffer size\n");
++			ret = EFI_BUFFER_TOO_SMALL;
++			goto out;
++		}
++
++		/* copy the data to the contiguous buffer */
++		efi_memcpy_runtime(corstone1000_capsule_buf, capsule, capsule->capsule_image_size);
++
++		/* invalidate the data cache */
++		invalidate_dcache_all();
++
++		/* issue buffer ready event */
++		ret = efi_corstone1000_buffer_ready_event(capsule->capsule_image_size);
++		if (ret) {
++			log_err("EFI: Corstone1000: Buffer ready event error (%d)\n", (int)ret);
++			ret = EFI_DEVICE_ERROR;
++		} else {
++			ret = EFI_SUCCESS;
++		}
++
++		goto out;
++#endif
++
+ 		if (!guidcmp(&capsule->capsule_guid,
+ 			     &efi_guid_firmware_management_capsule_id)) {
+ 			ret  = efi_capsule_update_firmware(capsule);
+@@ -589,7 +720,7 @@ out:
+  *
+  * Return:			status code
+  */
+-efi_status_t EFIAPI efi_query_capsule_caps(
++efi_status_t __efi_runtime EFIAPI efi_query_capsule_caps(
+ 		struct efi_capsule_header **capsule_header_array,
+ 		efi_uintn_t capsule_count,
+ 		u64 *maximum_capsule_size,
+diff --git a/lib/efi_loader/efi_setup.c b/lib/efi_loader/efi_setup.c
+index eee54e48784f..989380d4f8cd 100644
+--- a/lib/efi_loader/efi_setup.c
++++ b/lib/efi_loader/efi_setup.c
+@@ -16,6 +16,13 @@
+ 
+ efi_status_t efi_obj_list_initialized = OBJ_LIST_NOT_INITIALIZED;
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++/**
++ * efi_corstone1000_alloc_capsule_shared_buf - allocate capsule shared buffer
++ */
++extern efi_status_t efi_corstone1000_alloc_capsule_shared_buf(void);
++#endif
++
+ /*
+  * Allow unaligned memory access.
+  *
+@@ -128,6 +135,14 @@ static efi_status_t efi_init_capsule(void)
+ {
+ 	efi_status_t ret = EFI_SUCCESS;
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++	ret = efi_corstone1000_alloc_capsule_shared_buf();
++	if (ret != EFI_SUCCESS) {
++		printf("EFI: Corstone-1000: cannot allocate caspsule shared buffer\n");
++		return ret;
++	}
++#endif
++
+ 	if (IS_ENABLED(CONFIG_EFI_HAVE_CAPSULE_UPDATE)) {
+ 		ret = efi_set_variable_int(u"CapsuleMax",
+ 					   &efi_guid_capsule_report,
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0012-corstone1000-Update-FFA-shared-buffer-address.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0012-corstone1000-Update-FFA-shared-buffer-address.patch
new file mode 100644
index 0000000..60dc850
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0012-corstone1000-Update-FFA-shared-buffer-address.patch
@@ -0,0 +1,37 @@
+From b2d752b4bbd5b2dc4cb22d2d652a261287505926 Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Wed, 17 Nov 2021 15:28:06 +0000
+Subject: [PATCH 12/27] corstone1000: Update FFA shared buffer address
+
+FFA shared buffer address changed to 0x02000000.
+
+The existing address 0x023F8000 is currently being used by
+Optee so the virtual address returned to the SMM gateway is 0x0000.
+So the buffer is moved to 0x02000000.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+%% original patch: 0025-Update-FFA-shared-buffer-address.patch
+
+%% original patch: 0025-Update-FFA-shared-buffer-address.patch
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ include/configs/corstone1000.h | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index a400cdef69d0..db0f91335cef 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -45,7 +45,7 @@
+  * shared buffer physical address used for communication between
+  * u-boot and the MM SP
+  */
+-#define FFA_SHARED_MM_BUFFER_ADDR	(0x023F8000)
++#define FFA_SHARED_MM_BUFFER_ADDR	(0x02000000)
+ 
+ #define CONFIG_SYS_INIT_SP_ADDR		(CONFIG_SYS_SDRAM_BASE + 0x03f00000)
+ #define CONFIG_SKIP_LOWLEVEL_INIT
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0013-corstone1000-Make-sure-shared-buffer-contents-are-no.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0013-corstone1000-Make-sure-shared-buffer-contents-are-no.patch
new file mode 100644
index 0000000..2495538
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0013-corstone1000-Make-sure-shared-buffer-contents-are-no.patch
@@ -0,0 +1,52 @@
+From 67a755f74716068cfd44a8897c31151fe9ee4328 Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Thu, 18 Nov 2021 16:42:59 +0000
+Subject: [PATCH 13/27] corstone1000: Make sure shared buffer contents are not
+ cached
+
+After updating the shared buffer, it is required to flush the cache
+to ensure that the secure world sees expected the shared buffer
+contents.
+
+The MM communication shared buffer is configured in device region of optee
+which has cache disabled. So we need to invalidate the cache every time we
+update the buffer on uboot otherwise the secure world does not see the
+accurate values.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+%% original patch: 0027-Make-sure-shared-buffer-contents-are-not-cached.patch
+
+%% original patch: 0027-Make-sure-shared-buffer-contents-are-not-cached.patch
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_variable_tee.c | 6 ++++++
+ 1 file changed, 6 insertions(+)
+
+diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
+index 9cb8cfb9c779..b6be2b54a030 100644
+--- a/lib/efi_loader/efi_variable_tee.c
++++ b/lib/efi_loader/efi_variable_tee.c
+@@ -22,6 +22,7 @@
+ #if (IS_ENABLED(CONFIG_ARM_FFA_TRANSPORT))
+ 
+ #include <arm_ffa_helper.h>
++#include <cpu_func.h>
+ #include <mapmem.h>
+ 
+ /* MM return codes */
+@@ -335,6 +336,11 @@ static efi_status_t __efi_runtime ffa_mm_communicate(void *comm_buf, ulong comm_
+ 	virt_shared_buf = (void *)map_sysmem((phys_addr_t)FFA_SHARED_MM_BUFFER_ADDR, 0);
+ 	efi_memcpy_runtime(virt_shared_buf, comm_buf, tx_data_size);
+ 
++	/* The secure world has cache disabled for device region which we use for shared buffer
++	So, the secure world reads the data from DDR. Let's flush the cache so the DDR is
++	updated with the latest data */
++	invalidate_dcache_all();
++
+ 	/* Announce there is data in the shared buffer */
+ 
+ 	ffa_ret = ffa_notify_mm_sp();
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0014-arm-corstone1000-fix-unrecognized-filesystem-type.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0014-arm-corstone1000-fix-unrecognized-filesystem-type.patch
new file mode 100644
index 0000000..fa201eb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0014-arm-corstone1000-fix-unrecognized-filesystem-type.patch
@@ -0,0 +1,30 @@
+From e2463e3ef52260b38131085c0901de8708d52693 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Fri, 4 Mar 2022 15:56:09 +0000
+Subject: [PATCH 14/27] arm: corstone1000: fix unrecognized filesystem type
+
+Some usb sticks are not recognized by usb, just add a
+delay before checking status.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ common/usb_storage.c | 3 +++
+ 1 file changed, 3 insertions(+)
+
+diff --git a/common/usb_storage.c b/common/usb_storage.c
+index c9e2d7343ce2..ae72338323ba 100644
+--- a/common/usb_storage.c
++++ b/common/usb_storage.c
+@@ -769,6 +769,9 @@ static int usb_stor_BBB_transport(struct scsi_cmd *srb, struct us_data *us)
+ st:
+ 	retry = 0;
+ again:
++	if (srb->cmd[0] == SCSI_TST_U_RDY)
++		mdelay(100);
++
+ 	debug("STATUS phase\n");
+ 	result = usb_bulk_msg(us->pusb_dev, pipein, csw, UMASS_BBB_CSW_SIZE,
+ 				&actlen, USB_CNTL_TIMEOUT*5);
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0015-efi_capsule-corstone1000-pass-interface-id-and-buffe.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0015-efi_capsule-corstone1000-pass-interface-id-and-buffe.patch
new file mode 100644
index 0000000..0d1912c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0015-efi_capsule-corstone1000-pass-interface-id-and-buffe.patch
@@ -0,0 +1,73 @@
+From 81bf9ed7e8e858cef13cfc3d1435c44445e523df Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 10 Dec 2021 20:03:35 +0000
+Subject: [PATCH 15/27] efi_capsule: corstone1000: pass interface id and buffer
+ event id using register w4
+
+Initially the interface/event IDs are passed to the SP using register
+w3 and w5.
+
+Now the SE proxy SP requires this arguments to be in register w4.
+
+This change is to pass interface ID(31:16) and event ID(15:0)
+to SE proxy SP to trigger an event to secure enclave about
+firmware update.
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ include/configs/corstone1000.h |  6 ++++++
+ lib/efi_loader/efi_capsule.c   | 11 +++++++----
+ 2 files changed, 13 insertions(+), 4 deletions(-)
+
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index db0f91335cef..a7445e61348b 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -24,6 +24,12 @@
+ #define CORSTONE1000_BUFFER_READY_EVT		(0x1)
+ #define CORSTONE1000_KERNEL_STARTED_EVT		(0x2)
+ 
++#define PREP_SEPROXY_SVC_ID_MASK	GENMASK(31, 16)
++#define PREP_SEPROXY_SVC_ID(x)	 (FIELD_PREP(PREP_SEPROXY_SVC_ID_MASK, (x)))
++
++#define PREP_SEPROXY_EVT_MASK		GENMASK(15, 0)
++#define PREP_SEPROXY_EVT(x)	(FIELD_PREP(PREP_SEPROXY_EVT_MASK, (x)))
++
+ /* Size in 4KB pages of the EFI capsule buffer */
+ #define CORSTONE1000_CAPSULE_BUFFER_SIZE	(8192) /* 32 MB */
+ 
+diff --git a/lib/efi_loader/efi_capsule.c b/lib/efi_loader/efi_capsule.c
+index c100c1b95298..17d769803b2a 100644
+--- a/lib/efi_loader/efi_capsule.c
++++ b/lib/efi_loader/efi_capsule.c
+@@ -27,6 +27,8 @@
+ #ifdef CONFIG_TARGET_CORSTONE1000
+ #include <arm_ffa_helper.h>
+ #include <cpu_func.h>
++#include <linux/bitfield.h>
++#include <linux/bitops.h>
+ 
+ void *__efi_runtime_data corstone1000_capsule_buf; /* capsule shared buffer virtual address */
+ efi_guid_t corstone1000_capsule_guid = EFI_CORSTONE1000_CAPSULE_ID_GUID;
+@@ -587,11 +589,12 @@ static int __efi_runtime efi_corstone1000_buffer_ready_event(u32 capsule_image_s
+ 	func_data.data0 = &part_id;
+ 
+ 	/*
+-	 * setting the buffer ready event arguments
++	 * setting the buffer ready event arguments in register w4:
++	 * - capsule update interface ID (31:16)
++	 * - the buffer ready event ID (15:0)
+ 	 */
+-	msg.a3 = CORSTONE1000_SEPROXY_UPDATE_SVC_ID;
+-	msg.a4 = capsule_image_size;
+-	msg.a5 = CORSTONE1000_BUFFER_READY_EVT;
++	msg.a4 = PREP_SEPROXY_SVC_ID(CORSTONE1000_SEPROXY_UPDATE_SVC_ID) |
++		PREP_SEPROXY_EVT(CORSTONE1000_BUFFER_READY_EVT);
+ 
+ 	func_data.data1_size = sizeof(msg);
+ 	func_data.data1 = &msg;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0016-efi_boottime-corstone1000-pass-interface-id-and-kern.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0016-efi_boottime-corstone1000-pass-interface-id-and-kern.patch
new file mode 100644
index 0000000..f460fad
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0016-efi_boottime-corstone1000-pass-interface-id-and-kern.patch
@@ -0,0 +1,57 @@
+From 10d0ffc26ddcecd83921c2b3b37cb4eff54a154f Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 10 Dec 2021 20:10:41 +0000
+Subject: [PATCH 16/27] efi_boottime: corstone1000: pass interface id and
+ kernel event id using register w4
+
+Initially the interface/event IDs are passed to the SP using register
+w3 and w5.
+
+Now the SE proxy SP requires this arguments to be in register w4.
+
+This change is to pass interface ID(31:16) and kernel started
+event ID(15:0) to SE proxy SP to trigger an event to
+secure enclave just before ExitbootService().
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_boottime.c | 13 ++++++++++---
+ 1 file changed, 10 insertions(+), 3 deletions(-)
+
+diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
+index 5c77a40c3ebe..b58a8c98fd05 100644
+--- a/lib/efi_loader/efi_boottime.c
++++ b/lib/efi_loader/efi_boottime.c
+@@ -27,6 +27,11 @@
+ #include <arm_ffa_helper.h>
+ #endif
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++#include <linux/bitfield.h>
++#include <linux/bitops.h>
++#endif
++
+ DECLARE_GLOBAL_DATA_PTR;
+ 
+ /* Task priority level */
+@@ -2121,10 +2126,12 @@ static int efi_corstone1000_kernel_started_event(void)
+ 	func_data.data0 = &part_id;
+ 
+ 	/*
+-	 * setting the kernel started  event arguments
++	 * setting the kernel started  event arguments:
++	 * setting capsule update interface ID(31:16)
++	 * the kernel started event ID(15:0)
+ 	 */
+-	msg.a3 = CORSTONE1000_SEPROXY_UPDATE_SVC_ID;
+-	msg.a5 = CORSTONE1000_KERNEL_STARTED_EVT;
++	msg.a4 = PREP_SEPROXY_SVC_ID(CORSTONE1000_SEPROXY_UPDATE_SVC_ID) |
++	PREP_SEPROXY_EVT(CORSTONE1000_KERNEL_STARTED_EVT);
+ 
+ 	func_data.data1_size = sizeof(msg);
+ 	func_data.data1 = &msg;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0017-efi_loader-corstone1000-remove-guid-check-from-corst.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0017-efi_loader-corstone1000-remove-guid-check-from-corst.patch
new file mode 100644
index 0000000..fa6ab32
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0017-efi_loader-corstone1000-remove-guid-check-from-corst.patch
@@ -0,0 +1,52 @@
+From c463798489e41725f8ba33debeedc7c4011cda38 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Sat, 11 Dec 2021 13:23:55 +0000
+Subject: [PATCH 17/27] efi_loader: corstone1000: remove guid check from
+ corstone1000 config option
+
+Use generic fmp guid and no separte check is required for
+CORSTONE1000 target.
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_capsule.c | 16 +---------------
+ 1 file changed, 1 insertion(+), 15 deletions(-)
+
+diff --git a/lib/efi_loader/efi_capsule.c b/lib/efi_loader/efi_capsule.c
+index 17d769803b2a..939040d2755e 100644
+--- a/lib/efi_loader/efi_capsule.c
++++ b/lib/efi_loader/efi_capsule.c
+@@ -654,12 +654,6 @@ efi_status_t __efi_runtime EFIAPI efi_update_capsule(
+ 			  i, &capsule->capsule_guid);
+ 
+ #if CONFIG_IS_ENABLED(TARGET_CORSTONE1000)
+-		if (guidcmp(&corstone1000_capsule_guid, &capsule->capsule_guid)) {
+-			ret = EFI_INVALID_PARAMETER;
+-			log_err("Corstone1000: Invalid capsule GUID\n");
+-			goto out;
+-		}
+-
+ 		if (efi_size_in_pages(capsule->capsule_image_size) >
+ 		    CORSTONE1000_CAPSULE_BUFFER_SIZE) {
+ 			log_err("Corstone1000: Capsule data size exceeds the shared buffer size\n");
+@@ -685,15 +679,7 @@ efi_status_t __efi_runtime EFIAPI efi_update_capsule(
+ 		goto out;
+ #endif
+ 
+-		if (!guidcmp(&capsule->capsule_guid,
+-			     &efi_guid_firmware_management_capsule_id)) {
+-			ret  = efi_capsule_update_firmware(capsule);
+-		} else {
+-			log_err("Unsupported capsule type: %pUs\n",
+-				&capsule->capsule_guid);
+-			ret = EFI_UNSUPPORTED;
+-		}
+-
++		ret  = efi_capsule_update_firmware(capsule);
+ 		if (ret != EFI_SUCCESS)
+ 			goto out;
+ 	}
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0018-arm_ffa-removing-the-cast-when-using-binary-OR-on-FI.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0018-arm_ffa-removing-the-cast-when-using-binary-OR-on-FI.patch
new file mode 100644
index 0000000..4ee10a0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0018-arm_ffa-removing-the-cast-when-using-binary-OR-on-FI.patch
@@ -0,0 +1,40 @@
+From 1cfca60850727448bdbfe720d98d9e0d4523f6aa Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Sat, 11 Dec 2021 21:05:10 +0000
+Subject: [PATCH 18/27] arm_ffa: removing the cast when using binary OR on
+ FIELD_PREP macros
+
+When the GENMASK used is above 16-bits wide a u16 cast will cause
+loss of data.
+
+This commit fixes that.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ drivers/arm-ffa/arm_ffa_prv.h | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/drivers/arm-ffa/arm_ffa_prv.h b/drivers/arm-ffa/arm_ffa_prv.h
+index 38ea4ba83efc..d0db3ef508a1 100644
+--- a/drivers/arm-ffa/arm_ffa_prv.h
++++ b/drivers/arm-ffa/arm_ffa_prv.h
+@@ -40,13 +40,13 @@
+ 
+ #define PREP_SELF_ENDPOINT_ID_MASK		GENMASK(31, 16)
+ #define PREP_SELF_ENDPOINT_ID(x)		\
+-			((u16)(FIELD_PREP(PREP_SELF_ENDPOINT_ID_MASK, (x))))
++			(FIELD_PREP(PREP_SELF_ENDPOINT_ID_MASK, (x)))
+ 
+ /* Partition endpoint ID mask  (partition with which u-boot communicates with) */
+ 
+ #define PREP_PART_ENDPOINT_ID_MASK		GENMASK(15, 0)
+ #define PREP_PART_ENDPOINT_ID(x)		\
+-			((u16)(FIELD_PREP(PREP_PART_ENDPOINT_ID_MASK, (x))))
++			(FIELD_PREP(PREP_PART_ENDPOINT_ID_MASK, (x)))
+ 
+ /* The FF-A SMC function prototype definition */
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0019-Return-proper-error-code-when-rx-buffer-is-larger.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0019-Return-proper-error-code-when-rx-buffer-is-larger.patch
new file mode 100644
index 0000000..21a89a4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0019-Return-proper-error-code-when-rx-buffer-is-larger.patch
@@ -0,0 +1,31 @@
+From 7db27eeaba0fd5ddb1e49977bb7e342a1980aa3d Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Sun, 12 Dec 2021 17:51:17 +0000
+Subject: [PATCH 19/27] Return proper error code when rx buffer is larger
+
+ffa_mm_communicate should return EFI_BUFFER_TOO_SMALL when
+the buffer received from the secure world is larger than the
+comm buffer as this value is forwarded by mm_communicate.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_variable_tee.c | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
+index b6be2b54a030..38655a9dbb7c 100644
+--- a/lib/efi_loader/efi_variable_tee.c
++++ b/lib/efi_loader/efi_variable_tee.c
+@@ -358,7 +358,7 @@ static efi_status_t __efi_runtime ffa_mm_communicate(void *comm_buf, ulong comm_
+ 
+ 		if (rx_data_size > comm_buf_size) {
+ 			unmap_sysmem(virt_shared_buf);
+-			return EFI_OUT_OF_RESOURCES;
++			return EFI_BUFFER_TOO_SMALL;
+ 		}
+ 
+ 		efi_memcpy_runtime(comm_buf, virt_shared_buf, rx_data_size);
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0020-Use-correct-buffer-size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0020-Use-correct-buffer-size.patch
new file mode 100644
index 0000000..54328a7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0020-Use-correct-buffer-size.patch
@@ -0,0 +1,40 @@
+From 9ad9ead58e8e9e4f9e7a283c916421b443b424ce Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Mon, 13 Dec 2021 15:25:23 +0000
+Subject: [PATCH 20/27] Use correct buffer size
+
+The comm buffer created has additional 4 bytes length which
+needs to be trimmed. This change will reduce the size of the
+comm buffer to what is expected.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ include/mm_communication.h | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/include/mm_communication.h b/include/mm_communication.h
+index e65fbde60d0a..bb9919095649 100644
+--- a/include/mm_communication.h
++++ b/include/mm_communication.h
+@@ -123,7 +123,7 @@ struct __packed efi_mm_communicate_header {
+  *
+  * Defined in EDK2 as SMM_VARIABLE_COMMUNICATE_HEADER.
+  */
+-struct smm_variable_communicate_header {
++struct __packed smm_variable_communicate_header {
+ 	efi_uintn_t  function;
+ 	efi_status_t ret_status;
+ 	u8           data[];
+@@ -145,7 +145,7 @@ struct smm_variable_communicate_header {
+  * Defined in EDK2 as SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE.
+  *
+  */
+-struct smm_variable_access {
++struct __packed smm_variable_access {
+ 	efi_guid_t  guid;
+ 	efi_uintn_t data_size;
+ 	efi_uintn_t name_size;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0021-Update-comm_buf-when-EFI_BUFFER_TOO_SMALL.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0021-Update-comm_buf-when-EFI_BUFFER_TOO_SMALL.patch
new file mode 100644
index 0000000..c7ac38f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0021-Update-comm_buf-when-EFI_BUFFER_TOO_SMALL.patch
@@ -0,0 +1,30 @@
+From b81214dea7056c3877aa9eb775557dc4702660ec Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Sun, 12 Dec 2021 17:58:08 +0000
+Subject: [PATCH 21/27] Update comm_buf when EFI_BUFFER_TOO_SMALL
+
+When the received buffer is larger than the comm buffer,
+the contents of the shared buffer which can fit in the
+comm buffer should be read before returning.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_variable_tee.c | 1 +
+ 1 file changed, 1 insertion(+)
+
+diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
+index 38655a9dbb7c..67743d1f8fce 100644
+--- a/lib/efi_loader/efi_variable_tee.c
++++ b/lib/efi_loader/efi_variable_tee.c
+@@ -357,6 +357,7 @@ static efi_status_t __efi_runtime ffa_mm_communicate(void *comm_buf, ulong comm_
+ 			sizeof(size_t);
+ 
+ 		if (rx_data_size > comm_buf_size) {
++			efi_memcpy_runtime(comm_buf, virt_shared_buf, comm_buf_size);
+ 			unmap_sysmem(virt_shared_buf);
+ 			return EFI_BUFFER_TOO_SMALL;
+ 		}
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0022-efi_loader-populate-ESRT-table-if-EFI_ESRT-config-op.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0022-efi_loader-populate-ESRT-table-if-EFI_ESRT-config-op.patch
new file mode 100644
index 0000000..aaea20e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0022-efi_loader-populate-ESRT-table-if-EFI_ESRT-config-op.patch
@@ -0,0 +1,36 @@
+From 5fec641015f8f1ca80f55f05b5e1f67653321303 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 17 Dec 2021 19:49:02 +0000
+Subject: [PATCH 22/27] efi_loader: populate ESRT table if EFI_ESRT config
+ option is set
+
+This change is to call efi_esrt_populate function if CONFIG_EFI_ESRT
+is set. This will populte esrt table with firmware image info
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_capsule.c | 7 +++++++
+ 1 file changed, 7 insertions(+)
+
+diff --git a/lib/efi_loader/efi_capsule.c b/lib/efi_loader/efi_capsule.c
+index 939040d2755e..790d2ba8fe19 100644
+--- a/lib/efi_loader/efi_capsule.c
++++ b/lib/efi_loader/efi_capsule.c
+@@ -676,6 +676,13 @@ efi_status_t __efi_runtime EFIAPI efi_update_capsule(
+ 			ret = EFI_SUCCESS;
+ 		}
+ 
++		if (IS_ENABLED(CONFIG_EFI_ESRT)) {
++			/* Rebuild the ESRT to reflect any updated FW images. */
++			ret = efi_esrt_populate();
++	               if (ret != EFI_SUCCESS)
++				log_warning("EFI Capsule: failed to update ESRT\n");
++	       }
++
+ 		goto out;
+ #endif
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0023-efi_firmware-add-get_image_info-for-corstone1000.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0023-efi_firmware-add-get_image_info-for-corstone1000.patch
new file mode 100644
index 0000000..c86b658
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0023-efi_firmware-add-get_image_info-for-corstone1000.patch
@@ -0,0 +1,121 @@
+From 34fadec4f659248a6020676f5894895977ccf79d Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 17 Dec 2021 19:50:25 +0000
+Subject: [PATCH 23/27] efi_firmware: add get_image_info for corstone1000
+
+This change is to populate get_image_info which eventually
+will be populated in ESRT table
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+%% original patch: 0047-efi_firmware-add-get_image_info-for-corstone1000.patch
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_firmware.c | 64 ++++++++++++++++++++++++++++++++++-
+ 1 file changed, 63 insertions(+), 1 deletion(-)
+
+diff --git a/lib/efi_loader/efi_firmware.c b/lib/efi_loader/efi_firmware.c
+index a5ff32f121f4..9eb89849b28d 100644
+--- a/lib/efi_loader/efi_firmware.c
++++ b/lib/efi_loader/efi_firmware.c
+@@ -241,6 +241,7 @@ const efi_guid_t efi_firmware_image_type_uboot_fit =
+  *
+  * Return		status code
+  */
++
+ static
+ efi_status_t EFIAPI efi_firmware_fit_get_image_info(
+ 	struct efi_firmware_management_protocol *this,
+@@ -332,6 +333,56 @@ const struct efi_firmware_management_protocol efi_fmp_fit = {
+ const efi_guid_t efi_firmware_image_type_uboot_raw =
+ 	EFI_FIRMWARE_IMAGE_TYPE_UBOOT_RAW_GUID;
+ 
++#if CONFIG_IS_ENABLED(TARGET_CORSTONE1000)
++static efi_status_t efi_corstone1000_img_info_get (
++	efi_uintn_t *image_info_size,
++	struct efi_firmware_image_descriptor *image_info,
++	u32 *descriptor_version,
++	u8 *descriptor_count,
++	efi_uintn_t *descriptor_size,
++	u32 *package_version,
++	u16 **package_version_name,
++	const efi_guid_t *image_type)
++{
++	int i = 0;
++
++	*image_info_size = sizeof(*image_info);
++	*descriptor_version = EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION;
++	*descriptor_count = 1;//dfu_num;
++	*descriptor_size = sizeof(*image_info);
++	if (package_version)
++		*package_version = 0xffffffff; /* not supported */
++	if(package_version_name)
++		*package_version_name = NULL; /* not supported */
++
++	if(image_info == NULL) {
++		log_warning("image_info is null\n");
++		return EFI_BUFFER_TOO_SMALL;
++	}
++
++	image_info[i].image_index = i;
++	image_info[i].image_type_id = *image_type;
++	image_info[i].image_id = 0;
++	image_info[i].image_id_name = "wic";
++	image_info[i].version = 1;
++	image_info[i].version_name = NULL;
++	image_info[i].size = 0x1000;
++	image_info[i].attributes_supported = IMAGE_ATTRIBUTE_IMAGE_UPDATABLE |
++					     IMAGE_ATTRIBUTE_AUTHENTICATION_REQUIRED;
++	image_info[i].attributes_setting = IMAGE_ATTRIBUTE_IMAGE_UPDATABLE;
++	/* Check if the capsule authentication is enabled */
++	if (IS_ENABLED(CONFIG_EFI_CAPSULE_AUTHENTICATE))
++		image_info[0].attributes_setting |=
++			IMAGE_ATTRIBUTE_AUTHENTICATION_REQUIRED;
++	image_info[i].lowest_supported_image_version = 0;
++	image_info[i].last_attempt_version = 0;
++	image_info[i].last_attempt_status = LAST_ATTEMPT_STATUS_SUCCESS;
++	image_info[i].hardware_instance = 1;
++	image_info[i].dependencies = NULL;
++
++	return EFI_SUCCESS;
++}
++#endif
+ /**
+  * efi_firmware_raw_get_image_info - return information about the current
+ 				     firmware image
+@@ -376,12 +427,20 @@ efi_status_t EFIAPI efi_firmware_raw_get_image_info(
+ 	     !descriptor_size || !package_version || !package_version_name))
+ 		return EFI_EXIT(EFI_INVALID_PARAMETER);
+ 
+-	ret = efi_get_dfu_info(image_info_size, image_info,
++#if CONFIG_IS_ENABLED(TARGET_CORSTONE1000)
++	ret = efi_corstone1000_img_info_get(image_info_size, image_info,
+ 			       descriptor_version, descriptor_count,
+ 			       descriptor_size,
+ 			       package_version, package_version_name,
+ 			       &efi_firmware_image_type_uboot_raw);
++#else
+ 
++	ret = efi_get_dfu_info(image_info_size, image_info,
++			       descriptor_version, descriptor_count,
++			       descriptor_size,
++			       package_version, package_version_name,
++			       &efi_firmware_image_type_uboot_raw);
++#endif
+ 	return EFI_EXIT(ret);
+ }
+ 
+@@ -462,6 +521,9 @@ efi_status_t EFIAPI efi_firmware_raw_set_image(
+ 
+ 	}
+ 
++#if CONFIG_IS_ENABLED(TARGET_CORSTONE1000)
++	return EFI_EXIT(EFI_SUCCESS);
++#endif
+ 	if (dfu_write_by_alt(image_index - 1, (void *)image, image_size,
+ 			     NULL, NULL))
+ 		return EFI_EXIT(EFI_DEVICE_ERROR);
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0024-Comment-mm_communicate-failure-log.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0024-Comment-mm_communicate-failure-log.patch
new file mode 100644
index 0000000..c6a1aed
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0024-Comment-mm_communicate-failure-log.patch
@@ -0,0 +1,34 @@
+From c0c6e4c1166c4868afc36649b9ed98081a6966e1 Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Fri, 24 Dec 2021 14:22:52 +0000
+Subject: [PATCH 24/27] Comment mm_communicate failure log
+
+When a getVariable() call is made with data size set to 0,
+mm_communicate should return EFI_BUFFER_TOO_SMALL. This is
+an expected behavior. There should not be any failure logs
+in this case. So the error log is commented here.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_variable_tee.c | 5 ++++-
+ 1 file changed, 4 insertions(+), 1 deletion(-)
+
+diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
+index 67743d1f8fce..a34989efac83 100644
+--- a/lib/efi_loader/efi_variable_tee.c
++++ b/lib/efi_loader/efi_variable_tee.c
+@@ -411,7 +411,10 @@ static efi_status_t __efi_runtime mm_communicate(u8 *comm_buf, efi_uintn_t dsize
+ 	ret = ffa_mm_communicate(comm_buf, dsize);
+ 	#endif
+ 	if (ret != EFI_SUCCESS) {
+-		log_err("%s failed!\n", __func__);
++		/* mm_communicate failure is logged even when getVariable() is called
++		 * with data size set to 0. This is not expected so logging is commented.
++		*/
++		//log_err("%s failed!\n", __func__);
+ 		return ret;
+ 	}
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0025-efi_loader-send-bootcomplete-message-to-secure-encla.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0025-efi_loader-send-bootcomplete-message-to-secure-encla.patch
new file mode 100644
index 0000000..d5a0ec0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0025-efi_loader-send-bootcomplete-message-to-secure-encla.patch
@@ -0,0 +1,191 @@
+From af2defbfaffa4264052e30f269b91794068e4773 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Wed, 5 Jan 2022 17:56:09 +0000
+Subject: [PATCH 25/27] efi_loader: send bootcomplete message to secure enclave
+
+On corstone1000 platform, Secure Enclave will be expecting
+an event from uboot when it performs capsule update. Previously,
+an event is sent at exitbootservice level. This will create a problem
+when user wants to interrupt at UEFI shell, hence, it is required
+to send an uboot efi initialized event at efi sub-system initialization
+stage.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ include/configs/corstone1000.h |  2 +-
+ lib/efi_loader/efi_boottime.c  | 49 ----------------------------------
+ lib/efi_loader/efi_firmware.c  |  2 +-
+ lib/efi_loader/efi_setup.c     | 48 +++++++++++++++++++++++++++++++++
+ 4 files changed, 50 insertions(+), 51 deletions(-)
+
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index a7445e61348b..06b605e43bdf 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -22,7 +22,7 @@
+ 
+ /* Notification events used with SE Proxy update service */
+ #define CORSTONE1000_BUFFER_READY_EVT		(0x1)
+-#define CORSTONE1000_KERNEL_STARTED_EVT		(0x2)
++#define CORSTONE1000_UBOOT_EFI_STARTED_EVT	(0x2)
+ 
+ #define PREP_SEPROXY_SVC_ID_MASK	GENMASK(31, 16)
+ #define PREP_SEPROXY_SVC_ID(x)	 (FIELD_PREP(PREP_SEPROXY_SVC_ID_MASK, (x)))
+diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
+index b58a8c98fd05..d0703060491b 100644
+--- a/lib/efi_loader/efi_boottime.c
++++ b/lib/efi_loader/efi_boottime.c
+@@ -2101,46 +2101,6 @@ static void efi_exit_caches(void)
+ #endif
+ }
+ 
+-#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
+-/**
+- * efi_corstone1000_kernel_started_event - notifies SE Proxy FW update service
+- *
+- * This function notifies the SE Proxy update service that the kernel has already started
+- *
+- * Return:
+- *
+- * 0: on success, otherwise failure
+- */
+-static int efi_corstone1000_kernel_started_event(void)
+-{
+-	struct ffa_interface_data func_data = {0};
+-	struct ffa_send_direct_data msg = {0};
+-	u16 part_id = CORSTONE1000_SEPROXY_PART_ID;
+-
+-	log_debug("[%s]\n", __func__);
+-
+-	/*
+-	 * telling the driver which partition to use
+-	 */
+-	func_data.data0_size = sizeof(part_id);
+-	func_data.data0 = &part_id;
+-
+-	/*
+-	 * setting the kernel started  event arguments:
+-	 * setting capsule update interface ID(31:16)
+-	 * the kernel started event ID(15:0)
+-	 */
+-	msg.a4 = PREP_SEPROXY_SVC_ID(CORSTONE1000_SEPROXY_UPDATE_SVC_ID) |
+-	PREP_SEPROXY_EVT(CORSTONE1000_KERNEL_STARTED_EVT);
+-
+-	func_data.data1_size = sizeof(msg);
+-	func_data.data1 = &msg;
+-
+-	return ffa_helper_msg_send_direct_req(&func_data);
+-}
+-
+-#endif
+-
+ /**
+  * efi_exit_boot_services() - stop all boot services
+  * @image_handle: handle of the loaded image
+@@ -2254,15 +2214,6 @@ static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
+ 	/* Recalculate CRC32 */
+ 	efi_update_table_header_crc32(&systab.hdr);
+ 
+-#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
+-	/* Notifying SE Proxy FW update service */
+-	ffa_ret = efi_corstone1000_kernel_started_event();
+-	if (ffa_ret)
+-		debug("[efi_boottime][ERROR]: Failure to notify SE Proxy FW update service\n");
+-	else
+-		debug("[efi_boottime][INFO]: SE Proxy FW update service notified\n");
+-#endif
+-
+ 	/* Give the payload some time to boot */
+ 	efi_set_watchdog(0);
+ 	WATCHDOG_RESET();
+diff --git a/lib/efi_loader/efi_firmware.c b/lib/efi_loader/efi_firmware.c
+index 9eb89849b28d..477ad072070e 100644
+--- a/lib/efi_loader/efi_firmware.c
++++ b/lib/efi_loader/efi_firmware.c
+@@ -356,7 +356,7 @@ static efi_status_t efi_corstone1000_img_info_get (
+ 		*package_version_name = NULL; /* not supported */
+ 
+ 	if(image_info == NULL) {
+-		log_warning("image_info is null\n");
++		log_info("image_info is null\n");
+ 		return EFI_BUFFER_TOO_SMALL;
+ 	}
+ 
+diff --git a/lib/efi_loader/efi_setup.c b/lib/efi_loader/efi_setup.c
+index 989380d4f8cd..515a0bdf74ef 100644
+--- a/lib/efi_loader/efi_setup.c
++++ b/lib/efi_loader/efi_setup.c
+@@ -17,6 +17,9 @@
+ efi_status_t efi_obj_list_initialized = OBJ_LIST_NOT_INITIALIZED;
+ 
+ #if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++#include <linux/bitfield.h>
++#include <linux/bitops.h>
++#include <arm_ffa_helper.h>
+ /**
+  * efi_corstone1000_alloc_capsule_shared_buf - allocate capsule shared buffer
+  */
+@@ -126,6 +129,44 @@ static efi_status_t efi_init_secure_boot(void)
+ }
+ #endif /* CONFIG_EFI_SECURE_BOOT */
+ 
++#if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++/**
++ * efi_corstone1000_uboot-efi_started_event - notifies SE Proxy FW update service
++ *
++ * This function notifies the SE Proxy update service that uboot efi has already started
++ *
++ * Return:
++ *
++ * 0: on success, otherwise failure
++ * */
++static int efi_corstone1000_uboot_efi_started_event(void)
++{
++	struct ffa_interface_data func_data = {0};
++	struct ffa_send_direct_data msg = {0};
++	u16 part_id = CORSTONE1000_SEPROXY_PART_ID;
++
++	log_debug("[%s]\n", __func__);
++
++	/*
++	 * telling the driver which partition to use
++	 */
++	func_data.data0_size = sizeof(part_id);
++	func_data.data0 = &part_id;
++	/*
++	 * setting the uboot efi subsystem started  event arguments:
++	 * setting capsule update interface ID(31:16)
++	 * the uboot efi subsystem started event ID(15:0)
++	 */
++	msg.a4 = PREP_SEPROXY_SVC_ID(CORSTONE1000_SEPROXY_UPDATE_SVC_ID) |
++			PREP_SEPROXY_EVT(CORSTONE1000_UBOOT_EFI_STARTED_EVT);
++
++	func_data.data1_size = sizeof(msg);
++	func_data.data1 = &msg;
++
++	return ffa_helper_msg_send_direct_req(&func_data);
++}
++#endif
++
+ /**
+  * efi_init_capsule - initialize capsule update state
+  *
+@@ -134,8 +175,15 @@ static efi_status_t efi_init_secure_boot(void)
+ static efi_status_t efi_init_capsule(void)
+ {
+ 	efi_status_t ret = EFI_SUCCESS;
++	int ffa_ret;
+ 
+ #if IS_ENABLED(CONFIG_TARGET_CORSTONE1000)
++	ffa_ret = efi_corstone1000_uboot_efi_started_event();
++	if (ffa_ret)
++		debug("[efi_boottime][ERROR]: Failure to notify SE Proxy FW update service\n");
++	else
++		debug("[efi_boottime][INFO]: SE Proxy FW update service notified\n");
++
+ 	ret = efi_corstone1000_alloc_capsule_shared_buf();
+ 	if (ret != EFI_SUCCESS) {
+ 		printf("EFI: Corstone-1000: cannot allocate caspsule shared buffer\n");
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0026-efi_loader-fix-null-pointer-exception-with-get_image.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0026-efi_loader-fix-null-pointer-exception-with-get_image.patch
new file mode 100644
index 0000000..532e872
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0026-efi_loader-fix-null-pointer-exception-with-get_image.patch
@@ -0,0 +1,62 @@
+From 2da8554ab732c59c7ca624ac4b16412fa9c2e39c Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 14 Jan 2022 15:24:18 +0000
+Subject: [PATCH 26/27] efi_loader: fix null pointer exception with
+ get_image_info
+
+get_img_info API implemented for corstone1000 target does not
+check the input attributes and as a result uboot crash's with
+null pointer access. This change is to fix the null pointer
+exception.
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ lib/efi_loader/efi_firmware.c | 19 +++++++++++--------
+ 1 file changed, 11 insertions(+), 8 deletions(-)
+
+diff --git a/lib/efi_loader/efi_firmware.c b/lib/efi_loader/efi_firmware.c
+index 477ad072070e..f99c57fde576 100644
+--- a/lib/efi_loader/efi_firmware.c
++++ b/lib/efi_loader/efi_firmware.c
+@@ -347,26 +347,29 @@ static efi_status_t efi_corstone1000_img_info_get (
+ 	int i = 0;
+ 
+ 	*image_info_size = sizeof(*image_info);
+-	*descriptor_version = EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION;
+-	*descriptor_count = 1;//dfu_num;
+-	*descriptor_size = sizeof(*image_info);
++	if(descriptor_version)
++	    *descriptor_version = EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION;
++	if(descriptor_count)
++	    *descriptor_count = 1;
++	if(descriptor_size)
++	    *descriptor_size = sizeof(*image_info);
+ 	if (package_version)
+ 		*package_version = 0xffffffff; /* not supported */
+ 	if(package_version_name)
+ 		*package_version_name = NULL; /* not supported */
+ 
+ 	if(image_info == NULL) {
+-		log_info("image_info is null\n");
++		log_debug("image_info is null\n");
+ 		return EFI_BUFFER_TOO_SMALL;
+ 	}
+ 
+-	image_info[i].image_index = i;
++	image_info[i].image_index = 1;
+ 	image_info[i].image_type_id = *image_type;
+ 	image_info[i].image_id = 0;
+-	image_info[i].image_id_name = "wic";
+-	image_info[i].version = 1;
++	image_info[i].image_id_name = L"wic image";
++	image_info[i].version = 0;
+ 	image_info[i].version_name = NULL;
+-	image_info[i].size = 0x1000;
++	image_info[i].size = 0;
+ 	image_info[i].attributes_supported = IMAGE_ATTRIBUTE_IMAGE_UPDATABLE |
+ 					     IMAGE_ATTRIBUTE_AUTHENTICATION_REQUIRED;
+ 	image_info[i].attributes_setting = IMAGE_ATTRIBUTE_IMAGE_UPDATABLE;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0027-arm-corstone1000-add-mmc-for-fvp.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0027-arm-corstone1000-add-mmc-for-fvp.patch
new file mode 100644
index 0000000..bf95ed7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone1000/0027-arm-corstone1000-add-mmc-for-fvp.patch
@@ -0,0 +1,148 @@
+From cbf16548dc6dcc8eea97aa18c6ae17fb848e5c6c Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Tue, 5 Apr 2022 10:24:38 +0100
+Subject: [PATCH 27/27] arm:corstone1000: add mmc for fvp
+
+Enable support mmc/sdcard for the corstone1000 FVP.
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+---
+ arch/arm/dts/corstone1000-fvp.dts        | 28 +++++++++++++++
+ board/armltd/corstone1000/corstone1000.c | 46 ++++++++++++++++--------
+ configs/corstone1000_defconfig           |  8 ++++-
+ include/configs/corstone1000.h           |  4 ++-
+ 4 files changed, 69 insertions(+), 17 deletions(-)
+
+diff --git a/arch/arm/dts/corstone1000-fvp.dts b/arch/arm/dts/corstone1000-fvp.dts
+index 1fcc137a493c..26b0f1b3cea6 100644
+--- a/arch/arm/dts/corstone1000-fvp.dts
++++ b/arch/arm/dts/corstone1000-fvp.dts
+@@ -20,4 +20,32 @@
+ 		interrupts = <GIC_SPI 116 IRQ_TYPE_LEVEL_HIGH>;
+ 		reg-io-width = <2>;
+ 	};
++
++	vmmc_v3_3d: fixed_v3_3d {
++		compatible = "regulator-fixed";
++		regulator-name = "vmmc_supply";
++		regulator-min-microvolt = <3300000>;
++		regulator-max-microvolt = <3300000>;
++		regulator-always-on;
++	};
++
++	sdmmc0: mmc@40300000 {
++		compatible = "arm,pl18x", "arm,primecell";
++		reg = <0x40300000 0x1000>;
++		interrupts = <GIC_SPI 117 IRQ_TYPE_LEVEL_HIGH>;
++		max-frequency = <12000000>;
++		vmmc-supply = <&vmmc_v3_3d>;
++		clocks = <&smbclk>, <&refclk100mhz>;
++		clock-names = "smclk", "apb_pclk";
++	};
++
++	sdmmc1: mmc@50000000 {
++		compatible = "arm,pl18x", "arm,primecell";
++		reg = <0x50000000 0x10000>;
++		interrupts = <GIC_SPI 115 IRQ_TYPE_LEVEL_HIGH>;
++		max-frequency = <12000000>;
++		vmmc-supply = <&vmmc_v3_3d>;
++		clocks = <&smbclk>, <&refclk100mhz>;
++		clock-names = "smclk", "apb_pclk";
++	};
+ };
+diff --git a/board/armltd/corstone1000/corstone1000.c b/board/armltd/corstone1000/corstone1000.c
+index eff1739f0b02..936a6c9f8b89 100644
+--- a/board/armltd/corstone1000/corstone1000.c
++++ b/board/armltd/corstone1000/corstone1000.c
+@@ -46,22 +46,38 @@ static struct mm_region corstone1000_mem_map[] = {
+ 		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+ 			 PTE_BLOCK_NON_SHARE |
+ 			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+-        }, {
+-                /* USB */
+-                .virt = 0x40200000UL,
+-                .phys = 0x40200000UL,
+-                .size = 0x00100000UL,
+-                .attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+-                         PTE_BLOCK_NON_SHARE |
+-                         PTE_BLOCK_PXN | PTE_BLOCK_UXN
+ 	}, {
+-                 /* ethernet */
+-                .virt = 0x40100000UL,
+-                .phys = 0x40100000UL,
+-                .size = 0x00100000UL,
+-                .attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+-                         PTE_BLOCK_NON_SHARE |
+-                         PTE_BLOCK_PXN | PTE_BLOCK_UXN
++		/* USB */
++		.virt = 0x40200000UL,
++			.phys = 0x40200000UL,
++			.size = 0x00100000UL,
++			.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++				PTE_BLOCK_NON_SHARE |
++				PTE_BLOCK_PXN | PTE_BLOCK_UXN
++	}, {
++		/* MMC0 */
++		.virt = 0x40300000UL,
++		.phys = 0x40300000UL,
++		.size = 0x00100000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++				 PTE_BLOCK_NON_SHARE |
++				 PTE_BLOCK_PXN | PTE_BLOCK_UXN
++	}, {
++		/* ethernet */
++		.virt = 0x40100000UL,
++			.phys = 0x40100000UL,
++			.size = 0x00100000UL,
++			.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++				PTE_BLOCK_NON_SHARE |
++				PTE_BLOCK_PXN | PTE_BLOCK_UXN
++	}, {
++		/* MMC1 */
++		.virt = 0x50000000UL,
++		.phys = 0x50000000UL,
++		.size = 0x00100000UL,
++		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
++				 PTE_BLOCK_NON_SHARE |
++				 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+ 	}, {
+ 		/* OCVM */
+ 		.virt = 0x80000000UL,
+diff --git a/configs/corstone1000_defconfig b/configs/corstone1000_defconfig
+index b042d4e49419..147c14c94865 100644
+--- a/configs/corstone1000_defconfig
++++ b/configs/corstone1000_defconfig
+@@ -38,7 +38,13 @@ CONFIG_CMD_EFIDEBUG=y
+ CONFIG_CMD_FAT=y
+ CONFIG_OF_CONTROL=y
+ CONFIG_REGMAP=y
+-# CONFIG_MMC is not set
++CONFIG_CLK=y
++CONFIG_CMD_MMC=y
++CONFIG_DM_MMC=y
++CONFIG_ARM_PL180_MMCI=y
++CONFIG_MMC_SDHCI_ADMA_HELPERS=y
++CONFIG_MMC_WRITE=y
++CONFIG_DM_GPIO=y
+ CONFIG_DM_SERIAL=y
+ CONFIG_USB=y
+ CONFIG_DM_USB=y
+diff --git a/include/configs/corstone1000.h b/include/configs/corstone1000.h
+index 06b605e43bdf..d9855bf91ebf 100644
+--- a/include/configs/corstone1000.h
++++ b/include/configs/corstone1000.h
+@@ -95,7 +95,9 @@
+ #define CONFIG_SYS_MAXARGS	64	/* max command args */
+ 
+ #define BOOT_TARGET_DEVICES(func) \
+-	func(USB, usb, 0)
++	func(USB, usb, 0) \
++	func(MMC, mmc, 0) \
++	func(MMC, mmc, 1)
+ 
+ #include <config_distro_bootcmd.h>
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0001-armv7-adding-generic-timer-access-through-MMIO.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0001-armv7-adding-generic-timer-access-through-MMIO.patch
new file mode 100644
index 0000000..8a98f9d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0001-armv7-adding-generic-timer-access-through-MMIO.patch
@@ -0,0 +1,138 @@
+From fff63cfd7d9654dc9ed0c106f29d3a7ad01b0502 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Wed, 18 Dec 2019 21:52:34 +0000
+Subject: [PATCH 1/2] armv7: adding generic timer access through MMIO
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+
+This driver enables the ARMv7 generic timer.
+
+The access to the timer registers is through memory mapping (MMIO).
+
+This driver can be used by u-boot to access to the timer through MMIO
+when arch_timer is not available in the core (access using system
+instructions not possible), for example, in case of Cortex-A5.
+
+This driver configures and enables the generic timer at
+the u-boot initcall level (timer_init) before u-boot relocation.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+
+%% original patch: 0001-armv7-adding-generic-timer-access-through-MMIO.patch
+---
+ arch/arm/cpu/armv7/Makefile     |  1 +
+ arch/arm/cpu/armv7/mmio_timer.c | 75 +++++++++++++++++++++++++++++++++
+ scripts/config_whitelist.txt    |  1 +
+ 3 files changed, 77 insertions(+)
+ create mode 100644 arch/arm/cpu/armv7/mmio_timer.c
+
+diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
+index bfbd85ae64..1a0a24e531 100644
+--- a/arch/arm/cpu/armv7/Makefile
++++ b/arch/arm/cpu/armv7/Makefile
+@@ -28,6 +28,7 @@ obj-$(CONFIG_ARMV7_PSCI)	+= psci.o psci-common.o
+ obj-$(CONFIG_IPROC) += iproc-common/
+ obj-$(CONFIG_KONA) += kona-common/
+ obj-$(CONFIG_SYS_ARCH_TIMER) += arch_timer.o
++obj-$(CONFIG_SYS_MMIO_TIMER) += mmio_timer.o
+ 
+ ifneq (,$(filter s5pc1xx exynos,$(SOC)))
+ obj-y += s5p-common/
+diff --git a/arch/arm/cpu/armv7/mmio_timer.c b/arch/arm/cpu/armv7/mmio_timer.c
+new file mode 100644
+index 0000000000..edd806e06e
+--- /dev/null
++++ b/arch/arm/cpu/armv7/mmio_timer.c
+@@ -0,0 +1,75 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * Copyright (c) 2019, Arm Limited. All rights reserved.
++ *
++ */
++
++#include <common.h>
++#include <asm/io.h>
++#include <div64.h>
++#include <bootstage.h>
++#include <asm/global_data.h>
++
++DECLARE_GLOBAL_DATA_PTR;
++
++#define CNTCTLBASE    0x1a020000UL
++#define CNTREADBASE   0x1a030000UL
++#define CNTEN         (1 << 0)
++#define CNTFCREQ      (1 << 8)
++
++static inline uint32_t mmio_read32(uintptr_t addr)
++{
++	return *(volatile uint32_t*)addr;
++}
++
++static inline void mmio_write32(uintptr_t addr, uint32_t data)
++{
++	*(volatile uint32_t*)addr = data;
++}
++
++int timer_init(void)
++{
++	/* calculate the frequency in ms */
++	gd->arch.timer_rate_hz = COUNTER_FREQUENCY / CONFIG_SYS_HZ;
++
++	/* configure CNTFID0 register: set the base frequency */
++	mmio_write32(CNTCTLBASE + 0x20, COUNTER_FREQUENCY);
++
++	/*
++	 * configure CNTCR register:
++	 *    enable the generic counter and;
++	 *    select the first frequency entry
++	 */
++	mmio_write32(CNTCTLBASE, CNTFCREQ | CNTEN);
++
++	return 0;
++}
++
++unsigned long long get_ticks(void)
++{
++	return (((u64)(mmio_read32(CNTREADBASE + 0x4)) << 32) |
++		mmio_read32(CNTREADBASE));
++}
++
++ulong get_timer(ulong base)
++{
++	return lldiv(get_ticks(), gd->arch.timer_rate_hz) - base;
++}
++
++void __udelay(unsigned long usec)
++{
++	unsigned long endtime;
++
++	endtime = lldiv((unsigned long long)usec * gd->arch.timer_rate_hz,
++			1000UL);
++
++	endtime += get_ticks();
++
++	while (get_ticks() < endtime)
++		;
++}
++
++ulong get_tbclk(void)
++{
++	return gd->arch.timer_rate_hz;
++}
+diff --git a/scripts/config_whitelist.txt b/scripts/config_whitelist.txt
+index a6bc234f51..8d5cd67ace 100644
+--- a/scripts/config_whitelist.txt
++++ b/scripts/config_whitelist.txt
+@@ -1524,6 +1524,7 @@ CONFIG_SYS_MMC_U_BOOT_DST
+ CONFIG_SYS_MMC_U_BOOT_OFFS
+ CONFIG_SYS_MMC_U_BOOT_SIZE
+ CONFIG_SYS_MMC_U_BOOT_START
++CONFIG_SYS_MMIO_TIMER
+ CONFIG_SYS_MONITOR_BASE
+ CONFIG_SYS_MONITOR_LEN
+ CONFIG_SYS_MONITOR_SEC
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0002-board-arm-add-corstone500-board.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0002-board-arm-add-corstone500-board.patch
new file mode 100644
index 0000000..29b2943
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/corstone500/0002-board-arm-add-corstone500-board.patch
@@ -0,0 +1,307 @@
+From 73c319a1096259652853fa2538a733a8ebea96a8 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Wed, 8 Jan 2020 09:48:11 +0000
+Subject: [PATCH 2/2] board: arm: add corstone500 board
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+
+Add support for the Arm corstone500 platform, with a cortex-a5
+chip, add the default configuration, initialization and
+makefile for this system.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+
+%% original patch: 0002-board-arm-add-corstone500-board.patch
+---
+ arch/arm/Kconfig                       |  10 +++
+ board/armltd/corstone500/Kconfig       |  12 +++
+ board/armltd/corstone500/Makefile      |   8 ++
+ board/armltd/corstone500/corstone500.c |  48 +++++++++++
+ configs/corstone500_defconfig          |  40 +++++++++
+ include/configs/corstone500.h          | 109 +++++++++++++++++++++++++
+ 6 files changed, 227 insertions(+)
+ create mode 100644 board/armltd/corstone500/Kconfig
+ create mode 100644 board/armltd/corstone500/Makefile
+ create mode 100644 board/armltd/corstone500/corstone500.c
+ create mode 100644 configs/corstone500_defconfig
+ create mode 100644 include/configs/corstone500.h
+
+diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
+index 4567c183fb..66f99fdf4f 100644
+--- a/arch/arm/Kconfig
++++ b/arch/arm/Kconfig
+@@ -641,6 +641,15 @@ config ARCH_BCMSTB
+ 	  This enables support for Broadcom ARM-based set-top box
+ 	  chipsets, including the 7445 family of chips.
+ 
++config TARGET_CORSTONE500
++	bool "Support Corstone500"
++	select CPU_V7A
++	select SEMIHOSTING
++	select PL01X_SERIAL
++	help
++	  This enables support for Corstone500 ARM which is a
++	  Cortex-A5 system
++
+ config TARGET_VEXPRESS_CA9X4
+ 	bool "Support vexpress_ca9x4"
+ 	select CPU_V7A
+@@ -2202,6 +2211,7 @@ source "board/bosch/shc/Kconfig"
+ source "board/bosch/guardian/Kconfig"
+ source "board/Marvell/octeontx/Kconfig"
+ source "board/Marvell/octeontx2/Kconfig"
++source "board/armltd/corstone500/Kconfig"
+ source "board/armltd/vexpress/Kconfig"
+ source "board/armltd/vexpress64/Kconfig"
+ source "board/cortina/presidio-asic/Kconfig"
+diff --git a/board/armltd/corstone500/Kconfig b/board/armltd/corstone500/Kconfig
+new file mode 100644
+index 0000000000..8e689bd1fd
+--- /dev/null
++++ b/board/armltd/corstone500/Kconfig
+@@ -0,0 +1,12 @@
++if TARGET_CORSTONE500
++
++config SYS_BOARD
++	default "corstone500"
++
++config SYS_VENDOR
++	default "armltd"
++
++config SYS_CONFIG_NAME
++	default "corstone500"
++
++endif
+diff --git a/board/armltd/corstone500/Makefile b/board/armltd/corstone500/Makefile
+new file mode 100644
+index 0000000000..6598fdd3ae
+--- /dev/null
++++ b/board/armltd/corstone500/Makefile
+@@ -0,0 +1,8 @@
++# SPDX-License-Identifier: GPL-2.0+
++#
++# (C) Copyright 2022 ARM Limited
++# (C) Copyright 2022 Linaro
++# Rui Miguel Silva <rui.silva@linaro.org>
++#
++
++obj-y := corstone500.o
+diff --git a/board/armltd/corstone500/corstone500.c b/board/armltd/corstone500/corstone500.c
+new file mode 100644
+index 0000000000..e878f5c6a5
+--- /dev/null
++++ b/board/armltd/corstone500/corstone500.c
+@@ -0,0 +1,48 @@
++// SPDX-License-Identifier: GPL-2.0+
++/*
++ * (C) Copyright 2022 ARM Limited
++ * (C) Copyright 2022 Linaro
++ * Rui Miguel Silva <rui.silva@linaro.org>
++ */
++
++#include <common.h>
++#include <dm.h>
++#include <dm/platform_data/serial_pl01x.h>
++#include <malloc.h>
++#include <asm/global_data.h>
++
++static const struct pl01x_serial_plat serial_platdata = {
++	.base = V2M_UART0,
++	.type = TYPE_PL011,
++	.clock = CONFIG_PL011_CLOCK,
++};
++
++U_BOOT_DRVINFO(corstone500_serials) = {
++	.name = "serial_pl01x",
++	.plat = &serial_platdata,
++};
++
++int board_init(void)
++{
++	return 0;
++}
++
++int dram_init(void)
++{
++	gd->ram_size = PHYS_SDRAM_1_SIZE;
++
++	return 0;
++}
++
++int dram_init_banksize(void)
++{
++	gd->bd->bi_dram[0].start = PHYS_SDRAM_1;
++	gd->bd->bi_dram[0].size = PHYS_SDRAM_1_SIZE;
++
++	return 0;
++}
++
++void reset_cpu(ulong addr)
++{
++}
++
+diff --git a/configs/corstone500_defconfig b/configs/corstone500_defconfig
+new file mode 100644
+index 0000000000..d3161a4b40
+--- /dev/null
++++ b/configs/corstone500_defconfig
+@@ -0,0 +1,40 @@
++CONFIG_ARM=y
++CONFIG_SKIP_LOWLEVEL_INIT=y
++CONFIG_TARGET_CORSTONE500=y
++CONFIG_SYS_TEXT_BASE=0x88000000
++CONFIG_SYS_MALLOC_LEN=0x840000
++CONFIG_SYS_MALLOC_F_LEN=0x2000
++CONFIG_NR_DRAM_BANKS=1
++CONFIG_SYS_MEMTEST_START=0x80000000
++CONFIG_SYS_MEMTEST_END=0xff000000
++CONFIG_ENV_SIZE=0x40000
++CONFIG_IDENT_STRING=" corstone500 aarch32"
++CONFIG_SYS_LOAD_ADDR=0x90000000
++CONFIG_SUPPORT_RAW_INITRD=y
++CONFIG_BOOTDELAY=1
++CONFIG_USE_BOOTARGS=y
++CONFIG_BOOTARGS="console=ttyAMA0 earlycon=pl011,0x1a200000 root=/dev/ram0 rw loglevel=9"
++# CONFIG_DISPLAY_CPUINFO is not set
++# CONFIG_DISPLAY_BOARDINFO is not set
++CONFIG_HUSH_PARSER=y
++CONFIG_SYS_PROMPT="corstone500# "
++# CONFIG_CMD_CONSOLE is not set
++CONFIG_CMD_BOOTZ=y
++# CONFIG_CMD_XIMG is not set
++# CONFIG_CMD_EDITENV is not set
++# CONFIG_CMD_ENV_EXISTS is not set
++CONFIG_CMD_MEMTEST=y
++CONFIG_CMD_ARMFLASH=y
++# CONFIG_CMD_LOADS is not set
++# CONFIG_CMD_ITEST is not set
++# CONFIG_CMD_SETEXPR is not set
++CONFIG_CMD_DHCP=y
++# CONFIG_CMD_NFS is not set
++CONFIG_CMD_MII=y
++CONFIG_CMD_PING=y
++CONFIG_CMD_CACHE=y
++CONFIG_CMD_FAT=y
++CONFIG_DM=y
++CONFIG_MTD_NOR_FLASH=y
++CONFIG_DM_SERIAL=y
++CONFIG_OF_LIBFDT=y
+diff --git a/include/configs/corstone500.h b/include/configs/corstone500.h
+new file mode 100644
+index 0000000000..93c397d2f5
+--- /dev/null
++++ b/include/configs/corstone500.h
+@@ -0,0 +1,109 @@
++/* SPDX-License-Identifier: GPL-2.0+ */
++/*
++ * (C) Copyright 2022 ARM Limited
++ * (C) Copyright 2022 Linaro
++ * Rui Miguel Silva <rui.silva@linaro.org>
++ *
++ * Configuration for Cortex-A5 Corstone500. Parts were derived from other ARM
++ * configurations.
++ */
++
++#ifndef __CORSTONE500_H
++#define __CORSTONE500_H
++
++#define CONFIG_SYS_INIT_SP_ADDR		(CONFIG_SYS_SDRAM_BASE + 0x03f00000)
++
++/* Generic Timer Definitions */
++#define CONFIG_SYS_HZ_CLOCK	7500000
++#define CONFIG_SYS_HZ		1000
++#define COUNTER_FREQUENCY	CONFIG_SYS_HZ_CLOCK
++
++#ifdef CONFIG_CORSTONE500_MEMORY_MAP_EXTENDED
++#define V2M_SRAM0		0x00010000
++#define V2M_SRAM1		0x02200000
++#define V2M_QSPI		0x0a800000
++#else
++#define V2M_SRAM0		0x00000000
++#define V2M_SRAM1		0x02000000
++#define V2M_QSPI		0x08000000
++#endif
++
++#define V2M_DEBUG		0x10000000
++#define V2M_BASE_PERIPH		0x1a000000
++#define V2M_A5_PERIPH		0x1c000000
++#define V2M_L2CC_PERIPH		0x1c010000
++
++#define V2M_MASTER_EXPANSION0	0x40000000
++#define V2M_MASTER_EXPANSION1	0x60000000
++
++#define V2M_BASE		0x80000000
++
++#define V2M_PERIPH_OFFSET(x)  (x << 16)
++
++#define V2M_SYSID		(V2M_BASE_PERIPH)
++#define V2M_SYCTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(1))
++#define V2M_COUNTER_CTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(2))
++#define V2M_COUNTER_READ	(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(3))
++#define V2M_TIMER_CTL		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(4))
++#define V2M_TIMER0		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(5))
++
++#define V2M_WATCHDOG_CTL	(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(16))
++#define V2M_WATCHDOG_REFRESH	(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(17))
++
++#define V2M_UART0		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(32))
++#define V2M_UART1		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(33))
++
++#define V2M_RTC			(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(34))
++#define V2M_TRNG		(V2M_BASE_PERIPH + V2M_PERIPH_OFFSET(35))
++
++/* PL011 Serial Configuration */
++#define CONFIG_CONS_INDEX	0
++#define CONFIG_PL011_CLOCK	7500000
++
++/* Physical Memory Map */
++#define PHYS_SDRAM_1		(V2M_BASE)
++
++/* Top 16MB reserved for secure world use */
++#define DRAM_SEC_SIZE		0x01000000
++#define PHYS_SDRAM_1_SIZE	(0x80000000 - DRAM_SEC_SIZE)
++
++/* Miscellaneous configurable options */
++#define CONFIG_SYS_SDRAM_BASE	PHYS_SDRAM_1
++
++#define CONFIG_SYS_MMIO_TIMER
++
++#define CONFIG_EXTRA_ENV_SETTINGS     \
++				"kernel_name=Image\0"           \
++				"kernel_addr=0x80f00000\0"      \
++				"initrd_name=ramdisk.img\0"     \
++				"initrd_addr=0x84000000\0"      \
++				"fdt_name=devtree.dtb\0"        \
++				"fdt_addr=0x83000000\0"         \
++				"fdt_high=0xffffffff\0"         \
++				"initrd_high=0xffffffff\0"
++
++#define CONFIG_BOOTCOMMAND	"echo copy to RAM...; " \
++				"cp.b 0x80100000 $kernel_addr 0xb00000; " \
++				"cp.b 0x80d00000 $initrd_addr 0x800000; " \
++				"bootz $kernel_addr $initrd_addr:0x800000 $fdt_addr"
++
++/* Monitor Command Prompt */
++#define CONFIG_SYS_CBSIZE		512     /* Console I/O Buffer Size */
++#define CONFIG_SYS_MAXARGS		64      /* max command args */
++
++#define CONFIG_SYS_FLASH_BASE		0x80000000
++/* 256 x 256KiB sectors */
++#define CONFIG_SYS_MAX_FLASH_SECT	256
++/* Store environment at top of flash */
++#define CONFIG_ENV_ADDR			0x0a7c0000
++#define CONFIG_ENV_SECT_SIZE		0x0040000
++
++#define CONFIG_SYS_FLASH_CFI		1
++#define CONFIG_FLASH_CFI_DRIVER		1
++#define CONFIG_SYS_FLASH_CFI_WIDTH	FLASH_CFI_32BIT
++#define CONFIG_SYS_MAX_FLASH_BANKS	1
++
++#define CONFIG_SYS_FLASH_EMPTY_INFO   /* flinfo indicates empty blocks */
++#define FLASH_MAX_SECTOR_SI		0x00040000
++#define CONFIG_ENV_IS_IN_FLASH		1
++#endif
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0001-Add-vexpress_aemv8a_aarch32-variant.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0001-Add-vexpress_aemv8a_aarch32-variant.patch
new file mode 100644
index 0000000..5138335
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0001-Add-vexpress_aemv8a_aarch32-variant.patch
@@ -0,0 +1,184 @@
+From 424d186ab0a0c4dd62dfb13ac87e8d1fd26c101e Mon Sep 17 00:00:00 2001
+From: Anders Dellien <anders.dellien@arm.com>
+Date: Thu, 23 Jul 2020 17:32:55 +0100
+Subject: [PATCH 1/2] Add vexpress_aemv8a_aarch32 variant
+
+The ARM AEMv8 FVP model can be run in Aarch64 or Aarch32 mode. Aarch32
+support is enable per-CPU when launching the model, eg:
+
+-C cluster0.cpu0.CONFIG64=0
+
+This patch adds a new defconfig and some variant specific selections in
+vexpress_armv8a.h.
+
+This patch is co-authored with Soby Mathew <Soby.Mathew@arm.com>.
+
+Upstream-Status: Denied
+
+For upstream discussion, please visit
+https://www.mail-archive.com/u-boot@lists.denx.de/msg233429.html
+
+Signed-off-by: Ryan Harkin <ryan.harkin@linaro.org>
+Signed-off-by: Asha R <asha.r@arm.com>
+Signed-off-by: Anders Dellien <anders.dellien@arm.com>
+---
+ arch/arm/Kconfig                          |  5 +++
+ board/armltd/vexpress64/Kconfig           |  2 +-
+ configs/vexpress_aemv8a_aarch32_defconfig | 40 ++++++++++++++++++
+ include/configs/vexpress_aemv8.h          | 50 +++++++++++++++--------
+ 4 files changed, 80 insertions(+), 17 deletions(-)
+ create mode 100644 configs/vexpress_aemv8a_aarch32_defconfig
+
+diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
+index 4567c183fb84..99cc414d6760 100644
+--- a/arch/arm/Kconfig
++++ b/arch/arm/Kconfig
+@@ -1250,6 +1250,11 @@ config TARGET_VEXPRESS64_BASE_FVP
+ 	select PL01X_SERIAL
+ 	select SEMIHOSTING
+ 
++config TARGET_VEXPRESS64_BASE_FVP_AARCH32
++        bool "Support Versatile Express ARMv8a 32-bit FVP BASE model"
++        select CPU_V7A
++        select SEMIHOSTING
++
+ config TARGET_VEXPRESS64_JUNO
+ 	bool "Support Versatile Express Juno Development Platform"
+ 	select ARM64
+diff --git a/board/armltd/vexpress64/Kconfig b/board/armltd/vexpress64/Kconfig
+index 4aab3f092ecb..0a5e3fcc004a 100644
+--- a/board/armltd/vexpress64/Kconfig
++++ b/board/armltd/vexpress64/Kconfig
+@@ -1,4 +1,4 @@
+-if TARGET_VEXPRESS64_BASE_FVP || TARGET_VEXPRESS64_JUNO
++if TARGET_VEXPRESS64_BASE_FVP || TARGET_VEXPRESS64_JUNO || TARGET_VEXPRESS64_BASE_FVP_AARCH32
+ 
+ config SYS_BOARD
+ 	default "vexpress64"
+diff --git a/configs/vexpress_aemv8a_aarch32_defconfig b/configs/vexpress_aemv8a_aarch32_defconfig
+new file mode 100644
+index 000000000000..9c5c3367ec4d
+--- /dev/null
++++ b/configs/vexpress_aemv8a_aarch32_defconfig
+@@ -0,0 +1,40 @@
++CONFIG_ARM=y
++CONFIG_SYS_ARCH_TIMER=y
++CONFIG_TARGET_VEXPRESS64_BASE_FVP_AARCH32=y
++CONFIG_SYS_TEXT_BASE=0x88000000
++CONFIG_SYS_MALLOC_F_LEN=0x2000
++CONFIG_NR_DRAM_BANKS=2
++CONFIG_IDENT_STRING=" vexpress_aemv8a fvp aarch32"
++CONFIG_REMAKE_ELF=y
++CONFIG_SYS_LOAD_ADDR=0x90000000
++CONFIG_BOOTDELAY=1
++CONFIG_USE_BOOTARGS=y
++CONFIG_BOOTARGS="console=ttyAMA0 earlycon=pl011,0x1c090000 debug user_debug=31 systemd.log_target=null root=/dev/vda1 rw androidboot.hardware=fvpbase rootwait loglevel=9"
++# CONFIG_DISPLAY_CPUINFO is not set
++# CONFIG_DISPLAY_BOARDINFO is not set
++CONFIG_HUSH_PARSER=y
++CONFIG_SYS_PROMPT="fvp32# "
++# CONFIG_CMD_CONSOLE is not set
++CONFIG_CMD_BOOTZ=y
++# CONFIG_CMD_XIMG is not set
++# CONFIG_CMD_EDITENV is not set
++# CONFIG_CMD_ENV_EXISTS is not set
++CONFIG_CMD_MEMTEST=y
++CONFIG_CMD_ARMFLASH=y
++# CONFIG_CMD_LOADS is not set
++# CONFIG_CMD_ITEST is not set
++# CONFIG_CMD_SETEXPR is not set
++CONFIG_CMD_DHCP=y
++# CONFIG_CMD_NFS is not set
++CONFIG_CMD_MII=y
++CONFIG_CMD_PING=y
++CONFIG_CMD_CACHE=y
++CONFIG_CMD_FAT=y
++CONFIG_DM=y
++CONFIG_MTD_NOR_FLASH=y
++CONFIG_FLASH_CFI_DRIVER=y
++CONFIG_SYS_FLASH_CFI=y
++CONFIG_DM_SERIAL=y
++CONFIG_PL01X_SERIAL=y
++CONFIG_OF_LIBFDT=y
++CONFIG_REMAKE_ELF=y
+diff --git a/include/configs/vexpress_aemv8.h b/include/configs/vexpress_aemv8.h
+index f0c5ceb3849a..854fbb41bfc1 100644
+--- a/include/configs/vexpress_aemv8.h
++++ b/include/configs/vexpress_aemv8.h
+@@ -86,7 +86,7 @@
+ #endif
+ #endif /* !CONFIG_GICV3 */
+ 
+-#if defined(CONFIG_TARGET_VEXPRESS64_BASE_FVP) && !defined(CONFIG_DM_ETH)
++#if (defined(CONFIG_TARGET_VEXPRESS64_BASE_FVP) || defined(CONFIG_TARGET_VEXPRESS64_BASE_FVP_AARCH32)) && !defined(CONFIG_DM_ETH)
+ /* The Vexpress64 BASE_FVP simulator uses SMSC91C111 */
+ #define CONFIG_SMC91111			1
+ #define CONFIG_SMC91111_BASE		(V2M_PA_BASE + 0x01A000000)
+@@ -114,7 +114,7 @@
+ #ifdef CONFIG_TARGET_VEXPRESS64_JUNO
+ #define PHYS_SDRAM_2			(0x880000000)
+ #define PHYS_SDRAM_2_SIZE		0x180000000
+-#elif CONFIG_NR_DRAM_BANKS == 2
++#elif CONFIG_TARGET_VEXPRESS64_BASE_FVP && CONFIG_NR_DRAM_BANKS == 2
+ #define PHYS_SDRAM_2			(0x880000000)
+ #define PHYS_SDRAM_2_SIZE		0x80000000
+ #endif
+@@ -171,23 +171,41 @@
+ 				"fdt_addr_r=0x80000000\0" \
+ 				BOOTENV
+ 
+-#elif CONFIG_TARGET_VEXPRESS64_BASE_FVP
++#elif defined(CONFIG_TARGET_VEXPRESS64_BASE_FVP) || \
++        defined(CONFIG_TARGET_VEXPRESS64_BASE_FVP_AARCH32)
+ 
+-#define VEXPRESS_KERNEL_ADDR	0x80080000
+-#define VEXPRESS_FDT_ADDR	0x8fc00000
+-#define VEXPRESS_BOOT_ADDR	0x8fd00000
+-#define VEXPRESS_RAMDISK_ADDR	0x8fe00000
++#define VEXPRESS_KERNEL_ADDR   0x80080000
++#define VEXPRESS_FDT_ADDR      0x8fc00000
++#define VEXPRESS_BOOT_ADDR     0x8fd00000
++#define VEXPRESS_RAMDISK_ADDR  0x8fe00000
+ 
+-#define CONFIG_EXTRA_ENV_SETTINGS	\
++#define CONFIG_EXTRA_ENV_SETTINGS      \
+ 				"kernel_name=Image\0"		\
+-				"kernel_addr_r=" __stringify(VEXPRESS_KERNEL_ADDR) "\0"	\
+-				"ramdisk_name=ramdisk.img\0"	\
+-				"ramdisk_addr_r=" __stringify(VEXPRESS_RAMDISK_ADDR) "\0" \
+-				"fdtfile=devtree.dtb\0"	\
+-				"fdt_addr_r=" __stringify(VEXPRESS_FDT_ADDR) "\0"	\
+-				"boot_name=boot.img\0" \
+-				"boot_addr_r=" __stringify(VEXPRESS_BOOT_ADDR) "\0"
+-
++                                "kernel_addr_r=" __stringify(VEXPRESS_KERNEL_ADDR) "\0" \
++                                "ramdisk_name=ramdisk.img\0"    \
++                                "ramdisk_addr_r=" __stringify(VEXPRESS_RAMDISK_ADDR) "\0" \
++                                "fdtfile=devtree.dtb\0" \
++                                "fdt_addr_r=" __stringify(VEXPRESS_FDT_ADDR) "\0"       \
++                                "boot_name=boot.img\0" \
++                                "boot_addr_r=" __stringify(VEXPRESS_BOOT_ADDR) "\0"
++
++#ifndef CONFIG_BOOTCOMMAND
++#define CONFIG_BOOTCOMMAND	"if smhload ${boot_name} ${boot_addr_r}; then " \
++				"  set bootargs; " \
++				"  abootimg addr ${boot_addr_r}; " \
++				"  abootimg get dtb --index=0 fdt_addr_r; " \
++				"  bootm ${boot_addr_r} ${boot_addr_r} " \
++				"  ${fdt_addr_r}; " \
++				"else; " \
++				"  smhload ${kernel_name} ${kernel_addr_r}; " \
++				"  smhload ${fdtfile} ${fdt_addr_r}; " \
++				"  smhload ${ramdisk_name} ${initrd_addr_r} "\
++				"  initrd_end; " \
++				"  fdt addr ${fdt_addr_r}; fdt resize; " \
++				"  fdt chosen ${ramdisk_addr_r} ${initrd_end}; " \
++				"  bootz $kernel_addr_r - $fdt_addr_r; " \
++				"fi"
++#endif
+ #endif
+ 
+ /* Monitor Command Prompt */
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0002-Revert-vexpress64-Enable-OF_CONTROL-and-OF_BOARD-for.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0002-Revert-vexpress64-Enable-OF_CONTROL-and-OF_BOARD-for.patch
new file mode 100644
index 0000000..d916d42
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base-arm32/0002-Revert-vexpress64-Enable-OF_CONTROL-and-OF_BOARD-for.patch
@@ -0,0 +1,111 @@
+From e896d48c57d272327410416887f34ac0db550390 Mon Sep 17 00:00:00 2001
+From: Jon Mason <jdmason@kudzu.us>
+Date: Mon, 13 Jun 2022 10:59:53 -0400
+Subject: [PATCH 2/2] Revert "vexpress64: Enable OF_CONTROL and OF_BOARD for
+ VExpress64"
+
+This patch only works for aarch64 (as the 'x' registers are not
+available for ARMv7). Since this platform is ARMv7 in the previous
+patch, this either needs to be changed or removed.  I opted to remove
+it, as it doesn't seem to be necessary to boot the virtual hardware.
+Given that the previous patch was rejected upstream, it is not
+appropriate to fix this upstream.
+
+Upstream-Status: Inappropriate
+Signed-off-by: Jon Mason <jon.mason@arm.com>
+
+This reverts commit 2661397464e47d45cd25bbc5e6b9de7594b3268d.
+---
+ board/armltd/vexpress64/Makefile        |  2 +-
+ board/armltd/vexpress64/lowlevel_init.S | 12 ------------
+ board/armltd/vexpress64/vexpress64.c    | 26 -------------------------
+ 3 files changed, 1 insertion(+), 39 deletions(-)
+ delete mode 100644 board/armltd/vexpress64/lowlevel_init.S
+
+diff --git a/board/armltd/vexpress64/Makefile b/board/armltd/vexpress64/Makefile
+index 1878fbed4ec9..868dc4f629f2 100644
+--- a/board/armltd/vexpress64/Makefile
++++ b/board/armltd/vexpress64/Makefile
+@@ -3,5 +3,5 @@
+ # (C) Copyright 2000-2004
+ # Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ 
+-obj-y	:= vexpress64.o lowlevel_init.o
++obj-y	:= vexpress64.o
+ obj-$(CONFIG_TARGET_VEXPRESS64_JUNO)	+= pcie.o
+diff --git a/board/armltd/vexpress64/lowlevel_init.S b/board/armltd/vexpress64/lowlevel_init.S
+deleted file mode 100644
+index 3dcfb85d0e9a..000000000000
+--- a/board/armltd/vexpress64/lowlevel_init.S
++++ /dev/null
+@@ -1,12 +0,0 @@
+-/* SPDX-License-Identifier: GPL-2.0 */
+-/*
+- * (C) Copyright 2021 Arm Limited
+- */
+-
+-.global save_boot_params
+-save_boot_params:
+-
+-	adr	x8, prior_stage_fdt_address
+-	str	x0, [x8]
+-
+-	b	save_boot_params_ret
+diff --git a/board/armltd/vexpress64/vexpress64.c b/board/armltd/vexpress64/vexpress64.c
+index 5e22e89824ee..cedab86d984b 100644
+--- a/board/armltd/vexpress64/vexpress64.c
++++ b/board/armltd/vexpress64/vexpress64.c
+@@ -92,15 +92,7 @@ int dram_init_banksize(void)
+ 	return 0;
+ }
+ 
+-/* Assigned in lowlevel_init.S
+- * Push the variable into the .data section so that it
+- * does not get cleared later.
+- */
+-unsigned long __section(".data") prior_stage_fdt_address;
+-
+ #ifdef CONFIG_OF_BOARD
+-
+-#ifdef CONFIG_TARGET_VEXPRESS64_JUNO
+ #define JUNO_FLASH_SEC_SIZE	(256 * 1024)
+ static phys_addr_t find_dtb_in_nor_flash(const char *partname)
+ {
+@@ -145,11 +137,9 @@ static phys_addr_t find_dtb_in_nor_flash(const char *partname)
+ 
+ 	return ~0;
+ }
+-#endif
+ 
+ void *board_fdt_blob_setup(int *err)
+ {
+-#ifdef CONFIG_TARGET_VEXPRESS64_JUNO
+ 	phys_addr_t fdt_rom_addr = find_dtb_in_nor_flash(CONFIG_JUNO_DTB_PART);
+ 
+ 	*err = 0;
+@@ -159,22 +149,6 @@ void *board_fdt_blob_setup(int *err)
+ 	}
+ 
+ 	return (void *)fdt_rom_addr;
+-#endif
+-
+-#ifdef VEXPRESS_FDT_ADDR
+-	if (fdt_magic(VEXPRESS_FDT_ADDR) == FDT_MAGIC) {
+-		*err = 0;
+-		return (void *)VEXPRESS_FDT_ADDR;
+-	}
+-#endif
+-
+-	if (fdt_magic(prior_stage_fdt_address) == FDT_MAGIC) {
+-		*err = 0;
+-		return (void *)prior_stage_fdt_address;
+-	}
+-
+-	*err = -ENXIO;
+-	return NULL;
+ }
+ #endif
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base/bootargs.cfg b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base/bootargs.cfg
new file mode 100644
index 0000000..716600f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-base/bootargs.cfg
@@ -0,0 +1,3 @@
+CONFIG_BOOTARGS="console=ttyAMA0 earlycon=pl011,0x1c090000 root=/dev/vda1 rw rootwait"
+# Our FVP support CRC instructions
+CONFIG_ARM64_CRC32=y
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0001-armv8-Add-ARMv8-MPU-configuration-logic.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0001-armv8-Add-ARMv8-MPU-configuration-logic.patch
new file mode 100644
index 0000000..dd6b77d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0001-armv8-Add-ARMv8-MPU-configuration-logic.patch
@@ -0,0 +1,259 @@
+From e90aa7853ae32cb03c86249a6c572ec88cdebaa2 Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Wed, 26 May 2021 17:41:10 +0100
+Subject: [PATCH 1/9] armv8: Add ARMv8 MPU configuration logic
+
+Detect whether an MMU is present at the current exception level. If
+not, initialize the MPU instead of the MMU during init, and clear the
+MPU regions before transition to Linux.
+
+The MSA in use at EL1&0 may be configurable but can only by determined
+by inspecting VTCR_EL2 at EL2, so assume that there is an MMU for
+backwards compatibility.
+
+Provide a default (blank) MPU memory map, which can be overridden by
+board configurations.
+
+Issue-Id: SCM-2443
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: I0ee3879f9d7f03fe940664b3551c68eeaa458d17
+---
+ arch/arm/cpu/armv8/cache_v8.c    | 101 ++++++++++++++++++++++++++++++-
+ arch/arm/include/asm/armv8/mpu.h |  59 ++++++++++++++++++
+ arch/arm/include/asm/system.h    |  19 ++++++
+ 3 files changed, 176 insertions(+), 3 deletions(-)
+ create mode 100644 arch/arm/include/asm/armv8/mpu.h
+
+diff --git a/arch/arm/cpu/armv8/cache_v8.c b/arch/arm/cpu/armv8/cache_v8.c
+index e4736e5643..798aed8058 100644
+--- a/arch/arm/cpu/armv8/cache_v8.c
++++ b/arch/arm/cpu/armv8/cache_v8.c
+@@ -15,6 +15,7 @@
+ #include <asm/global_data.h>
+ #include <asm/system.h>
+ #include <asm/armv8/mmu.h>
++#include <asm/armv8/mpu.h>
+ 
+ DECLARE_GLOBAL_DATA_PTR;
+ 
+@@ -385,6 +386,91 @@ __weak u64 get_page_table_size(void)
+ 	return size;
+ }
+ 
++static void mpu_clear_regions(void)
++{
++	int i;
++
++	for (i = 0; mpu_mem_map[i].end || mpu_mem_map[i].attrs; i++) {
++		setup_el2_mpu_region(i, 0, 0);
++	}
++}
++
++static struct mpu_region default_mpu_mem_map[] = {{0,}};
++__weak struct mpu_region *mpu_mem_map = default_mpu_mem_map;
++
++static void mpu_setup(void)
++{
++	int i;
++
++	if (current_el() != 2) {
++		panic("MPU configuration is only supported at EL2");
++	}
++
++	set_sctlr(get_sctlr() & ~(CR_M | CR_WXN));
++
++	asm volatile("msr MAIR_EL2, %0" : : "r" MEMORY_ATTRIBUTES);
++
++	for (i = 0; mpu_mem_map[i].end || mpu_mem_map[i].attrs; i++) {
++		setup_el2_mpu_region(i,
++			PRBAR_ADDRESS(mpu_mem_map[i].start)
++				| PRBAR_OUTER_SH | PRBAR_AP_RW_ANY,
++			PRLAR_ADDRESS(mpu_mem_map[i].end)
++				| mpu_mem_map[i].attrs | PRLAR_EN_BIT
++			);
++	}
++
++	set_sctlr(get_sctlr() | CR_M);
++}
++
++static bool el_has_mmu(void)
++{
++	if (current_el() < 2) {
++		// We have no way of knowing, so assuming we have an MMU
++		return true;
++	}
++
++	uint64_t id_aa64mmfr0;
++	asm volatile("mrs %0, id_aa64mmfr0_el1"
++			: "=r" (id_aa64mmfr0) : : "cc");
++	uint64_t msa = id_aa64mmfr0 & ID_AA64MMFR0_EL1_MSA_MASK;
++	uint64_t msa_frac = id_aa64mmfr0 & ID_AA64MMFR0_EL1_MSA_FRAC_MASK;
++
++	switch (msa) {
++		case ID_AA64MMFR0_EL1_MSA_VMSA:
++			/*
++			 * VMSA supported in all translation regimes.
++			 * No support for PMSA.
++			 */
++			return true;
++		case ID_AA64MMFR0_EL1_MSA_USE_FRAC:
++			/* See MSA_frac for the supported MSAs. */
++			switch (msa_frac) {
++				case ID_AA64MMFR0_EL1_MSA_FRAC_NO_PMSA:
++					/*
++					 * PMSA not supported in any translation
++					 * regime.
++					 */
++					return true;
++				case ID_AA64MMFR0_EL1_MSA_FRAC_VMSA:
++					/*
++					* PMSA supported in all translation
++					* regimes. No support for VMSA.
++					*/
++				case ID_AA64MMFR0_EL1_MSA_FRAC_PMSA:
++					/*
++					 * PMSA supported in all translation
++					 * regimes.
++					 */
++					return false;
++				default:
++					panic("Unsupported id_aa64mmfr0_el1 " \
++						"MSA_frac value");
++			}
++		default:
++			panic("Unsupported id_aa64mmfr0_el1 MSA value");
++	}
++}
++
+ void setup_pgtables(void)
+ {
+ 	int i;
+@@ -499,8 +585,13 @@ void dcache_enable(void)
+ 	/* The data cache is not active unless the mmu is enabled */
+ 	if (!(get_sctlr() & CR_M)) {
+ 		invalidate_dcache_all();
+-		__asm_invalidate_tlb_all();
+-		mmu_setup();
++
++		if (el_has_mmu()) {
++			__asm_invalidate_tlb_all();
++			mmu_setup();
++		} else {
++			mpu_setup();
++		}
+ 	}
+ 
+ 	set_sctlr(get_sctlr() | CR_C);
+@@ -519,7 +610,11 @@ void dcache_disable(void)
+ 	set_sctlr(sctlr & ~(CR_C|CR_M));
+ 
+ 	flush_dcache_all();
+-	__asm_invalidate_tlb_all();
++
++	if (el_has_mmu())
++		__asm_invalidate_tlb_all();
++	else
++		mpu_clear_regions();
+ }
+ 
+ int dcache_status(void)
+diff --git a/arch/arm/include/asm/armv8/mpu.h b/arch/arm/include/asm/armv8/mpu.h
+new file mode 100644
+index 0000000000..8de627cafd
+--- /dev/null
++++ b/arch/arm/include/asm/armv8/mpu.h
+@@ -0,0 +1,59 @@
++/*
++ * SPDX-License-Identifier: GPL-2.0+
++ *
++ * (C) Copyright 2021 Arm Limited
++ */
++
++#ifndef _ASM_ARMV8_MPU_H_
++#define _ASM_ARMV8_MPU_H_
++
++#include <asm/armv8/mmu.h>
++#include <linux/stringify.h>
++
++#define PRSELR_EL2		S3_4_c6_c2_1
++#define PRBAR_EL2		S3_4_c6_c8_0
++#define PRLAR_EL2		S3_4_c6_c8_1
++#define MPUIR_EL2		S3_4_c0_c0_4
++
++#define PRBAR_ADDRESS(addr)	((addr) & ~(0x3fULL))
++
++/* Access permissions */
++#define PRBAR_AP(val)		(((val) & 0x3) << 2)
++#define PRBAR_AP_RW_HYP		PRBAR_AP(0x0)
++#define PRBAR_AP_RW_ANY		PRBAR_AP(0x1)
++#define PRBAR_AP_RO_HYP		PRBAR_AP(0x2)
++#define PRBAR_AP_RO_ANY		PRBAR_AP(0x3)
++
++/* Shareability */
++#define PRBAR_SH(val)		(((val) & 0x3) << 4)
++#define PRBAR_NON_SH		PRBAR_SH(0x0)
++#define PRBAR_OUTER_SH		PRBAR_SH(0x2)
++#define PRBAR_INNER_SH		PRBAR_SH(0x3)
++
++/* Memory attribute (MAIR idx) */
++#define PRLAR_ATTRIDX(val)	(((val) & 0x7) << 1)
++#define PRLAR_EN_BIT		(0x1)
++#define PRLAR_ADDRESS(addr)	((addr) & ~(0x3fULL))
++
++#ifndef __ASSEMBLY__
++
++static inline void setup_el2_mpu_region(uint8_t region, uint64_t base, uint64_t limit)
++{
++	asm volatile("msr " __stringify(PRSELR_EL2) ", %0" : : "r" (region));
++	asm volatile("msr " __stringify(PRBAR_EL2) ", %0" : : "r" (base));
++	asm volatile("msr " __stringify(PRLAR_EL2) ", %0" : : "r" (limit));
++
++	asm volatile("isb");
++}
++
++#endif
++
++struct mpu_region {
++	u64 start;
++	u64 end;
++	u64 attrs;
++};
++
++extern struct mpu_region *mpu_mem_map;
++
++#endif /* _ASM_ARMV8_MPU_H_ */
+diff --git a/arch/arm/include/asm/system.h b/arch/arm/include/asm/system.h
+index 87d1c77e8b..4510db98a2 100644
+--- a/arch/arm/include/asm/system.h
++++ b/arch/arm/include/asm/system.h
+@@ -95,6 +95,25 @@
+ 				               auth algorithm                 */
+ #define ID_AA64ISAR1_EL1_APA	(0xF << 4)  /* QARMA address auth algorithm   */
+ 
++/*
++ * ID_AA64MMFR0_EL1 bits definitions
++ */
++#define ID_AA64MMFR0_EL1_MSA_FRAC_MASK		(0xFUL << 52) /* Memory system
++								 architecture
++								 frac         */
++#define ID_AA64MMFR0_EL1_MSA_FRAC_VMSA		(0x2UL << 52) /* EL1&0 supports
++								 VMSA         */
++#define ID_AA64MMFR0_EL1_MSA_FRAC_PMSA		(0x1UL << 52) /* EL1&0 only
++							         supports PMSA*/
++#define ID_AA64MMFR0_EL1_MSA_FRAC_NO_PMSA	(0x0UL << 52) /* No PMSA
++								 support      */
++#define ID_AA64MMFR0_EL1_MSA_MASK		(0xFUL << 48) /* Memory system
++								 architecture */
++#define ID_AA64MMFR0_EL1_MSA_USE_FRAC		(0xFUL << 48) /* Use MSA_FRAC */
++#define ID_AA64MMFR0_EL1_MSA_VMSA		(0x0UL << 48) /* Memory system
++								 architecture
++								 is VMSA      */
++
+ /*
+  * ID_AA64PFR0_EL1 bits definitions
+  */
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0002-vexpress64-add-MPU-memory-map-for-the-BASER_FVP.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0002-vexpress64-add-MPU-memory-map-for-the-BASER_FVP.patch
new file mode 100644
index 0000000..f405457
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0002-vexpress64-add-MPU-memory-map-for-the-BASER_FVP.patch
@@ -0,0 +1,61 @@
+From 80c64d0c035a5cd0cb23c7b01be39f25e15c7193 Mon Sep 17 00:00:00 2001
+From: Qi Feng <qi.feng@arm.com>
+Date: Tue, 26 Jul 2022 18:13:23 +0800
+Subject: [PATCH 2/9] vexpress64: add MPU memory map for the BASER_FVP
+
+The previous patch added support for initializing an Armv8 MPU. There is only an
+MPU at S-EL2 on the BASER_FVP, so add a platform-specific MPU memory map.
+
+See https://developer.arm.com/documentation/100964/1117/Base-Platform/Base---memory/BaseR-Platform-memory-map
+
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Signed-off-by: Qi Feng <qi.feng@arm.com>
+---
+ board/armltd/vexpress64/vexpress64.c | 22 ++++++++++++++++++++++
+ 1 file changed, 22 insertions(+)
+
+diff --git a/board/armltd/vexpress64/vexpress64.c b/board/armltd/vexpress64/vexpress64.c
+index 709ebf3fb0..1791cd986f 100644
+--- a/board/armltd/vexpress64/vexpress64.c
++++ b/board/armltd/vexpress64/vexpress64.c
+@@ -19,6 +19,7 @@
+ #include <dm/platform_data/serial_pl01x.h>
+ #include "pcie.h"
+ #include <asm/armv8/mmu.h>
++#include <asm/armv8/mpu.h>
+ #ifdef CONFIG_VIRTIO_NET
+ #include <virtio_types.h>
+ #include <virtio.h>
+@@ -37,6 +38,27 @@ U_BOOT_DRVINFO(vexpress_serials) = {
+ 	.plat = &serial_plat,
+ };
+ 
++static struct mpu_region vexpress64_aemv8r_mem_map[] = {
++	{
++		.start = 0x0UL,
++		.end = 0x7fffffffUL,
++		.attrs = PRLAR_ATTRIDX(MT_NORMAL)
++	}, {
++		.start = 0x80000000UL,
++		.end = 0xffffffffUL,
++		.attrs = PRLAR_ATTRIDX(MT_DEVICE_NGNRNE)
++	}, {
++		.start = 0x100000000UL,
++		.end = 0xffffffffffUL,
++		.attrs = PRLAR_ATTRIDX(MT_NORMAL)
++	}, {
++		/* List terminator */
++		0,
++	}
++};
++
++struct mpu_region *mpu_mem_map = vexpress64_aemv8r_mem_map;
++
+ static struct mm_region vexpress64_mem_map[] = {
+ 	{
+ 		.virt = V2M_PA_BASE,
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0003-armv8-Allow-disabling-exception-vectors-on-non-SPL-b.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0003-armv8-Allow-disabling-exception-vectors-on-non-SPL-b.patch
new file mode 100644
index 0000000..e6ccc17
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0003-armv8-Allow-disabling-exception-vectors-on-non-SPL-b.patch
@@ -0,0 +1,107 @@
+From 61a2df4467c13bbc83bff8a3429e9ebb4b957b56 Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Fri, 10 Dec 2021 11:41:19 +0000
+Subject: [PATCH 3/9] armv8: Allow disabling exception vectors on non-SPL
+ builds
+
+On the BASER_FVP, U-Boot shares EL2 with another bootloader, so we do
+not wish to overide the exception vector, but we are also not using an
+SPL build.
+
+Therefore, add ARMV8_EXCEPTION_VECTORS, which disables exception vectors
+in a similar way to ARMV8_SPL_EXCEPTION_VECTORS.
+
+Rename ARMV8_SPL_EXCEPTION_VECTORS -> SPL_ARMV8_EXCEPTION_VECTORS so
+that both config flags be be targeted using CONFIG_IS_ENABLED.
+
+Issue-Id: SCM-3728
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: I0cf0fc6d7ef4d45791411cf1f67c65e198cc8b2b
+---
+ arch/arm/cpu/armv8/Kconfig        | 10 ++++++++--
+ arch/arm/cpu/armv8/Makefile       |  6 ++----
+ arch/arm/cpu/armv8/start.S        |  4 ++--
+ configs/vexpress_aemv8r_defconfig |  1 +
+ 4 files changed, 13 insertions(+), 8 deletions(-)
+
+diff --git a/arch/arm/cpu/armv8/Kconfig b/arch/arm/cpu/armv8/Kconfig
+index 09f3f50fa2..031faa909c 100644
+--- a/arch/arm/cpu/armv8/Kconfig
++++ b/arch/arm/cpu/armv8/Kconfig
+@@ -1,8 +1,8 @@
+ if ARM64
+ 
+-config ARMV8_SPL_EXCEPTION_VECTORS
++config ARMV8_EXCEPTION_VECTORS
+ 	bool "Install crash dump exception vectors"
+-	depends on SPL
++	default y
+ 	help
+ 	  The default exception vector table is only used for the crash
+ 	  dump, but still takes quite a lot of space in the image size.
+@@ -10,6 +10,12 @@ config ARMV8_SPL_EXCEPTION_VECTORS
+ 	  Say N here if you are running out of code space in the image
+ 	  and want to save some space at the cost of less debugging info.
+ 
++config SPL_ARMV8_EXCEPTION_VECTORS
++	bool "Install crash dump exception vectors in the SPL"
++	depends on SPL
++	help
++	  Same as ARMV8_EXCEPTION_VECTORS, but for SPL builds
++
+ config ARMV8_MULTIENTRY
+         bool "Enable multiple CPUs to enter into U-Boot"
+ 
+diff --git a/arch/arm/cpu/armv8/Makefile b/arch/arm/cpu/armv8/Makefile
+index 85fe0475c8..8b3f695835 100644
+--- a/arch/arm/cpu/armv8/Makefile
++++ b/arch/arm/cpu/armv8/Makefile
+@@ -13,10 +13,8 @@ ifndef CONFIG_$(SPL_)SYS_DCACHE_OFF
+ obj-y	+= cache_v8.o
+ obj-y	+= cache.o
+ endif
+-ifdef CONFIG_SPL_BUILD
+-obj-$(CONFIG_ARMV8_SPL_EXCEPTION_VECTORS) += exceptions.o
+-else
+-obj-y	+= exceptions.o
++obj-$(CONFIG_$(SPL_)ARMV8_EXCEPTION_VECTORS) += exceptions.o
++ifndef CONFIG_SPL_BUILD
+ obj-y	+= exception_level.o
+ endif
+ obj-y	+= tlb.o
+diff --git a/arch/arm/cpu/armv8/start.S b/arch/arm/cpu/armv8/start.S
+index 28f0df13f0..f831e77af3 100644
+--- a/arch/arm/cpu/armv8/start.S
++++ b/arch/arm/cpu/armv8/start.S
+@@ -104,7 +104,7 @@ pie_skip_reloc:
+ pie_fixup_done:
+ #endif
+ 
+-#if defined(CONFIG_ARMV8_SPL_EXCEPTION_VECTORS) || !defined(CONFIG_SPL_BUILD)
++#if CONFIG_IS_ENABLED(ARMV8_EXCEPTION_VECTORS)
+ .macro	set_vbar, regname, reg
+ 	msr	\regname, \reg
+ .endm
+@@ -354,7 +354,7 @@ ENDPROC(smp_kick_all_cpus)
+ /*-----------------------------------------------------------------------*/
+ 
+ ENTRY(c_runtime_cpu_setup)
+-#if defined(CONFIG_ARMV8_SPL_EXCEPTION_VECTORS) || !defined(CONFIG_SPL_BUILD)
++#if CONFIG_IS_ENABLED(ARMV8_EXCEPTION_VECTORS)
+ 	/* Relocate vBAR */
+ 	adr	x0, vectors
+ 	switch_el x1, 3f, 2f, 1f
+diff --git a/configs/vexpress_aemv8r_defconfig b/configs/vexpress_aemv8r_defconfig
+index a1c5d88717..1d5b7411f0 100644
+--- a/configs/vexpress_aemv8r_defconfig
++++ b/configs/vexpress_aemv8r_defconfig
+@@ -13,3 +13,4 @@ CONFIG_BOOTARGS="console=ttyAMA0 earlycon=pl011,0x9c090000 rootfstype=ext4 root=
+ CONFIG_SYS_PROMPT="VExpress64# "
+ # CONFIG_MMC is not set
+ CONFIG_VIRTIO_MMIO=y
++CONFIG_ARMV8_EXCEPTION_VECTORS=n
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0004-armv8-ARMV8_SWITCH_TO_EL1-improvements.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0004-armv8-ARMV8_SWITCH_TO_EL1-improvements.patch
new file mode 100644
index 0000000..84f6b4b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0004-armv8-ARMV8_SWITCH_TO_EL1-improvements.patch
@@ -0,0 +1,160 @@
+From dbc1a218e9837e39cd50dd3c19f603f29a08ddba Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Wed, 14 Jul 2021 12:44:27 +0100
+Subject: [PATCH 4/9] armv8: ARMV8_SWITCH_TO_EL1 improvements
+
+Convert CONFIG_ARMV8_SWITCH_TO_EL1 to a Kconfig variable.
+
+Add support for switching to EL1 to bootefi.
+
+Add the environment variable armv8_switch_to_el1 to allow configuring
+whether to switch to EL1 at runtime. This overrides the compile-time
+option.
+
+Issue-Id: SCM-3728
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: If98478148d6d8d1f732acac5439276700614815f
+---
+ arch/arm/cpu/armv8/Kconfig           |  8 +++++++
+ arch/arm/cpu/armv8/exception_level.c | 21 ++++++++++++++--
+ arch/arm/lib/bootm.c                 | 36 ++++++++++++++++------------
+ configs/vexpress_aemv8r_defconfig    |  1 +
+ 4 files changed, 49 insertions(+), 17 deletions(-)
+
+diff --git a/arch/arm/cpu/armv8/Kconfig b/arch/arm/cpu/armv8/Kconfig
+index 031faa909c..110adf63b3 100644
+--- a/arch/arm/cpu/armv8/Kconfig
++++ b/arch/arm/cpu/armv8/Kconfig
+@@ -191,4 +191,12 @@ config ARMV8_EA_EL3_FIRST
+ 	  Exception handling at all exception levels for External Abort and
+ 	  SError interrupt exception are taken in EL3.
+ 
++config ARMV8_SWITCH_TO_EL1
++	bool "Switch to EL1 before booting the operating system"
++	default n
++	help
++	  Switch to EL1 before booting the operating system, if for example the
++	  operating system does not support booting at EL2, or you wish to prevent
++	  any hypervisors from running. Supported for bootm, booti and bootefi.
++
+ endif
+diff --git a/arch/arm/cpu/armv8/exception_level.c b/arch/arm/cpu/armv8/exception_level.c
+index b11936548f..4aad1550f4 100644
+--- a/arch/arm/cpu/armv8/exception_level.c
++++ b/arch/arm/cpu/armv8/exception_level.c
+@@ -40,19 +40,36 @@ static void entry_non_secure(struct jmp_buf_data *non_secure_jmp)
+  * trusted firmware being one embodiment). The operating system shall be
+  * started at exception level EL2. So here we check the exception level
+  * and switch it if necessary.
++ *
++ * If armv8_switch_to_el1 (config or env var) is enabled, also switch to EL1
++ * before booting the operating system.
+  */
+ void switch_to_non_secure_mode(void)
+ {
+ 	struct jmp_buf_data non_secure_jmp;
+ 
+ 	/* On AArch64 we need to make sure we call our payload in < EL3 */
+-	if (current_el() == 3) {
++
++	int switch_to_el1 = env_get_yesno("armv8_switch_to_el1");
++#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
++	if (switch_to_el1 == -1) {
++			switch_to_el1 = 1;
++	}
++#endif
++
++	if (current_el() > 2) {
+ 		if (setjmp(&non_secure_jmp))
+ 			return;
+ 		dcache_disable();	/* flush cache before switch to EL2 */
+-
+ 		/* Move into EL2 and keep running there */
+ 		armv8_switch_to_el2((uintptr_t)&non_secure_jmp, 0, 0, 0,
+ 				    (uintptr_t)entry_non_secure, ES_TO_AARCH64);
++	} else if (switch_to_el1 == 1 && current_el() > 1) {
++		if (setjmp(&non_secure_jmp))
++			return;
++		dcache_disable();	/* flush cache before switch to EL1 */
++		/* Move into EL1 and keep running there */
++		armv8_switch_to_el1((uintptr_t)&non_secure_jmp, 0, 0, 0,
++				    (uintptr_t)entry_non_secure, ES_TO_AARCH64);
+ 	}
+ }
+diff --git a/arch/arm/lib/bootm.c b/arch/arm/lib/bootm.c
+index a59a5e6c0e..e2cf2e6ec4 100644
+--- a/arch/arm/lib/bootm.c
++++ b/arch/arm/lib/bootm.c
+@@ -272,7 +272,6 @@ __weak void update_os_arch_secondary_cores(uint8_t os_arch)
+ {
+ }
+ 
+-#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+ static void switch_to_el1(void)
+ {
+ 	if ((IH_ARCH_DEFAULT == IH_ARCH_ARM64) &&
+@@ -287,7 +286,6 @@ static void switch_to_el1(void)
+ 				    ES_TO_AARCH64);
+ }
+ #endif
+-#endif
+ 
+ /* Subcommand: GO */
+ static void boot_jump_linux(bootm_headers_t *images, int flag)
+@@ -314,21 +312,29 @@ static void boot_jump_linux(bootm_headers_t *images, int flag)
+ 
+ 		update_os_arch_secondary_cores(images->os.arch);
+ 
++		int armv8_switch_to_el1 = env_get_yesno("armv8_switch_to_el1");
+ #ifdef CONFIG_ARMV8_SWITCH_TO_EL1
+-		armv8_switch_to_el2((u64)images->ft_addr, 0, 0, 0,
+-				    (u64)switch_to_el1, ES_TO_AARCH64);
+-#else
+-		if ((IH_ARCH_DEFAULT == IH_ARCH_ARM64) &&
+-		    (images->os.arch == IH_ARCH_ARM))
+-			armv8_switch_to_el2(0, (u64)gd->bd->bi_arch_number,
+-					    (u64)images->ft_addr, 0,
+-					    (u64)images->ep,
+-					    ES_TO_AARCH32);
+-		else
+-			armv8_switch_to_el2((u64)images->ft_addr, 0, 0, 0,
+-					    images->ep,
+-					    ES_TO_AARCH64);
++		if (armv8_switch_to_el1 == -1) {
++			armv8_switch_to_el1 = 1;
++		}
+ #endif
++		if (armv8_switch_to_el1 == 1) {
++			armv8_switch_to_el2((u64)images->ft_addr, 0, 0, 0,
++					    (u64)switch_to_el1, ES_TO_AARCH64);
++		} else {
++			if ((IH_ARCH_DEFAULT == IH_ARCH_ARM64) &&
++					(images->os.arch == IH_ARCH_ARM))
++				armv8_switch_to_el2(0,
++						    (u64)gd->bd->bi_arch_number,
++						    (u64)images->ft_addr, 0,
++						    (u64)images->ep,
++						    ES_TO_AARCH32);
++			else
++				armv8_switch_to_el2((u64)images->ft_addr,
++						    0, 0, 0,
++						    images->ep,
++						    ES_TO_AARCH64);
++		}
+ 	}
+ #else
+ 	unsigned long machid = gd->bd->bi_arch_number;
+diff --git a/configs/vexpress_aemv8r_defconfig b/configs/vexpress_aemv8r_defconfig
+index 1d5b7411f0..35e5e8a5e1 100644
+--- a/configs/vexpress_aemv8r_defconfig
++++ b/configs/vexpress_aemv8r_defconfig
+@@ -14,3 +14,4 @@ CONFIG_SYS_PROMPT="VExpress64# "
+ # CONFIG_MMC is not set
+ CONFIG_VIRTIO_MMIO=y
+ CONFIG_ARMV8_EXCEPTION_VECTORS=n
++CONFIG_ARMV8_SWITCH_TO_EL1=y
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0005-armv8-Make-disabling-HVC-configurable-when-switching.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0005-armv8-Make-disabling-HVC-configurable-when-switching.patch
new file mode 100644
index 0000000..b546026
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0005-armv8-Make-disabling-HVC-configurable-when-switching.patch
@@ -0,0 +1,83 @@
+From f75060be04aaed4bb5a07cb40361d694b3775f4a Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Fri, 10 Dec 2021 16:37:26 +0000
+Subject: [PATCH 5/9] armv8: Make disabling HVC configurable when switching to
+ EL1
+
+On the BASER_FVP there is no EL3, so HVC is used to provide PSCI
+services. Therefore we cannot disable hypercalls.
+
+Create CONFIG_ARMV8_DISABLE_HVC (dependent on CONFIG_ARMV8_TO_EL1) to
+control whether to disable HVC exceptions in HCR_EL2->HCD
+
+Issue-Id: SCM-3728
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: I463d82f1db8a3cafcab40a9c0c208753569cc300
+---
+ arch/arm/cpu/armv8/Kconfig        |  9 +++++++++
+ arch/arm/include/asm/macro.h      | 10 ++++++++--
+ configs/vexpress_aemv8r_defconfig |  1 +
+ 3 files changed, 18 insertions(+), 2 deletions(-)
+
+diff --git a/arch/arm/cpu/armv8/Kconfig b/arch/arm/cpu/armv8/Kconfig
+index 110adf63b3..a821e219d8 100644
+--- a/arch/arm/cpu/armv8/Kconfig
++++ b/arch/arm/cpu/armv8/Kconfig
+@@ -199,4 +199,13 @@ config ARMV8_SWITCH_TO_EL1
+ 	  operating system does not support booting at EL2, or you wish to prevent
+ 	  any hypervisors from running. Supported for bootm, booti and bootefi.
+ 
++config ARMV8_DISABLE_HVC
++	bool "Disable HVC calls before switching to EL1"
++	depends on ARMV8_SWITCH_TO_EL1
++	default y
++	help
++	  If switching to EL1 before loading the operating system, disable taking
++	  hypercalls back to EL2. May be disabled if, for example, PSCI services are
++	  running at EL2.
++
+ endif
+diff --git a/arch/arm/include/asm/macro.h b/arch/arm/include/asm/macro.h
+index 1a1edc9870..7167739210 100644
+--- a/arch/arm/include/asm/macro.h
++++ b/arch/arm/include/asm/macro.h
+@@ -296,9 +296,12 @@ lr	.req	x30
+ 	ldr	\tmp2, =(ID_AA64ISAR1_EL1_GPI | ID_AA64ISAR1_EL1_GPA | \
+ 		      ID_AA64ISAR1_EL1_API | ID_AA64ISAR1_EL1_APA)
+ 	tst	\tmp, \tmp2
+-	mov	\tmp2, #(HCR_EL2_RW_AARCH64 | HCR_EL2_HCD_DIS)
++	mov	\tmp2, #(HCR_EL2_RW_AARCH64)
+ 	orr	\tmp, \tmp2, #(HCR_EL2_APK | HCR_EL2_API)
+ 	csel	\tmp, \tmp2, \tmp, eq
++#ifdef CONFIG_ARMV8_DISABLE_HVC
++	orr	\tmp, \tmp, #(HCR_EL2_HCD_DIS)
++#endif
+ 	msr	hcr_el2, \tmp
+ 
+ 	/* Return to the EL1_SP1 mode from EL2 */
+@@ -311,7 +314,10 @@ lr	.req	x30
+ 
+ 1:
+ 	/* Initialize HCR_EL2 */
+-	ldr	\tmp, =(HCR_EL2_RW_AARCH32 | HCR_EL2_HCD_DIS)
++	ldr	\tmp, =(HCR_EL2_RW_AARCH32)
++#ifdef CONFIG_ARMV8_DISABLE_HVC
++	orr	\tmp, \tmp, #(HCR_EL2_HCD_DIS)
++#endif
+ 	msr	hcr_el2, \tmp
+ 
+ 	/* Return to AArch32 Supervisor mode from EL2 */
+diff --git a/configs/vexpress_aemv8r_defconfig b/configs/vexpress_aemv8r_defconfig
+index 35e5e8a5e1..605a724a61 100644
+--- a/configs/vexpress_aemv8r_defconfig
++++ b/configs/vexpress_aemv8r_defconfig
+@@ -15,3 +15,4 @@ CONFIG_SYS_PROMPT="VExpress64# "
+ CONFIG_VIRTIO_MMIO=y
+ CONFIG_ARMV8_EXCEPTION_VECTORS=n
+ CONFIG_ARMV8_SWITCH_TO_EL1=y
++CONFIG_ARMV8_DISABLE_HVC=n
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0006-vexpress64-Do-not-set-COUNTER_FREQUENCY.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0006-vexpress64-Do-not-set-COUNTER_FREQUENCY.patch
new file mode 100644
index 0000000..ca99d0e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0006-vexpress64-Do-not-set-COUNTER_FREQUENCY.patch
@@ -0,0 +1,32 @@
+From db7d4cc91ae19d0cbd05c0ff1b9bf6a55ac0d04a Mon Sep 17 00:00:00 2001
+From: Qi Feng <qi.feng@arm.com>
+Date: Thu, 28 Jul 2022 17:47:18 +0800
+Subject: [PATCH 6/9] vexpress64: Do not set COUNTER_FREQUENCY
+
+VExpress boards normally run as a second-stage bootloader so should not
+need to modify CNTFRQ_EL0. On the BASER_FVP, U-Boot can modify it if
+running at EL2, but shouldn't because it might be different from the
+value being used by the first-stage bootloader (which might be
+providing PSCI services).
+
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Signed-off-by: Qi Feng <qi.feng@arm.com>
+---
+ configs/vexpress_aemv8r_defconfig | 1 -
+ 1 file changed, 1 deletion(-)
+
+diff --git a/configs/vexpress_aemv8r_defconfig b/configs/vexpress_aemv8r_defconfig
+index 605a724a61..153fa14efc 100644
+--- a/configs/vexpress_aemv8r_defconfig
++++ b/configs/vexpress_aemv8r_defconfig
+@@ -1,5 +1,4 @@
+ CONFIG_ARM=y
+-CONFIG_COUNTER_FREQUENCY=24000000
+ CONFIG_ARCH_VEXPRESS64=y
+ CONFIG_NR_DRAM_BANKS=2
+ CONFIG_DEFAULT_DEVICE_TREE="arm_fvp"
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0007-vexpress64-Enable-LIBFDT_OVERLAY-in-the-vexpress_aem.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0007-vexpress64-Enable-LIBFDT_OVERLAY-in-the-vexpress_aem.patch
new file mode 100644
index 0000000..467fb40
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0007-vexpress64-Enable-LIBFDT_OVERLAY-in-the-vexpress_aem.patch
@@ -0,0 +1,27 @@
+From f4986ed954ffad36abfa27db5520e702cba2531e Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Tue, 22 Feb 2022 15:32:51 +0000
+Subject: [PATCH 7/9] vexpress64: Enable LIBFDT_OVERLAY in the vexpress_aemv8r
+ defconfig
+
+Issue-Id: SCM-3874
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Change-Id: Ide0532cf2de89f1bca9c8d4bd2ed0c1a1c57599f
+---
+ configs/vexpress_aemv8r_defconfig | 1 +
+ 1 file changed, 1 insertion(+)
+
+diff --git a/configs/vexpress_aemv8r_defconfig b/configs/vexpress_aemv8r_defconfig
+index 153fa14efc..94651768b3 100644
+--- a/configs/vexpress_aemv8r_defconfig
++++ b/configs/vexpress_aemv8r_defconfig
+@@ -15,3 +15,4 @@ CONFIG_VIRTIO_MMIO=y
+ CONFIG_ARMV8_EXCEPTION_VECTORS=n
+ CONFIG_ARMV8_SWITCH_TO_EL1=y
+ CONFIG_ARMV8_DISABLE_HVC=n
++CONFIG_OF_LIBFDT_OVERLAY=y
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0008-armv8-Allow-PRBAR-MPU-attributes-to-be-configured.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0008-armv8-Allow-PRBAR-MPU-attributes-to-be-configured.patch
new file mode 100644
index 0000000..4b6daf8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0008-armv8-Allow-PRBAR-MPU-attributes-to-be-configured.patch
@@ -0,0 +1,105 @@
+From af05764e7b64e0704291ba2e71138f2345737afa Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Wed, 18 May 2022 15:24:19 +0100
+Subject: [PATCH 8/9] armv8: Allow PRBAR MPU attributes to be configured
+
+In a previous patch, support was added to initialize an S-EL2 MPU on
+armv8r64 machines. This implementation allowed the PRLAR attribute
+index to be configured, but not the shareability and access permission
+attributes in PRBAR. These attributes were hard-coded as "outer
+shareable" and "read/write at EL1 and EL0".
+
+Add separate prlar_attrs and prbar_attrs to the MPU region struct so
+that these attributes can be configured on a per-region basis.
+
+For the BASER_FVP, ensure the MPU memory attributes match those in the
+existing vexpress64 board MMU configuration ("non shareable" for device
+memory and "inner shareable" for normal memory).
+
+Issue-Id: SCM-4641
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Change-Id: I6b72aead91ad12412262aa32c61a53e12eab3984
+---
+ arch/arm/cpu/armv8/cache_v8.c        | 12 ++++++++----
+ arch/arm/include/asm/armv8/mpu.h     |  3 ++-
+ board/armltd/vexpress64/vexpress64.c |  9 ++++++---
+ 3 files changed, 16 insertions(+), 8 deletions(-)
+
+diff --git a/arch/arm/cpu/armv8/cache_v8.c b/arch/arm/cpu/armv8/cache_v8.c
+index 798aed8058..e336339281 100644
+--- a/arch/arm/cpu/armv8/cache_v8.c
++++ b/arch/arm/cpu/armv8/cache_v8.c
+@@ -390,7 +390,9 @@ static void mpu_clear_regions(void)
+ {
+ 	int i;
+ 
+-	for (i = 0; mpu_mem_map[i].end || mpu_mem_map[i].attrs; i++) {
++	for (i = 0; mpu_mem_map[i].end ||
++	     mpu_mem_map[i].prbar_attrs ||
++	     mpu_mem_map[i].prlar_attrs; i++) {
+ 		setup_el2_mpu_region(i, 0, 0);
+ 	}
+ }
+@@ -410,12 +412,14 @@ static void mpu_setup(void)
+ 
+ 	asm volatile("msr MAIR_EL2, %0" : : "r" MEMORY_ATTRIBUTES);
+ 
+-	for (i = 0; mpu_mem_map[i].end || mpu_mem_map[i].attrs; i++) {
++	for (i = 0; mpu_mem_map[i].end ||
++	     mpu_mem_map[i].prbar_attrs ||
++	     mpu_mem_map[i].prlar_attrs; i++) {
+ 		setup_el2_mpu_region(i,
+ 			PRBAR_ADDRESS(mpu_mem_map[i].start)
+-				| PRBAR_OUTER_SH | PRBAR_AP_RW_ANY,
++				| mpu_mem_map[i].prbar_attrs,
+ 			PRLAR_ADDRESS(mpu_mem_map[i].end)
+-				| mpu_mem_map[i].attrs | PRLAR_EN_BIT
++				| mpu_mem_map[i].prlar_attrs | PRLAR_EN_BIT
+ 			);
+ 	}
+ 
+diff --git a/arch/arm/include/asm/armv8/mpu.h b/arch/arm/include/asm/armv8/mpu.h
+index 8de627cafd..dd4c689ea6 100644
+--- a/arch/arm/include/asm/armv8/mpu.h
++++ b/arch/arm/include/asm/armv8/mpu.h
+@@ -51,7 +51,8 @@ static inline void setup_el2_mpu_region(uint8_t region, uint64_t base, uint64_t
+ struct mpu_region {
+ 	u64 start;
+ 	u64 end;
+-	u64 attrs;
++	u64 prbar_attrs;
++	u64 prlar_attrs;
+ };
+ 
+ extern struct mpu_region *mpu_mem_map;
+diff --git a/board/armltd/vexpress64/vexpress64.c b/board/armltd/vexpress64/vexpress64.c
+index 1791cd986f..7858521fec 100644
+--- a/board/armltd/vexpress64/vexpress64.c
++++ b/board/armltd/vexpress64/vexpress64.c
+@@ -42,15 +42,18 @@ static struct mpu_region vexpress64_aemv8r_mem_map[] = {
+ 	{
+ 		.start = 0x0UL,
+ 		.end = 0x7fffffffUL,
+-		.attrs = PRLAR_ATTRIDX(MT_NORMAL)
++		.prbar_attrs = PRBAR_INNER_SH | PRBAR_AP_RW_ANY,
++		.prlar_attrs = PRLAR_ATTRIDX(MT_NORMAL)
+ 	}, {
+ 		.start = 0x80000000UL,
+ 		.end = 0xffffffffUL,
+-		.attrs = PRLAR_ATTRIDX(MT_DEVICE_NGNRNE)
++		.prbar_attrs = PRBAR_OUTER_SH | PRBAR_AP_RW_ANY,
++		.prlar_attrs = PRLAR_ATTRIDX(MT_DEVICE_NGNRNE)
+ 	}, {
+ 		.start = 0x100000000UL,
+ 		.end = 0xffffffffffUL,
+-		.attrs = PRLAR_ATTRIDX(MT_NORMAL)
++		.prbar_attrs = PRBAR_INNER_SH | PRBAR_AP_RW_ANY,
++		.prlar_attrs = PRLAR_ATTRIDX(MT_NORMAL)
+ 	}, {
+ 		/* List terminator */
+ 		0,
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0009-armv8-Enable-icache-when-switching-exception-levels-.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0009-armv8-Enable-icache-when-switching-exception-levels-.patch
new file mode 100644
index 0000000..0e0a248
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/fvp-baser-aemv8r64/0009-armv8-Enable-icache-when-switching-exception-levels-.patch
@@ -0,0 +1,63 @@
+From 0f15f6b02825b042ddc1d753f62cf87f30b1fe12 Mon Sep 17 00:00:00 2001
+From: Peter Hoyes <Peter.Hoyes@arm.com>
+Date: Thu, 19 May 2022 09:02:32 +0100
+Subject: [PATCH 9/9] armv8: Enable icache when switching exception levels in
+ bootefi
+
+bootefi calls the function switch_to_non_secure_mode before calling the
+UEFI payload to handle the case where U-Boot is running at EL3.
+
+For AArch64, the UEFI specification states that:
+   The core will be configured as follows:
+     * MMU enabled
+     * Instruction and data caches enabled
+
+These requirements should be followed when switching exception levels
+for EFI applications.
+
+This function already disables and re-enables the data cache prior to
+switching exception levels, but omits the instruction cache, meaning
+the function returns with the instruction cache disabled at the new
+exception level. Fix this by calling icache_disable prior to switching
+exception levels and icache_enable afterwards.
+
+Issue-Id: SCM-4641
+Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
+Upstream-Status: Inappropriate [other]
+  Implementation pending further discussion
+Change-Id: I678cd5ba39b56e124ab7854608289cd14651ce65
+---
+ arch/arm/cpu/armv8/exception_level.c | 3 +++
+ 1 file changed, 3 insertions(+)
+
+diff --git a/arch/arm/cpu/armv8/exception_level.c b/arch/arm/cpu/armv8/exception_level.c
+index 4aad1550f4..0a3e5428e7 100644
+--- a/arch/arm/cpu/armv8/exception_level.c
++++ b/arch/arm/cpu/armv8/exception_level.c
+@@ -27,6 +27,7 @@
+ static void entry_non_secure(struct jmp_buf_data *non_secure_jmp)
+ {
+ 	dcache_enable();
++	icache_enable();
+ 	debug("Reached non-secure mode\n");
+ 
+ 	/* Restore stack and registers saved in switch_to_non_secure_mode() */
+@@ -61,6 +62,7 @@ void switch_to_non_secure_mode(void)
+ 		if (setjmp(&non_secure_jmp))
+ 			return;
+ 		dcache_disable();	/* flush cache before switch to EL2 */
++		icache_disable();
+ 		/* Move into EL2 and keep running there */
+ 		armv8_switch_to_el2((uintptr_t)&non_secure_jmp, 0, 0, 0,
+ 				    (uintptr_t)entry_non_secure, ES_TO_AARCH64);
+@@ -68,6 +70,7 @@ void switch_to_non_secure_mode(void)
+ 		if (setjmp(&non_secure_jmp))
+ 			return;
+ 		dcache_disable();	/* flush cache before switch to EL1 */
++		icache_disable();
+ 		/* Move into EL1 and keep running there */
+ 		armv8_switch_to_el1((uintptr_t)&non_secure_jmp, 0, 0, 0,
+ 				    (uintptr_t)entry_non_secure, ES_TO_AARCH64);
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/juno/0001-arm-juno-add-custom-bootcmd-to-autoboot-from-uEnv.tx.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/juno/0001-arm-juno-add-custom-bootcmd-to-autoboot-from-uEnv.tx.patch
new file mode 100644
index 0000000..a683839
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/juno/0001-arm-juno-add-custom-bootcmd-to-autoboot-from-uEnv.tx.patch
@@ -0,0 +1,50 @@
+From 13dde05efae99c5261ed213108087d0f7ac9581e Mon Sep 17 00:00:00 2001
+From: Damodar Santhapuri <damodar.santhapuri@arm.com>
+Date: Thu, 5 Nov 2020 22:40:48 +0530
+Subject: [PATCH] arm: juno: add custom bootcmd to autoboot from uEnv.txt file
+
+enable autoboot support with custom bootcmd loads uEnv.txt
+from NOR to DRAM and import.
+
+Signed-off-by: Damodar Santhapuri <damodar.santhapuri@arm.com>
+Upstream-Status: Pending
+---
+ include/configs/vexpress_aemv8.h | 12 ++++++------
+ 1 file changed, 6 insertions(+), 6 deletions(-)
+
+diff --git a/include/configs/vexpress_aemv8.h b/include/configs/vexpress_aemv8.h
+index f0c5ceb3849a..cd7f6c1b9ba0 100644
+--- a/include/configs/vexpress_aemv8.h
++++ b/include/configs/vexpress_aemv8.h
+@@ -137,13 +137,13 @@
+ 		"  afs load ${fdt_alt_name} ${fdt_addr_r}; "\
+ 		"fi ; "\
+ 		"fdt addr ${fdt_addr_r}; fdt resize; " \
+-		"if afs load  ${ramdisk_name} ${ramdisk_addr_r} ; "\
++		"if afs load  ${initrd_name} ${initrd_addr_r} ; "\
+ 		"then "\
+-		"  setenv ramdisk_param ${ramdisk_addr_r}; "\
++		"  setenv initrd_param ${initrd_addr_r}; "\
+ 		"else "\
+-		"  setenv ramdisk_param -; "\
++		"  setenv initrd_param -; "\
+ 		"fi ; " \
+-		"booti ${kernel_addr_r} ${ramdisk_param} ${fdt_addr_r}\0"
++		"booti ${kernel_addr_r} ${initrd_param} ${fdt_addr_r}"
+ #define BOOTENV_DEV_NAME_AFS(devtypeu, devtypel, instance) "afs "
+ 
+ #define BOOT_TARGET_DEVICES(func)	\
+@@ -164,8 +164,8 @@
+ 				"kernel_name=norkern\0"	\
+ 				"kernel_alt_name=Image\0"	\
+ 				"kernel_addr_r=0x80080000\0" \
+-				"ramdisk_name=ramdisk.img\0"	\
+-				"ramdisk_addr_r=0x88000000\0"	\
++				"initrd_name=ramdisk.img\0"	\
++				"initrd_addr_r=0x88000000\0"	\
+ 				"fdtfile=board.dtb\0" \
+ 				"fdt_alt_name=juno\0" \
+ 				"fdt_addr_r=0x80000000\0" \
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/0001-arm-total_compute-update-secure-dram-size.patch b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/0001-arm-total_compute-update-secure-dram-size.patch
new file mode 100644
index 0000000..85b14b0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/0001-arm-total_compute-update-secure-dram-size.patch
@@ -0,0 +1,33 @@
+From f7c24393604e45012447b16aaa95eb5e7224ba07 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Tue, 12 Apr 2022 12:43:49 +0100
+Subject: [PATCH] arm: total_compute: update secure dram size
+
+Update secure DRAM size as it is increased by 64MB for additional
+secure partitions.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Id8ce99c7a5330d3c28d473009c4db04141e6fa4d
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ include/configs/total_compute.h | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/include/configs/total_compute.h b/include/configs/total_compute.h
+index 0324b1e1b217..62bdb4f6a3ae 100644
+--- a/include/configs/total_compute.h
++++ b/include/configs/total_compute.h
+@@ -23,8 +23,8 @@
+ 
+ /* Physical Memory Map */
+ #define PHYS_SDRAM_1		0x80000000
+-/* Top 48MB reserved for secure world use */
+-#define DRAM_SEC_SIZE		0x03000000
++/* Top 112MB reserved for secure world use */
++#define DRAM_SEC_SIZE		0x07000000
+ #define PHYS_SDRAM_1_SIZE	0x80000000 - DRAM_SEC_SIZE
+ #define CONFIG_SYS_SDRAM_BASE	PHYS_SDRAM_1
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/bootargs.cfg b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/bootargs.cfg
new file mode 100644
index 0000000..8c31602
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot/tc/bootargs.cfg
@@ -0,0 +1,2 @@
+CONFIG_USE_BOOTARGS=y
+CONFIG_BOOTARGS="console=ttyAMA0 debug user_debug=31 earlycon=pl011,0x7ff80000 loglevel=9 androidboot.hardware=total_compute androidboot.boot_devices=1c050000.mmci ip=dhcp androidboot.selinux=permissive allow_mismatched_32bit_el0"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_%.bbappend
new file mode 100644
index 0000000..e254d41
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_%.bbappend
@@ -0,0 +1,90 @@
+FILESEXTRAPATHS:prepend := "${THISDIR}/${PN}:"
+
+#
+# Corstone-500 MACHINE
+#
+SRC_URI:append:corstone500 = " \
+                   file://0001-armv7-adding-generic-timer-access-through-MMIO.patch \
+                   file://0002-board-arm-add-corstone500-board.patch"
+
+#
+# Corstone1000 64-bit machines
+#
+DEPENDS:append:corstone1000 = " gnutls-native"
+CORSTONE1000_DEVICE_TREE:corstone1000-mps3 = "corstone1000-mps3"
+CORSTONE1000_DEVICE_TREE:corstone1000-fvp = "corstone1000-fvp"
+EXTRA_OEMAKE:append:corstone1000 = ' DEVICE_TREE=${CORSTONE1000_DEVICE_TREE}'
+
+SYSROOT_DIRS:append:corstone1000 = " /boot"
+
+SRC_URI:append:corstone1000 = " \
+        file://0001-cmd-load-add-load-command-for-memory-mapped.patch \
+        file://0002-arm-add-support-to-corstone1000-platform.patch \
+        file://0003-usb-common-move-urb-code-to-common.patch \
+        file://0004-usb-add-isp1760-family-driver.patch \
+        file://0005-corstone1000-enable-isp1763-usb-controller.patch \
+        file://0006-arm_ffa-introducing-Arm-FF-A-low-level-driver.patch \
+        file://0007-arm_ffa-introducing-armffa-command.patch \
+        file://0008-arm_ffa-introducing-MM-communication-with-FF-A.patch \
+        file://0009-arm_ffa-introducing-test-module-for-UCLASS_FFA.patch \
+        file://0010-arm_ffa-corstone1000-enable-FF-A-and-MM-support.patch \
+        file://0011-efi-corstone1000-introduce-EFI-capsule-update.patch \
+        file://0012-corstone1000-Update-FFA-shared-buffer-address.patch \
+        file://0013-corstone1000-Make-sure-shared-buffer-contents-are-no.patch \
+        file://0014-arm-corstone1000-fix-unrecognized-filesystem-type.patch \
+        file://0015-efi_capsule-corstone1000-pass-interface-id-and-buffe.patch \
+        file://0016-efi_boottime-corstone1000-pass-interface-id-and-kern.patch \
+        file://0017-efi_loader-corstone1000-remove-guid-check-from-corst.patch \
+        file://0018-arm_ffa-removing-the-cast-when-using-binary-OR-on-FI.patch \
+        file://0019-Return-proper-error-code-when-rx-buffer-is-larger.patch \
+        file://0020-Use-correct-buffer-size.patch \
+        file://0021-Update-comm_buf-when-EFI_BUFFER_TOO_SMALL.patch \
+        file://0022-efi_loader-populate-ESRT-table-if-EFI_ESRT-config-op.patch \
+        file://0023-efi_firmware-add-get_image_info-for-corstone1000.patch \
+        file://0024-Comment-mm_communicate-failure-log.patch \
+        file://0025-efi_loader-send-bootcomplete-message-to-secure-encla.patch \
+        file://0026-efi_loader-fix-null-pointer-exception-with-get_image.patch \
+        file://0027-arm-corstone1000-add-mmc-for-fvp.patch \
+      "
+
+#
+# FVP BASE
+#
+SRC_URI:append:fvp-base = " file://bootargs.cfg"
+
+#
+# FVP BASE ARM32
+#
+SRC_URI:append:fvp-base-arm32 = " file://0001-Add-vexpress_aemv8a_aarch32-variant.patch \
+				  file://0002-Revert-vexpress64-Enable-OF_CONTROL-and-OF_BOARD-for.patch \
+				"
+
+#
+# FVP BASER
+#
+SRC_URI:append:fvp-baser-aemv8r64 = " \
+    file://0001-armv8-Add-ARMv8-MPU-configuration-logic.patch \
+    file://0002-vexpress64-add-MPU-memory-map-for-the-BASER_FVP.patch \
+    file://0003-armv8-Allow-disabling-exception-vectors-on-non-SPL-b.patch \
+    file://0004-armv8-ARMV8_SWITCH_TO_EL1-improvements.patch \
+    file://0005-armv8-Make-disabling-HVC-configurable-when-switching.patch \
+    file://0006-vexpress64-Do-not-set-COUNTER_FREQUENCY.patch \
+    file://0007-vexpress64-Enable-LIBFDT_OVERLAY-in-the-vexpress_aem.patch \
+    file://0008-armv8-Allow-PRBAR-MPU-attributes-to-be-configured.patch \
+    file://0009-armv8-Enable-icache-when-switching-exception-levels-.patch \
+    "
+
+
+#
+# Juno Machines
+#
+SRC_URI:append:juno = " file://0001-arm-juno-add-custom-bootcmd-to-autoboot-from-uEnv.tx.patch"
+
+
+#
+# TC0 and TC1 MACHINES
+#
+SRC_URI:append:tc = " \
+        file://bootargs.cfg \
+        file://0001-arm-total_compute-update-secure-dram-size.patch \
+        "
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_2022.04.bb b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_2022.04.bb
new file mode 100644
index 0000000..bc2d6d4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/u-boot/u-boot_2022.04.bb
@@ -0,0 +1,26 @@
+HOMEPAGE = "http://www.denx.de/wiki/U-Boot/WebHome"
+DESCRIPTION = "U-Boot, a boot loader for Embedded boards based on PowerPC, \
+ARM, MIPS and several other processors, which can be installed in a boot \
+ROM and used to initialize and test the hardware or to download and run \
+application code."
+SECTION = "bootloaders"
+DEPENDS += "flex-native bison-native"
+
+LICENSE = "GPL-2.0-or-later"
+LIC_FILES_CHKSUM = "file://Licenses/README;md5=5a7450c57ffe5ae63fd732446b988025"
+PE = "1"
+
+# We use the revision in order to avoid having to fetch it from the
+# repo during parse
+SRCREV = "e4b6ebd3de982ae7185dbf689a030e73fd06e0d2"
+
+SRC_URI = "git://git.denx.de/u-boot.git;branch=master \
+          "
+
+S = "${WORKDIR}/git"
+B = "${WORKDIR}/build"
+do_configure[cleandirs] = "${B}"
+
+require recipes-bsp/u-boot/u-boot.inc
+
+DEPENDS += "bc-native dtc-native"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-fvp-base.inc b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-fvp-base.inc
new file mode 100644
index 0000000..7069c4d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-fvp-base.inc
@@ -0,0 +1,6 @@
+EDK2_BUILD_RELEASE = "0"
+EDK2_PLATFORM      = "ArmVExpress-FVP-AArch64"
+EDK2_PLATFORM_DSC  = "Platform/ARM/VExpressPkg/ArmVExpress-FVP-AArch64.dsc"
+EDK2_BIN_NAME      = "FVP_AARCH64_EFI.fd"
+
+COMPATIBLE_MACHINE = "fvp-base"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-juno.inc b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-juno.inc
new file mode 100644
index 0000000..aac0d1c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-juno.inc
@@ -0,0 +1,9 @@
+EDK2_PLATFORM      = "ArmJuno"
+EDK2_PLATFORM_DSC  = "Platform/ARM/JunoPkg/ArmJuno.dsc"
+EDK2_BIN_NAME      = "BL33_AP_UEFI.fd"
+
+COMPATIBLE_MACHINE = "juno"
+
+# As of 2022-06-14 with 2022.05, clang builds fail:
+# "The required fv image size 0x104048 exceeds the set fv image size 0xf9000"
+TOOLCHAIN = "gcc"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-n1sdp.inc b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-n1sdp.inc
new file mode 100644
index 0000000..90c3f2a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-n1sdp.inc
@@ -0,0 +1,23 @@
+# N1SDP specific EDK2 configurations
+EDK2_BUILD_RELEASE = "0"
+EDK2_PLATFORM      = "n1sdp"
+EDK2_PLATFORM_DSC  = "Platform/ARM/N1Sdp/N1SdpPlatform.dsc"
+EDK2_BIN_NAME      = "BL33_AP_UEFI.fd"
+
+COMPATIBLE_MACHINE = "n1sdp"
+
+# UEFI EDK2 on N1SDP is unable to detect FS2 during boot resulting in launching of
+# EDK2 shell instead of launching grub. The startup.nsh will force launching of grub
+EFIDIR             = "/EFI/BOOT"
+EFI_BOOT_IMAGE     = "bootaa64.efi"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/edk2-platforms:"
+
+SRC_URI:append = "\
+    file://add-nt-fw-config.patch;patchdir=edk2-platforms \
+"
+
+do_deploy:append() {
+    EFIPATH=$(echo "${EFIDIR}" | sed 's/\//\\/g')
+    printf 'FS2:%s\%s\n' "$EFIPATH" "${EFI_BOOT_IMAGE}" > ${DEPLOYDIR}/startup.nsh
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-sgi575.inc b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-sgi575.inc
new file mode 100644
index 0000000..e26225f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware-sgi575.inc
@@ -0,0 +1,7 @@
+# SGI575 specific EDK2 configurations
+EDK2_BUILD_RELEASE = "0"
+EDK2_PLATFORM      = "Sgi575"
+EDK2_PLATFORM_DSC  = "Platform/ARM/SgiPkg/Sgi575/Sgi575.dsc"
+EDK2_BIN_NAME      = "BL33_AP_UEFI.fd"
+
+COMPATIBLE_MACHINE = "sgi575"
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware_%.bbappend b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware_%.bbappend
new file mode 100644
index 0000000..e5018bb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/edk2-firmware_%.bbappend
@@ -0,0 +1,10 @@
+# Include machine specific configurations for UEFI EDK2
+
+MACHINE_EDK2_REQUIRE ?= ""
+
+MACHINE_EDK2_REQUIRE:fvp-base = "edk2-firmware-fvp-base.inc"
+MACHINE_EDK2_REQUIRE:juno = "edk2-firmware-juno.inc"
+MACHINE_EDK2_REQUIRE:sgi575 = "edk2-firmware-sgi575.inc"
+MACHINE_EDK2_REQUIRE:n1sdp = "edk2-firmware-n1sdp.inc"
+
+require ${MACHINE_EDK2_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-bsp/uefi/files/edk2-platforms/add-nt-fw-config.patch b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/files/edk2-platforms/add-nt-fw-config.patch
new file mode 100644
index 0000000..f6f1895
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-bsp/uefi/files/edk2-platforms/add-nt-fw-config.patch
@@ -0,0 +1,474 @@
+From cc58709b32d74273736886ccfc08e4723a436ea4 Mon Sep 17 00:00:00 2001
+From: sahil <sahil@arm.com>
+Date: Thu, 17 Mar 2022 16:28:05 +0530
+Subject: [PATCH] Platform/ARM/N1sdp: Add support to parse NT_FW_CONFIG
+
+NT_FW_CONFIG DTB contains platform information passed by
+Tf-A boot stage.
+This information is used for Virtual memory map generation
+during PEI phase and passed on to DXE phase as a HOB, where
+it is used in ConfigurationManagerDxe.
+
+Upstream-Status: Pending
+Signed-off-by: Adam Johnston <adam.johnston@arm.com>
+Signed-off-by: sahil <sahil@arm.com>
+Change-Id: Ib82571280bf1ca5febe5766e618de09e7b70bb02
+
+---
+ .../ConfigurationManager.c                    |  24 ++--
+ .../ConfigurationManagerDxe.inf               |   3 +-
+ .../ARM/NeoverseN1Soc/Include/NeoverseN1Soc.h |  16 +--
+ .../Library/PlatformLib/AArch64/Helper.S      |   4 +-
+ .../Library/PlatformLib/PlatformLib.c         |  12 +-
+ .../Library/PlatformLib/PlatformLib.inf       |   8 +-
+ .../Library/PlatformLib/PlatformLibMem.c      | 103 +++++++++++++++++-
+ Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec   |   7 +-
+ 8 files changed, 152 insertions(+), 25 deletions(-)
+
+diff --git a/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManager.c b/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManager.c
+index f50623ae3f..e023d47cfd 100644
+--- a/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManager.c
++++ b/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManager.c
+@@ -1,7 +1,7 @@
+ /** @file
+   Configuration Manager Dxe
+ 
+-  Copyright (c) 2021, ARM Limited. All rights reserved.<BR>
++  Copyright (c) 2021 - 2022, ARM Limited. All rights reserved.<BR>
+ 
+   SPDX-License-Identifier: BSD-2-Clause-Patent
+ 
+@@ -16,6 +16,7 @@
+ #include <IndustryStandard/SerialPortConsoleRedirectionTable.h>
+ #include <Library/ArmLib.h>
+ #include <Library/DebugLib.h>
++#include <Library/HobLib.h>
+ #include <Library/IoLib.h>
+ #include <Library/PcdLib.h>
+ #include <Library/UefiBootServicesTableLib.h>
+@@ -28,6 +29,7 @@
+ #include "Platform.h"
+ 
+ extern struct EFI_ACPI_HETEROGENEOUS_MEMORY_ATTRIBUTE_TABLE Hmat;
++static NEOVERSEN1SOC_PLAT_INFO *PlatInfo;
+ 
+ /** The platform configuration repository information.
+ */
+@@ -1242,13 +1244,11 @@ InitializePlatformRepository (
+   IN  EDKII_PLATFORM_REPOSITORY_INFO  * CONST PlatRepoInfo
+   )
+ {
+-  NEOVERSEN1SOC_PLAT_INFO       *PlatInfo;
+   UINT64                        Dram2Size;
+   UINT64                        RemoteDdrSize;
+ 
+   RemoteDdrSize = 0;
+ 
+-  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE;
+   Dram2Size = ((PlatInfo->LocalDdrSize - 2) * SIZE_1GB);
+ 
+   PlatRepoInfo->MemAffInfo[LOCAL_DDR_REGION2].Length = Dram2Size;
+@@ -1512,7 +1512,6 @@ GetGicCInfo (
+   )
+ {
+   EDKII_PLATFORM_REPOSITORY_INFO  * PlatformRepo;
+-  NEOVERSEN1SOC_PLAT_INFO           *PlatInfo;
+   UINT32                            TotalObjCount;
+   UINT32                            ObjIndex;
+ 
+@@ -1523,7 +1522,6 @@ GetGicCInfo (
+   }
+ 
+   PlatformRepo = This->PlatRepoInfo;
+-  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE;
+ 
+   if (PlatInfo->MultichipMode == 1) {
+     TotalObjCount = PLAT_CPU_COUNT * 2;
+@@ -1623,7 +1621,6 @@ GetStandardNameSpaceObject (
+ {
+   EFI_STATUS                        Status;
+   EDKII_PLATFORM_REPOSITORY_INFO  * PlatformRepo;
+-  NEOVERSEN1SOC_PLAT_INFO           *PlatInfo;
+   UINT32                            AcpiTableCount;
+ 
+   if ((This == NULL) || (CmObject == NULL)) {
+@@ -1634,7 +1631,7 @@ GetStandardNameSpaceObject (
+ 
+   Status = EFI_NOT_FOUND;
+   PlatformRepo = This->PlatRepoInfo;
+-  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE;
++
+   AcpiTableCount = ARRAY_SIZE (PlatformRepo->CmAcpiTableList);
+   if (PlatInfo->MultichipMode == 0)
+         AcpiTableCount -= 1;
+@@ -1697,7 +1694,6 @@ GetArmNameSpaceObject (
+ {
+   EFI_STATUS                        Status;
+   EDKII_PLATFORM_REPOSITORY_INFO  * PlatformRepo;
+-  NEOVERSEN1SOC_PLAT_INFO           *PlatInfo;
+   UINT32                            GicRedistCount;
+   UINT32                            GicCpuCount;
+   UINT32                            ProcHierarchyInfoCount;
+@@ -1718,8 +1714,6 @@ GetArmNameSpaceObject (
+   Status = EFI_NOT_FOUND;
+   PlatformRepo = This->PlatRepoInfo;
+ 
+-  // Probe for multi chip information
+-  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE;
+   if (PlatInfo->MultichipMode == 1) {
+     GicRedistCount = 2;
+     GicCpuCount = PLAT_CPU_COUNT * 2;
+@@ -2162,8 +2156,18 @@ ConfigurationManagerDxeInitialize (
+   IN EFI_SYSTEM_TABLE  * SystemTable
+   )
+ {
++  VOID *PlatInfoHob;
+   EFI_STATUS  Status;
+ 
++  PlatInfoHob = GetFirstGuidHob (&gArmNeoverseN1SocPlatformInfoDescriptorGuid);
++
++  if (PlatInfoHob == NULL) {
++    DEBUG ((DEBUG_ERROR, "Platform HOB is NULL\n"));
++    return EFI_NOT_FOUND;
++  }
++
++  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)GET_GUID_HOB_DATA (PlatInfoHob);
++
+   // Initialize the Platform Configuration Repository before installing the
+   // Configuration Manager Protocol
+   Status = InitializePlatformRepository (
+diff --git a/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManagerDxe.inf b/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManagerDxe.inf
+index 4f8e7f1302..fb59c29501 100644
+--- a/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManagerDxe.inf
++++ b/Platform/ARM/N1Sdp/ConfigurationManager/ConfigurationManagerDxe/ConfigurationManagerDxe.inf
+@@ -1,7 +1,7 @@
+ ## @file
+ #  Configuration Manager Dxe
+ #
+-#  Copyright (c) 2021, ARM Limited. All rights reserved.<BR>
++#  Copyright (c) 2021 - 2022, ARM Limited. All rights reserved.<BR>
+ #
+ #  SPDX-License-Identifier: BSD-2-Clause-Patent
+ #
+@@ -42,6 +42,7 @@
+ 
+ [LibraryClasses]
+   ArmPlatformLib
++  HobLib
+   PrintLib
+   UefiBootServicesTableLib
+   UefiDriverEntryPoint
+diff --git a/Silicon/ARM/NeoverseN1Soc/Include/NeoverseN1Soc.h b/Silicon/ARM/NeoverseN1Soc/Include/NeoverseN1Soc.h
+index 097160c7e2..63cebaf0e0 100644
+--- a/Silicon/ARM/NeoverseN1Soc/Include/NeoverseN1Soc.h
++++ b/Silicon/ARM/NeoverseN1Soc/Include/NeoverseN1Soc.h
+@@ -1,6 +1,6 @@
+ /** @file
+ *
+-* Copyright (c) 2018 - 2020, ARM Limited. All rights reserved.
++* Copyright (c) 2018 - 2022, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-2-Clause-Patent
+ *
+@@ -41,11 +41,6 @@
+ #define NEOVERSEN1SOC_EXP_PERIPH_BASE0               0x1C000000
+ #define NEOVERSEN1SOC_EXP_PERIPH_BASE0_SZ            0x1300000
+ 
+-// Base address to a structure of type NEOVERSEN1SOC_PLAT_INFO which is
+-// pre-populated by a earlier boot stage
+-#define NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE          (NEOVERSEN1SOC_NON_SECURE_SRAM_BASE + \
+-                                                      0x00008000)
+-
+ /*
+  * Platform information structure stored in Non-secure SRAM. Platform
+  * information are passed from the trusted firmware with the below structure
+@@ -55,12 +50,17 @@
+ typedef struct {
+   /*! 0 - Single Chip, 1 - Chip to Chip (C2C) */
+   UINT8   MultichipMode;
+-  /*! Slave count in C2C mode */
+-  UINT8   SlaveCount;
++  /*! Secondary chip count in C2C mode */
++  UINT8   SecondaryChipCount;
+   /*! Local DDR memory size in GigaBytes */
+   UINT8   LocalDdrSize;
+   /*! Remote DDR memory size in GigaBytes */
+   UINT8   RemoteDdrSize;
+ } NEOVERSEN1SOC_PLAT_INFO;
+ 
++// NT_FW_CONFIG DT structure
++typedef struct {
++  UINT64                  NtFwConfigDtAddr;
++} NEOVERSEN1SOC_NT_FW_CONFIG_INFO_PPI;
++
+ #endif
+diff --git a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/AArch64/Helper.S b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/AArch64/Helper.S
+index 8d2069dea8..88ed640d29 100644
+--- a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/AArch64/Helper.S
++++ b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/AArch64/Helper.S
+@@ -1,6 +1,6 @@
+ /** @file
+ *
+-*  Copyright (c) 2019 - 2020, ARM Limited. All rights reserved.
++*  Copyright (c) 2019 - 2022, ARM Limited. All rights reserved.
+ *
+ *  SPDX-License-Identifier: BSD-2-Clause-Patent
+ *
+@@ -25,6 +25,8 @@ GCC_ASM_EXPORT(ArmPlatformIsPrimaryCore)
+ // the UEFI firmware through the CPU registers.
+ //
+ ASM_PFX(ArmPlatformPeiBootAction):
++  adr  x10, NtFwConfigDtBlob
++  str  x0, [x10]
+   ret
+ 
+ //
+diff --git a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.c b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.c
+index c0effd37f3..fabe902cd0 100644
+--- a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.c
++++ b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.c
+@@ -1,6 +1,6 @@
+ /** @file
+ 
+-  Copyright (c) 2018-2021, ARM Limited. All rights reserved.<BR>
++  Copyright (c) 2018 - 2022, ARM Limited. All rights reserved.<BR>
+ 
+   SPDX-License-Identifier: BSD-2-Clause-Patent
+ 
+@@ -8,8 +8,12 @@
+ 
+ #include <Library/ArmPlatformLib.h>
+ #include <Library/BaseLib.h>
++#include <NeoverseN1Soc.h>
+ #include <Ppi/ArmMpCoreInfo.h>
+ 
++UINT64 NtFwConfigDtBlob;
++STATIC NEOVERSEN1SOC_NT_FW_CONFIG_INFO_PPI mNtFwConfigDtInfoPpi;
++
+ STATIC ARM_CORE_INFO mCoreInfoTable[] = {
+   { 0x0, 0x0 }, // Cluster 0, Core 0
+   { 0x0, 0x1 }, // Cluster 0, Core 1
+@@ -46,6 +50,7 @@ ArmPlatformInitialize (
+   IN     UINTN                  MpId
+   )
+ {
++  mNtFwConfigDtInfoPpi.NtFwConfigDtAddr = NtFwConfigDtBlob;
+   return RETURN_SUCCESS;
+ }
+ 
+@@ -80,6 +85,11 @@ EFI_PEI_PPI_DESCRIPTOR gPlatformPpiTable[] = {
+     EFI_PEI_PPI_DESCRIPTOR_PPI,
+     &gArmMpCoreInfoPpiGuid,
+     &mMpCoreInfoPpi
++  },
++  {
++    EFI_PEI_PPI_DESCRIPTOR_PPI,
++    &gNtFwConfigDtInfoPpiGuid,
++    &mNtFwConfigDtInfoPpi
+   }
+ };
+ 
+diff --git a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.inf b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.inf
+index 96e590cdd8..6f9c9d5ab6 100644
+--- a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.inf
++++ b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLib.inf
+@@ -1,7 +1,7 @@
+ ## @file
+ #  Platform Library for N1Sdp.
+ #
+-#  Copyright (c) 2018-2021, ARM Limited. All rights reserved.<BR>
++#  Copyright (c) 2018 - 2022, ARM Limited. All rights reserved.<BR>
+ #
+ #  SPDX-License-Identifier: BSD-2-Clause-Patent
+ #
+@@ -18,10 +18,14 @@
+ [Packages]
+   ArmPkg/ArmPkg.dec
+   ArmPlatformPkg/ArmPlatformPkg.dec
++  EmbeddedPkg/EmbeddedPkg.dec
+   MdeModulePkg/MdeModulePkg.dec
+   MdePkg/MdePkg.dec
+   Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec
+ 
++[LibraryClasses]
++  FdtLib
++
+ [Sources.common]
+   PlatformLibMem.c
+   PlatformLib.c
+@@ -59,7 +63,9 @@
+   gEfiMdePkgTokenSpaceGuid.PcdPciExpressBaseAddress
+ 
+ [Guids]
++  gArmNeoverseN1SocPlatformInfoDescriptorGuid
+   gEfiHobListGuid          ## CONSUMES  ## SystemTable
+ 
+ [Ppis]
+   gArmMpCoreInfoPpiGuid
++  gNtFwConfigDtInfoPpiGuid
+diff --git a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLibMem.c b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLibMem.c
+index 339fa07b32..b58bda4b76 100644
+--- a/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLibMem.c
++++ b/Silicon/ARM/NeoverseN1Soc/Library/PlatformLib/PlatformLibMem.c
+@@ -1,6 +1,6 @@
+ /** @file
+ 
+-  Copyright (c) 2018 - 2021, ARM Limited. All rights reserved.<BR>
++  Copyright (c) 2018 - 2022, ARM Limited. All rights reserved.<BR>
+ 
+   SPDX-License-Identifier: BSD-2-Clause-Patent
+ 
+@@ -10,11 +10,95 @@
+ #include <Library/DebugLib.h>
+ #include <Library/HobLib.h>
+ #include <Library/MemoryAllocationLib.h>
++#include <Library/PeiServicesLib.h>
++#include <libfdt.h>
+ #include <NeoverseN1Soc.h>
+ 
+ // The total number of descriptors, including the final "end-of-table" descriptor.
+ #define MAX_VIRTUAL_MEMORY_MAP_DESCRIPTORS 19
+ 
++/** A helper function to locate the NtFwConfig PPI and get the base address of
++  NT_FW_CONFIG DT from which values are obtained using FDT helper functions.
++
++  @param [out]  plat_info  Pointer to the NeoverseN1Soc PLATFORM_INFO HOB
++
++  @retval EFI_SUCCESS            Success.
++  returns EFI_INVALID_PARAMETER  A parameter is invalid.
++**/
++EFI_STATUS
++GetNeoverseN1SocPlatInfo (
++  OUT NEOVERSEN1SOC_PLAT_INFO *plat_info
++  )
++{
++  CONST UINT32                   *Property;
++  INT32                          Offset;
++  CONST VOID                     *NtFwCfgDtBlob;
++  NEOVERSEN1SOC_NT_FW_CONFIG_INFO_PPI  *NtFwConfigInfoPpi;
++  EFI_STATUS                     Status;
++
++  Status = PeiServicesLocatePpi (
++             &gNtFwConfigDtInfoPpiGuid,
++             0,
++             NULL,
++             (VOID **)&NtFwConfigInfoPpi
++             );
++
++  if (EFI_ERROR (Status)) {
++    DEBUG ((
++      DEBUG_ERROR,
++      "PeiServicesLocatePpi failed with error %r\n",
++      Status
++      ));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  NtFwCfgDtBlob = (VOID *)(UINTN)NtFwConfigInfoPpi->NtFwConfigDtAddr;
++  if (fdt_check_header (NtFwCfgDtBlob) != 0) {
++    DEBUG ((DEBUG_ERROR, "Invalid DTB file %p passed\n", NtFwCfgDtBlob));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  Offset = fdt_subnode_offset (NtFwCfgDtBlob, 0, "platform-info");
++  if (Offset == -FDT_ERR_NOTFOUND) {
++    DEBUG ((DEBUG_ERROR, "Invalid DTB : platform-info node not found\n"));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  Property = fdt_getprop (NtFwCfgDtBlob, Offset, "local-ddr-size", NULL);
++  if (Property == NULL) {
++    DEBUG ((DEBUG_ERROR, "local-ddr-size property not found\n"));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  plat_info->LocalDdrSize = fdt32_to_cpu (*Property);
++
++  Property = fdt_getprop (NtFwCfgDtBlob, Offset, "remote-ddr-size", NULL);
++  if (Property == NULL) {
++    DEBUG ((DEBUG_ERROR, "remote-ddr-size property not found\n"));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  plat_info->RemoteDdrSize = fdt32_to_cpu (*Property);
++
++  Property = fdt_getprop (NtFwCfgDtBlob, Offset, "secondary-chip-count", NULL);
++  if (Property == NULL) {
++    DEBUG ((DEBUG_ERROR, "secondary-chip-count property not found\n"));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  plat_info->SecondaryChipCount = fdt32_to_cpu (*Property);
++
++  Property = fdt_getprop (NtFwCfgDtBlob, Offset, "multichip-mode", NULL);
++  if (Property == NULL) {
++    DEBUG ((DEBUG_ERROR, "multichip-mode property not found\n"));
++    return EFI_INVALID_PARAMETER;
++  }
++
++  plat_info->MultichipMode = fdt32_to_cpu (*Property);
++
++  return EFI_SUCCESS;
++}
++
+ /**
+   Returns the Virtual Memory Map of the platform.
+ 
+@@ -36,9 +120,24 @@ ArmPlatformGetVirtualMemoryMap (
+   NEOVERSEN1SOC_PLAT_INFO       *PlatInfo;
+   UINT64                        DramBlock2Size;
+   UINT64                        RemoteDdrSize;
++  EFI_STATUS                    Status;
+ 
+   Index = 0;
+-  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)NEOVERSEN1SOC_PLAT_INFO_STRUCT_BASE;
++
++  // Create platform info HOB
++  PlatInfo = (NEOVERSEN1SOC_PLAT_INFO *)BuildGuidHob (
++                                        &gArmNeoverseN1SocPlatformInfoDescriptorGuid,
++                                        sizeof (NEOVERSEN1SOC_PLAT_INFO)
++                                        );
++
++  if (PlatInfo == NULL) {
++    DEBUG ((DEBUG_ERROR, "Platform HOB is NULL\n"));
++    ASSERT (FALSE);
++    return;
++  }
++
++  Status = GetNeoverseN1SocPlatInfo (PlatInfo);
++  ASSERT (Status == 0);
+   DramBlock2Size = ((UINT64)(PlatInfo->LocalDdrSize -
+                              NEOVERSEN1SOC_DRAM_BLOCK1_SIZE / SIZE_1GB) *
+                             (UINT64)SIZE_1GB);
+diff --git a/Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec b/Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec
+index d59f25a5b9..4dea8fe1e8 100644
+--- a/Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec
++++ b/Silicon/ARM/NeoverseN1Soc/NeoverseN1Soc.dec
+@@ -1,7 +1,7 @@
+ ## @file
+ #  Describes the entire platform configuration.
+ #
+-#  Copyright (c) 2018 - 2021, ARM Limited. All rights reserved.<BR>
++#  Copyright (c) 2018 - 2022, ARM Limited. All rights reserved.<BR>
+ #
+ #  SPDX-License-Identifier: BSD-2-Clause-Patent
+ #
+@@ -22,6 +22,8 @@
+   Include                        # Root include for the package
+ 
+ [Guids.common]
++  # ARM NeoverseN1Soc Platform Info descriptor
++  gArmNeoverseN1SocPlatformInfoDescriptorGuid = { 0x095cb024, 0x1e00, 0x4d6f, { 0xaa, 0x34, 0x4a, 0xf8, 0xaf, 0x0e, 0xad, 0x99 } }
+   gArmNeoverseN1SocTokenSpaceGuid = { 0xab93eb78, 0x60d7, 0x4099, { 0xac, 0xeb, 0x6d, 0xb5, 0x02, 0x58, 0x7c, 0x24 } }
+ 
+ [PcdsFixedAtBuild]
+@@ -83,3 +85,6 @@
+   gArmNeoverseN1SocTokenSpaceGuid.PcdRemotePcieMmio32Translation|0x40000000000|UINT64|0x0000004F
+   gArmNeoverseN1SocTokenSpaceGuid.PcdRemotePcieMmio64Translation|0x40000000000|UINT64|0x00000050
+   gArmNeoverseN1SocTokenSpaceGuid.PcdRemotePcieSegmentNumber|2|UINT32|0x00000051
++
++[Ppis]
++  gNtFwConfigDtInfoPpiGuid =  { 0xb50dee0e, 0x577f, 0x47fb, { 0x83, 0xd0, 0x41, 0x78, 0x61, 0x8b, 0x33, 0x8a } }
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-core/init-ifupdown/files/juno/interfaces b/meta-arm/meta-arm-bsp/recipes-core/init-ifupdown/files/juno/interfaces
new file mode 100644
index 0000000..8c323c2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-core/init-ifupdown/files/juno/interfaces
@@ -0,0 +1,32 @@
+# /etc/network/interfaces -- configuration file for ifup(8), ifdown(8)
+ 
+# The loopback interface
+auto lo
+iface lo inet loopback
+
+# Wireless interfaces
+iface wlan0 inet dhcp
+	wireless_mode managed
+	wireless_essid any
+	wpa-driver wext
+	wpa-conf /etc/wpa_supplicant.conf
+
+
+# Wired or wireless interfaces
+auto eth0
+# Juno has 2 ethernet (front and back)
+auto eth1
+iface eth0 inet dhcp
+iface eth1 inet dhcp
+
+# Ethernet/RNDIS gadget (g_ether)
+# ... or on host side, usbnet and random hwaddr
+iface usb0 inet static
+	address 192.168.7.2
+	netmask 255.255.255.0
+	network 192.168.7.0
+	gateway 192.168.7.1
+
+# Bluetooth networking
+iface bnep0 inet dhcp
+
diff --git a/meta-arm/meta-arm-bsp/recipes-core/init-ifupdown/init-ifupdown_1.0.bbappend b/meta-arm/meta-arm-bsp/recipes-core/init-ifupdown/init-ifupdown_1.0.bbappend
new file mode 100644
index 0000000..65e37d8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-core/init-ifupdown/init-ifupdown_1.0.bbappend
@@ -0,0 +1,8 @@
+# Use custom interface file for Juno
+
+#
+# Enable second network interface on startup
+#
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files:"
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/README.md b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/README.md
new file mode 100644
index 0000000..ba61ca3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/README.md
@@ -0,0 +1,4 @@
+Arm platforms BSPs
+==================
+
+This directory contains Arm platforms definitions and configuration for Linux.
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/arm64.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/arm64.cfg
new file mode 100644
index 0000000..62c0238
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/arm64.cfg
@@ -0,0 +1,17 @@
+# SPDX-License-Identifier: MIT
+#
+# ARM64
+#
+CONFIG_ARM64=y
+CONFIG_64BIT=y
+CONFIG_ARCH_VEXPRESS=y
+
+#
+# Bus support
+#
+CONFIG_ARM_AMBA=y
+
+#
+# Bus devices
+#
+CONFIG_VEXPRESS_CONFIG=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/corstone1000-standard.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/corstone1000-standard.scc
new file mode 100644
index 0000000..9278ce1
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/corstone1000-standard.scc
@@ -0,0 +1,5 @@
+define KMACHINE corstone1000
+define KTYPE standard
+define KARCH arm64
+
+kconf hardware corstone1000/base.cfg
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/corstone1000/base.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/corstone1000/base.cfg
new file mode 100644
index 0000000..aea1d84
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/corstone1000/base.cfg
@@ -0,0 +1,29 @@
+CONFIG_LOCALVERSION="-yocto-standard"
+# CONFIG_LOCALVERSION_AUTO is not set
+CONFIG_LOG_BUF_SHIFT=12
+# CONFIG_UTS_NS is not set
+# CONFIG_PID_NS is not set
+# CONFIG_NET_NS is not set
+# CONFIG_BLK_DEV_BSG is not set
+CONFIG_ARM64=y
+CONFIG_THUMB2_KERNEL=y
+CONFIG_ZBOOT_ROM_TEXT=0x0
+CONFIG_ZBOOT_ROM_BSS=0x0
+CONFIG_VFP=y
+# CONFIG_CORE_DUMP_DEFAULT_ELF_HEADERS is not set
+CONFIG_NET=y
+CONFIG_PACKET=y
+CONFIG_UNIX=y
+CONFIG_INET=y
+# CONFIG_IPV6 is not set
+# CONFIG_WIRELESS is not set
+CONFIG_DEVTMPFS=y
+CONFIG_TMPFS=y
+# CONFIG_WLAN is not set
+# CONFIG_SERIO_SERPORT is not set
+CONFIG_SERIAL_AMBA_PL011=y
+CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_DRV_PL031=y
+CONFIG_MAILBOX=y
+# CONFIG_CRYPTO_HW is not set
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32-standard.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32-standard.scc
new file mode 100644
index 0000000..6569027
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32-standard.scc
@@ -0,0 +1,8 @@
+define KMACHINE fvp-arm32
+define KTYPE standard
+define KARCH arm
+
+include ktypes/standard/standard.scc
+
+include fvp-arm32.scc
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32.scc
new file mode 100644
index 0000000..ff7ce57
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32.scc
@@ -0,0 +1,14 @@
+include features/input/input.scc
+include features/net/net.scc
+include cfg/timer/no_hz.scc
+include cfg/virtio.scc
+
+kconf hardware fvp-arm32/fvp-board.cfg
+kconf hardware fvp-arm32/fvp-features.cfg
+kconf hardware fvp/fvp-net.cfg
+kconf hardware fvp/fvp-rtc.cfg
+kconf hardware fvp/fvp-serial.cfg
+kconf hardware fvp/fvp-cfi.cfg
+kconf hardware fvp/fvp-drm.cfg
+kconf hardware fvp/fvp-timer.cfg
+kconf hardware fvp/fvp-watchdog.cfg
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32/fvp-board.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32/fvp-board.cfg
new file mode 100644
index 0000000..e49a1e3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32/fvp-board.cfg
@@ -0,0 +1,12 @@
+CONFIG_ARM=y
+
+CONFIG_ARCH_VEXPRESS=y
+CONFIG_SMP=y
+CONFIG_NR_CPUS=8
+CONFIG_HOTPLUG_CPU=y
+
+CONFIG_REGULATOR=y
+CONFIG_REGULATOR_FIXED_VOLTAGE=y
+
+CONFIG_CPU_IDLE=y
+CONFIG_ARM_CPUIDLE=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32/fvp-features.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32/fvp-features.cfg
new file mode 100644
index 0000000..12e7697
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-arm32/fvp-features.cfg
@@ -0,0 +1,9 @@
+CONFIG_BINFMT_MISC=y
+CONFIG_BOUNCE=y
+CONFIG_HIGHMEM=y
+CONFIG_HIGHPTE=y
+CONFIG_KERNEL_MODE_NEON=y
+CONFIG_NEON=y
+CONFIG_VFP=y
+CONFIG_VFPv3=y
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-baser-aemv8r64-preempt-rt.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-baser-aemv8r64-preempt-rt.scc
new file mode 100644
index 0000000..e8fea0b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-baser-aemv8r64-preempt-rt.scc
@@ -0,0 +1,6 @@
+define KMACHINE fvp-baser-aemv8r64
+define KTYPE preempt-rt
+define KARCH arm64
+
+include ktypes/preempt-rt/preempt-rt.scc
+include fvp-baser-aemv8r64.scc
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-baser-aemv8r64-standard.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-baser-aemv8r64-standard.scc
new file mode 100644
index 0000000..fd1fb28
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-baser-aemv8r64-standard.scc
@@ -0,0 +1,7 @@
+define KMACHINE fvp-baser-aemv8r64
+define KTYPE standard
+define KARCH arm64
+
+include ktypes/standard/standard.scc
+
+include fvp-baser-aemv8r64.scc
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-baser-aemv8r64.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-baser-aemv8r64.scc
new file mode 100644
index 0000000..a8d7967
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-baser-aemv8r64.scc
@@ -0,0 +1,4 @@
+kconf hardware arm64.cfg
+kconf hardware fvp-common-peripherals.cfg
+include cfg/virtio.scc
+include virtio-9p.scc
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-common-peripherals.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-common-peripherals.cfg
new file mode 100644
index 0000000..ecb3cc9
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-common-peripherals.cfg
@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: MIT
+CONFIG_SERIAL_AMBA_PL011=y
+CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
+
+CONFIG_ARM_SP805_WATCHDOG=y
+
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_DRV_PL031=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-standard.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-standard.scc
new file mode 100644
index 0000000..d29e0b8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp-standard.scc
@@ -0,0 +1,11 @@
+define KMACHINE fvp
+define KTYPE standard
+define KARCH arm64
+
+include ktypes/standard/standard.scc
+
+include fvp.scc
+
+# default policy for standard kernels
+#include features/latencytop/latencytop.scc
+#include features/profiling/profiling.scc
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp.scc
new file mode 100644
index 0000000..80b8581
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp.scc
@@ -0,0 +1,13 @@
+include features/input/input.scc
+include features/net/net.scc
+include cfg/timer/no_hz.scc
+include cfg/virtio.scc
+
+kconf hardware fvp/fvp-board.cfg
+kconf hardware fvp/fvp-net.cfg
+kconf hardware fvp/fvp-rtc.cfg
+kconf hardware fvp/fvp-serial.cfg
+kconf hardware fvp/fvp-cfi.cfg
+kconf hardware fvp/fvp-drm.cfg
+kconf hardware fvp/fvp-timer.cfg
+kconf hardware fvp/fvp-watchdog.cfg
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-board.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-board.cfg
new file mode 100644
index 0000000..2fd0264
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-board.cfg
@@ -0,0 +1,11 @@
+CONFIG_ARM64=y
+CONFIG_ARCH_VEXPRESS=y
+CONFIG_SMP=y
+CONFIG_NR_CPUS=8
+CONFIG_HOTPLUG_CPU=y
+
+CONFIG_REGULATOR=y
+CONFIG_REGULATOR_FIXED_VOLTAGE=y
+
+CONFIG_CPU_IDLE=y
+CONFIG_ARM_CPUIDLE=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-cfi.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-cfi.cfg
new file mode 100644
index 0000000..f28e0d9
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-cfi.cfg
@@ -0,0 +1,3 @@
+# CFI Flash
+CONFIG_MTD=y
+CONFIG_MTD_CFI=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-drm.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-drm.cfg
new file mode 100644
index 0000000..77133a9
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-drm.cfg
@@ -0,0 +1,5 @@
+# DRM CLCD
+CONFIG_DRM=y
+CONFIG_DRM_PL111=y
+CONFIG_FB=y
+CONFIG_FB_ARMCLCD=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-net.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-net.cfg
new file mode 100644
index 0000000..20cc408
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-net.cfg
@@ -0,0 +1,3 @@
+CONFIG_NET_VENDOR_SMSC=y
+CONFIG_SMSC911X=y
+CONFIG_SMC91X=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-rtc.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-rtc.cfg
new file mode 100644
index 0000000..5d377b3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-rtc.cfg
@@ -0,0 +1,2 @@
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_DRV_PL031=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-serial.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-serial.cfg
new file mode 100644
index 0000000..4457164
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-serial.cfg
@@ -0,0 +1,2 @@
+CONFIG_SERIAL_AMBA_PL011=y
+CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-timer.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-timer.cfg
new file mode 100644
index 0000000..b33c65c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-timer.cfg
@@ -0,0 +1,4 @@
+# Dual timer module
+CONFIG_COMPILE_TEST=y
+CONFIG_ARM_TIMER_SP804=y
+CONFIG_CLK_SP810=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-watchdog.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-watchdog.cfg
new file mode 100644
index 0000000..977f317
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/fvp/fvp-watchdog.cfg
@@ -0,0 +1,3 @@
+# Watchdog
+CONFIG_WATCHDOG=y
+CONFIG_ARM_SP805_WATCHDOG=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno-standard.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno-standard.scc
new file mode 100644
index 0000000..c9d2405
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno-standard.scc
@@ -0,0 +1,11 @@
+define KMACHINE juno
+define KTYPE standard
+define KARCH arm64
+
+include ktypes/standard/standard.scc
+
+include juno.scc
+
+# default policy for standard kernels
+#include features/latencytop/latencytop.scc
+#include features/profiling/profiling.scc
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno.scc
new file mode 100644
index 0000000..2980b39
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno.scc
@@ -0,0 +1,22 @@
+include features/input/input.scc
+include features/net/net.scc
+include cfg/timer/no_hz.scc
+include cfg/usb-mass-storage.scc
+
+kconf hardware juno/juno-board.cfg
+kconf hardware juno/juno-devfreq.cfg
+kconf hardware juno/juno-dma.cfg
+kconf hardware juno/juno-drm.cfg
+kconf hardware juno/juno-fb.cfg
+kconf hardware juno/juno-i2c.cfg
+# kconf hardware juno/juno-mali-midgard.cfg
+kconf hardware juno/juno-mmc.cfg
+kconf hardware juno/juno-net.cfg
+kconf hardware juno/juno-pci.cfg
+kconf hardware juno/juno-rtc.cfg
+kconf hardware juno/juno-sata.cfg
+kconf hardware juno/juno-serial.cfg
+kconf hardware juno/juno-sound.cfg
+kconf hardware juno/juno-thermal.cfg
+kconf hardware juno/juno-usb.cfg
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-board.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-board.cfg
new file mode 100644
index 0000000..654efa4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-board.cfg
@@ -0,0 +1,41 @@
+CONFIG_ARM64=y
+CONFIG_ARCH_VEXPRESS=y
+CONFIG_SMP=y
+CONFIG_NR_CPUS=8
+CONFIG_HOTPLUG_CPU=y
+
+# Keyboard over AMBA
+CONFIG_SERIO=y
+CONFIG_SERIO_AMBAKMI=y
+
+# Hardware mailbox
+CONFIG_MAILBOX=y
+CONFIG_ARM_MHU=y
+
+# SCMI support
+CONFIG_ARM_SCMI_PROTOCOL=y
+CONFIG_ARM_SCMI_POWER_DOMAIN=y
+CONFIG_SENSORS_ARM_SCMI=y
+CONFIG_COMMON_CLK_SCMI=y
+
+# Power Interface and system control
+CONFIG_ARM_SCPI_PROTOCOL=y
+CONFIG_ARM_SCPI_POWER_DOMAIN=y
+CONFIG_SENSORS_ARM_SCPI=y
+CONFIG_COMMON_CLK_SCPI=y
+
+CONFIG_REGULATOR=y
+CONFIG_REGULATOR_FIXED_VOLTAGE=y
+
+CONFIG_CPU_IDLE=y
+CONFIG_ARM_CPUIDLE=y
+
+CONFIG_CPU_FREQ=y
+CONFIG_ARM_SCPI_CPUFREQ=y
+
+CONFIG_CPU_FREQ_GOV_PERFORMANCE=y
+CONFIG_CPU_FREQ_GOV_USERSPACE=y
+CONFIG_CPU_FREQ_GOV_ONDEMAND=y
+
+CONFIG_CONNECTOR=y
+CONFIG_ARM_TIMER_SP804=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-devfreq.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-devfreq.cfg
new file mode 100644
index 0000000..474e010
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-devfreq.cfg
@@ -0,0 +1,4 @@
+CONFIG_PM_DEVFREQ=y
+CONFIG_DEVFREQ_THERMAL=y
+CONFIG_DEVFREQ_GOV_SIMPLE_ONDEMAND=y
+CONFIG_DEVFREQ_GOV_PERFORMANCE=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-dma.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-dma.cfg
new file mode 100644
index 0000000..cbdffa3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-dma.cfg
@@ -0,0 +1,5 @@
+CONFIG_DMADEVICES=y
+CONFIG_PL330_DMA=y
+CONFIG_CMA=y
+CONFIG_DMA_CMA=y
+CONFIG_CMA_SIZE_MBYTES=96
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-drm.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-drm.cfg
new file mode 100644
index 0000000..1216297
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-drm.cfg
@@ -0,0 +1,5 @@
+CONFIG_DRM=y
+CONFIG_DRM_HDLCD=y
+CONFIG_DRM_I2C_NXP_TDA998X=y
+CONFIG_FB=y
+CONFIG_FB_ARMCLCD=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-fb.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-fb.cfg
new file mode 100644
index 0000000..59499fa
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-fb.cfg
@@ -0,0 +1,4 @@
+CONFIG_FB=y
+CONFIG_FB_ARMCLCD=y
+CONFIG_FRAMEBUFFER_CONSOLE=y
+# CONFIG_VGA_CONSOLE is not set
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-i2c.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-i2c.cfg
new file mode 100644
index 0000000..97f80c4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-i2c.cfg
@@ -0,0 +1,2 @@
+CONFIG_I2C=y
+CONFIG_I2C_DESIGNWARE_PLATFORM=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-mali-midgard.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-mali-midgard.cfg
new file mode 100644
index 0000000..adf02b7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-mali-midgard.cfg
@@ -0,0 +1,7 @@
+CONFIG_MALI_MIDGARD=y
+CONFIG_MALI_EXPERT=y
+CONFIG_MALI_PLATFORM_FAKE=y
+CONFIG_MALI_PLATFORM_THIRDPARTY=y
+CONFIG_MALI_PLATFORM_THIRDPARTY_NAME="juno_soc"
+CONFIG_MALI_PLATFORM_DEVICETREE=y
+CONFIG_MALI_DEVFREQ=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-mmc.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-mmc.cfg
new file mode 100644
index 0000000..41af527
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-mmc.cfg
@@ -0,0 +1,2 @@
+CONFIG_MMC=y
+CONFIG_MMC_ARMMMCI=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-net.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-net.cfg
new file mode 100644
index 0000000..54e3686
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-net.cfg
@@ -0,0 +1,2 @@
+CONFIG_SMSC911X=y
+CONFIG_SMC91X=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-pci.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-pci.cfg
new file mode 100644
index 0000000..295d190
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-pci.cfg
@@ -0,0 +1,11 @@
+CONFIG_PCI=y
+CONFIG_PCI_MSI=y
+CONFIG_PCI_REALLOC_ENABLE_AUTO=y
+CONFIG_PCI_PRI=y
+CONFIG_PCI_PASID=y
+CONFIG_PCI_HOST_GENERIC=y
+CONFIG_PCIEPORTBUS=y
+CONFIG_HOTPLUG_PCI=y
+CONFIG_HOTPLUG_PCI_PCIE=y
+CONFIG_PCIEAER=y
+CONFIG_PCIE_ECRC=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-rtc.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-rtc.cfg
new file mode 100644
index 0000000..5d377b3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-rtc.cfg
@@ -0,0 +1,2 @@
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_DRV_PL031=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-sata.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-sata.cfg
new file mode 100644
index 0000000..a159af8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-sata.cfg
@@ -0,0 +1,3 @@
+CONFIG_ATA=y
+CONFIG_SATA_SIL24=y
+CONFIG_SKY2=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-serial.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-serial.cfg
new file mode 100644
index 0000000..4457164
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-serial.cfg
@@ -0,0 +1,2 @@
+CONFIG_SERIAL_AMBA_PL011=y
+CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-sound.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-sound.cfg
new file mode 100644
index 0000000..d3419ef
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-sound.cfg
@@ -0,0 +1,14 @@
+CONFIG_SOUND=y
+CONFIG_SND=y
+CONFIG_SND_SEQUENCER=y
+CONFIG_SND_SEQ_DUMMY=y
+CONFIG_SND_OSSEMUL=y
+CONFIG_SND_MIXER_OSS=y
+CONFIG_SND_PCM_OSS=y
+CONFIG_SND_SEQUENCER_OSS=y
+# CONFIG_SND_USB is not set
+CONFIG_SND_SOC=y
+CONFIG_SND_DESIGNWARE_I2S=y
+CONFIG_SND_SOC_HDMI_CODEC=y
+CONFIG_SND_SOC_SPDIF=y
+CONFIG_SND_SIMPLE_CARD=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-thermal.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-thermal.cfg
new file mode 100644
index 0000000..6241374
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-thermal.cfg
@@ -0,0 +1,5 @@
+CONFIG_THERMAL=y
+CONFIG_THERMAL_DEFAULT_GOV_POWER_ALLOCATOR=y
+CONFIG_THERMAL_GOV_USER_SPACE=y
+CONFIG_CPU_THERMAL=y
+CONFIG_THERMAL_WRITABLE_TRIPS=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-usb.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-usb.cfg
new file mode 100644
index 0000000..9159de1
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/juno/juno-usb.cfg
@@ -0,0 +1,7 @@
+CONFIG_USB_STORAGE=y
+CONFIG_USB=y
+CONFIG_USB_ULPI=y
+CONFIG_USB_EHCI_HCD=y
+CONFIG_USB_EHCI_HCD_PLATFORM=y
+CONFIG_NOP_USB_XCEIV=y
+CONFIG_USB_OHCI_HCD=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/n1sdp-preempt-rt.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/n1sdp-preempt-rt.scc
new file mode 100644
index 0000000..dc84445
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/n1sdp-preempt-rt.scc
@@ -0,0 +1,6 @@
+define KMACHINE n1sdp
+define KTYPE preempt-rt
+define KARCH arm64
+
+include ktypes/preempt-rt/preempt-rt.scc
+include n1sdp/disable-kvm.cfg
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/n1sdp-standard.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/n1sdp-standard.scc
new file mode 100644
index 0000000..8536c81
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/n1sdp-standard.scc
@@ -0,0 +1,5 @@
+define KMACHINE n1sdp
+define KTYPE standard
+define KARCH arm64
+
+include ktypes/standard/standard.scc
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/n1sdp/disable-kvm.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/n1sdp/disable-kvm.cfg
new file mode 100644
index 0000000..617d3e5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/n1sdp/disable-kvm.cfg
@@ -0,0 +1 @@
+# CONFIG_KVM is not set
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc-autofdo.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc-autofdo.scc
new file mode 100644
index 0000000..5b20b6c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc-autofdo.scc
@@ -0,0 +1 @@
+kconf non-hardware tc/autofdo.cfg
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc.scc
new file mode 100644
index 0000000..43d2153
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc.scc
@@ -0,0 +1,9 @@
+kconf hardware tc/base.cfg
+kconf non-hardware tc/dhcp.cfg
+kconf non-hardware tc/devtmpfs.cfg
+kconf non-hardware tc/gralloc.cfg
+kconf non-hardware tc/mali.cfg
+kconf non-hardware tc/tee.cfg
+kconf non-hardware tc/virtio.cfg
+kconf non-hardware tc/ci700.cfg
+kconf non-hardware tc/trusty.cfg
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/autofdo.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/autofdo.cfg
new file mode 100644
index 0000000..8530c88
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/autofdo.cfg
@@ -0,0 +1,3 @@
+CONFIG_CORESIGHT=y
+CONFIG_CORESIGHT_SOURCE_ETM4X=y
+CONFIG_CORESIGHT_TRBE=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/base.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/base.cfg
new file mode 100644
index 0000000..90b08f1
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/base.cfg
@@ -0,0 +1,12 @@
+CONFIG_ARCH_VEXPRESS=y
+CONFIG_ARM_MHU=y
+CONFIG_ARM_MHU_V2=y
+CONFIG_ARM_SMMU_V3=y
+CONFIG_ARM64_VA_BITS_48=y
+CONFIG_COMMON_CLK_SCMI=y
+CONFIG_DRM_HDLCD=y
+CONFIG_DRM_KOMEDA=y
+CONFIG_DRM_VIRT_ENCODER=y
+CONFIG_MMC_ARMMMCI=y
+CONFIG_SERIO_AMBAKMI=y
+CONFIG_SMC91X=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/ci700.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/ci700.cfg
new file mode 100644
index 0000000..50c0153
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/ci700.cfg
@@ -0,0 +1 @@
+CONFIG_ARM_CMN=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/devtmpfs.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/devtmpfs.cfg
new file mode 100644
index 0000000..abde412
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/devtmpfs.cfg
@@ -0,0 +1,3 @@
+CONFIG_DEVTMPFS=y
+CONFIG_DEVTMPFS_MOUNT=y
+CONFIG_VT=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/dhcp.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/dhcp.cfg
new file mode 100644
index 0000000..78c5a04
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/dhcp.cfg
@@ -0,0 +1,2 @@
+CONFIG_IP_PNP=y
+CONFIG_IP_PNP_DHCP=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/gralloc.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/gralloc.cfg
new file mode 100644
index 0000000..22abcb5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/gralloc.cfg
@@ -0,0 +1,2 @@
+CONFIG_DMABUF_HEAPS_SYSTEM=y
+CONFIG_DMABUF_HEAPS_CMA=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/mali.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/mali.cfg
new file mode 100644
index 0000000..166818f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/mali.cfg
@@ -0,0 +1 @@
+CONFIG_FW_LOADER_USER_HELPER_FALLBACK=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/tee.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/tee.cfg
new file mode 100644
index 0000000..4b9cbd8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/tee.cfg
@@ -0,0 +1,3 @@
+CONFIG_TEE=y
+CONFIG_OPTEE=y
+CONFIG_ARM_FFA_TRANSPORT=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/trusty.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/trusty.cfg
new file mode 100644
index 0000000..54e8657
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/trusty.cfg
@@ -0,0 +1 @@
+CONFIG_TRUSTY=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/virtio.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/virtio.cfg
new file mode 100644
index 0000000..9e6d21c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/tc/virtio.cfg
@@ -0,0 +1,2 @@
+CONFIG_VIRTIO_BLK=y
+CONFIG_VIRTIO_MMIO=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/virtio-9p.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/virtio-9p.cfg
new file mode 100644
index 0000000..c9fefa1
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/virtio-9p.cfg
@@ -0,0 +1,4 @@
+CONFIG_NET_9P=y
+CONFIG_NET_9P_VIRTIO=y
+CONFIG_9P_FS=y
+CONFIG_9P_FS_POSIX_ACL=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/virtio-9p.scc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/virtio-9p.scc
new file mode 100644
index 0000000..33c0465
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/arm-platforms-kmeta/bsp/arm-platforms/virtio-9p.scc
@@ -0,0 +1 @@
+kconf non-hardware virtio-9p.cfg
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/corstone1000/0001-UPSTREAM-firmware-arm_ffa-Handle-compatibility-with-.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/corstone1000/0001-UPSTREAM-firmware-arm_ffa-Handle-compatibility-with-.patch
new file mode 100644
index 0000000..a4fd673
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/corstone1000/0001-UPSTREAM-firmware-arm_ffa-Handle-compatibility-with-.patch
@@ -0,0 +1,90 @@
+Upstream-Status: Backport [https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/commit/?id=8e3f9da608f14cfebac2659d8dd8737b79d01308]
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+
+From a8f3e351c07c48774be2a45e184b9f08dc92f1db Mon Sep 17 00:00:00 2001
+From: Sudeep Holla <sudeep.holla@arm.com>
+Date: Wed, 13 Apr 2022 15:43:19 +0100
+Subject: [PATCH] UPSTREAM: firmware: arm_ffa: Handle compatibility with
+ different firmware versions
+
+The driver currently just support v1.0 of Arm FFA specification. It also
+expects the firmware implementation to match the same and bail out if it
+doesn't match. This is causing issue when running with higher version of
+firmware implementation(e.g. v1.1 which will released soon).
+
+In order to support compatibility with different firmware versions, let
+us add additional checks and find the compatible version the driver can
+work with.
+
+Link: https://lore.kernel.org/r/20211013091127.990992-1-sudeep.holla@arm.com
+Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>
+Signed-off-by: Sudeep Holla <sudeep.holla@arm.com>
+(cherry picked from commit 8e3f9da608f14cfebac2659d8dd8737b79d01308)
+Change-Id: I7bc9a3b172a9067bfd4e9bb9d50b4729e915b5a5
+Bug: 168585974
+---
+ drivers/firmware/arm_ffa/driver.c | 37 ++++++++++++++++++++++++++-----
+ 1 file changed, 32 insertions(+), 5 deletions(-)
+
+diff --git a/drivers/firmware/arm_ffa/driver.c b/drivers/firmware/arm_ffa/driver.c
+index c9fb56afbcb4..6e0c883ab708 100644
+--- a/drivers/firmware/arm_ffa/driver.c
++++ b/drivers/firmware/arm_ffa/driver.c
+@@ -167,6 +167,28 @@ struct ffa_drv_info {
+ 
+ static struct ffa_drv_info *drv_info;
+ 
++/*
++ * The driver must be able to support all the versions from the earliest
++ * supported FFA_MIN_VERSION to the latest supported FFA_DRIVER_VERSION.
++ * The specification states that if firmware supports a FFA implementation
++ * that is incompatible with and at a greater version number than specified
++ * by the caller(FFA_DRIVER_VERSION passed as parameter to FFA_VERSION),
++ * it must return the NOT_SUPPORTED error code.
++ */
++static u32 ffa_compatible_version_find(u32 version)
++{
++	u32 compat_version;
++	u16 major = MAJOR_VERSION(version), minor = MINOR_VERSION(version);
++	u16 drv_major = MAJOR_VERSION(FFA_DRIVER_VERSION);
++	u16 drv_minor = MINOR_VERSION(FFA_DRIVER_VERSION);
++
++	if ((major < drv_major) || (major == drv_major && minor <= drv_minor))
++		return version;
++
++	pr_info("Firmware version higher than driver version, downgrading\n");
++	return FFA_DRIVER_VERSION;
++}
++
+ static int ffa_version_check(u32 *version)
+ {
+ 	ffa_value_t ver;
+@@ -180,15 +202,20 @@ static int ffa_version_check(u32 *version)
+ 		return -EOPNOTSUPP;
+ 	}
+ 
+-	if (ver.a0 < FFA_MIN_VERSION || ver.a0 > FFA_DRIVER_VERSION) {
+-		pr_err("Incompatible version %d.%d found\n",
+-		       MAJOR_VERSION(ver.a0), MINOR_VERSION(ver.a0));
++	if (ver.a0 < FFA_MIN_VERSION) {
++		pr_err("Incompatible v%d.%d! Earliest supported v%d.%d\n",
++		       MAJOR_VERSION(ver.a0), MINOR_VERSION(ver.a0),
++		       MAJOR_VERSION(FFA_MIN_VERSION),
++		       MINOR_VERSION(FFA_MIN_VERSION));
+ 		return -EINVAL;
+ 	}
+ 
+-	*version = ver.a0;
+-	pr_info("Version %d.%d found\n", MAJOR_VERSION(ver.a0),
++	pr_info("Driver version %d.%d\n", MAJOR_VERSION(FFA_DRIVER_VERSION),
++		MINOR_VERSION(FFA_DRIVER_VERSION));
++	pr_info("Firmware version %d.%d found\n", MAJOR_VERSION(ver.a0),
+ 		MINOR_VERSION(ver.a0));
++	*version = ffa_compatible_version_find(ver.a0);
++
+ 	return 0;
+ }
+ 
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/corstone1000/corstone1000_kernel_debug.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/corstone1000/corstone1000_kernel_debug.cfg
new file mode 100644
index 0000000..aad9e93
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/corstone1000/corstone1000_kernel_debug.cfg
@@ -0,0 +1,3 @@
+CONFIG_DEBUG_INFO=y
+CONFIG_DEBUG_KERNEL=y
+CONFIG_DEBUG_INFO_DWARF4=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/corstone1000/defconfig b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/corstone1000/defconfig
new file mode 100644
index 0000000..5f0a7e9
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/corstone1000/defconfig
@@ -0,0 +1,94 @@
+CONFIG_LOCALVERSION="-yocto-standard"
+# CONFIG_LOCALVERSION_AUTO is not set
+CONFIG_HIGH_RES_TIMERS=y
+CONFIG_PREEMPT=y
+CONFIG_LOG_BUF_SHIFT=13
+CONFIG_LOG_CPU_MAX_BUF_SHIFT=13
+CONFIG_RELAY=y
+CONFIG_BOOT_CONFIG=y
+CONFIG_ARCH_VEXPRESS=y
+CONFIG_CMDLINE="console=ttyAMA0 loglevel=9"
+CONFIG_EFI=y
+# CONFIG_SUSPEND is not set
+CONFIG_ARM_FFA_TRANSPORT=y
+CONFIG_EFI_BOOTLOADER_CONTROL=y
+CONFIG_EFI_CAPSULE_LOADER=y
+CONFIG_EFI_TEST=y
+CONFIG_RESET_ATTACK_MITIGATION=y
+# CONFIG_STACKPROTECTOR is not set
+CONFIG_MODULES=y
+# CONFIG_BLK_DEV_BSG is not set
+# CONFIG_CORE_DUMP_DEFAULT_ELF_HEADERS is not set
+CONFIG_NET=y
+CONFIG_PACKET=y
+CONFIG_UNIX=y
+CONFIG_INET=y
+CONFIG_IP_PNP=y
+CONFIG_IP_PNP_DHCP=y
+CONFIG_IP_PNP_BOOTP=y
+CONFIG_SYN_COOKIES=y
+CONFIG_NET_SCHED=y
+CONFIG_DEVTMPFS=y
+CONFIG_SCSI=y
+CONFIG_BLK_DEV_SD=y
+CONFIG_NETDEVICES=y
+# CONFIG_NET_VENDOR_ALACRITECH is not set
+# CONFIG_NET_VENDOR_AMAZON is not set
+# CONFIG_NET_VENDOR_AMD is not set
+# CONFIG_NET_VENDOR_AQUANTIA is not set
+# CONFIG_NET_VENDOR_ARC is not set
+# CONFIG_NET_VENDOR_BROADCOM is not set
+# CONFIG_NET_VENDOR_CADENCE is not set
+# CONFIG_NET_VENDOR_CAVIUM is not set
+# CONFIG_NET_VENDOR_CORTINA is not set
+# CONFIG_NET_VENDOR_EZCHIP is not set
+# CONFIG_NET_VENDOR_GOOGLE is not set
+# CONFIG_NET_VENDOR_HISILICON is not set
+# CONFIG_NET_VENDOR_HUAWEI is not set
+# CONFIG_NET_VENDOR_INTEL is not set
+# CONFIG_NET_VENDOR_MARVELL is not set
+# CONFIG_NET_VENDOR_MICREL is not set
+# CONFIG_NET_VENDOR_MICROCHIP is not set
+# CONFIG_NET_VENDOR_MICROSEMI is not set
+# CONFIG_NET_VENDOR_NATSEMI is not set
+# CONFIG_NET_VENDOR_NETRONOME is not set
+# CONFIG_NET_VENDOR_NI is not set
+# CONFIG_NET_VENDOR_PENSANDO is not set
+# CONFIG_NET_VENDOR_QUALCOMM is not set
+# CONFIG_NET_VENDOR_RENESAS is not set
+# CONFIG_NET_VENDOR_ROCKER is not set
+# CONFIG_NET_VENDOR_SAMSUNG is not set
+# CONFIG_NET_VENDOR_SEEQ is not set
+# CONFIG_NET_VENDOR_SOLARFLARE is not set
+CONFIG_SMC91X=y
+CONFIG_SMSC911X=y
+# CONFIG_NET_VENDOR_SOCIONEXT is not set
+# CONFIG_NET_VENDOR_STMICRO is not set
+# CONFIG_NET_VENDOR_SYNOPSYS is not set
+# CONFIG_NET_VENDOR_VIA is not set
+# CONFIG_NET_VENDOR_WIZNET is not set
+# CONFIG_NET_VENDOR_XILINX is not set
+# CONFIG_SERIO_SERPORT is not set
+CONFIG_SERIAL_AMBA_PL011=y
+CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
+CONFIG_USB=y
+CONFIG_USB_STORAGE=y
+CONFIG_USB_UAS=y
+CONFIG_USB_ISP1760=y
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_DRV_PL031=y
+CONFIG_TEE=y
+CONFIG_OPTEE=y
+CONFIG_VFAT_FS=y
+CONFIG_TMPFS=y
+CONFIG_CONFIGFS_FS=y
+CONFIG_NLS_CODEPAGE_437=y
+CONFIG_NLS_CODEPAGE_860=y
+CONFIG_NLS_ISO8859_1=y
+CONFIG_NLS_ISO8859_15=y
+CONFIG_NLS_UTF8=y
+CONFIG_LIBCRC32C=y
+# CONFIG_SECTION_MISMATCH_WARN_ONLY is not set
+CONFIG_DEBUG_FS=y
+CONFIG_PANIC_TIMEOUT=5
+CONFIG_STACKTRACE=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/fvp-base-arm32/0001-ARM-vexpress-enable-GICv3.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/fvp-base-arm32/0001-ARM-vexpress-enable-GICv3.patch
new file mode 100644
index 0000000..d0a05c2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/fvp-base-arm32/0001-ARM-vexpress-enable-GICv3.patch
@@ -0,0 +1,31 @@
+From 5dbb6c4267b1e46ed08359be363d8bc9b6a79397 Mon Sep 17 00:00:00 2001
+From: Ryan Harkin <ryan.harkin@linaro.org>
+Date: Wed, 16 Nov 2016 14:43:02 +0000
+Subject: [PATCH] ARM: vexpress: enable GICv3
+
+Upstream-Status: Pending
+
+ARMv8 targets such as ARM's FVP Cortex-A32 model can run the 32-bit
+ARMv7 kernel.  And these targets often contain GICv3.
+
+Signed-off-by: Ryan Harkin <ryan.harkin@linaro.org>
+Signed-off-by: Jon Medhurst <tixy@linaro.org>
+---
+ arch/arm/mach-vexpress/Kconfig | 1 +
+ 1 file changed, 1 insertion(+)
+
+diff --git a/arch/arm/mach-vexpress/Kconfig b/arch/arm/mach-vexpress/Kconfig
+index 7c728ebc0b33..ed579382d41f 100644
+--- a/arch/arm/mach-vexpress/Kconfig
++++ b/arch/arm/mach-vexpress/Kconfig
+@@ -4,6 +4,7 @@ menuconfig ARCH_VEXPRESS
+ 	select ARCH_SUPPORTS_BIG_ENDIAN
+ 	select ARM_AMBA
+ 	select ARM_GIC
++	select ARM_GIC_V3
+ 	select ARM_GLOBAL_TIMER
+ 	select ARM_TIMER_SP804
+ 	select COMMON_CLK_VERSATILE
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/fvp-baser-aemv8r64/fvp-baser-aemv8r64.dts b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/fvp-baser-aemv8r64/fvp-baser-aemv8r64.dts
new file mode 100644
index 0000000..6911a59
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/fvp-baser-aemv8r64/fvp-baser-aemv8r64.dts
@@ -0,0 +1,212 @@
+/dts-v1/;
+
+/ {
+
+        #address-cells = <0x2>;
+        #size-cells = <0x2>;
+        interrupt-parent = <0x1>;
+        model = "Generated";
+        compatible = "arm,base";
+
+        memory@0 {
+                #address-cells = <0x2>;
+                #size-cells = <0x2>;
+                device_type = "memory";
+                reg = <0x0        0x0        0x0 0x80000000>,
+                      <0x00000008 0x80000000 0x0 0x80000000>;
+        };
+
+        cpus {
+                #address-cells = <0x2>;
+                #size-cells = <0x0>;
+
+                cpu-map {
+                        cluster0 {
+                                core0 { thread0 { cpu = <&CPU_0>; }; };
+                                core1 { thread0 { cpu = <&CPU_1>; }; };
+                                core2 { thread0 { cpu = <&CPU_2>; }; };
+                                core3 { thread0 { cpu = <&CPU_3>; }; };
+                        };
+                };
+
+                CPU_0: cpu@0 {
+                        device_type = "cpu";
+                        compatible = "arm,armv8";
+                        reg = <0x0 0x0>;
+                        enable-method = "spin-table";
+                        cpu-release-addr = <0x0 0x7f800>;
+                };
+
+                CPU_1: cpu@1 {
+                        device_type = "cpu";
+                        compatible = "arm,armv8";
+                        reg = <0x0 0x1>;
+                        enable-method = "spin-table";
+                        cpu-release-addr = <0x0 0x7f808>;
+                };
+
+                CPU_2: cpu@2 {
+                        device_type = "cpu";
+                        compatible = "arm,armv8";
+                        reg = <0x0 0x2>;
+                        enable-method = "spin-table";
+                        cpu-release-addr = <0x0 0x7f810>;
+                };
+
+                CPU_3: cpu@3 {
+                        device_type = "cpu";
+                        compatible = "arm,armv8";
+                        reg = <0x0 0x3>;
+                        enable-method = "spin-table";
+                        cpu-release-addr = <0x0 0x7f818>;
+                };
+        };
+
+        interrupt-controller@af000000 {
+                compatible = "arm,gic-v3";
+                #interrupt-cells = <0x3>;
+                #address-cells = <0x2>;
+                #size-cells = <0x2>;
+                ranges;
+                interrupt-controller;
+                #redistributor-regions = <0x1>;
+                reg = <0x0 0xaf000000 0x0 0x10000>,     // GICD
+                      <0x0 0xaf100000 0x0 0x100000>,    // GICR
+                      <0x0 0xac000000 0x0 0x2000>,      // GICC
+                      <0x0 0xac010000 0x0 0x2000>,      // GICH
+                      <0x0 0xac02f000 0x0 0x2000>;      // GICV
+                interrupts = <0x1 9 0x4>;
+                linux,phandle = <0x1>;
+                phandle = <0x1>;
+
+                its: msi-controller@2f020000 {
+                        #msi-cells = <1>;
+                        compatible = "arm,gic-v3-its";
+                        reg = <0x0 0xaf020000 0x0 0x20000>; // GITS
+                        msi-controller;
+                };
+
+        };
+
+        refclk100mhz: refclk100mhz {
+                compatible = "fixed-clock";
+                #clock-cells = <0>;
+                clock-frequency = <100000000>;
+                clock-output-names = "apb_pclk";
+        };
+
+        refclk24mhz: refclk24mhz {
+                compatible = "fixed-clock";
+                #clock-cells = <0>;
+                clock-frequency = <24000000>;
+                clock-output-names = "refclk24mhz";
+        };
+
+        refclk1hz: refclk1hz {
+                compatible = "fixed-clock";
+                #clock-cells = <0>;
+                clock-frequency = <1>;
+                clock-output-names = "refclk1hz";
+        };
+
+        uart@9c090000 {
+                compatible = "arm,pl011", "arm,primecell";
+                reg = <0x0 0x9c090000 0x0 0x1000>;
+                interrupts = <0x0 5 0x4>;
+                clocks = <&refclk24mhz>, <&refclk100mhz>;
+                clock-names = "uartclk", "apb_pclk";
+        };
+
+        uart@9c0a0000 {
+                compatible = "arm,pl011", "arm,primecell";
+                reg = <0x0 0x9c0a0000 0x0 0x1000>;
+                interrupts = <0x0 6 0x4>;
+                clocks = <&refclk24mhz>, <&refclk100mhz>;
+                clock-names = "uartclk", "apb_pclk";
+        };
+
+        uart@9c0b0000 {
+                compatible = "arm,pl011", "arm,primecell";
+                reg = <0x0 0x9c0b0000 0x0 0x1000>;
+                interrupts = <0x0 7 0x4>;
+                clocks = <&refclk24mhz>, <&refclk100mhz>;
+                clock-names = "uartclk", "apb_pclk";
+        };
+
+        uart@9c0c0000 {
+                compatible = "arm,pl011", "arm,primecell";
+                reg = <0x0 0x9c0c0000 0x0 0x1000>;
+                interrupts = <0x0 8 0x4>;
+                clocks = <&refclk24mhz>, <&refclk100mhz>;
+                clock-names = "uartclk", "apb_pclk";
+        };
+
+        wdt@9c0f0000 {
+                compatible = "arm,sp805", "arm,primecell";
+                reg = <0x0 0x9c0f0000 0x0 0x1000>;
+                interrupts = <0x0 0 0x4>;
+                clocks = <&refclk24mhz>, <&refclk100mhz>;
+                clock-names = "wdog_clk", "apb_pclk";
+        };
+
+        rtc@9c170000 {
+                compatible = "arm,pl031", "arm,primecell";
+                reg = <0x0 0x9c170000 0x0 0x1000>;
+                interrupts = <0x0 4 0x4>;
+                clocks = <&refclk1hz>;
+                clock-names = "apb_pclk";
+        };
+
+        virtio-block@9c130000 {
+                compatible = "virtio,mmio";
+                reg = <0 0x9c130000 0 0x200>;
+                interrupts = <0x0 42 0x4>;
+        };
+
+        virtio-p9@9c140000{
+                compatible = "virtio,mmio";
+                reg = <0x0 0x9c140000 0x0 0x1000>;
+                interrupts = <0x0 43 0x4>;
+        };
+
+        virtio-net@9c150000 {
+                compatible = "virtio,mmio";
+                reg = <0 0x9c150000 0 0x200>;
+                interrupts = <0x0 44 0x4>;
+        };
+
+        virtio-rng@9c200000 {
+                compatible = "virtio,mmio";
+                reg = <0 0x9c200000 0 0x200>;
+                interrupts = <0x0 46 0x4>;
+        };
+
+        timer {
+                compatible = "arm,armv8-timer";
+                interrupts = <0x1 13 0xff08>,
+                             <0x1 14 0xff08>,
+                             <0x1 11 0xff08>,
+                             <0x1  4 0xff08>;
+                clock-frequency = <100000000>;
+        };
+
+        aliases {
+                serial0 = "/uart@9c090000";
+                serial1 = "/uart@9c0a0000";
+                serial2 = "/uart@9c0b0000";
+                serial3 = "/uart@9c0c0000";
+        };
+
+        pmu {
+                compatible = "arm,armv8-pmuv3";
+                interrupts = <0 60 4>,
+                             <0 61 4>,
+                             <0 62 4>,
+                             <0 63 4>;
+        };
+
+        chosen {
+                bootargs = "earlycon console=ttyAMA0 loglevel=8 rootfstype=ext4 root=/dev/vda1 rw";
+                stdout-path = "serial0";
+        };
+};
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/juno/juno-dts-mhu-doorbell.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/juno/juno-dts-mhu-doorbell.patch
new file mode 100644
index 0000000..81f641c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/files/juno/juno-dts-mhu-doorbell.patch
@@ -0,0 +1,616 @@
+Add MHU doorbell support and SCMI device nodes to the Juno DeviceTree.
+
+Patch taken from https://git.kernel.org/pub/scm/linux/kernel/git/sudeep.holla/linux.git/log/?h=scmi_dt_defconfig
+
+Upstream-Status: Pending
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+From 821ffd8e5dc4d2fb2716d5fb912b343b932e1e77 Mon Sep 17 00:00:00 2001
+From: Sudeep Holla <sudeep.holla@arm.com>
+Date: Thu, 20 Apr 2017 11:58:01 +0100
+Subject: [PATCH] arm64: dts: juno: add mhu doorbell support and scmi device
+ nodes
+
+Signed-off-by: Sudeep Holla <sudeep.holla@arm.com>
+---
+ arch/arm64/boot/dts/arm/juno-base.dtsi    | 139 ++++++++++++----------
+ arch/arm64/boot/dts/arm/juno-cs-r1r2.dtsi |   6 +-
+ arch/arm64/boot/dts/arm/juno-r1.dts       |  12 +-
+ arch/arm64/boot/dts/arm/juno-r2.dts       |  12 +-
+ arch/arm64/boot/dts/arm/juno.dts          |  12 +-
+ 5 files changed, 96 insertions(+), 85 deletions(-)
+
+diff --git a/arch/arm64/boot/dts/arm/juno-base.dtsi b/arch/arm64/boot/dts/arm/juno-base.dtsi
+index 6288e104a089..36844f7d861e 100644
+--- a/arch/arm64/boot/dts/arm/juno-base.dtsi
++++ b/arch/arm64/boot/dts/arm/juno-base.dtsi
+@@ -23,11 +23,12 @@ frame@2a830000 {
+ 	};
+ 
+ 	mailbox: mhu@2b1f0000 {
+-		compatible = "arm,mhu", "arm,primecell";
++		compatible = "arm,mhu-doorbell", "arm,primecell";
+ 		reg = <0x0 0x2b1f0000 0x0 0x1000>;
+ 		interrupts = <GIC_SPI 36 IRQ_TYPE_LEVEL_HIGH>,
+ 			     <GIC_SPI 35 IRQ_TYPE_LEVEL_HIGH>;
+-		#mbox-cells = <1>;
++		#mbox-cells = <2>;
++		mbox-name = "ARM-MHU";
+ 		clocks = <&soc_refclk100mhz>;
+ 		clock-names = "apb_pclk";
+ 	};
+@@ -39,7 +40,7 @@ smmu_gpu: iommu@2b400000 {
+ 			     <GIC_SPI 38 IRQ_TYPE_LEVEL_HIGH>;
+ 		#iommu-cells = <1>;
+ 		#global-interrupts = <1>;
+-		power-domains = <&scpi_devpd 1>;
++		power-domains = <&scmi_devpd 9>;
+ 		dma-coherent;
+ 		status = "disabled";
+ 	};
+@@ -63,7 +64,7 @@ smmu_etr: iommu@2b600000 {
+ 		#iommu-cells = <1>;
+ 		#global-interrupts = <1>;
+ 		dma-coherent;
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 	};
+ 
+ 	gic: interrupt-controller@2c010000 {
+@@ -123,7 +124,7 @@ etf@20010000 { /* etf0 */
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 
+ 		in-ports {
+ 			port {
+@@ -147,7 +148,7 @@ tpiu@20030000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		in-ports {
+ 			port {
+ 				tpiu_in_port: endpoint {
+@@ -164,7 +165,7 @@ main_funnel: funnel@20040000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 
+ 		out-ports {
+ 			port {
+@@ -201,7 +202,7 @@ etr@20070000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		arm,scatter-gather;
+ 		in-ports {
+ 			port {
+@@ -220,7 +221,7 @@ stm@20100000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				stm_out_port: endpoint {
+@@ -235,7 +236,7 @@ replicator@20120000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 
+ 		out-ports {
+ 			#address-cells = <1>;
+@@ -270,7 +271,7 @@ cpu_debug0: cpu-debug@22010000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 	};
+ 
+ 	etm0: etm@22040000 {
+@@ -279,7 +280,7 @@ etm0: etm@22040000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				cluster0_etm0_out_port: endpoint {
+@@ -295,7 +296,7 @@ funnel@220c0000 { /* cluster0 funnel */
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				cluster0_funnel_out_port: endpoint {
+@@ -330,7 +331,7 @@ cpu_debug1: cpu-debug@22110000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 	};
+ 
+ 	etm1: etm@22140000 {
+@@ -339,7 +340,7 @@ etm1: etm@22140000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				cluster0_etm1_out_port: endpoint {
+@@ -355,7 +356,7 @@ cpu_debug2: cpu-debug@23010000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 	};
+ 
+ 	etm2: etm@23040000 {
+@@ -364,7 +365,7 @@ etm2: etm@23040000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				cluster1_etm0_out_port: endpoint {
+@@ -380,7 +381,7 @@ funnel@230c0000 { /* cluster1 funnel */
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				cluster1_funnel_out_port: endpoint {
+@@ -427,7 +428,7 @@ cpu_debug3: cpu-debug@23110000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 	};
+ 
+ 	etm3: etm@23140000 {
+@@ -436,7 +437,7 @@ etm3: etm@23140000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				cluster1_etm1_out_port: endpoint {
+@@ -452,7 +453,7 @@ cpu_debug4: cpu-debug@23210000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 	};
+ 
+ 	etm4: etm@23240000 {
+@@ -461,7 +462,7 @@ etm4: etm@23240000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				cluster1_etm2_out_port: endpoint {
+@@ -477,7 +478,7 @@ cpu_debug5: cpu-debug@23310000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 	};
+ 
+ 	etm5: etm@23340000 {
+@@ -486,7 +487,7 @@ etm5: etm@23340000 {
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				cluster1_etm3_out_port: endpoint {
+@@ -503,8 +504,8 @@ gpu: gpu@2d000000 {
+ 			     <GIC_SPI 34 IRQ_TYPE_LEVEL_HIGH>,
+ 			     <GIC_SPI 32 IRQ_TYPE_LEVEL_HIGH>;
+ 		interrupt-names = "job", "mmu", "gpu";
+-		clocks = <&scpi_dvfs 2>;
+-		power-domains = <&scpi_devpd 1>;
++		clocks = <&scmi_dvfs 2>;
++		power-domains = <&scmi_devpd 9>;
+ 		dma-coherent;
+ 		/* The SMMU is only really of interest to bare-metal hypervisors */
+ 		/* iommus = <&smmu_gpu 0>; */
+@@ -519,14 +520,24 @@ sram: sram@2e000000 {
+ 		#size-cells = <1>;
+ 		ranges = <0 0x0 0x2e000000 0x8000>;
+ 
+-		cpu_scp_lpri: scp-sram@0 {
+-			compatible = "arm,juno-scp-shmem";
+-			reg = <0x0 0x200>;
++		cpu_scp_lpri0: scp-sram@0 {
++			compatible = "arm,scmi-shmem";
++			reg = <0x0 0x80>;
+ 		};
+ 
+-		cpu_scp_hpri: scp-sram@200 {
+-			compatible = "arm,juno-scp-shmem";
+-			reg = <0x200 0x200>;
++		cpu_scp_lpri1: scp-sram@80 {
++			compatible = "arm,scmi-shmem";
++			reg = <0x80 0x80>;
++		};
++
++		cpu_scp_hpri0: scp-sram@100 {
++			compatible = "arm,scmi-shmem";
++			reg = <0x100 0x80>;
++		};
++
++		cpu_scp_hpri1: scp-sram@180 {
++			compatible = "arm,scmi-shmem";
++			reg = <0x180 0x80>;
+ 		};
+ 	};
+ 
+@@ -558,37 +569,37 @@ pcie_ctlr: pcie@40000000 {
+ 		iommu-map = <0x0 &smmu_pcie 0x0 0x1>;
+ 	};
+ 
+-	scpi {
+-		compatible = "arm,scpi";
+-		mboxes = <&mailbox 1>;
+-		shmem = <&cpu_scp_hpri>;
++	firmware {
++		scmi {
++			compatible = "arm,scmi";
++			mbox-names = "tx", "rx";
++			mboxes = <&mailbox 0 0 &mailbox 0 1>;
++			shmem = <&cpu_scp_lpri0 &cpu_scp_lpri1>;
++			#address-cells = <1>;
++			#size-cells = <0>;
+ 
+-		clocks {
+-			compatible = "arm,scpi-clocks";
++			scmi_devpd: protocol@11 {
++				reg = <0x11>;
++				#power-domain-cells = <1>;
++			};
+ 
+-			scpi_dvfs: clocks-0 {
+-				compatible = "arm,scpi-dvfs-clocks";
++			scmi_dvfs: protocol@13 {
++				reg = <0x13>;
+ 				#clock-cells = <1>;
+-				clock-indices = <0>, <1>, <2>;
+-				clock-output-names = "atlclk", "aplclk","gpuclk";
++				mbox-names = "tx", "rx";
++				mboxes = <&mailbox 1 0 &mailbox 1 1>;
++				shmem = <&cpu_scp_hpri0 &cpu_scp_hpri1>;
+ 			};
+-			scpi_clk: clocks-1 {
+-				compatible = "arm,scpi-variable-clocks";
++
++			scmi_clk: protocol@14 {
++				reg = <0x14>;
+ 				#clock-cells = <1>;
+-				clock-indices = <3>;
+-				clock-output-names = "pxlclk";
+ 			};
+-		};
+ 
+-		scpi_devpd: power-controller {
+-			compatible = "arm,scpi-power-domains";
+-			num-domains = <2>;
+-			#power-domain-cells = <1>;
+-		};
+-
+-		scpi_sensors0: sensors {
+-			compatible = "arm,scpi-sensors";
+-			#thermal-sensor-cells = <1>;
++			scmi_sensors0: protocol@15 {
++				reg = <0x15>;
++				#thermal-sensor-cells = <1>;
++			};
+ 		};
+ 	};
+ 
+@@ -596,40 +607,40 @@ thermal-zones {
+ 		pmic {
+ 			polling-delay = <1000>;
+ 			polling-delay-passive = <100>;
+-			thermal-sensors = <&scpi_sensors0 0>;
++			thermal-sensors = <&scmi_sensors0 0>;
+ 		};
+ 
+ 		soc {
+ 			polling-delay = <1000>;
+ 			polling-delay-passive = <100>;
+-			thermal-sensors = <&scpi_sensors0 3>;
++			thermal-sensors = <&scmi_sensors0 3>;
+ 		};
+ 
+ 		big_cluster_thermal_zone: big-cluster {
+ 			polling-delay = <1000>;
+ 			polling-delay-passive = <100>;
+-			thermal-sensors = <&scpi_sensors0 21>;
++			thermal-sensors = <&scmi_sensors0 21>;
+ 			status = "disabled";
+ 		};
+ 
+ 		little_cluster_thermal_zone: little-cluster {
+ 			polling-delay = <1000>;
+ 			polling-delay-passive = <100>;
+-			thermal-sensors = <&scpi_sensors0 22>;
++			thermal-sensors = <&scmi_sensors0 22>;
+ 			status = "disabled";
+ 		};
+ 
+ 		gpu0_thermal_zone: gpu0 {
+ 			polling-delay = <1000>;
+ 			polling-delay-passive = <100>;
+-			thermal-sensors = <&scpi_sensors0 23>;
++			thermal-sensors = <&scmi_sensors0 23>;
+ 			status = "disabled";
+ 		};
+ 
+ 		gpu1_thermal_zone: gpu1 {
+ 			polling-delay = <1000>;
+ 			polling-delay-passive = <100>;
+-			thermal-sensors = <&scpi_sensors0 24>;
++			thermal-sensors = <&scmi_sensors0 24>;
+ 			status = "disabled";
+ 		};
+ 	};
+@@ -705,7 +716,7 @@ hdlcd@7ff50000 {
+ 		reg = <0 0x7ff50000 0 0x1000>;
+ 		interrupts = <GIC_SPI 93 IRQ_TYPE_LEVEL_HIGH>;
+ 		iommus = <&smmu_hdlcd1 0>;
+-		clocks = <&scpi_clk 3>;
++		clocks = <&scmi_clk 3>;
+ 		clock-names = "pxlclk";
+ 
+ 		port {
+@@ -720,7 +731,7 @@ hdlcd@7ff60000 {
+ 		reg = <0 0x7ff60000 0 0x1000>;
+ 		interrupts = <GIC_SPI 85 IRQ_TYPE_LEVEL_HIGH>;
+ 		iommus = <&smmu_hdlcd0 0>;
+-		clocks = <&scpi_clk 3>;
++		clocks = <&scmi_clk 3>;
+ 		clock-names = "pxlclk";
+ 
+ 		port {
+diff --git a/arch/arm64/boot/dts/arm/juno-cs-r1r2.dtsi b/arch/arm64/boot/dts/arm/juno-cs-r1r2.dtsi
+index eda3d9e18af6..e6ecb0dfcbcd 100644
+--- a/arch/arm64/boot/dts/arm/juno-cs-r1r2.dtsi
++++ b/arch/arm64/boot/dts/arm/juno-cs-r1r2.dtsi
+@@ -6,7 +6,7 @@ funnel@20130000 { /* cssys1 */
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				csys1_funnel_out_port: endpoint {
+@@ -29,7 +29,7 @@ etf@20140000 { /* etf1 */
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		in-ports {
+ 			port {
+ 				etf1_in_port: endpoint {
+@@ -52,7 +52,7 @@ funnel@20150000 { /* cssys2 */
+ 
+ 		clocks = <&soc_smc50mhz>;
+ 		clock-names = "apb_pclk";
+-		power-domains = <&scpi_devpd 0>;
++		power-domains = <&scmi_devpd 8>;
+ 		out-ports {
+ 			port {
+ 				csys2_funnel_out_port: endpoint {
+diff --git a/arch/arm64/boot/dts/arm/juno-r1.dts b/arch/arm64/boot/dts/arm/juno-r1.dts
+index 0e24e29eb9b1..fee67943f4d5 100644
+--- a/arch/arm64/boot/dts/arm/juno-r1.dts
++++ b/arch/arm64/boot/dts/arm/juno-r1.dts
+@@ -96,7 +96,7 @@ A57_0: cpu@0 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <256>;
+ 			next-level-cache = <&A57_L2>;
+-			clocks = <&scpi_dvfs 0>;
++			clocks = <&scmi_dvfs 0>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <1024>;
+ 		};
+@@ -113,7 +113,7 @@ A57_1: cpu@1 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <256>;
+ 			next-level-cache = <&A57_L2>;
+-			clocks = <&scpi_dvfs 0>;
++			clocks = <&scmi_dvfs 0>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <1024>;
+ 		};
+@@ -130,7 +130,7 @@ A53_0: cpu@100 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <578>;
+ 		};
+@@ -147,7 +147,7 @@ A53_1: cpu@101 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <578>;
+ 		};
+@@ -164,7 +164,7 @@ A53_2: cpu@102 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <578>;
+ 		};
+@@ -181,7 +181,7 @@ A53_3: cpu@103 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <578>;
+ 		};
+diff --git a/arch/arm64/boot/dts/arm/juno-r2.dts b/arch/arm64/boot/dts/arm/juno-r2.dts
+index e609420ce3e4..7792626eb29e 100644
+--- a/arch/arm64/boot/dts/arm/juno-r2.dts
++++ b/arch/arm64/boot/dts/arm/juno-r2.dts
+@@ -96,7 +96,7 @@ A72_0: cpu@0 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <256>;
+ 			next-level-cache = <&A72_L2>;
+-			clocks = <&scpi_dvfs 0>;
++			clocks = <&scmi_dvfs 0>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <1024>;
+ 			dynamic-power-coefficient = <450>;
+@@ -114,7 +114,7 @@ A72_1: cpu@1 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <256>;
+ 			next-level-cache = <&A72_L2>;
+-			clocks = <&scpi_dvfs 0>;
++			clocks = <&scmi_dvfs 0>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <1024>;
+ 			dynamic-power-coefficient = <450>;
+@@ -132,7 +132,7 @@ A53_0: cpu@100 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <485>;
+ 			dynamic-power-coefficient = <140>;
+@@ -150,7 +150,7 @@ A53_1: cpu@101 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <485>;
+ 			dynamic-power-coefficient = <140>;
+@@ -168,7 +168,7 @@ A53_2: cpu@102 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <485>;
+ 			dynamic-power-coefficient = <140>;
+@@ -186,7 +186,7 @@ A53_3: cpu@103 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <485>;
+ 			dynamic-power-coefficient = <140>;
+diff --git a/arch/arm64/boot/dts/arm/juno.dts b/arch/arm64/boot/dts/arm/juno.dts
+index f00cffbd032c..a28316c65c1b 100644
+--- a/arch/arm64/boot/dts/arm/juno.dts
++++ b/arch/arm64/boot/dts/arm/juno.dts
+@@ -95,7 +95,7 @@ A57_0: cpu@0 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <256>;
+ 			next-level-cache = <&A57_L2>;
+-			clocks = <&scpi_dvfs 0>;
++			clocks = <&scmi_dvfs 0>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <1024>;
+ 			dynamic-power-coefficient = <530>;
+@@ -113,7 +113,7 @@ A57_1: cpu@1 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <256>;
+ 			next-level-cache = <&A57_L2>;
+-			clocks = <&scpi_dvfs 0>;
++			clocks = <&scmi_dvfs 0>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <1024>;
+ 			dynamic-power-coefficient = <530>;
+@@ -131,7 +131,7 @@ A53_0: cpu@100 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <578>;
+ 			dynamic-power-coefficient = <140>;
+@@ -149,7 +149,7 @@ A53_1: cpu@101 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <578>;
+ 			dynamic-power-coefficient = <140>;
+@@ -167,7 +167,7 @@ A53_2: cpu@102 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <578>;
+ 			dynamic-power-coefficient = <140>;
+@@ -185,7 +185,7 @@ A53_3: cpu@103 {
+ 			d-cache-line-size = <64>;
+ 			d-cache-sets = <128>;
+ 			next-level-cache = <&A53_L2>;
+-			clocks = <&scpi_dvfs 1>;
++			clocks = <&scmi_dvfs 1>;
+ 			cpu-idle-states = <&CPU_SLEEP_0 &CLUSTER_SLEEP_0>;
+ 			capacity-dmips-mhz = <578>;
+ 			dynamic-power-coefficient = <140>;
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm-platforms.inc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm-platforms.inc
new file mode 100644
index 0000000..f05c5ff
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm-platforms.inc
@@ -0,0 +1,185 @@
+# Kernel configuration and dts specific information
+
+#
+# Kernel configurations and dts (If not using Linux provided ones) are captured
+# in this file. Update SRC_URI and do_patch for building images with custom dts
+#
+
+# We can't set FILESEXTRAPATHS once because of how the kernel classes search for
+# config fragments. Discussion is ongoing as to whether this is the correct
+# solution, or a workaround.
+# https://bugzilla.yoctoproject.org/show_bug.cgi?id=14154
+ARMBSPFILESPATHS := "${THISDIR}:${THISDIR}/files:"
+
+# Arm platforms kmeta
+SRC_URI_KMETA = "file://arm-platforms-kmeta;type=kmeta;name=arm-platforms-kmeta;destsuffix=arm-platforms-kmeta"
+SRC_URI:append:fvp-base = " ${SRC_URI_KMETA}"
+SRC_URI:append:fvp-base-arm32 = " ${SRC_URI_KMETA}"
+SRC_URI:append:fvp-baser-aemv8r64 = " ${SRC_URI_KMETA}"
+SRC_URI:append:juno = " ${SRC_URI_KMETA}"
+SRC_URI:append:n1sdp = " ${SRC_URI_KMETA}"
+SRC_URI:append:tc = " ${SRC_URI_KMETA}"
+SRCREV:arm-platforms-kmeta = "6147e82375aa9df8f2a162d42ea6406c79c854c5"
+
+#
+# Corstone-500 KMACHINE
+#
+COMPATIBLE_MACHINE:corstone500 = "corstone500"
+KBUILD_DEFCONFIG:corstone500  = "multi_v7_defconfig"
+KCONFIG_MODE:corstone500 = "--alldefconfig"
+
+#
+# Corstone1000 KMACHINE
+#
+FILESEXTRAPATHS:prepend:corstone1000 := "${ARMBSPFILESPATHS}"
+COMPATIBLE_MACHINE:corstone1000 = "${MACHINE}"
+KCONFIG_MODE:corstone1000 = "--alldefconfig"
+KMACHINE:corstone1000 = "corstone1000"
+LINUX_KERNEL_TYPE:corstone1000 = "standard"
+#disabling the rootfs cpio file compression so it is not compressed twice when bundled with the kernel
+KERNEL_EXTRA_ARGS:corstone1000 += "CONFIG_INITRAMFS_COMPRESSION_NONE=y"
+SRC_URI:append:corstone1000 = " \
+           file://defconfig  \
+           file://0001-UPSTREAM-firmware-arm_ffa-Handle-compatibility-with-.patch  \
+        "
+
+SRC_URI:append:corstone1000 = " ${@bb.utils.contains('MACHINE_FEATURES', \
+                                                            'corstone1000_kernel_debug', \
+                                                            'file://corstone1000_kernel_debug.cfg', \
+                                                            '', \
+                                                             d)}"
+
+# Default kernel features not needed for corstone1000
+# otherwise the extra kernel modules will increase the rootfs size
+# corstone1000 has limited flash memory constraints
+KERNEL_EXTRA_FEATURES:corstone1000 = ""
+KERNEL_FEATURES:corstone1000 = ""
+
+#
+# FVP BASE KMACHINE
+#
+COMPATIBLE_MACHINE:fvp-base = "fvp-base"
+KMACHINE:fvp-base = "fvp"
+FILESEXTRAPATHS:prepend:fvp-base := "${ARMBSPFILESPATHS}"
+
+#
+# FVP BASE ARM32 KMACHINE
+#
+COMPATIBLE_MACHINE:fvp-base-arm32 = "fvp-base-arm32"
+KMACHINE:fvp-base-arm32 = "fvp-arm32"
+FILESEXTRAPATHS:prepend:fvp-base-arm32 := "${ARMBSPFILESPATHS}"
+SRC_URI:append:fvp-base-arm32 = " file://0001-ARM-vexpress-enable-GICv3.patch"
+# We want to use the DT in the arm64 tree but the kernel build doesn't like that, so symlink it
+do_compile:prepend:fvp-base-arm32() {
+    mkdir --parents ${S}/arch/arm/boot/dts/arm
+    for file in fvp-base-revc.dts rtsm_ve-motherboard.dtsi rtsm_ve-motherboard-rs2.dtsi; do
+        ln -fsr ${S}/arch/arm64/boot/dts/arm/$file ${S}/arch/arm/boot/dts/arm
+    done
+}
+
+#
+# FVP BaseR AEMv8r64 Machine
+#
+COMPATIBLE_MACHINE:fvp-baser-aemv8r64 = "fvp-baser-aemv8r64"
+FILESEXTRAPATHS:prepend:fvp-baser-aemv8r64 := "${ARMBSPFILESPATHS}"
+SRC_URI:append:fvp-baser-aemv8r64 = " file://fvp-baser-aemv8r64.dts;subdir=git/arch/arm64/boot/dts/arm"
+
+#
+# Juno KMACHINE
+#
+COMPATIBLE_MACHINE:juno = "juno"
+KBUILD_DEFCONFIG:juno = "defconfig"
+KCONFIG_MODE:juno = "--alldefconfig"
+FILESEXTRAPATHS:prepend:juno := "${ARMBSPFILESPATHS}"
+SRC_URI:append:juno = " file://juno-dts-mhu-doorbell.patch"
+
+#
+# Musca B1/S2 can't run Linux
+#
+COMPATIBLE_MACHINE:musca-b1 = "(^$)"
+COMPATIBLE_MACHINE:musca-s1 = "(^$)"
+
+#
+# N1SDP KMACHINE
+#
+FILESEXTRAPATHS:prepend:n1sdp := "${THISDIR}/linux-yocto-5.15/n1sdp:"
+COMPATIBLE_MACHINE:n1sdp = "n1sdp"
+KBUILD_DEFCONFIG:n1sdp = "defconfig"
+KCONFIG_MODE:n1sdp = "--alldefconfig"
+FILESEXTRAPATHS:prepend:n1sdp := "${ARMBSPFILESPATHS}"
+SRC_URI:append:n1sdp = " \
+    file://0001-iommu-arm-smmu-v3-workaround-for-ATC_INV_SIZE_ALL-in.patch \
+    file://0002-n1sdp-pci_quirk-add-acs-override-for-PCI-devices.patch \
+    file://0003-pcie-Add-quirk-for-the-Arm-Neoverse-N1SDP-platform.patch \
+    file://0004-n1sdp-pcie-add-quirk-support-enabling-remote-chip-PC.patch \
+    file://0005-arm64-kpti-Whitelist-early-Arm-Neoverse-N1-revisions.patch \
+    file://enable-realtek-R8169.cfg \
+    file://enable-usb_conn_gpio.cfg \
+    file://usb_xhci_pci_renesas.cfg \
+    "
+# Since we use the intree defconfig and the preempt-rt turns off some configs
+# do_kernel_configcheck will display warnings. So, lets disable it.
+KCONF_AUDIT_LEVEL:n1sdp:pn-linux-yocto-rt = "0"
+
+#
+# SGI575 KMACHINE
+#
+COMPATIBLE_MACHINE:sgi575 = "sgi575"
+KBUILD_DEFCONFIG:sgi575 = "defconfig"
+KCONFIG_MODE:sgi575 = "--alldefconfig"
+
+#
+# Total Compute (TC0/TC1) KMACHINE
+#
+COMPATIBLE_MACHINE:tc = "(tc0|tc1)"
+KCONFIG_MODE:tc = "--alldefconfig"
+FILESEXTRAPATHS:prepend:tc := "${ARMBSPFILESPATHS}:${THISDIR}/linux-arm64-ack-5.10/tc:"
+SRC_URI:append:tc = " \
+    file://gki_defconfig \
+    file://0001-drm-Add-component-aware-simple-encoder.patch \
+    file://0002-drm-arm-komeda-add-RENDER-capability-to-the-device-n.patch \
+    file://0003-dt-bindings-mailbox-arm-mhuv2-Add-bindings.patch \
+    file://0004-mailbox-arm_mhuv2-Add-driver.patch \
+    file://0005-mailbox-arm_mhuv2-Fix-sparse-warnings.patch \
+    file://0006-mailbox-arm_mhuv2-make-remove-callback-return-void.patch \
+    file://0007-mailbox-arm_mhuv2-Skip-calling-kfree-with-invalid-po.patch \
+    file://0008-firmware-arm_ffa-Backport-of-arm_ffa-driver.patch \
+    file://0009-tee-add-sec_world_id-to-struct-tee_shm.patch \
+    file://0010-optee-simplify-optee_release.patch \
+    file://0011-optee-sync-OP-TEE-headers.patch \
+    file://0012-optee-refactor-driver-with-internal-callbacks.patch \
+    file://0013-optee-add-a-FF-A-memory-pool.patch \
+    file://0014-optee-add-FF-A-support.patch \
+    file://0015-coresight-etm4x-Save-restore-TRFCR_EL1.patch \
+    file://0016-coresight-etm4x-Use-Trace-Filtering-controls-dynamic.patch \
+    file://0017-perf-arm-cmn-Use-irq_set_affinity.patch \
+    file://0018-perf-arm-cmn-Fix-CPU-hotplug-unregistration.patch \
+    file://0019-perf-arm-cmn-Account-for-NUMA-affinity.patch \
+    file://0020-perf-arm-cmn-Drop-compile-test-restriction.patch \
+    file://0021-perf-arm-cmn-Refactor-node-ID-handling.patch \
+    file://0022-perf-arm-cmn-Streamline-node-iteration.patch \
+    file://0023-drivers-perf-arm-cmn-Add-space-after.patch \
+    file://0024-perf-arm-cmn-Refactor-DTM-handling.patch \
+    file://0025-perf-arm-cmn-Optimise-DTM-counter-reads.patch \
+    file://0026-perf-arm-cmn-Optimise-DTC-counter-accesses.patch \
+    file://0027-perf-arm-cmn-Move-group-validation-data-off-stack.patch \
+    file://0028-perf-arm-cmn-Demarcate-CMN-600-specifics.patch \
+    file://0029-perf-arm-cmn-Support-new-IP-features.patch \
+    file://0030-perf-arm-cmn-Add-CI-700-Support.patch \
+    file://0031-firmware-arm_ffa-Fix-uuid-argument-passed-to-ffa_par.patch \
+    file://0032-firmware-arm_ffa-Add-ffa_dev_get_drvdata.patch \
+    file://0033-firmware-arm_ffa-extern-ffa_bus_type.patch \
+    file://0034-firmware-arm_ffa-Fix-FFA_MEM_SHARE-and-FFA_MEM_FRAG_.patch \
+    file://0035-ANDROID-trusty-Backport-of-trusty-driver.patch \
+    file://0036-ANDROID-trusty-Remove-FFA-specific-initilization.patch \
+    file://0037-ANDROID-trusty-Rename-transfer-memory-function-to-le.patch \
+    file://0038-ANDROID-trusty-Separate-out-SMC-based-transport.patch \
+    file://0039-ANDROID-trusty-Modify-device-compatible-string.patch \
+    file://0040-ANDROID-trusty-Add-transport-descriptor.patch \
+    file://0041-ANDROID-trusty-Add-trusty-ffa-driver.patch \
+    file://0042-ANDROID-trusty-ffa-Add-support-for-FFA-memory-operat.patch \
+    file://0043-ANDROID-trusty-ffa-Enable-FFA-transport-for-both-mem.patch \
+    file://0044-ANDROID-trusty-Make-trusty-transports-configurable.patch \
+    "
+KERNEL_FEATURES:append:tc = " bsp/arm-platforms/tc.scc"
+KERNEL_FEATURES:append:tc1 = " bsp/arm-platforms/tc-autofdo.scc"
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0001-drm-Add-component-aware-simple-encoder.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0001-drm-Add-component-aware-simple-encoder.patch
new file mode 100644
index 0000000..1586034
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0001-drm-Add-component-aware-simple-encoder.patch
@@ -0,0 +1,365 @@
+From 39e6b51150c36dd659b85de0c4339594da389da9 Mon Sep 17 00:00:00 2001
+From: Tushar Khandelwal <tushar.khandelwal@arm.com>
+Date: Tue, 16 Jun 2020 12:39:06 +0000
+Subject: [PATCH 01/22] drm: Add component-aware simple encoder
+
+This is a simple DRM encoder that gets its connector timings information
+from a OF subnode in the device tree and exposes that as a "discovered"
+panel. It can be used together with component-based DRM drivers in an
+emulated environment where no real encoder or connector hardware exists
+and the display output is configured outside the kernel.
+
+Signed-off-by: Tushar Khandelwal <tushar.khandelwal@arm.com>
+
+Upstream-Status: Backport [https://git.linaro.org/landing-teams/working/arm/kernel-release.git/commit/?h=latest-armlt&id=15283f7be4b1e586702551e85b4caf06531ac2fc]
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ drivers/gpu/drm/Kconfig               |  11 +
+ drivers/gpu/drm/Makefile              |   2 +
+ drivers/gpu/drm/drm_virtual_encoder.c | 299 ++++++++++++++++++++++++++
+ 3 files changed, 312 insertions(+)
+ create mode 100644 drivers/gpu/drm/drm_virtual_encoder.c
+
+diff --git a/drivers/gpu/drm/Kconfig b/drivers/gpu/drm/Kconfig
+index ca868271f4c4..6ae8ba3ca7b3 100644
+--- a/drivers/gpu/drm/Kconfig
++++ b/drivers/gpu/drm/Kconfig
+@@ -300,6 +300,17 @@ config DRM_VKMS
+ 
+ 	  If M is selected the module will be called vkms.
+ 
++config DRM_VIRT_ENCODER
++       tristate "Virtual OF-based encoder"
++       depends on DRM && OF
++       select VIDEOMODE_HELPERS
++       help
++         Choose this option to get a virtual encoder and its associated
++         connector that will use the device tree to read the display
++         timings information. If M is selected the module will be called
++         drm_vencoder.
++
++
+ source "drivers/gpu/drm/exynos/Kconfig"
+ 
+ source "drivers/gpu/drm/rockchip/Kconfig"
+diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
+index 81569009f884..a3429152c613 100644
+--- a/drivers/gpu/drm/Makefile
++++ b/drivers/gpu/drm/Makefile
+@@ -56,6 +56,8 @@ drm_kms_helper-$(CONFIG_DRM_DP_CEC) += drm_dp_cec.o
+ 
+ obj-$(CONFIG_DRM_KMS_HELPER) += drm_kms_helper.o
+ obj-$(CONFIG_DRM_DEBUG_SELFTEST) += selftests/
++drm_vencoder-y := drm_virtual_encoder.o
++obj-$(CONFIG_DRM_VIRT_ENCODER) += drm_vencoder.o
+ 
+ obj-$(CONFIG_DRM)	+= drm.o
+ obj-$(CONFIG_DRM_MIPI_DBI) += drm_mipi_dbi.o
+diff --git a/drivers/gpu/drm/drm_virtual_encoder.c b/drivers/gpu/drm/drm_virtual_encoder.c
+new file mode 100644
+index 000000000000..2f65c6b47d00
+--- /dev/null
++++ b/drivers/gpu/drm/drm_virtual_encoder.c
+@@ -0,0 +1,299 @@
++/*
++ * Copyright (C) 2016 ARM Limited
++ * Author: Liviu Dudau <Liviu.Dudau@arm.com>
++ *
++ * Dummy encoder and connector that use the OF to "discover" the attached
++ * display timings. Can be used in situations where the encoder and connector's
++ * functionality are emulated and no setup steps are needed, or to describe
++ * attached panels for which no driver exists but can be used without
++ * additional hardware setup.
++ *
++ * The encoder also uses the component framework so that it can be a quick
++ * replacement for existing drivers when testing in an emulated environment.
++ *
++ * This file is subject to the terms and conditions of the GNU General Public
++ * License.  See the file COPYING in the main directory of this archive
++ * for more details.
++ *
++ */
++
++#include <drm/drm_crtc.h>
++#include <drm/drm_atomic_helper.h>
++#include <drm/drm_crtc_helper.h>
++#include <drm/drm_probe_helper.h>
++#include <drm/drm_print.h>
++#include <linux/platform_device.h>
++#include <drm/drm_of.h>
++#include <linux/component.h>
++#include <video/display_timing.h>
++#include <video/of_display_timing.h>
++#include <video/videomode.h>
++
++struct drm_virt_priv {
++	struct drm_connector connector;
++	struct drm_encoder encoder;
++	struct display_timings *timings;
++};
++
++#define connector_to_drm_virt_priv(x) \
++	container_of(x, struct drm_virt_priv, connector)
++
++#define encoder_to_drm_virt_priv(x) \
++	container_of(x, struct drm_virt_priv, encoder)
++
++static void drm_virtcon_destroy(struct drm_connector *connector)
++{
++	struct drm_virt_priv *conn = connector_to_drm_virt_priv(connector);
++
++	drm_connector_cleanup(connector);
++	display_timings_release(conn->timings);
++}
++
++static enum drm_connector_status
++drm_virtcon_detect(struct drm_connector *connector, bool force)
++{
++	return connector_status_connected;
++}
++
++static const struct drm_connector_funcs drm_virtcon_funcs = {
++	.reset = drm_atomic_helper_connector_reset,
++	.detect	= drm_virtcon_detect,
++	.fill_modes = drm_helper_probe_single_connector_modes,
++	.destroy = drm_virtcon_destroy,
++	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
++	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
++};
++
++static int drm_virtcon_get_modes(struct drm_connector *connector)
++{
++	struct drm_virt_priv *conn = connector_to_drm_virt_priv(connector);
++	struct display_timings *timings = conn->timings;
++	int i;
++
++	for (i = 0; i < timings->num_timings; i++) {
++		struct drm_display_mode *mode = drm_mode_create(connector->dev);
++		struct videomode vm;
++
++		if (videomode_from_timings(timings, &vm, i))
++			break;
++
++		drm_display_mode_from_videomode(&vm, mode);
++		mode->type = DRM_MODE_TYPE_DRIVER;
++		if (timings->native_mode == i)
++			mode->type = DRM_MODE_TYPE_PREFERRED;
++
++		drm_mode_set_name(mode);
++		drm_mode_probed_add(connector, mode);
++	}
++
++	return i;
++}
++
++static int drm_virtcon_mode_valid(struct drm_connector *connector,
++				   struct drm_display_mode *mode)
++{
++	return MODE_OK;
++}
++
++struct drm_encoder *drm_virtcon_best_encoder(struct drm_connector *connector)
++{
++	struct drm_virt_priv *priv = connector_to_drm_virt_priv(connector);
++
++	return &priv->encoder;
++}
++
++struct drm_encoder *
++drm_virtcon_atomic_best_encoder(struct drm_connector *connector,
++				 struct drm_connector_state *connector_state)
++{
++	struct drm_virt_priv *priv = connector_to_drm_virt_priv(connector);
++
++	return &priv->encoder;
++}
++
++static const struct drm_connector_helper_funcs drm_virtcon_helper_funcs = {
++	.get_modes = drm_virtcon_get_modes,
++	.mode_valid = drm_virtcon_mode_valid,
++	.best_encoder = drm_virtcon_best_encoder,
++	.atomic_best_encoder = drm_virtcon_atomic_best_encoder,
++};
++
++static void drm_vencoder_destroy(struct drm_encoder *encoder)
++{
++	drm_encoder_cleanup(encoder);
++}
++
++static const struct drm_encoder_funcs drm_vencoder_funcs = {
++	.destroy = drm_vencoder_destroy,
++};
++
++static void drm_vencoder_dpms(struct drm_encoder *encoder, int mode)
++{
++	/* nothing needed */
++}
++
++static bool drm_vencoder_mode_fixup(struct drm_encoder *encoder,
++				    const struct drm_display_mode *mode,
++				    struct drm_display_mode *adjusted_mode)
++{
++	/* nothing needed */
++	return true;
++}
++
++static void drm_vencoder_prepare(struct drm_encoder *encoder)
++{
++	drm_vencoder_dpms(encoder, DRM_MODE_DPMS_OFF);
++}
++
++static void drm_vencoder_commit(struct drm_encoder *encoder)
++{
++	drm_vencoder_dpms(encoder, DRM_MODE_DPMS_ON);
++}
++
++static void drm_vencoder_mode_set(struct drm_encoder *encoder,
++				  struct drm_display_mode *mode,
++				  struct drm_display_mode *adjusted_mode)
++{
++	/* nothing needed */
++}
++
++static const struct drm_encoder_helper_funcs drm_vencoder_helper_funcs = {
++	.dpms		= drm_vencoder_dpms,
++	.mode_fixup	= drm_vencoder_mode_fixup,
++	.prepare	= drm_vencoder_prepare,
++	.commit		= drm_vencoder_commit,
++	.mode_set	= drm_vencoder_mode_set,
++};
++
++static int drm_vencoder_bind(struct device *dev, struct device *master,
++			     void *data)
++{
++	struct drm_encoder *encoder;
++	struct drm_virt_priv *con;
++	struct drm_connector *connector;
++	struct drm_device *drm = data;
++	u32 crtcs = 0;
++	int ret;
++
++	con = devm_kzalloc(dev, sizeof(*con), GFP_KERNEL);
++	if (!con)
++		return -ENOMEM;
++
++	dev_set_drvdata(dev, con);
++	connector = &con->connector;
++	encoder = &con->encoder;
++
++	if (dev->of_node) {
++		struct drm_bridge *bridge;
++		crtcs = drm_of_find_possible_crtcs(drm, dev->of_node);
++		bridge = of_drm_find_bridge(dev->of_node);
++		if (bridge) {
++			ret = drm_bridge_attach(encoder, bridge, NULL, 0);
++			if (ret) {
++				DRM_ERROR("Failed to initialize bridge\n");
++				return ret;
++			}
++		}
++		con->timings = of_get_display_timings(dev->of_node);
++		if (!con->timings) {
++			dev_err(dev, "failed to get display panel timings\n");
++			return ENXIO;
++		}
++	}
++
++	/* If no CRTCs were found, fall back to the old encoder's behaviour */
++	if (crtcs == 0) {
++		dev_warn(dev, "Falling back to first CRTC\n");
++		crtcs = 1 << 0;
++	}
++
++	encoder->possible_crtcs = crtcs ? crtcs : 1;
++	encoder->possible_clones = 0;
++
++	ret = drm_encoder_init(drm, encoder, &drm_vencoder_funcs,
++			       DRM_MODE_ENCODER_VIRTUAL, NULL);
++	if (ret)
++		goto encoder_init_err;
++
++	drm_encoder_helper_add(encoder, &drm_vencoder_helper_funcs);
++
++	/* bogus values, pretend we're a 24" screen for DPI calculations */
++	connector->display_info.width_mm = 519;
++	connector->display_info.height_mm = 324;
++	connector->interlace_allowed = false;
++	connector->doublescan_allowed = false;
++	connector->polled = 0;
++
++	ret = drm_connector_init(drm, connector, &drm_virtcon_funcs,
++				 DRM_MODE_CONNECTOR_VIRTUAL);
++	if (ret)
++		goto connector_init_err;
++
++	drm_connector_helper_add(connector, &drm_virtcon_helper_funcs);
++
++	drm_connector_register(connector);
++
++	ret = drm_connector_attach_encoder(connector, encoder);
++	if (ret)
++		goto attach_err;
++
++	return ret;
++
++attach_err:
++	drm_connector_unregister(connector);
++	drm_connector_cleanup(connector);
++connector_init_err:
++	drm_encoder_cleanup(encoder);
++encoder_init_err:
++	display_timings_release(con->timings);
++
++	return ret;
++};
++
++static void drm_vencoder_unbind(struct device *dev, struct device *master,
++				void *data)
++{
++	struct drm_virt_priv *con = dev_get_drvdata(dev);
++
++	drm_connector_unregister(&con->connector);
++	drm_connector_cleanup(&con->connector);
++	drm_encoder_cleanup(&con->encoder);
++	display_timings_release(con->timings);
++}
++
++static const struct component_ops drm_vencoder_ops = {
++	.bind = drm_vencoder_bind,
++	.unbind = drm_vencoder_unbind,
++};
++
++static int drm_vencoder_probe(struct platform_device *pdev)
++{
++	return component_add(&pdev->dev, &drm_vencoder_ops);
++}
++
++static int drm_vencoder_remove(struct platform_device *pdev)
++{
++	component_del(&pdev->dev, &drm_vencoder_ops);
++	return 0;
++}
++
++static const struct of_device_id drm_vencoder_of_match[] = {
++	{ .compatible = "drm,virtual-encoder", },
++	{},
++};
++MODULE_DEVICE_TABLE(of, drm_vencoder_of_match);
++
++static struct platform_driver drm_vencoder_driver = {
++	.probe = drm_vencoder_probe,
++	.remove = drm_vencoder_remove,
++	.driver = {
++		.name = "drm_vencoder",
++		.of_match_table = drm_vencoder_of_match,
++	},
++};
++
++module_platform_driver(drm_vencoder_driver);
++
++MODULE_AUTHOR("Liviu Dudau");
++MODULE_DESCRIPTION("Virtual DRM Encoder");
++MODULE_LICENSE("GPL v2");
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0002-drm-arm-komeda-add-RENDER-capability-to-the-device-n.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0002-drm-arm-komeda-add-RENDER-capability-to-the-device-n.patch
new file mode 100644
index 0000000..77519f1
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0002-drm-arm-komeda-add-RENDER-capability-to-the-device-n.patch
@@ -0,0 +1,32 @@
+From 1b2c200673b4a08324f3a6575b30bd16030ed586 Mon Sep 17 00:00:00 2001
+From: Tushar Khandelwal <tushar.khandelwal@arm.com>
+Date: Wed, 17 Jun 2020 10:49:26 +0000
+Subject: [PATCH 02/22] drm: arm: komeda: add RENDER capability to the device
+ node
+
+this is required to make this driver work with android framework
+
+Signed-off-by: Tushar Khandelwal <tushar.khandelwal@arm.com>
+
+Upstream-Status: Inappropriate [Product specific configuration]
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ drivers/gpu/drm/arm/display/komeda/komeda_kms.c | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/drivers/gpu/drm/arm/display/komeda/komeda_kms.c b/drivers/gpu/drm/arm/display/komeda/komeda_kms.c
+index 1f6682032ca4..9d1a1942e673 100644
+--- a/drivers/gpu/drm/arm/display/komeda/komeda_kms.c
++++ b/drivers/gpu/drm/arm/display/komeda/komeda_kms.c
+@@ -59,7 +59,7 @@ static irqreturn_t komeda_kms_irq_handler(int irq, void *data)
+ }
+ 
+ static struct drm_driver komeda_kms_driver = {
+-	.driver_features = DRIVER_GEM | DRIVER_MODESET | DRIVER_ATOMIC,
++	.driver_features = DRIVER_GEM | DRIVER_MODESET | DRIVER_ATOMIC | DRIVER_RENDER,
+ 	.lastclose			= drm_fb_helper_lastclose,
+ 	DRM_GEM_CMA_DRIVER_OPS_WITH_DUMB_CREATE(komeda_gem_cma_dumb_create),
+ 	.fops = &komeda_cma_fops,
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0003-dt-bindings-mailbox-arm-mhuv2-Add-bindings.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0003-dt-bindings-mailbox-arm-mhuv2-Add-bindings.patch
new file mode 100644
index 0000000..1da75ea
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0003-dt-bindings-mailbox-arm-mhuv2-Add-bindings.patch
@@ -0,0 +1,241 @@
+From cdda49168d42c897574388356555f8130c021bb5 Mon Sep 17 00:00:00 2001
+From: Viresh Kumar <viresh.kumar@linaro.org>
+Date: Tue, 17 Nov 2020 15:32:05 +0530
+Subject: [PATCH 03/22] dt-bindings: mailbox : arm,mhuv2: Add bindings
+
+This patch adds device tree binding for ARM Message Handling Unit (MHU)
+controller version 2.
+
+Based on earlier work by Morten Borup Petersen.
+
+Reviewed-by: Rob Herring <robh@kernel.org>
+Co-developed-by: Tushar Khandelwal <tushar.khandelwal@arm.com>
+Signed-off-by: Tushar Khandelwal <tushar.khandelwal@arm.com>
+Signed-off-by: Viresh Kumar <viresh.kumar@linaro.org>
+Signed-off-by: Jassi Brar <jaswinder.singh@linaro.org>
+
+Upstream-Status: Backport [https://lkml.org/lkml/2020/11/17/234]
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ .../bindings/mailbox/arm,mhuv2.yaml           | 209 ++++++++++++++++++
+ 1 file changed, 209 insertions(+)
+ create mode 100644 Documentation/devicetree/bindings/mailbox/arm,mhuv2.yaml
+
+diff --git a/Documentation/devicetree/bindings/mailbox/arm,mhuv2.yaml b/Documentation/devicetree/bindings/mailbox/arm,mhuv2.yaml
+new file mode 100644
+index 000000000000..6608545ea66f
+--- /dev/null
++++ b/Documentation/devicetree/bindings/mailbox/arm,mhuv2.yaml
+@@ -0,0 +1,209 @@
++# SPDX-License-Identifier: (GPL-2.0-only OR BSD-2-Clause)
++%YAML 1.2
++---
++$id: http://devicetree.org/schemas/mailbox/arm,mhuv2.yaml#
++$schema: http://devicetree.org/meta-schemas/core.yaml#
++
++title: ARM MHUv2 Mailbox Controller
++
++maintainers:
++  - Tushar Khandelwal <tushar.khandelwal@arm.com>
++  - Viresh Kumar <viresh.kumar@linaro.org>
++
++description: |
++  The Arm Message Handling Unit (MHU) Version 2 is a mailbox controller that has
++  between 1 and 124 channel windows (each 32-bit wide) to provide unidirectional
++  communication with remote processor(s), where the number of channel windows
++  are implementation dependent.
++
++  Given the unidirectional nature of the controller, an MHUv2 mailbox may only
++  be written to or read from. If a pair of MHU controllers is implemented
++  between two processing elements to provide bidirectional communication, these
++  must be specified as two separate mailboxes.
++
++  If the interrupts property is present in device tree node, then its treated as
++  a "receiver" mailbox, otherwise a "sender".
++
++  An MHU controller must be specified along with the supported transport
++  protocols. The transport protocols determine the method of data transmission
++  as well as the number of provided mailbox channels.
++
++  Following are the possible transport protocols.
++
++  - Data-transfer: Each transfer is made of one or more words, using one or more
++    channel windows.
++
++  - Doorbell: Each transfer is made up of single bit flag, using any one of the
++    bits in a channel window. A channel window can support up to 32 doorbells
++    and the entire window shall be used in doorbell protocol.  Optionally, data
++    may be transmitted through a shared memory region, wherein the MHU is used
++    strictly as an interrupt generation mechanism but that is out of the scope
++    of these bindings.
++
++# We need a select here so we don't match all nodes with 'arm,primecell'
++select:
++  properties:
++    compatible:
++      contains:
++        enum:
++          - arm,mhuv2-tx
++          - arm,mhuv2-rx
++  required:
++    - compatible
++
++properties:
++  compatible:
++    oneOf:
++      - description: Sender mode
++        items:
++          - const: arm,mhuv2-tx
++          - const: arm,primecell
++
++      - description: Receiver-mode
++        items:
++          - const: arm,mhuv2-rx
++          - const: arm,primecell
++
++  reg:
++    maxItems: 1
++
++  interrupts:
++    description: |
++      The MHUv2 controller always implements an interrupt in the "receiver"
++      mode, while the interrupt in the "sender" mode was not available in the
++      version MHUv2.0, but the later versions do have it.
++    maxItems: 1
++
++  clocks:
++    maxItems: 1
++
++  clock-names:
++    maxItems: 1
++
++  arm,mhuv2-protocols:
++    $ref: /schemas/types.yaml#/definitions/uint32-matrix
++    description: |
++      The MHUv2 controller may contain up to 124 channel windows (each 32-bit
++      wide). The hardware and the DT bindings allows any combination of those to
++      be used for various transport protocols.
++
++      This property allows a platform to describe how these channel windows are
++      used in various transport protocols. The entries in this property shall be
++      present as an array of tuples, where each tuple describes details about
++      one of the transport protocol being implemented over some channel
++      window(s).
++
++      The first field of a tuple signifies the transfer protocol, 0 is reserved
++      for doorbell protocol, and 1 is reserved for data-transfer protocol.
++      Using any other value in the first field of a tuple makes it invalid.
++
++      The second field of a tuple signifies the number of channel windows where
++      the protocol would be used and should be set to a non zero value. For
++      doorbell protocol this field signifies the number of 32-bit channel
++      windows that implement the doorbell protocol. For data-transfer protocol,
++      this field signifies the number of 32-bit channel windows that implement
++      the data-transfer protocol.
++
++      The total number of channel windows specified here shouldn't be more than
++      the ones implemented by the platform, though one can specify lesser number
++      of windows here than what the platform implements.
++
++      mhu: mailbox@2b1f0000 {
++          ...
++
++          arm,mhuv2-protocols = <0 2>, <1 1>, <1 5>, <1 7>;
++      }
++
++      The above example defines the protocols of an ARM MHUv2 mailbox
++      controller, where a total of 15 channel windows are used. The first two
++      windows are used in doorbell protocol (64 doorbells), followed by 1, 5 and
++      7 windows (separately) used in data-transfer protocol.
++
++    minItems: 1
++    maxItems: 124
++    items:
++      items:
++        - enum: [ 0, 1 ]
++        - minimum: 0
++          maximum: 124
++
++
++  '#mbox-cells':
++    description: |
++      It is always set to 2. The first argument in the consumers 'mboxes'
++      property represents the channel window group, which may be used in
++      doorbell, or data-transfer protocol, and the second argument (only
++      relevant in doorbell protocol, should be 0 otherwise) represents the
++      doorbell number within the 32 bit wide channel window.
++
++      From the example given above for arm,mhuv2-protocols, here is how a client
++      node can reference them.
++
++      mboxes = <&mhu 0 5>; // Channel Window Group 0, doorbell 5.
++      mboxes = <&mhu 1 7>; // Channel Window Group 1, doorbell 7.
++      mboxes = <&mhu 2 0>; // Channel Window Group 2, data transfer protocol with 1 window.
++      mboxes = <&mhu 3 0>; // Channel Window Group 3, data transfer protocol with 5 windows.
++      mboxes = <&mhu 4 0>; // Channel Window Group 4, data transfer protocol with 7 windows.
++
++    const: 2
++
++if:
++  # Interrupt is compulsory for receiver
++  properties:
++    compatible:
++      contains:
++        const: arm,mhuv2-rx
++then:
++  required:
++    - interrupts
++
++required:
++  - compatible
++  - reg
++  - '#mbox-cells'
++  - arm,mhuv2-protocols
++
++additionalProperties: false
++
++examples:
++  # Multiple transport protocols implemented by the mailbox controllers
++  - |
++    soc {
++        #address-cells = <2>;
++        #size-cells = <2>;
++
++        mhu_tx: mailbox@2b1f0000 {
++            #mbox-cells = <2>;
++            compatible = "arm,mhuv2-tx", "arm,primecell";
++            reg = <0 0x2b1f0000 0 0x1000>;
++            clocks = <&clock 0>;
++            clock-names = "apb_pclk";
++            interrupts = <0 45 4>;
++            arm,mhuv2-protocols = <1 5>, <1 2>, <1 5>, <1 7>, <0 2>;
++        };
++
++        mhu_rx: mailbox@2b1f1000 {
++            #mbox-cells = <2>;
++            compatible = "arm,mhuv2-rx", "arm,primecell";
++            reg = <0 0x2b1f1000 0 0x1000>;
++            clocks = <&clock 0>;
++            clock-names = "apb_pclk";
++            interrupts = <0 46 4>;
++            arm,mhuv2-protocols = <1 1>, <1 7>, <0 2>;
++        };
++
++        mhu_client: scb@2e000000 {
++            compatible = "fujitsu,mb86s70-scb-1.0";
++            reg = <0 0x2e000000 0 0x4000>;
++
++            mboxes =
++                     //data-transfer protocol with 5 windows, mhu-tx
++                     <&mhu_tx 2 0>,
++                     //data-transfer protocol with 7 windows, mhu-tx
++                     <&mhu_tx 3 0>,
++                     //doorbell protocol channel 4, doorbell 27, mhu-tx
++                     <&mhu_tx 4 27>,
++                     //data-transfer protocol with 1 window, mhu-rx
++                     <&mhu_rx 0 0>;
++        };
++    };
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0004-mailbox-arm_mhuv2-Add-driver.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0004-mailbox-arm_mhuv2-Add-driver.patch
new file mode 100644
index 0000000..a4dd961
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0004-mailbox-arm_mhuv2-Add-driver.patch
@@ -0,0 +1,1256 @@
+From eadd9235d084da8022df2d232c90590f2160e433 Mon Sep 17 00:00:00 2001
+From: Viresh Kumar <viresh.kumar@linaro.org>
+Date: Tue, 17 Nov 2020 15:32:06 +0530
+Subject: [PATCH 04/22] mailbox: arm_mhuv2: Add driver
+
+This adds driver for the ARM MHUv2 (Message Handling Unit) mailbox
+controller.
+
+This is based on the accepted DT bindings of the controller and supports
+combination of both transport protocols, i.e. doorbell and data-transfer.
+
+Transmitting and receiving data through the mailbox framework is done
+through struct arm_mhuv2_mbox_msg.
+
+Based on the initial work done by Morten Borup Petersen from ARM.
+
+Co-developed-by: Tushar Khandelwal <tushar.khandelwal@arm.com>
+Signed-off-by: Tushar Khandelwal <tushar.khandelwal@arm.com>
+Tested-by: Usama Arif <usama.arif@arm.com>
+Signed-off-by: Viresh Kumar <viresh.kumar@linaro.org>
+Signed-off-by: Jassi Brar <jaswinder.singh@linaro.org>
+
+Upstream-Status: Backport [https://www.lkml.org/lkml/2020/11/17/235]
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ MAINTAINERS                               |    9 +
+ drivers/mailbox/Kconfig                   |    7 +
+ drivers/mailbox/Makefile                  |    2 +
+ drivers/mailbox/arm_mhuv2.c               | 1136 +++++++++++++++++++++
+ include/linux/mailbox/arm_mhuv2_message.h |   20 +
+ 5 files changed, 1174 insertions(+)
+ create mode 100644 drivers/mailbox/arm_mhuv2.c
+ create mode 100644 include/linux/mailbox/arm_mhuv2_message.h
+
+diff --git a/MAINTAINERS b/MAINTAINERS
+index 354831907474..5234423c477a 100644
+--- a/MAINTAINERS
++++ b/MAINTAINERS
+@@ -10459,6 +10459,15 @@ F:	drivers/mailbox/
+ F:	include/linux/mailbox_client.h
+ F:	include/linux/mailbox_controller.h
+ 
++MAILBOX ARM MHUv2
++M:	Viresh Kumar <viresh.kumar@linaro.org>
++M:	Tushar Khandelwal <Tushar.Khandelwal@arm.com>
++L:	linux-kernel@vger.kernel.org
++S:	Maintained
++F:	drivers/mailbox/arm_mhuv2.c
++F:	include/linux/mailbox/arm_mhuv2_message.h
++F:	Documentation/devicetree/bindings/mailbox/arm,mhuv2.yaml
++
+ MAN-PAGES: MANUAL PAGES FOR LINUX -- Sections 2, 3, 4, 5, and 7
+ M:	Michael Kerrisk <mtk.manpages@gmail.com>
+ L:	linux-man@vger.kernel.org
+diff --git a/drivers/mailbox/Kconfig b/drivers/mailbox/Kconfig
+index 05b1009e2820..3c0ea96a0a8b 100644
+--- a/drivers/mailbox/Kconfig
++++ b/drivers/mailbox/Kconfig
+@@ -16,6 +16,13 @@ config ARM_MHU
+ 	  The controller has 3 mailbox channels, the last of which can be
+ 	  used in Secure mode only.
+ 
++config ARM_MHU_V2
++	tristate "ARM MHUv2 Mailbox"
++	depends on ARM_AMBA
++	help
++	  Say Y here if you want to build the ARM MHUv2 controller driver,
++	  which provides unidirectional mailboxes between processing elements.
++
+ config IMX_MBOX
+ 	tristate "i.MX Mailbox"
+ 	depends on ARCH_MXC || COMPILE_TEST
+diff --git a/drivers/mailbox/Makefile b/drivers/mailbox/Makefile
+index 2e06e02b2e03..7194fa92c787 100644
+--- a/drivers/mailbox/Makefile
++++ b/drivers/mailbox/Makefile
+@@ -7,6 +7,8 @@ obj-$(CONFIG_MAILBOX_TEST)	+= mailbox-test.o
+ 
+ obj-$(CONFIG_ARM_MHU)	+= arm_mhu.o arm_mhu_db.o
+ 
++obj-$(CONFIG_ARM_MHU_V2)	+= arm_mhuv2.o
++
+ obj-$(CONFIG_IMX_MBOX)	+= imx-mailbox.o
+ 
+ obj-$(CONFIG_ARMADA_37XX_RWTM_MBOX)	+= armada-37xx-rwtm-mailbox.o
+diff --git a/drivers/mailbox/arm_mhuv2.c b/drivers/mailbox/arm_mhuv2.c
+new file mode 100644
+index 000000000000..67fb10885bb4
+--- /dev/null
++++ b/drivers/mailbox/arm_mhuv2.c
+@@ -0,0 +1,1136 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * ARM Message Handling Unit Version 2 (MHUv2) driver.
++ *
++ * Copyright (C) 2020 ARM Ltd.
++ * Copyright (C) 2020 Linaro Ltd.
++ *
++ * An MHUv2 mailbox controller can provide up to 124 channel windows (each 32
++ * bit long) and the driver allows any combination of both the transport
++ * protocol modes: data-transfer and doorbell, to be used on those channel
++ * windows.
++ *
++ * The transport protocols should be specified in the device tree entry for the
++ * device. The transport protocols determine how the underlying hardware
++ * resources of the device are utilized when transmitting data. Refer to the
++ * device tree bindings of the ARM MHUv2 controller for more details.
++ *
++ * The number of registered mailbox channels is dependent on both the underlying
++ * hardware - mainly the number of channel windows implemented by the platform,
++ * as well as the selected transport protocols.
++ *
++ * The MHUv2 controller can work both as a sender and receiver, but the driver
++ * and the DT bindings support unidirectional transfers for better allocation of
++ * the channels. That is, this driver will be probed for two separate devices
++ * for each mailbox controller, a sender device and a receiver device.
++ */
++
++#include <linux/amba/bus.h>
++#include <linux/interrupt.h>
++#include <linux/mailbox_controller.h>
++#include <linux/mailbox/arm_mhuv2_message.h>
++#include <linux/module.h>
++#include <linux/of_address.h>
++#include <linux/spinlock.h>
++
++/* ====== MHUv2 Registers ====== */
++
++/* Maximum number of channel windows */
++#define MHUV2_CH_WN_MAX			124
++/* Number of combined interrupt status registers */
++#define MHUV2_CMB_INT_ST_REG_CNT	4
++#define MHUV2_STAT_BYTES		(sizeof(u32))
++#define MHUV2_STAT_BITS			(MHUV2_STAT_BYTES * __CHAR_BIT__)
++
++#define LSB_MASK(n)			((1 << (n * __CHAR_BIT__)) - 1)
++#define MHUV2_PROTOCOL_PROP		"arm,mhuv2-protocols"
++
++/* Register Message Handling Unit Configuration fields */
++struct mhu_cfg_t {
++	u32 num_ch : 7;
++	u32 pad : 25;
++} __packed;
++
++/* register Interrupt Status fields */
++struct int_st_t {
++	u32 nr2r : 1;
++	u32 r2nr : 1;
++	u32 pad : 30;
++} __packed;
++
++/* Register Interrupt Clear fields */
++struct int_clr_t {
++	u32 nr2r : 1;
++	u32 r2nr : 1;
++	u32 pad : 30;
++} __packed;
++
++/* Register Interrupt Enable fields */
++struct int_en_t {
++	u32 r2nr : 1;
++	u32 nr2r : 1;
++	u32 chcomb : 1;
++	u32 pad : 29;
++} __packed;
++
++/* Register Implementer Identification fields */
++struct iidr_t {
++	u32 implementer : 12;
++	u32 revision : 4;
++	u32 variant : 4;
++	u32 product_id : 12;
++} __packed;
++
++/* Register Architecture Identification Register fields */
++struct aidr_t {
++	u32 arch_minor_rev : 4;
++	u32 arch_major_rev : 4;
++	u32 pad : 24;
++} __packed;
++
++/* Sender Channel Window fields */
++struct mhu2_send_ch_wn_reg {
++	u32 stat;
++	u8 pad1[0x0C - 0x04];
++	u32 stat_set;
++	u32 int_st;
++	u32 int_clr;
++	u32 int_en;
++	u8 pad2[0x20 - 0x1C];
++} __packed;
++
++/* Sender frame register fields */
++struct mhu2_send_frame_reg {
++	struct mhu2_send_ch_wn_reg ch_wn[MHUV2_CH_WN_MAX];
++	struct mhu_cfg_t mhu_cfg;
++	u32 resp_cfg;
++	u32 access_request;
++	u32 access_ready;
++	struct int_st_t int_st;
++	struct int_clr_t int_clr;
++	struct int_en_t int_en;
++	u32 reserved0;
++	u32 chcomb_int_st[MHUV2_CMB_INT_ST_REG_CNT];
++	u8 pad[0xFC8 - 0xFB0];
++	struct iidr_t iidr;
++	struct aidr_t aidr;
++} __packed;
++
++/* Receiver Channel Window fields */
++struct mhu2_recv_ch_wn_reg {
++	u32 stat;
++	u32 stat_masked;
++	u32 stat_clear;
++	u8 reserved0[0x10 - 0x0C];
++	u32 mask;
++	u32 mask_set;
++	u32 mask_clear;
++	u8 pad[0x20 - 0x1C];
++} __packed;
++
++/* Receiver frame register fields */
++struct mhu2_recv_frame_reg {
++	struct mhu2_recv_ch_wn_reg ch_wn[MHUV2_CH_WN_MAX];
++	struct mhu_cfg_t mhu_cfg;
++	u8 reserved0[0xF90 - 0xF84];
++	struct int_st_t int_st;
++	struct int_clr_t int_clr;
++	struct int_en_t int_en;
++	u32 pad;
++	u32 chcomb_int_st[MHUV2_CMB_INT_ST_REG_CNT];
++	u8 reserved2[0xFC8 - 0xFB0];
++	struct iidr_t iidr;
++	struct aidr_t aidr;
++} __packed;
++
++
++/* ====== MHUv2 data structures ====== */
++
++enum mhuv2_transport_protocol {
++	DOORBELL = 0,
++	DATA_TRANSFER = 1
++};
++
++enum mhuv2_frame {
++	RECEIVER_FRAME,
++	SENDER_FRAME
++};
++
++/**
++ * struct mhuv2 - MHUv2 mailbox controller data
++ *
++ * @mbox:	Mailbox controller belonging to the MHU frame.
++ * @send/recv:	Base address of the register mapping region.
++ * @frame:	Frame type: RECEIVER_FRAME or SENDER_FRAME.
++ * @irq:	Interrupt.
++ * @windows:	Channel windows implemented by the platform.
++ * @minor:	Minor version of the controller.
++ * @length:	Length of the protocols array in bytes.
++ * @protocols:	Raw protocol information, derived from device tree.
++ * @doorbell_pending_lock: spinlock required for correct operation of Tx
++ *		interrupt for doorbells.
++ */
++struct mhuv2 {
++	struct mbox_controller mbox;
++	union {
++		struct mhu2_send_frame_reg __iomem *send;
++		struct mhu2_recv_frame_reg __iomem *recv;
++	};
++	enum mhuv2_frame frame;
++	unsigned int irq;
++	unsigned int windows;
++	unsigned int minor;
++	unsigned int length;
++	u32 *protocols;
++
++	spinlock_t doorbell_pending_lock;
++};
++
++#define mhu_from_mbox(_mbox) container_of(_mbox, struct mhuv2, mbox)
++
++/**
++ * struct mhuv2_protocol_ops - MHUv2 operations
++ *
++ * Each transport protocol must provide an implementation of the operations
++ * provided here.
++ *
++ * @rx_startup: Startup callback for receiver.
++ * @rx_shutdown: Shutdown callback for receiver.
++ * @read_data: Reads and clears newly available data.
++ * @tx_startup: Startup callback for receiver.
++ * @tx_shutdown: Shutdown callback for receiver.
++ * @last_tx_done: Report back if the last tx is completed or not.
++ * @send_data: Send data to the receiver.
++ */
++struct mhuv2_protocol_ops {
++	int (*rx_startup)(struct mhuv2 *mhu, struct mbox_chan *chan);
++	void (*rx_shutdown)(struct mhuv2 *mhu, struct mbox_chan *chan);
++	void *(*read_data)(struct mhuv2 *mhu, struct mbox_chan *chan);
++
++	void (*tx_startup)(struct mhuv2 *mhu, struct mbox_chan *chan);
++	void (*tx_shutdown)(struct mhuv2 *mhu, struct mbox_chan *chan);
++	int (*last_tx_done)(struct mhuv2 *mhu, struct mbox_chan *chan);
++	int (*send_data)(struct mhuv2 *mhu, struct mbox_chan *chan, void *arg);
++};
++
++/*
++ * MHUv2 mailbox channel's private information
++ *
++ * @ops:	protocol specific ops for the channel.
++ * @ch_wn_idx:	Channel window index allocated to the channel.
++ * @windows:	Total number of windows consumed by the channel, only relevant
++ *		in DATA_TRANSFER protocol.
++ * @doorbell:	Doorbell bit number within the ch_wn_idx window, only relevant
++ *		in DOORBELL protocol.
++ * @pending:	Flag indicating pending doorbell interrupt, only relevant in
++ *		DOORBELL protocol.
++ */
++struct mhuv2_mbox_chan_priv {
++	const struct mhuv2_protocol_ops *ops;
++	u32 ch_wn_idx;
++	union {
++		u32 windows;
++		struct {
++			u32 doorbell;
++			u32 pending;
++		};
++	};
++};
++
++/* Macro for reading a bitfield within a physically mapped packed struct */
++#define readl_relaxed_bitfield(_regptr, _field)				\
++	({								\
++		u32 _regval;						\
++		_regval = readl_relaxed((_regptr));			\
++		(*(typeof((_regptr)))(&_regval))._field;		\
++	})
++
++/* Macro for writing a bitfield within a physically mapped packed struct */
++#define writel_relaxed_bitfield(_value, _regptr, _field)		\
++	({								\
++		u32 _regval;						\
++		_regval = readl_relaxed(_regptr);			\
++		(*(typeof(_regptr))(&_regval))._field = _value;		\
++		writel_relaxed(_regval, _regptr);			\
++	})
++
++
++/* =================== Doorbell transport protocol operations =============== */
++
++static int mhuv2_doorbell_rx_startup(struct mhuv2 *mhu, struct mbox_chan *chan)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++
++	writel_relaxed(BIT(priv->doorbell),
++		       &mhu->recv->ch_wn[priv->ch_wn_idx].mask_clear);
++	return 0;
++}
++
++static void mhuv2_doorbell_rx_shutdown(struct mhuv2 *mhu,
++				       struct mbox_chan *chan)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++
++	writel_relaxed(BIT(priv->doorbell),
++		       &mhu->recv->ch_wn[priv->ch_wn_idx].mask_set);
++}
++
++static void *mhuv2_doorbell_read_data(struct mhuv2 *mhu, struct mbox_chan *chan)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++
++	writel_relaxed(BIT(priv->doorbell),
++		       &mhu->recv->ch_wn[priv->ch_wn_idx].stat_clear);
++	return NULL;
++}
++
++static int mhuv2_doorbell_last_tx_done(struct mhuv2 *mhu,
++				       struct mbox_chan *chan)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++
++	return !(readl_relaxed(&mhu->send->ch_wn[priv->ch_wn_idx].stat) &
++		 BIT(priv->doorbell));
++}
++
++static int mhuv2_doorbell_send_data(struct mhuv2 *mhu, struct mbox_chan *chan,
++				    void *arg)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++	unsigned long flags;
++
++	spin_lock_irqsave(&mhu->doorbell_pending_lock, flags);
++
++	priv->pending = 1;
++	writel_relaxed(BIT(priv->doorbell),
++		       &mhu->send->ch_wn[priv->ch_wn_idx].stat_set);
++
++	spin_unlock_irqrestore(&mhu->doorbell_pending_lock, flags);
++
++	return 0;
++}
++
++static const struct mhuv2_protocol_ops mhuv2_doorbell_ops = {
++	.rx_startup = mhuv2_doorbell_rx_startup,
++	.rx_shutdown = mhuv2_doorbell_rx_shutdown,
++	.read_data = mhuv2_doorbell_read_data,
++	.last_tx_done = mhuv2_doorbell_last_tx_done,
++	.send_data = mhuv2_doorbell_send_data,
++};
++#define IS_PROTOCOL_DOORBELL(_priv) (_priv->ops == &mhuv2_doorbell_ops)
++
++/* ============= Data transfer transport protocol operations ================ */
++
++static int mhuv2_data_transfer_rx_startup(struct mhuv2 *mhu,
++					  struct mbox_chan *chan)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++	int i = priv->ch_wn_idx + priv->windows - 1;
++
++	/*
++	 * The protocol mandates that all but the last status register must be
++	 * masked.
++	 */
++	writel_relaxed(0xFFFFFFFF, &mhu->recv->ch_wn[i].mask_clear);
++	return 0;
++}
++
++static void mhuv2_data_transfer_rx_shutdown(struct mhuv2 *mhu,
++					    struct mbox_chan *chan)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++	int i = priv->ch_wn_idx + priv->windows - 1;
++
++	writel_relaxed(0xFFFFFFFF, &mhu->recv->ch_wn[i].mask_set);
++}
++
++static void *mhuv2_data_transfer_read_data(struct mhuv2 *mhu,
++					   struct mbox_chan *chan)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++	const int windows = priv->windows;
++	struct arm_mhuv2_mbox_msg *msg;
++	u32 *data;
++	int i, idx;
++
++	msg = kzalloc(sizeof(*msg) + windows * MHUV2_STAT_BYTES, GFP_KERNEL);
++	if (!msg)
++		return ERR_PTR(-ENOMEM);
++
++	data = msg->data = msg + 1;
++	msg->len = windows * MHUV2_STAT_BYTES;
++
++	/*
++	 * Messages are expected in order of most significant word to least
++	 * significant word. Refer mhuv2_data_transfer_send_data() for more
++	 * details.
++	 *
++	 * We also need to read the stat register instead of stat_masked, as we
++	 * masked all but the last window.
++	 *
++	 * Last channel window must be cleared as the final operation. Upon
++	 * clearing the last channel window register, which is unmasked in
++	 * data-transfer protocol, the interrupt is de-asserted.
++	 */
++	for (i = 0; i < windows; i++) {
++		idx = priv->ch_wn_idx + i;
++		data[windows - 1 - i] = readl_relaxed(&mhu->recv->ch_wn[idx].stat);
++		writel_relaxed(0xFFFFFFFF, &mhu->recv->ch_wn[idx].stat_clear);
++	}
++
++	return msg;
++}
++
++static void mhuv2_data_transfer_tx_startup(struct mhuv2 *mhu,
++					   struct mbox_chan *chan)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++	int i = priv->ch_wn_idx + priv->windows - 1;
++
++	/* Enable interrupts only for the last window */
++	if (mhu->minor) {
++		writel_relaxed(0x1, &mhu->send->ch_wn[i].int_clr);
++		writel_relaxed(0x1, &mhu->send->ch_wn[i].int_en);
++	}
++}
++
++static void mhuv2_data_transfer_tx_shutdown(struct mhuv2 *mhu,
++					    struct mbox_chan *chan)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++	int i = priv->ch_wn_idx + priv->windows - 1;
++
++	if (mhu->minor)
++		writel_relaxed(0x0, &mhu->send->ch_wn[i].int_en);
++}
++
++static int mhuv2_data_transfer_last_tx_done(struct mhuv2 *mhu,
++					    struct mbox_chan *chan)
++{
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++	int i = priv->ch_wn_idx + priv->windows - 1;
++
++	/* Just checking the last channel window should be enough */
++	return !readl_relaxed(&mhu->send->ch_wn[i].stat);
++}
++
++/*
++ * Message will be transmitted from most significant to least significant word.
++ * This is to allow for messages shorter than channel windows to still trigger
++ * the receiver interrupt which gets activated when the last stat register is
++ * written. As an example, a 6-word message is to be written on a 4-channel MHU
++ * connection: Registers marked with '*' are masked, and will not generate an
++ * interrupt on the receiver side once written.
++ *
++ * u32 *data =	[0x00000001], [0x00000002], [0x00000003], [0x00000004],
++ *		[0x00000005], [0x00000006]
++ *
++ * ROUND 1:
++ * stat reg		To write	Write sequence
++ * [ stat 3 ]	<-	[0x00000001]	4 <- triggers interrupt on receiver
++ * [ stat 2 ]	<-	[0x00000002]	3
++ * [ stat 1 ]	<-	[0x00000003]	2
++ * [ stat 0 ]	<-	[0x00000004]	1
++ *
++ * data += 4 // Increment data pointer by number of stat regs
++ *
++ * ROUND 2:
++ * stat reg		To write	Write sequence
++ * [ stat 3 ]	<-	[0x00000005]	2 <- triggers interrupt on receiver
++ * [ stat 2 ]	<-	[0x00000006]	1
++ * [ stat 1 ]	<-	[0x00000000]
++ * [ stat 0 ]	<-	[0x00000000]
++ */
++static int mhuv2_data_transfer_send_data(struct mhuv2 *mhu,
++					 struct mbox_chan *chan, void *arg)
++{
++	const struct arm_mhuv2_mbox_msg *msg = arg;
++	int bytes_left = msg->len, bytes_to_send, bytes_in_round, i;
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++	int windows = priv->windows;
++	u32 *data = msg->data, word;
++
++	while (bytes_left) {
++		if (!data[0]) {
++			dev_err(mhu->mbox.dev, "Data aligned at first window can't be zero to guarantee interrupt generation at receiver");
++			return -EINVAL;
++		}
++
++		while(!mhuv2_data_transfer_last_tx_done(mhu, chan))
++			continue;
++
++		bytes_in_round = min(bytes_left, (int)(windows * MHUV2_STAT_BYTES));
++
++		for (i = windows - 1; i >= 0; i--) {
++			/* Data less than windows can transfer ? */
++			if (unlikely(bytes_in_round <= i * MHUV2_STAT_BYTES))
++				continue;
++
++			word = data[i];
++			bytes_to_send = bytes_in_round & (MHUV2_STAT_BYTES - 1);
++			if (unlikely(bytes_to_send))
++				word &= LSB_MASK(bytes_to_send);
++			else
++				bytes_to_send = MHUV2_STAT_BYTES;
++
++			writel_relaxed(word, &mhu->send->ch_wn[priv->ch_wn_idx + windows - 1 - i].stat_set);
++			bytes_left -= bytes_to_send;
++			bytes_in_round -= bytes_to_send;
++		}
++
++		data += windows;
++	}
++
++	return 0;
++}
++
++static const struct mhuv2_protocol_ops mhuv2_data_transfer_ops = {
++	.rx_startup = mhuv2_data_transfer_rx_startup,
++	.rx_shutdown = mhuv2_data_transfer_rx_shutdown,
++	.read_data = mhuv2_data_transfer_read_data,
++	.tx_startup = mhuv2_data_transfer_tx_startup,
++	.tx_shutdown = mhuv2_data_transfer_tx_shutdown,
++	.last_tx_done = mhuv2_data_transfer_last_tx_done,
++	.send_data = mhuv2_data_transfer_send_data,
++};
++
++/* Interrupt handlers */
++
++static struct mbox_chan *get_irq_chan_comb(struct mhuv2 *mhu, u32 *reg)
++{
++	struct mbox_chan *chans = mhu->mbox.chans;
++	int channel = 0, i, offset = 0, windows, protocol, ch_wn;
++	u32 stat;
++
++	for (i = 0; i < MHUV2_CMB_INT_ST_REG_CNT; i++) {
++		stat = readl_relaxed(reg + i);
++		if (!stat)
++			continue;
++
++		ch_wn = i * MHUV2_STAT_BITS + __builtin_ctz(stat);
++
++		for (i = 0; i < mhu->length; i += 2) {
++			protocol = mhu->protocols[i];
++			windows = mhu->protocols[i + 1];
++
++			if (ch_wn >= offset + windows) {
++				if (protocol == DOORBELL)
++					channel += MHUV2_STAT_BITS * windows;
++				else
++					channel++;
++
++				offset += windows;
++				continue;
++			}
++
++			/* Return first chan of the window in doorbell mode */
++			if (protocol == DOORBELL)
++				channel += MHUV2_STAT_BITS * (ch_wn - offset);
++
++			return &chans[channel];
++		}
++	}
++
++	return ERR_PTR(-EIO);
++}
++
++static irqreturn_t mhuv2_sender_interrupt(int irq, void *data)
++{
++	struct mhuv2 *mhu = data;
++	struct device *dev = mhu->mbox.dev;
++	struct mhuv2_mbox_chan_priv *priv;
++	struct mbox_chan *chan;
++	unsigned long flags;
++	int i, found = 0;
++	u32 stat;
++
++	chan = get_irq_chan_comb(mhu, mhu->send->chcomb_int_st);
++	if (IS_ERR(chan)) {
++		dev_warn(dev, "Failed to find channel for the Tx interrupt\n");
++		return IRQ_NONE;
++	}
++	priv = chan->con_priv;
++
++	if (!IS_PROTOCOL_DOORBELL(priv)) {
++		writel_relaxed(1, &mhu->send->ch_wn[priv->ch_wn_idx + priv->windows - 1].int_clr);
++
++		if (chan->cl) {
++			mbox_chan_txdone(chan, 0);
++			return IRQ_HANDLED;
++		}
++
++		dev_warn(dev, "Tx interrupt Received on channel (%u) not currently attached to a mailbox client\n",
++			 priv->ch_wn_idx);
++		return IRQ_NONE;
++	}
++
++	/* Clear the interrupt first, so we don't miss any doorbell later */
++	writel_relaxed(1, &mhu->send->ch_wn[priv->ch_wn_idx].int_clr);
++
++	/*
++	 * In Doorbell mode, make sure no new transitions happen while the
++	 * interrupt handler is trying to find the finished doorbell tx
++	 * operations, else we may think few of the transfers were complete
++	 * before they actually were.
++	 */
++	spin_lock_irqsave(&mhu->doorbell_pending_lock, flags);
++
++	/*
++	 * In case of doorbell mode, the first channel of the window is returned
++	 * by get_irq_chan_comb(). Find all the pending channels here.
++	 */
++	stat = readl_relaxed(&mhu->send->ch_wn[priv->ch_wn_idx].stat);
++
++	for (i = 0; i < MHUV2_STAT_BITS; i++) {
++		priv = chan[i].con_priv;
++
++		/* Find cases where pending was 1, but stat's bit is cleared */
++		if (priv->pending ^ ((stat >> i) & 0x1)) {
++			BUG_ON(!priv->pending);
++
++			if (!chan->cl) {
++				dev_warn(dev, "Tx interrupt received on doorbell (%u : %u) channel not currently attached to a mailbox client\n",
++					 priv->ch_wn_idx, i);
++				continue;
++			}
++
++			mbox_chan_txdone(&chan[i], 0);
++			priv->pending = 0;
++			found++;
++		}
++	}
++
++	spin_unlock_irqrestore(&mhu->doorbell_pending_lock, flags);
++
++	if (!found) {
++		/*
++		 * We may have already processed the doorbell in the previous
++		 * iteration if the interrupt came right after we cleared it but
++		 * before we read the stat register.
++		 */
++		dev_dbg(dev, "Couldn't find the doorbell (%u) for the Tx interrupt interrupt\n",
++			priv->ch_wn_idx);
++		return IRQ_NONE;
++	}
++
++	return IRQ_HANDLED;
++}
++
++static struct mbox_chan *get_irq_chan_comb_rx(struct mhuv2 *mhu)
++{
++	struct mhuv2_mbox_chan_priv *priv;
++	struct mbox_chan *chan;
++	u32 stat;
++
++	chan = get_irq_chan_comb(mhu, mhu->recv->chcomb_int_st);
++	if (IS_ERR(chan))
++		return chan;
++
++	priv = chan->con_priv;
++	if (!IS_PROTOCOL_DOORBELL(priv))
++		return chan;
++
++	/*
++	 * In case of doorbell mode, the first channel of the window is returned
++	 * by the routine. Find the exact channel here.
++	 */
++	stat = readl_relaxed(&mhu->recv->ch_wn[priv->ch_wn_idx].stat_masked);
++	BUG_ON(!stat);
++
++	return chan + __builtin_ctz(stat);
++}
++
++static struct mbox_chan *get_irq_chan_stat_rx(struct mhuv2 *mhu)
++{
++	struct mbox_chan *chans = mhu->mbox.chans;
++	struct mhuv2_mbox_chan_priv *priv;
++	u32 stat;
++	int i = 0;
++
++	while (i < mhu->mbox.num_chans) {
++		priv = chans[i].con_priv;
++		stat = readl_relaxed(&mhu->recv->ch_wn[priv->ch_wn_idx].stat_masked);
++
++		if (stat) {
++			if (IS_PROTOCOL_DOORBELL(priv))
++				i += __builtin_ctz(stat);
++			return &chans[i];
++		}
++
++		i += IS_PROTOCOL_DOORBELL(priv) ? MHUV2_STAT_BITS : 1;
++	}
++
++	return ERR_PTR(-EIO);
++}
++
++static struct mbox_chan *get_irq_chan_rx(struct mhuv2 *mhu)
++{
++	if (!mhu->minor)
++		return get_irq_chan_stat_rx(mhu);
++
++	return get_irq_chan_comb_rx(mhu);
++}
++
++static irqreturn_t mhuv2_receiver_interrupt(int irq, void *arg)
++{
++	struct mhuv2 *mhu = arg;
++	struct mbox_chan *chan = get_irq_chan_rx(mhu);
++	struct device *dev = mhu->mbox.dev;
++	struct mhuv2_mbox_chan_priv *priv;
++	int ret = IRQ_NONE;
++	void *data;
++
++	if (IS_ERR(chan)) {
++		dev_warn(dev, "Failed to find channel for the rx interrupt\n");
++		return IRQ_NONE;
++	}
++	priv = chan->con_priv;
++
++	/* Read and clear the data first */
++	data = priv->ops->read_data(mhu, chan);
++
++	if (!chan->cl) {
++		dev_warn(dev, "Received data on channel (%u) not currently attached to a mailbox client\n",
++			 priv->ch_wn_idx);
++	} else if (IS_ERR(data)) {
++		dev_err(dev, "Failed to read data: %lu\n", PTR_ERR(data));
++	} else {
++		mbox_chan_received_data(chan, data);
++		ret = IRQ_HANDLED;
++	}
++
++	kfree(data);
++	return ret;
++}
++
++/* Sender and receiver ops */
++static bool mhuv2_sender_last_tx_done(struct mbox_chan *chan)
++{
++	struct mhuv2 *mhu = mhu_from_mbox(chan->mbox);
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++
++	return priv->ops->last_tx_done(mhu, chan);
++}
++
++static int mhuv2_sender_send_data(struct mbox_chan *chan, void *data)
++{
++	struct mhuv2 *mhu = mhu_from_mbox(chan->mbox);
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++
++	if (!priv->ops->last_tx_done(mhu, chan))
++		return -EBUSY;
++
++	return priv->ops->send_data(mhu, chan, data);
++}
++
++static int mhuv2_sender_startup(struct mbox_chan *chan)
++{
++	struct mhuv2 *mhu = mhu_from_mbox(chan->mbox);
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++
++	if (priv->ops->tx_startup)
++		priv->ops->tx_startup(mhu, chan);
++	return 0;
++}
++
++static void mhuv2_sender_shutdown(struct mbox_chan *chan)
++{
++	struct mhuv2 *mhu = mhu_from_mbox(chan->mbox);
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++
++	if (priv->ops->tx_shutdown)
++		priv->ops->tx_shutdown(mhu, chan);
++}
++
++static const struct mbox_chan_ops mhuv2_sender_ops = {
++	.send_data = mhuv2_sender_send_data,
++	.startup = mhuv2_sender_startup,
++	.shutdown = mhuv2_sender_shutdown,
++	.last_tx_done = mhuv2_sender_last_tx_done,
++};
++
++static int mhuv2_receiver_startup(struct mbox_chan *chan)
++{
++	struct mhuv2 *mhu = mhu_from_mbox(chan->mbox);
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++
++	return priv->ops->rx_startup(mhu, chan);
++}
++
++static void mhuv2_receiver_shutdown(struct mbox_chan *chan)
++{
++	struct mhuv2 *mhu = mhu_from_mbox(chan->mbox);
++	struct mhuv2_mbox_chan_priv *priv = chan->con_priv;
++
++	priv->ops->rx_shutdown(mhu, chan);
++}
++
++static int mhuv2_receiver_send_data(struct mbox_chan *chan, void *data)
++{
++	dev_err(chan->mbox->dev,
++		"Trying to transmit on a receiver MHU frame\n");
++	return -EIO;
++}
++
++static bool mhuv2_receiver_last_tx_done(struct mbox_chan *chan)
++{
++	dev_err(chan->mbox->dev, "Trying to Tx poll on a receiver MHU frame\n");
++	return true;
++}
++
++static const struct mbox_chan_ops mhuv2_receiver_ops = {
++	.send_data = mhuv2_receiver_send_data,
++	.startup = mhuv2_receiver_startup,
++	.shutdown = mhuv2_receiver_shutdown,
++	.last_tx_done = mhuv2_receiver_last_tx_done,
++};
++
++static struct mbox_chan *mhuv2_mbox_of_xlate(struct mbox_controller *mbox,
++					     const struct of_phandle_args *pa)
++{
++	struct mhuv2 *mhu = mhu_from_mbox(mbox);
++	struct mbox_chan *chans = mbox->chans;
++	int channel = 0, i, offset, doorbell, protocol, windows;
++
++	if (pa->args_count != 2)
++		return ERR_PTR(-EINVAL);
++
++	offset = pa->args[0];
++	doorbell = pa->args[1];
++	if (doorbell >= MHUV2_STAT_BITS)
++		goto out;
++
++	for (i = 0; i < mhu->length; i += 2) {
++		protocol = mhu->protocols[i];
++		windows = mhu->protocols[i + 1];
++
++		if (protocol == DOORBELL) {
++			if (offset < windows)
++				return &chans[channel + MHUV2_STAT_BITS * offset + doorbell];
++
++			channel += MHUV2_STAT_BITS * windows;
++			offset -= windows;
++		} else {
++			if (offset == 0) {
++				if (doorbell)
++					goto out;
++
++				return &chans[channel];
++			}
++
++			channel++;
++			offset--;
++		}
++	}
++
++out:
++	dev_err(mbox->dev, "Couldn't xlate to a valid channel (%d: %d)\n",
++		pa->args[0], doorbell);
++	return ERR_PTR(-ENODEV);
++}
++
++static int mhuv2_verify_protocol(struct mhuv2 *mhu)
++{
++	struct device *dev = mhu->mbox.dev;
++	int protocol, windows, channels = 0, total_windows = 0, i;
++
++	for (i = 0; i < mhu->length; i += 2) {
++		protocol = mhu->protocols[i];
++		windows = mhu->protocols[i + 1];
++
++		if (!windows) {
++			dev_err(dev, "Window size can't be zero (%d)\n", i);
++			return -EINVAL;
++		}
++		total_windows += windows;
++
++		if (protocol == DOORBELL) {
++			channels += MHUV2_STAT_BITS * windows;
++		} else if (protocol == DATA_TRANSFER) {
++			channels++;
++		} else {
++			dev_err(dev, "Invalid protocol (%d) present in %s property at index %d\n",
++				protocol, MHUV2_PROTOCOL_PROP, i);
++			return -EINVAL;
++		}
++	}
++
++	if (total_windows > mhu->windows) {
++		dev_err(dev, "Channel windows can't be more than what's implemented by the hardware ( %d: %d)\n",
++			total_windows, mhu->windows);
++		return -EINVAL;
++	}
++
++	mhu->mbox.num_chans = channels;
++	return 0;
++}
++
++static int mhuv2_allocate_channels(struct mhuv2 *mhu)
++{
++	struct mbox_controller *mbox = &mhu->mbox;
++	struct mhuv2_mbox_chan_priv *priv;
++	struct device *dev = mbox->dev;
++	struct mbox_chan *chans;
++	int protocol, windows = 0, next_window = 0, i, j, k;
++
++	chans = devm_kcalloc(dev, mbox->num_chans, sizeof(*chans), GFP_KERNEL);
++	if (!chans)
++		return -ENOMEM;
++
++	mbox->chans = chans;
++
++	for (i = 0; i < mhu->length; i += 2) {
++		next_window += windows;
++
++		protocol = mhu->protocols[i];
++		windows = mhu->protocols[i + 1];
++
++		if (protocol == DATA_TRANSFER) {
++			priv = devm_kmalloc(dev, sizeof(*priv), GFP_KERNEL);
++			if (!priv)
++				return -ENOMEM;
++
++			priv->ch_wn_idx = next_window;
++			priv->ops = &mhuv2_data_transfer_ops;
++			priv->windows = windows;
++			chans++->con_priv = priv;
++			continue;
++		}
++
++		for (j = 0; j < windows; j++) {
++			for (k = 0; k < MHUV2_STAT_BITS; k++) {
++				priv = devm_kmalloc(dev, sizeof(*priv), GFP_KERNEL);
++				if (!priv)
++					return -ENOMEM;
++
++				priv->ch_wn_idx = next_window + j;
++				priv->ops = &mhuv2_doorbell_ops;
++				priv->doorbell = k;
++				chans++->con_priv = priv;
++			}
++
++			/*
++			 * Permanently enable interrupt as we can't
++			 * control it per doorbell.
++			 */
++			if (mhu->frame == SENDER_FRAME && mhu->minor)
++				writel_relaxed(0x1, &mhu->send->ch_wn[priv->ch_wn_idx].int_en);
++		}
++	}
++
++	/* Make sure we have initialized all channels */
++	BUG_ON(chans - mbox->chans != mbox->num_chans);
++
++	return 0;
++}
++
++static int mhuv2_parse_channels(struct mhuv2 *mhu)
++{
++	struct device *dev = mhu->mbox.dev;
++	const struct device_node *np = dev->of_node;
++	int ret, count;
++	u32 *protocols;
++
++	count = of_property_count_u32_elems(np, MHUV2_PROTOCOL_PROP);
++	if (count <= 0 || count % 2) {
++		dev_err(dev, "Invalid %s property (%d)\n", MHUV2_PROTOCOL_PROP,
++			count);
++		return -EINVAL;
++	}
++
++	protocols = devm_kmalloc_array(dev, count, sizeof(*protocols), GFP_KERNEL);
++	if (!protocols)
++		return -ENOMEM;
++
++	ret = of_property_read_u32_array(np, MHUV2_PROTOCOL_PROP, protocols, count);
++	if (ret) {
++		dev_err(dev, "Failed to read %s property: %d\n",
++			MHUV2_PROTOCOL_PROP, ret);
++		return ret;
++	}
++
++	mhu->protocols = protocols;
++	mhu->length = count;
++
++	ret = mhuv2_verify_protocol(mhu);
++	if (ret)
++		return ret;
++
++	return mhuv2_allocate_channels(mhu);
++}
++
++static int mhuv2_tx_init(struct amba_device *adev, struct mhuv2 *mhu,
++			 void __iomem *reg)
++{
++	struct device *dev = mhu->mbox.dev;
++	int ret, i;
++
++	mhu->frame = SENDER_FRAME;
++	mhu->mbox.ops = &mhuv2_sender_ops;
++	mhu->send = reg;
++
++	mhu->windows = readl_relaxed_bitfield(&mhu->send->mhu_cfg, num_ch);
++	mhu->minor = readl_relaxed_bitfield(&mhu->send->aidr, arch_minor_rev);
++
++	spin_lock_init(&mhu->doorbell_pending_lock);
++
++	/*
++	 * For minor version 1 and forward, tx interrupt is provided by
++	 * the controller.
++	 */
++	if (mhu->minor && adev->irq[0]) {
++		ret = devm_request_threaded_irq(dev, adev->irq[0], NULL,
++						mhuv2_sender_interrupt,
++						IRQF_ONESHOT, "mhuv2-tx", mhu);
++		if (ret) {
++			dev_err(dev, "Failed to request tx IRQ, fallback to polling mode: %d\n",
++				ret);
++		} else {
++			mhu->mbox.txdone_irq = true;
++			mhu->mbox.txdone_poll = false;
++			mhu->irq = adev->irq[0];
++
++			writel_relaxed_bitfield(1, &mhu->send->int_en, chcomb);
++
++			/* Disable all channel interrupts */
++			for (i = 0; i < mhu->windows; i++)
++				writel_relaxed(0x0, &mhu->send->ch_wn[i].int_en);
++
++			goto out;
++		}
++	}
++
++	mhu->mbox.txdone_irq = false;
++	mhu->mbox.txdone_poll = true;
++	mhu->mbox.txpoll_period = 1;
++
++out:
++	/* Wait for receiver to be ready */
++	writel_relaxed(0x1, &mhu->send->access_request);
++	while (!readl_relaxed(&mhu->send->access_ready))
++		continue;
++
++	return 0;
++}
++
++static int mhuv2_rx_init(struct amba_device *adev, struct mhuv2 *mhu,
++			 void __iomem *reg)
++{
++	struct device *dev = mhu->mbox.dev;
++	int ret, i;
++
++	mhu->frame = RECEIVER_FRAME;
++	mhu->mbox.ops = &mhuv2_receiver_ops;
++	mhu->recv = reg;
++
++	mhu->windows = readl_relaxed_bitfield(&mhu->recv->mhu_cfg, num_ch);
++	mhu->minor = readl_relaxed_bitfield(&mhu->recv->aidr, arch_minor_rev);
++
++	mhu->irq = adev->irq[0];
++	if (!mhu->irq) {
++		dev_err(dev, "Missing receiver IRQ\n");
++		return -EINVAL;
++	}
++
++	ret = devm_request_threaded_irq(dev, mhu->irq, NULL,
++					mhuv2_receiver_interrupt, IRQF_ONESHOT,
++					"mhuv2-rx", mhu);
++	if (ret) {
++		dev_err(dev, "Failed to request rx IRQ\n");
++		return ret;
++	}
++
++	/* Mask all the channel windows */
++	for (i = 0; i < mhu->windows; i++)
++		writel_relaxed(0xFFFFFFFF, &mhu->recv->ch_wn[i].mask_set);
++
++	if (mhu->minor)
++		writel_relaxed_bitfield(1, &mhu->recv->int_en, chcomb);
++
++	return 0;
++}
++
++static int mhuv2_probe(struct amba_device *adev, const struct amba_id *id)
++{
++	struct device *dev = &adev->dev;
++	const struct device_node *np = dev->of_node;
++	struct mhuv2 *mhu;
++	void __iomem *reg;
++	int ret = -EINVAL;
++
++	reg = devm_of_iomap(dev, dev->of_node, 0, NULL);
++	if (!reg)
++		return -ENOMEM;
++
++	mhu = devm_kzalloc(dev, sizeof(*mhu), GFP_KERNEL);
++	if (!mhu)
++		return -ENOMEM;
++
++	mhu->mbox.dev = dev;
++	mhu->mbox.of_xlate = mhuv2_mbox_of_xlate;
++
++	if (of_device_is_compatible(np, "arm,mhuv2-tx"))
++		ret = mhuv2_tx_init(adev, mhu, reg);
++	else if (of_device_is_compatible(np, "arm,mhuv2-rx"))
++		ret = mhuv2_rx_init(adev, mhu, reg);
++	else
++		dev_err(dev, "Invalid compatible property\n");
++
++	if (ret)
++		return ret;
++
++	/* Channel windows can't be 0 */
++	BUG_ON(!mhu->windows);
++
++	ret = mhuv2_parse_channels(mhu);
++	if (ret)
++		return ret;
++
++	amba_set_drvdata(adev, mhu);
++
++	ret = devm_mbox_controller_register(dev, &mhu->mbox);
++	if (ret)
++		dev_err(dev, "failed to register ARM MHUv2 driver %d\n", ret);
++
++	return ret;
++}
++
++static int mhuv2_remove(struct amba_device *adev)
++{
++	struct mhuv2 *mhu = amba_get_drvdata(adev);
++
++	if (mhu->frame == SENDER_FRAME)
++		writel_relaxed(0x0, &mhu->send->access_request);
++
++	return 0;
++}
++
++static struct amba_id mhuv2_ids[] = {
++	{
++		/* 2.0 */
++		.id = 0xbb0d1,
++		.mask = 0xfffff,
++	},
++	{
++		/* 2.1 */
++		.id = 0xbb076,
++		.mask = 0xfffff,
++	},
++	{ 0, 0 },
++};
++MODULE_DEVICE_TABLE(amba, mhuv2_ids);
++
++static struct amba_driver mhuv2_driver = {
++	.drv = {
++		.name	= "arm-mhuv2",
++	},
++	.id_table	= mhuv2_ids,
++	.probe		= mhuv2_probe,
++	.remove		= mhuv2_remove,
++};
++module_amba_driver(mhuv2_driver);
++
++MODULE_LICENSE("GPL v2");
++MODULE_DESCRIPTION("ARM MHUv2 Driver");
++MODULE_AUTHOR("Viresh Kumar <viresh.kumar@linaro.org>");
++MODULE_AUTHOR("Tushar Khandelwal <tushar.khandelwal@arm.com>");
+diff --git a/include/linux/mailbox/arm_mhuv2_message.h b/include/linux/mailbox/arm_mhuv2_message.h
+new file mode 100644
+index 000000000000..821b9d96daa4
+--- /dev/null
++++ b/include/linux/mailbox/arm_mhuv2_message.h
+@@ -0,0 +1,20 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * ARM MHUv2 Mailbox Message
++ *
++ * Copyright (C) 2020 Arm Ltd.
++ * Copyright (C) 2020 Linaro Ltd.
++ */
++
++#ifndef _LINUX_ARM_MHUV2_MESSAGE_H_
++#define _LINUX_ARM_MHUV2_MESSAGE_H_
++
++#include <linux/types.h>
++
++/* Data structure for data-transfer protocol */
++struct arm_mhuv2_mbox_msg {
++	void *data;
++	size_t len;
++};
++
++#endif /* _LINUX_ARM_MHUV2_MESSAGE_H_ */
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0005-mailbox-arm_mhuv2-Fix-sparse-warnings.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0005-mailbox-arm_mhuv2-Fix-sparse-warnings.patch
new file mode 100644
index 0000000..8905f74
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0005-mailbox-arm_mhuv2-Fix-sparse-warnings.patch
@@ -0,0 +1,114 @@
+From 1c75e7d566e29258e9daf7b1548f2d681efb4aea Mon Sep 17 00:00:00 2001
+From: Viresh Kumar <viresh.kumar@linaro.org>
+Date: Wed, 30 Dec 2020 10:12:04 +0530
+Subject: [PATCH 05/22] mailbox: arm_mhuv2: Fix sparse warnings
+
+This patch fixes a bunch of sparse warnings in the newly added arm_mhuv2
+driver.
+
+drivers/mailbox/arm_mhuv2.c:506:24: warning: incorrect type in argument 1 (different address spaces)
+drivers/mailbox/arm_mhuv2.c:506:24:    expected void const volatile [noderef] __iomem *addr
+drivers/mailbox/arm_mhuv2.c:506:24:    got unsigned int [usertype] *
+drivers/mailbox/arm_mhuv2.c:547:42: warning: incorrect type in argument 2 (different address spaces)
+drivers/mailbox/arm_mhuv2.c:547:42:    expected unsigned int [usertype] *reg
+drivers/mailbox/arm_mhuv2.c:547:42:    got unsigned int [noderef] __iomem *
+drivers/mailbox/arm_mhuv2.c:625:42: warning: incorrect type in argument 2 (different address spaces)
+drivers/mailbox/arm_mhuv2.c:625:42:    expected unsigned int [usertype] *reg
+drivers/mailbox/arm_mhuv2.c:625:42:    got unsigned int [noderef] __iomem *
+drivers/mailbox/arm_mhuv2.c:972:24: warning: dereference of noderef expression
+drivers/mailbox/arm_mhuv2.c:973:22: warning: dereference of noderef expression
+drivers/mailbox/arm_mhuv2.c:993:25: warning: dereference of noderef expression
+drivers/mailbox/arm_mhuv2.c:1026:24: warning: dereference of noderef expression
+drivers/mailbox/arm_mhuv2.c:1027:22: warning: dereference of noderef expression
+drivers/mailbox/arm_mhuv2.c:1048:17: warning: dereference of noderef expression
+
+Reported-by: kernel test robot <lkp@intel.com>
+Signed-off-by: Viresh Kumar <viresh.kumar@linaro.org>
+Signed-off-by: Jassi Brar <jaswinder.singh@linaro.org>
+
+Upstream-Status: Backport [https://lkml.org/lkml/2021/2/9/428]
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ drivers/mailbox/arm_mhuv2.c | 22 +++++++++++-----------
+ 1 file changed, 11 insertions(+), 11 deletions(-)
+
+diff --git a/drivers/mailbox/arm_mhuv2.c b/drivers/mailbox/arm_mhuv2.c
+index 67fb10885bb4..8223c1005254 100644
+--- a/drivers/mailbox/arm_mhuv2.c
++++ b/drivers/mailbox/arm_mhuv2.c
+@@ -238,19 +238,19 @@ struct mhuv2_mbox_chan_priv {
+ };
+ 
+ /* Macro for reading a bitfield within a physically mapped packed struct */
+-#define readl_relaxed_bitfield(_regptr, _field)				\
++#define readl_relaxed_bitfield(_regptr, _type, _field)			\
+ 	({								\
+ 		u32 _regval;						\
+ 		_regval = readl_relaxed((_regptr));			\
+-		(*(typeof((_regptr)))(&_regval))._field;		\
++		(*(_type *)(&_regval))._field;				\
+ 	})
+ 
+ /* Macro for writing a bitfield within a physically mapped packed struct */
+-#define writel_relaxed_bitfield(_value, _regptr, _field)		\
++#define writel_relaxed_bitfield(_value, _regptr, _type, _field)		\
+ 	({								\
+ 		u32 _regval;						\
+ 		_regval = readl_relaxed(_regptr);			\
+-		(*(typeof(_regptr))(&_regval))._field = _value;		\
++		(*(_type *)(&_regval))._field = _value;			\
+ 		writel_relaxed(_regval, _regptr);			\
+ 	})
+ 
+@@ -496,7 +496,7 @@ static const struct mhuv2_protocol_ops mhuv2_data_transfer_ops = {
+ 
+ /* Interrupt handlers */
+ 
+-static struct mbox_chan *get_irq_chan_comb(struct mhuv2 *mhu, u32 *reg)
++static struct mbox_chan *get_irq_chan_comb(struct mhuv2 *mhu, u32 __iomem *reg)
+ {
+ 	struct mbox_chan *chans = mhu->mbox.chans;
+ 	int channel = 0, i, offset = 0, windows, protocol, ch_wn;
+@@ -969,8 +969,8 @@ static int mhuv2_tx_init(struct amba_device *adev, struct mhuv2 *mhu,
+ 	mhu->mbox.ops = &mhuv2_sender_ops;
+ 	mhu->send = reg;
+ 
+-	mhu->windows = readl_relaxed_bitfield(&mhu->send->mhu_cfg, num_ch);
+-	mhu->minor = readl_relaxed_bitfield(&mhu->send->aidr, arch_minor_rev);
++	mhu->windows = readl_relaxed_bitfield(&mhu->send->mhu_cfg, struct mhu_cfg_t, num_ch);
++	mhu->minor = readl_relaxed_bitfield(&mhu->send->aidr, struct aidr_t, arch_minor_rev);
+ 
+ 	spin_lock_init(&mhu->doorbell_pending_lock);
+ 
+@@ -990,7 +990,7 @@ static int mhuv2_tx_init(struct amba_device *adev, struct mhuv2 *mhu,
+ 			mhu->mbox.txdone_poll = false;
+ 			mhu->irq = adev->irq[0];
+ 
+-			writel_relaxed_bitfield(1, &mhu->send->int_en, chcomb);
++			writel_relaxed_bitfield(1, &mhu->send->int_en, struct int_en_t, chcomb);
+ 
+ 			/* Disable all channel interrupts */
+ 			for (i = 0; i < mhu->windows; i++)
+@@ -1023,8 +1023,8 @@ static int mhuv2_rx_init(struct amba_device *adev, struct mhuv2 *mhu,
+ 	mhu->mbox.ops = &mhuv2_receiver_ops;
+ 	mhu->recv = reg;
+ 
+-	mhu->windows = readl_relaxed_bitfield(&mhu->recv->mhu_cfg, num_ch);
+-	mhu->minor = readl_relaxed_bitfield(&mhu->recv->aidr, arch_minor_rev);
++	mhu->windows = readl_relaxed_bitfield(&mhu->recv->mhu_cfg, struct mhu_cfg_t, num_ch);
++	mhu->minor = readl_relaxed_bitfield(&mhu->recv->aidr, struct aidr_t, arch_minor_rev);
+ 
+ 	mhu->irq = adev->irq[0];
+ 	if (!mhu->irq) {
+@@ -1045,7 +1045,7 @@ static int mhuv2_rx_init(struct amba_device *adev, struct mhuv2 *mhu,
+ 		writel_relaxed(0xFFFFFFFF, &mhu->recv->ch_wn[i].mask_set);
+ 
+ 	if (mhu->minor)
+-		writel_relaxed_bitfield(1, &mhu->recv->int_en, chcomb);
++		writel_relaxed_bitfield(1, &mhu->recv->int_en, struct int_en_t, chcomb);
+ 
+ 	return 0;
+ }
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0006-mailbox-arm_mhuv2-make-remove-callback-return-void.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0006-mailbox-arm_mhuv2-make-remove-callback-return-void.patch
new file mode 100644
index 0000000..a353f31
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0006-mailbox-arm_mhuv2-make-remove-callback-return-void.patch
@@ -0,0 +1,47 @@
+From 107f39e7741bb77688df47ce3f56b25cceb301c3 Mon Sep 17 00:00:00 2001
+From: =?UTF-8?q?Uwe=20Kleine-K=C3=B6nig?= <u.kleine-koenig@pengutronix.de>
+Date: Tue, 2 Feb 2021 20:43:08 +0100
+Subject: [PATCH 06/22] mailbox: arm_mhuv2: make remove callback return void
+MIME-Version: 1.0
+Content-Type: text/plain; charset=UTF-8
+Content-Transfer-Encoding: 8bit
+
+My build tests failed to catch that amba driver that would have needed
+adaption in commit 3fd269e74f2f ("amba: Make the remove callback return
+void"). Change the remove function to make the driver build again.
+
+Reported-by: kernel test robot <lkp@intel.com>
+Fixes: 3fd269e74f2f ("amba: Make the remove callback return void")
+Signed-off-by: Uwe Kleine-König <u.kleine-koenig@pengutronix.de>
+Acked-by: Viresh Kumar <viresh.kumar@linaro.org>
+Signed-off-by: Jassi Brar <jaswinder.singh@linaro.org>
+
+Upstream-Status: Backport [https://lkml.org/lkml/2021/2/2/1525]
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ drivers/mailbox/arm_mhuv2.c | 4 +---
+ 1 file changed, 1 insertion(+), 3 deletions(-)
+
+diff --git a/drivers/mailbox/arm_mhuv2.c b/drivers/mailbox/arm_mhuv2.c
+index 8223c1005254..cdfb1939fabf 100644
+--- a/drivers/mailbox/arm_mhuv2.c
++++ b/drivers/mailbox/arm_mhuv2.c
+@@ -1095,14 +1095,12 @@ static int mhuv2_probe(struct amba_device *adev, const struct amba_id *id)
+ 	return ret;
+ }
+ 
+-static int mhuv2_remove(struct amba_device *adev)
++static void mhuv2_remove(struct amba_device *adev)
+ {
+ 	struct mhuv2 *mhu = amba_get_drvdata(adev);
+ 
+ 	if (mhu->frame == SENDER_FRAME)
+ 		writel_relaxed(0x0, &mhu->send->access_request);
+-
+-	return 0;
+ }
+ 
+ static struct amba_id mhuv2_ids[] = {
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0007-mailbox-arm_mhuv2-Skip-calling-kfree-with-invalid-po.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0007-mailbox-arm_mhuv2-Skip-calling-kfree-with-invalid-po.patch
new file mode 100644
index 0000000..cf5b0b0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0007-mailbox-arm_mhuv2-Skip-calling-kfree-with-invalid-po.patch
@@ -0,0 +1,41 @@
+From 81d76e92b03a6f33acefd8aef168948c5f595205 Mon Sep 17 00:00:00 2001
+From: Viresh Kumar <viresh.kumar@linaro.org>
+Date: Mon, 22 Feb 2021 12:48:06 +0530
+Subject: [PATCH 07/22] mailbox: arm_mhuv2: Skip calling kfree() with invalid
+ pointer
+
+It is possible that 'data' passed to kfree() is set to a error value
+instead of allocated space. Make sure it doesn't get called with invalid
+pointer.
+
+Fixes: 5a6338cce9f4 ("mailbox: arm_mhuv2: Add driver")
+Cc: v5.11 <stable@vger.kernel.org> # v5.11
+Reported-by: kernel test robot <lkp@intel.com>
+Reported-by: Dan Carpenter <dan.carpenter@oracle.com>
+Signed-off-by: Viresh Kumar <viresh.kumar@linaro.org>
+Signed-off-by: Jassi Brar <jaswinder.singh@linaro.org>
+
+Upstream-Status: Backport [https://lkml.org/lkml/2021/2/22/57]
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ drivers/mailbox/arm_mhuv2.c | 4 +++-
+ 1 file changed, 3 insertions(+), 1 deletion(-)
+
+diff --git a/drivers/mailbox/arm_mhuv2.c b/drivers/mailbox/arm_mhuv2.c
+index cdfb1939fabf..d997f8ebfa98 100644
+--- a/drivers/mailbox/arm_mhuv2.c
++++ b/drivers/mailbox/arm_mhuv2.c
+@@ -699,7 +699,9 @@ static irqreturn_t mhuv2_receiver_interrupt(int irq, void *arg)
+ 		ret = IRQ_HANDLED;
+ 	}
+ 
+-	kfree(data);
++	if (!IS_ERR(data))
++		kfree(data);
++
+ 	return ret;
+ }
+ 
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0008-firmware-arm_ffa-Backport-of-arm_ffa-driver.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0008-firmware-arm_ffa-Backport-of-arm_ffa-driver.patch
new file mode 100644
index 0000000..d91dbc5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0008-firmware-arm_ffa-Backport-of-arm_ffa-driver.patch
@@ -0,0 +1,1684 @@
+From 85df9333f0adf60fd76eb5ebb21b89c5b0a86c10 Mon Sep 17 00:00:00 2001
+From: Sudeep Holla <sudeep.holla@arm.com>
+Date: Tue, 18 May 2021 17:36:18 +0100
+Subject: [PATCH 01/32] firmware: arm_ffa: Backport of arm_ffa driver
+
+This is a backport of upstream ARM FFA driver from:
+https://git.kernel.org/pub/scm/linux/kernel/git/sudeep.holla/linux.git/commit/?h=v5.10/ffa&id=c0aff30cb9ad6a00c82acef0f2a48f99adf997c0
+
+to branch=android12-5.10-lts
+
+   arm64: smccc: Add support for SMCCCv1.2 extended input/output registers
+   commit 3fdc0cb59d97f87e2cc708d424f1538e31744286 upstream.
+
+   firmware: arm_ffa: Add initial FFA bus support for device enumeration
+   commit e781858488b918e30a6ff28e9eab6058b787e3b3 upstream.
+
+   firmware: arm_ffa: Add initial Arm FFA driver support
+   commit 3bbfe9871005f38df2955b2e125933edf1d2feef upstream.
+
+   firmware: arm_ffa: Add support for SMCCC as transport to FFA driver
+   commit 714be77e976a4b013b935b3223b2ef68856084d0 upstream.
+
+   firmware: arm_ffa: Setup in-kernel users of FFA partitions
+   commit d0c0bce831223b08e5bade2cefc93c3ddb790796 upstream.
+
+   firmware: arm_ffa: Add support for MEM_* interfaces
+   commit cc2195fe536c28e192df5d07e6dd277af36814b4 upstream.
+
+   firmware: arm_ffa: Ensure drivers provide a probe function
+   commit 92743071464fca5acbbe812d9a0d88de3eaaad36 upstream.
+
+   firmware: arm_ffa: Simplify probe function
+   commit e362547addc39e4bb18ad5bdfd59ce4d512d0c08 upstream.
+
+   firmware: arm_ffa: Fix the comment style
+   commit ba684a31d3626c86cd9097e12d6ed57d224d077d upstream.
+
+   firmware: arm_ffa: Fix a possible ffa_linux_errmap buffer overflow
+   commit dd925db6f07556061c11ab1fbfa4a0145ae6b438 upstream.
+
+   firmware: arm_ffa: Add missing remove callback to ffa_bus_type
+   commit 244f5d597e1ea519c2085fbd9819458688775e42 upstream.
+
+   firmware: arm_ffa: Fix __ffa_devices_unregister
+   commit eb7b52e6db7c21400b9b2d539f9343fb6e94bd94 upstream.
+
+   firmware: arm_ffa: Handle compatibility with different firmware versions
+   commit 8e3f9da608f14cfebac2659d8dd8737b79d01308 upstream.
+
+   firmware: arm_ffa: Add support for MEM_LEND
+   commit 82a8daaecfd9382e9450a05f86be8a274cf69a27 upstream.
+
+   firmware: arm_ffa: Remove unused 'compat_version' variable
+   commit 01537a078b86917c7bb69aa4b756b42b980c158b upstream.
+
+Signed-off-by: Sudeep Holla <sudeep.holla@arm.com>
+Change-Id: If9df40d2d10be9e3c95298820bc20c201ea1774c
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+
+Upstream-Status: Backport
+Change-Id: I8e6197d8b7ef6654dacd21450069b8e284a3cec5
+---
+ MAINTAINERS                       |   7 +
+ arch/arm64/kernel/asm-offsets.c   |   9 +
+ arch/arm64/kernel/smccc-call.S    |  57 +++
+ drivers/firmware/Kconfig          |   1 +
+ drivers/firmware/Makefile         |   1 +
+ drivers/firmware/arm_ffa/Kconfig  |  21 +
+ drivers/firmware/arm_ffa/Makefile |   6 +
+ drivers/firmware/arm_ffa/bus.c    | 220 +++++++++
+ drivers/firmware/arm_ffa/common.h |  31 ++
+ drivers/firmware/arm_ffa/driver.c | 776 ++++++++++++++++++++++++++++++
+ drivers/firmware/arm_ffa/smccc.c  |  39 ++
+ include/linux/arm-smccc.h         |  55 +++
+ include/linux/arm_ffa.h           | 269 +++++++++++
+ 13 files changed, 1492 insertions(+)
+ create mode 100644 drivers/firmware/arm_ffa/Kconfig
+ create mode 100644 drivers/firmware/arm_ffa/Makefile
+ create mode 100644 drivers/firmware/arm_ffa/bus.c
+ create mode 100644 drivers/firmware/arm_ffa/common.h
+ create mode 100644 drivers/firmware/arm_ffa/driver.c
+ create mode 100644 drivers/firmware/arm_ffa/smccc.c
+ create mode 100644 include/linux/arm_ffa.h
+
+diff --git a/MAINTAINERS b/MAINTAINERS
+index 5234423c477a..d5fdc9e68c89 100644
+--- a/MAINTAINERS
++++ b/MAINTAINERS
+@@ -6847,6 +6847,13 @@ F:	include/linux/firewire.h
+ F:	include/uapi/linux/firewire*.h
+ F:	tools/firewire/
+ 
++FIRMWARE FRAMEWORK FOR ARMV8-A
++M:	Sudeep Holla <sudeep.holla@arm.com>
++L:	linux-arm-kernel@lists.infradead.org
++S:	Maintained
++F:	drivers/firmware/arm_ffa/
++F:	include/linux/arm_ffa.h
++
+ FIRMWARE LOADER (request_firmware)
+ M:	Luis Chamberlain <mcgrof@kernel.org>
+ L:	linux-kernel@vger.kernel.org
+diff --git a/arch/arm64/kernel/asm-offsets.c b/arch/arm64/kernel/asm-offsets.c
+index 93da876a58e6..bad4a367da28 100644
+--- a/arch/arm64/kernel/asm-offsets.c
++++ b/arch/arm64/kernel/asm-offsets.c
+@@ -139,6 +139,15 @@ int main(void)
+   DEFINE(ARM_SMCCC_RES_X2_OFFS,		offsetof(struct arm_smccc_res, a2));
+   DEFINE(ARM_SMCCC_QUIRK_ID_OFFS,	offsetof(struct arm_smccc_quirk, id));
+   DEFINE(ARM_SMCCC_QUIRK_STATE_OFFS,	offsetof(struct arm_smccc_quirk, state));
++  DEFINE(ARM_SMCCC_1_2_REGS_X0_OFFS,	offsetof(struct arm_smccc_1_2_regs, a0));
++  DEFINE(ARM_SMCCC_1_2_REGS_X2_OFFS,	offsetof(struct arm_smccc_1_2_regs, a2));
++  DEFINE(ARM_SMCCC_1_2_REGS_X4_OFFS,	offsetof(struct arm_smccc_1_2_regs, a4));
++  DEFINE(ARM_SMCCC_1_2_REGS_X6_OFFS,	offsetof(struct arm_smccc_1_2_regs, a6));
++  DEFINE(ARM_SMCCC_1_2_REGS_X8_OFFS,	offsetof(struct arm_smccc_1_2_regs, a8));
++  DEFINE(ARM_SMCCC_1_2_REGS_X10_OFFS,	offsetof(struct arm_smccc_1_2_regs, a10));
++  DEFINE(ARM_SMCCC_1_2_REGS_X12_OFFS,	offsetof(struct arm_smccc_1_2_regs, a12));
++  DEFINE(ARM_SMCCC_1_2_REGS_X14_OFFS,	offsetof(struct arm_smccc_1_2_regs, a14));
++  DEFINE(ARM_SMCCC_1_2_REGS_X16_OFFS,	offsetof(struct arm_smccc_1_2_regs, a16));
+   BLANK();
+   DEFINE(HIBERN_PBE_ORIG,	offsetof(struct pbe, orig_address));
+   DEFINE(HIBERN_PBE_ADDR,	offsetof(struct pbe, address));
+diff --git a/arch/arm64/kernel/smccc-call.S b/arch/arm64/kernel/smccc-call.S
+index d62447964ed9..2def9d0dd3dd 100644
+--- a/arch/arm64/kernel/smccc-call.S
++++ b/arch/arm64/kernel/smccc-call.S
+@@ -43,3 +43,60 @@ SYM_FUNC_START(__arm_smccc_hvc)
+ 	SMCCC	hvc
+ SYM_FUNC_END(__arm_smccc_hvc)
+ EXPORT_SYMBOL(__arm_smccc_hvc)
++
++	.macro SMCCC_1_2 instr
++	/* Save `res` and free a GPR that won't be clobbered */
++	stp     x1, x19, [sp, #-16]!
++
++	/* Ensure `args` won't be clobbered while loading regs in next step */
++	mov	x19, x0
++
++	/* Load the registers x0 - x17 from the struct arm_smccc_1_2_regs */
++	ldp	x0, x1, [x19, #ARM_SMCCC_1_2_REGS_X0_OFFS]
++	ldp	x2, x3, [x19, #ARM_SMCCC_1_2_REGS_X2_OFFS]
++	ldp	x4, x5, [x19, #ARM_SMCCC_1_2_REGS_X4_OFFS]
++	ldp	x6, x7, [x19, #ARM_SMCCC_1_2_REGS_X6_OFFS]
++	ldp	x8, x9, [x19, #ARM_SMCCC_1_2_REGS_X8_OFFS]
++	ldp	x10, x11, [x19, #ARM_SMCCC_1_2_REGS_X10_OFFS]
++	ldp	x12, x13, [x19, #ARM_SMCCC_1_2_REGS_X12_OFFS]
++	ldp	x14, x15, [x19, #ARM_SMCCC_1_2_REGS_X14_OFFS]
++	ldp	x16, x17, [x19, #ARM_SMCCC_1_2_REGS_X16_OFFS]
++
++	\instr #0
++
++	/* Load the `res` from the stack */
++	ldr	x19, [sp]
++
++	/* Store the registers x0 - x17 into the result structure */
++	stp	x0, x1, [x19, #ARM_SMCCC_1_2_REGS_X0_OFFS]
++	stp	x2, x3, [x19, #ARM_SMCCC_1_2_REGS_X2_OFFS]
++	stp	x4, x5, [x19, #ARM_SMCCC_1_2_REGS_X4_OFFS]
++	stp	x6, x7, [x19, #ARM_SMCCC_1_2_REGS_X6_OFFS]
++	stp	x8, x9, [x19, #ARM_SMCCC_1_2_REGS_X8_OFFS]
++	stp	x10, x11, [x19, #ARM_SMCCC_1_2_REGS_X10_OFFS]
++	stp	x12, x13, [x19, #ARM_SMCCC_1_2_REGS_X12_OFFS]
++	stp	x14, x15, [x19, #ARM_SMCCC_1_2_REGS_X14_OFFS]
++	stp	x16, x17, [x19, #ARM_SMCCC_1_2_REGS_X16_OFFS]
++
++	/* Restore original x19 */
++	ldp     xzr, x19, [sp], #16
++	ret
++.endm
++
++/*
++ * void arm_smccc_1_2_hvc(const struct arm_smccc_1_2_regs *args,
++ *			  struct arm_smccc_1_2_regs *res);
++ */
++SYM_FUNC_START(arm_smccc_1_2_hvc)
++	SMCCC_1_2 hvc
++SYM_FUNC_END(arm_smccc_1_2_hvc)
++EXPORT_SYMBOL(arm_smccc_1_2_hvc)
++
++/*
++ * void arm_smccc_1_2_smc(const struct arm_smccc_1_2_regs *args,
++ *			  struct arm_smccc_1_2_regs *res);
++ */
++SYM_FUNC_START(arm_smccc_1_2_smc)
++	SMCCC_1_2 smc
++SYM_FUNC_END(arm_smccc_1_2_smc)
++EXPORT_SYMBOL(arm_smccc_1_2_smc)
+diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
+index bfef3d8d14e7..90e6dd32f2cd 100644
+--- a/drivers/firmware/Kconfig
++++ b/drivers/firmware/Kconfig
+@@ -296,6 +296,7 @@ config TURRIS_MOX_RWTM
+ 	  other manufacturing data and also utilize the Entropy Bit Generator
+ 	  for hardware random number generation.
+ 
++source "drivers/firmware/arm_ffa/Kconfig"
+ source "drivers/firmware/broadcom/Kconfig"
+ source "drivers/firmware/google/Kconfig"
+ source "drivers/firmware/efi/Kconfig"
+diff --git a/drivers/firmware/Makefile b/drivers/firmware/Makefile
+index 523173cbff33..3c2af2e98def 100644
+--- a/drivers/firmware/Makefile
++++ b/drivers/firmware/Makefile
+@@ -23,6 +23,7 @@ obj-$(CONFIG_TI_SCI_PROTOCOL)	+= ti_sci.o
+ obj-$(CONFIG_TRUSTED_FOUNDATIONS) += trusted_foundations.o
+ obj-$(CONFIG_TURRIS_MOX_RWTM)	+= turris-mox-rwtm.o
+ 
++obj-y				+= arm_ffa/
+ obj-y				+= arm_scmi/
+ obj-y				+= broadcom/
+ obj-y				+= meson/
+diff --git a/drivers/firmware/arm_ffa/Kconfig b/drivers/firmware/arm_ffa/Kconfig
+new file mode 100644
+index 000000000000..5e3ae5cf82e8
+--- /dev/null
++++ b/drivers/firmware/arm_ffa/Kconfig
+@@ -0,0 +1,21 @@
++# SPDX-License-Identifier: GPL-2.0-only
++config ARM_FFA_TRANSPORT
++	tristate "Arm Firmware Framework for Armv8-A"
++	depends on OF
++	depends on ARM64
++	default n
++	help
++	  This Firmware Framework(FF) for Arm A-profile processors describes
++	  interfaces that standardize communication between the various
++	  software images which includes communication between images in
++	  the Secure world and Normal world. It also leverages the
++	  virtualization extension to isolate software images provided
++	  by an ecosystem of vendors from each other.
++
++	  This driver provides interface for all the client drivers making
++	  use of the features offered by ARM FF-A.
++
++config ARM_FFA_SMCCC
++	bool
++	default ARM_FFA_TRANSPORT
++	depends on ARM64 && HAVE_ARM_SMCCC_DISCOVERY
+diff --git a/drivers/firmware/arm_ffa/Makefile b/drivers/firmware/arm_ffa/Makefile
+new file mode 100644
+index 000000000000..9d9f37523200
+--- /dev/null
++++ b/drivers/firmware/arm_ffa/Makefile
+@@ -0,0 +1,6 @@
++# SPDX-License-Identifier: GPL-2.0-only
++ffa-bus-y = bus.o
++ffa-driver-y = driver.o
++ffa-transport-$(CONFIG_ARM_FFA_SMCCC) += smccc.o
++ffa-module-objs := $(ffa-bus-y) $(ffa-driver-y) $(ffa-transport-y)
++obj-$(CONFIG_ARM_FFA_TRANSPORT) = ffa-module.o
+diff --git a/drivers/firmware/arm_ffa/bus.c b/drivers/firmware/arm_ffa/bus.c
+new file mode 100644
+index 000000000000..fca1e311ea6c
+--- /dev/null
++++ b/drivers/firmware/arm_ffa/bus.c
+@@ -0,0 +1,220 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Copyright (C) 2021 ARM Ltd.
++ */
++
++#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
++
++#include <linux/arm_ffa.h>
++#include <linux/device.h>
++#include <linux/fs.h>
++#include <linux/kernel.h>
++#include <linux/module.h>
++#include <linux/slab.h>
++#include <linux/types.h>
++
++#include "common.h"
++
++static int ffa_device_match(struct device *dev, struct device_driver *drv)
++{
++	const struct ffa_device_id *id_table;
++	struct ffa_device *ffa_dev;
++
++	id_table = to_ffa_driver(drv)->id_table;
++	ffa_dev = to_ffa_dev(dev);
++
++	while (!uuid_is_null(&id_table->uuid)) {
++		/*
++		 * FF-A v1.0 doesn't provide discovery of UUIDs, just the
++		 * partition IDs, so fetch the partitions IDs for this
++		 * id_table UUID and assign the UUID to the device if the
++		 * partition ID matches
++		 */
++		if (uuid_is_null(&ffa_dev->uuid))
++			ffa_device_match_uuid(ffa_dev, &id_table->uuid);
++
++		if (uuid_equal(&ffa_dev->uuid, &id_table->uuid))
++			return 1;
++		id_table++;
++	}
++
++	return 0;
++}
++
++static int ffa_device_probe(struct device *dev)
++{
++	struct ffa_driver *ffa_drv = to_ffa_driver(dev->driver);
++	struct ffa_device *ffa_dev = to_ffa_dev(dev);
++
++	return ffa_drv->probe(ffa_dev);
++}
++
++static int ffa_device_remove(struct device *dev)
++{
++	struct ffa_driver *ffa_drv = to_ffa_driver(dev->driver);
++
++	ffa_drv->remove(to_ffa_dev(dev));
++
++	return 0;
++}
++
++static int ffa_device_uevent(struct device *dev, struct kobj_uevent_env *env)
++{
++	struct ffa_device *ffa_dev = to_ffa_dev(dev);
++
++	return add_uevent_var(env, "MODALIAS=arm_ffa:%04x:%pUb",
++			      ffa_dev->vm_id, &ffa_dev->uuid);
++}
++
++static ssize_t partition_id_show(struct device *dev,
++				 struct device_attribute *attr, char *buf)
++{
++	struct ffa_device *ffa_dev = to_ffa_dev(dev);
++
++	return sprintf(buf, "0x%04x\n", ffa_dev->vm_id);
++}
++static DEVICE_ATTR_RO(partition_id);
++
++static ssize_t uuid_show(struct device *dev, struct device_attribute *attr,
++			 char *buf)
++{
++	struct ffa_device *ffa_dev = to_ffa_dev(dev);
++
++	return sprintf(buf, "%pUb\n", &ffa_dev->uuid);
++}
++static DEVICE_ATTR_RO(uuid);
++
++static struct attribute *ffa_device_attributes_attrs[] = {
++	&dev_attr_partition_id.attr,
++	&dev_attr_uuid.attr,
++	NULL,
++};
++ATTRIBUTE_GROUPS(ffa_device_attributes);
++
++struct bus_type ffa_bus_type = {
++	.name		= "arm_ffa",
++	.match		= ffa_device_match,
++	.probe		= ffa_device_probe,
++	.remove		= ffa_device_remove,
++	.uevent		= ffa_device_uevent,
++	.dev_groups	= ffa_device_attributes_groups,
++};
++EXPORT_SYMBOL_GPL(ffa_bus_type);
++
++int ffa_driver_register(struct ffa_driver *driver, struct module *owner,
++			const char *mod_name)
++{
++	int ret;
++
++	if (!driver->probe)
++		return -EINVAL;
++
++	driver->driver.bus = &ffa_bus_type;
++	driver->driver.name = driver->name;
++	driver->driver.owner = owner;
++	driver->driver.mod_name = mod_name;
++
++	ret = driver_register(&driver->driver);
++	if (!ret)
++		pr_debug("registered new ffa driver %s\n", driver->name);
++
++	return ret;
++}
++EXPORT_SYMBOL_GPL(ffa_driver_register);
++
++void ffa_driver_unregister(struct ffa_driver *driver)
++{
++	driver_unregister(&driver->driver);
++}
++EXPORT_SYMBOL_GPL(ffa_driver_unregister);
++
++static void ffa_release_device(struct device *dev)
++{
++	struct ffa_device *ffa_dev = to_ffa_dev(dev);
++
++	kfree(ffa_dev);
++}
++
++static int __ffa_devices_unregister(struct device *dev, void *data)
++{
++	device_unregister(dev);
++
++	return 0;
++}
++
++static void ffa_devices_unregister(void)
++{
++	bus_for_each_dev(&ffa_bus_type, NULL, NULL,
++			 __ffa_devices_unregister);
++}
++
++bool ffa_device_is_valid(struct ffa_device *ffa_dev)
++{
++	bool valid = false;
++	struct device *dev = NULL;
++	struct ffa_device *tmp_dev;
++
++	do {
++		dev = bus_find_next_device(&ffa_bus_type, dev);
++		tmp_dev = to_ffa_dev(dev);
++		if (tmp_dev == ffa_dev) {
++			valid = true;
++			break;
++		}
++		put_device(dev);
++	} while (dev);
++
++	put_device(dev);
++
++	return valid;
++}
++
++struct ffa_device *ffa_device_register(const uuid_t *uuid, int vm_id)
++{
++	int ret;
++	struct device *dev;
++	struct ffa_device *ffa_dev;
++
++	ffa_dev = kzalloc(sizeof(*ffa_dev), GFP_KERNEL);
++	if (!ffa_dev)
++		return NULL;
++
++	dev = &ffa_dev->dev;
++	dev->bus = &ffa_bus_type;
++	dev->release = ffa_release_device;
++	dev_set_name(&ffa_dev->dev, "arm-ffa-%04x", vm_id);
++
++	ffa_dev->vm_id = vm_id;
++	uuid_copy(&ffa_dev->uuid, uuid);
++
++	ret = device_register(&ffa_dev->dev);
++	if (ret) {
++		dev_err(dev, "unable to register device %s err=%d\n",
++			dev_name(dev), ret);
++		put_device(dev);
++		return NULL;
++	}
++
++	return ffa_dev;
++}
++EXPORT_SYMBOL_GPL(ffa_device_register);
++
++void ffa_device_unregister(struct ffa_device *ffa_dev)
++{
++	if (!ffa_dev)
++		return;
++
++	device_unregister(&ffa_dev->dev);
++}
++EXPORT_SYMBOL_GPL(ffa_device_unregister);
++
++int arm_ffa_bus_init(void)
++{
++	return bus_register(&ffa_bus_type);
++}
++
++void arm_ffa_bus_exit(void)
++{
++	ffa_devices_unregister();
++	bus_unregister(&ffa_bus_type);
++}
+diff --git a/drivers/firmware/arm_ffa/common.h b/drivers/firmware/arm_ffa/common.h
+new file mode 100644
+index 000000000000..d6eccf1fd3f6
+--- /dev/null
++++ b/drivers/firmware/arm_ffa/common.h
+@@ -0,0 +1,31 @@
++/* SPDX-License-Identifier: GPL-2.0 */
++/*
++ * Copyright (C) 2021 ARM Ltd.
++ */
++
++#ifndef _FFA_COMMON_H
++#define _FFA_COMMON_H
++
++#include <linux/arm_ffa.h>
++#include <linux/arm-smccc.h>
++#include <linux/err.h>
++
++typedef struct arm_smccc_1_2_regs ffa_value_t;
++
++typedef void (ffa_fn)(ffa_value_t, ffa_value_t *);
++
++int arm_ffa_bus_init(void);
++void arm_ffa_bus_exit(void);
++bool ffa_device_is_valid(struct ffa_device *ffa_dev);
++void ffa_device_match_uuid(struct ffa_device *ffa_dev, const uuid_t *uuid);
++
++#ifdef CONFIG_ARM_FFA_SMCCC
++int __init ffa_transport_init(ffa_fn **invoke_ffa_fn);
++#else
++static inline int __init ffa_transport_init(ffa_fn **invoke_ffa_fn)
++{
++	return -EOPNOTSUPP;
++}
++#endif
++
++#endif /* _FFA_COMMON_H */
+diff --git a/drivers/firmware/arm_ffa/driver.c b/drivers/firmware/arm_ffa/driver.c
+new file mode 100644
+index 000000000000..14f900047ac0
+--- /dev/null
++++ b/drivers/firmware/arm_ffa/driver.c
+@@ -0,0 +1,776 @@
++// SPDX-License-Identifier: GPL-2.0-only
++/*
++ * Arm Firmware Framework for ARMv8-A(FFA) interface driver
++ *
++ * The Arm FFA specification[1] describes a software architecture to
++ * leverages the virtualization extension to isolate software images
++ * provided by an ecosystem of vendors from each other and describes
++ * interfaces that standardize communication between the various software
++ * images including communication between images in the Secure world and
++ * Normal world. Any Hypervisor could use the FFA interfaces to enable
++ * communication between VMs it manages.
++ *
++ * The Hypervisor a.k.a Partition managers in FFA terminology can assign
++ * system resources(Memory regions, Devices, CPU cycles) to the partitions
++ * and manage isolation amongst them.
++ *
++ * [1] https://developer.arm.com/docs/den0077/latest
++ *
++ * Copyright (C) 2021 ARM Ltd.
++ */
++
++#define DRIVER_NAME "ARM FF-A"
++#define pr_fmt(fmt) DRIVER_NAME ": " fmt
++
++#include <linux/arm_ffa.h>
++#include <linux/bitfield.h>
++#include <linux/device.h>
++#include <linux/io.h>
++#include <linux/kernel.h>
++#include <linux/module.h>
++#include <linux/mm.h>
++#include <linux/scatterlist.h>
++#include <linux/slab.h>
++#include <linux/uuid.h>
++
++#include "common.h"
++
++#define FFA_DRIVER_VERSION	FFA_VERSION_1_0
++
++#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_SMC_64(func_num)	FFA_SMC(ARM_SMCCC_SMC_64, (func_num))
++
++#define FFA_ERROR			FFA_SMC_32(0x60)
++#define FFA_SUCCESS			FFA_SMC_32(0x61)
++#define FFA_INTERRUPT			FFA_SMC_32(0x62)
++#define FFA_VERSION			FFA_SMC_32(0x63)
++#define FFA_FEATURES			FFA_SMC_32(0x64)
++#define FFA_RX_RELEASE			FFA_SMC_32(0x65)
++#define FFA_RXTX_MAP			FFA_SMC_32(0x66)
++#define FFA_FN64_RXTX_MAP		FFA_SMC_64(0x66)
++#define FFA_RXTX_UNMAP			FFA_SMC_32(0x67)
++#define FFA_PARTITION_INFO_GET		FFA_SMC_32(0x68)
++#define FFA_ID_GET			FFA_SMC_32(0x69)
++#define FFA_MSG_POLL			FFA_SMC_32(0x6A)
++#define FFA_MSG_WAIT			FFA_SMC_32(0x6B)
++#define FFA_YIELD			FFA_SMC_32(0x6C)
++#define FFA_RUN				FFA_SMC_32(0x6D)
++#define FFA_MSG_SEND			FFA_SMC_32(0x6E)
++#define FFA_MSG_SEND_DIRECT_REQ		FFA_SMC_32(0x6F)
++#define FFA_FN64_MSG_SEND_DIRECT_REQ	FFA_SMC_64(0x6F)
++#define FFA_MSG_SEND_DIRECT_RESP	FFA_SMC_32(0x70)
++#define FFA_FN64_MSG_SEND_DIRECT_RESP	FFA_SMC_64(0x70)
++#define FFA_MEM_DONATE			FFA_SMC_32(0x71)
++#define FFA_FN64_MEM_DONATE		FFA_SMC_64(0x71)
++#define FFA_MEM_LEND			FFA_SMC_32(0x72)
++#define FFA_FN64_MEM_LEND		FFA_SMC_64(0x72)
++#define FFA_MEM_SHARE			FFA_SMC_32(0x73)
++#define FFA_FN64_MEM_SHARE		FFA_SMC_64(0x73)
++#define FFA_MEM_RETRIEVE_REQ		FFA_SMC_32(0x74)
++#define FFA_FN64_MEM_RETRIEVE_REQ	FFA_SMC_64(0x74)
++#define FFA_MEM_RETRIEVE_RESP		FFA_SMC_32(0x75)
++#define FFA_MEM_RELINQUISH		FFA_SMC_32(0x76)
++#define FFA_MEM_RECLAIM			FFA_SMC_32(0x77)
++#define FFA_MEM_OP_PAUSE		FFA_SMC_32(0x78)
++#define FFA_MEM_OP_RESUME		FFA_SMC_32(0x79)
++#define FFA_MEM_FRAG_RX			FFA_SMC_32(0x7A)
++#define FFA_MEM_FRAG_TX			FFA_SMC_32(0x7B)
++#define FFA_NORMAL_WORLD_RESUME		FFA_SMC_32(0x7C)
++
++/*
++ * For some calls it is necessary to use SMC64 to pass or return 64-bit values.
++ * For such calls FFA_FN_NATIVE(name) will choose the appropriate
++ * (native-width) function ID.
++ */
++#ifdef CONFIG_64BIT
++#define FFA_FN_NATIVE(name)	FFA_FN64_##name
++#else
++#define FFA_FN_NATIVE(name)	FFA_##name
++#endif
++
++/* FFA error codes. */
++#define FFA_RET_SUCCESS            (0)
++#define FFA_RET_NOT_SUPPORTED      (-1)
++#define FFA_RET_INVALID_PARAMETERS (-2)
++#define FFA_RET_NO_MEMORY          (-3)
++#define FFA_RET_BUSY               (-4)
++#define FFA_RET_INTERRUPTED        (-5)
++#define FFA_RET_DENIED             (-6)
++#define FFA_RET_RETRY              (-7)
++#define FFA_RET_ABORTED            (-8)
++
++#define MAJOR_VERSION_MASK	GENMASK(30, 16)
++#define MINOR_VERSION_MASK	GENMASK(15, 0)
++#define MAJOR_VERSION(x)	((u16)(FIELD_GET(MAJOR_VERSION_MASK, (x))))
++#define 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_VERSION_1_0		PACK_VERSION_INFO(1, 0)
++#define FFA_MIN_VERSION		FFA_VERSION_1_0
++
++#define SENDER_ID_MASK		GENMASK(31, 16)
++#define RECEIVER_ID_MASK	GENMASK(15, 0)
++#define SENDER_ID(x)		((u16)(FIELD_GET(SENDER_ID_MASK, (x))))
++#define RECEIVER_ID(x)		((u16)(FIELD_GET(RECEIVER_ID_MASK, (x))))
++#define PACK_TARGET_INFO(s, r)		\
++	(FIELD_PREP(SENDER_ID_MASK, (s)) | FIELD_PREP(RECEIVER_ID_MASK, (r)))
++
++/*
++ * FF-A specification mentions explicitly about '4K pages'. This should
++ * not be confused with the kernel PAGE_SIZE, which is the translation
++ * granule kernel is configured and may be one among 4K, 16K and 64K.
++ */
++#define FFA_PAGE_SIZE		SZ_4K
++/*
++ * Keeping RX TX buffer size as 4K for now
++ * 64K may be preferred to keep it min a page in 64K PAGE_SIZE config
++ */
++#define RXTX_BUFFER_SIZE	SZ_4K
++
++static ffa_fn *invoke_ffa_fn;
++
++static const int ffa_linux_errmap[] = {
++	/* better than switch case as long as return value is continuous */
++	0,		/* FFA_RET_SUCCESS */
++	-EOPNOTSUPP,	/* FFA_RET_NOT_SUPPORTED */
++	-EINVAL,	/* FFA_RET_INVALID_PARAMETERS */
++	-ENOMEM,	/* FFA_RET_NO_MEMORY */
++	-EBUSY,		/* FFA_RET_BUSY */
++	-EINTR,		/* FFA_RET_INTERRUPTED */
++	-EACCES,	/* FFA_RET_DENIED */
++	-EAGAIN,	/* FFA_RET_RETRY */
++	-ECANCELED,	/* FFA_RET_ABORTED */
++};
++
++static inline int ffa_to_linux_errno(int errno)
++{
++	int err_idx = -errno;
++
++	if (err_idx >= 0 && err_idx < ARRAY_SIZE(ffa_linux_errmap))
++		return ffa_linux_errmap[err_idx];
++	return -EINVAL;
++}
++
++struct ffa_drv_info {
++	u32 version;
++	u16 vm_id;
++	struct mutex rx_lock; /* lock to protect Rx buffer */
++	struct mutex tx_lock; /* lock to protect Tx buffer */
++	void *rx_buffer;
++	void *tx_buffer;
++};
++
++static struct ffa_drv_info *drv_info;
++
++/*
++ * The driver must be able to support all the versions from the earliest
++ * supported FFA_MIN_VERSION to the latest supported FFA_DRIVER_VERSION.
++ * The specification states that if firmware supports a FFA implementation
++ * that is incompatible with and at a greater version number than specified
++ * by the caller(FFA_DRIVER_VERSION passed as parameter to FFA_VERSION),
++ * it must return the NOT_SUPPORTED error code.
++ */
++static u32 ffa_compatible_version_find(u32 version)
++{
++	u16 major = MAJOR_VERSION(version), minor = MINOR_VERSION(version);
++	u16 drv_major = MAJOR_VERSION(FFA_DRIVER_VERSION);
++	u16 drv_minor = MINOR_VERSION(FFA_DRIVER_VERSION);
++
++	if ((major < drv_major) || (major == drv_major && minor <= drv_minor))
++		return version;
++
++	pr_info("Firmware version higher than driver version, downgrading\n");
++	return FFA_DRIVER_VERSION;
++}
++
++static int ffa_version_check(u32 *version)
++{
++	ffa_value_t ver;
++
++	invoke_ffa_fn((ffa_value_t){
++		      .a0 = FFA_VERSION, .a1 = FFA_DRIVER_VERSION,
++		      }, &ver);
++
++	if (ver.a0 == FFA_RET_NOT_SUPPORTED) {
++		pr_info("FFA_VERSION returned not supported\n");
++		return -EOPNOTSUPP;
++	}
++
++	if (ver.a0 < FFA_MIN_VERSION) {
++		pr_err("Incompatible v%d.%d! Earliest supported v%d.%d\n",
++		       MAJOR_VERSION(ver.a0), MINOR_VERSION(ver.a0),
++		       MAJOR_VERSION(FFA_MIN_VERSION),
++		       MINOR_VERSION(FFA_MIN_VERSION));
++		return -EINVAL;
++	}
++
++	pr_info("Driver version %d.%d\n", MAJOR_VERSION(FFA_DRIVER_VERSION),
++		MINOR_VERSION(FFA_DRIVER_VERSION));
++	pr_info("Firmware version %d.%d found\n", MAJOR_VERSION(ver.a0),
++		MINOR_VERSION(ver.a0));
++	*version = ffa_compatible_version_find(ver.a0);
++
++	return 0;
++}
++
++static int ffa_rx_release(void)
++{
++	ffa_value_t ret;
++
++	invoke_ffa_fn((ffa_value_t){
++		      .a0 = FFA_RX_RELEASE,
++		      }, &ret);
++
++	if (ret.a0 == FFA_ERROR)
++		return ffa_to_linux_errno((int)ret.a2);
++
++	/* check for ret.a0 == FFA_RX_RELEASE ? */
++
++	return 0;
++}
++
++static int ffa_rxtx_map(phys_addr_t tx_buf, phys_addr_t rx_buf, u32 pg_cnt)
++{
++	ffa_value_t ret;
++
++	invoke_ffa_fn((ffa_value_t){
++		      .a0 = FFA_FN_NATIVE(RXTX_MAP),
++		      .a1 = tx_buf, .a2 = rx_buf, .a3 = pg_cnt,
++		      }, &ret);
++
++	if (ret.a0 == FFA_ERROR)
++		return ffa_to_linux_errno((int)ret.a2);
++
++	return 0;
++}
++
++static int ffa_rxtx_unmap(u16 vm_id)
++{
++	ffa_value_t ret;
++
++	invoke_ffa_fn((ffa_value_t){
++		      .a0 = FFA_RXTX_UNMAP, .a1 = PACK_TARGET_INFO(vm_id, 0),
++		      }, &ret);
++
++	if (ret.a0 == FFA_ERROR)
++		return ffa_to_linux_errno((int)ret.a2);
++
++	return 0;
++}
++
++/* buffer must be sizeof(struct ffa_partition_info) * num_partitions */
++static int
++__ffa_partition_info_get(u32 uuid0, u32 uuid1, u32 uuid2, u32 uuid3,
++			 struct ffa_partition_info *buffer, int num_partitions)
++{
++	int count;
++	ffa_value_t partition_info;
++
++	mutex_lock(&drv_info->rx_lock);
++	invoke_ffa_fn((ffa_value_t){
++		      .a0 = FFA_PARTITION_INFO_GET,
++		      .a1 = uuid0, .a2 = uuid1, .a3 = uuid2, .a4 = uuid3,
++		      }, &partition_info);
++
++	if (partition_info.a0 == FFA_ERROR) {
++		mutex_unlock(&drv_info->rx_lock);
++		return ffa_to_linux_errno((int)partition_info.a2);
++	}
++
++	count = partition_info.a2;
++
++	if (buffer && count <= num_partitions)
++		memcpy(buffer, drv_info->rx_buffer, sizeof(*buffer) * count);
++
++	ffa_rx_release();
++
++	mutex_unlock(&drv_info->rx_lock);
++
++	return count;
++}
++
++/* buffer is allocated and caller must free the same if returned count > 0 */
++static int
++ffa_partition_probe(const uuid_t *uuid, struct ffa_partition_info **buffer)
++{
++	int count;
++	u32 uuid0_4[4];
++	struct ffa_partition_info *pbuf;
++
++	export_uuid((u8 *)uuid0_4, uuid);
++	count = __ffa_partition_info_get(uuid0_4[0], uuid0_4[1], uuid0_4[2],
++					 uuid0_4[3], NULL, 0);
++	if (count <= 0)
++		return count;
++
++	pbuf = kcalloc(count, sizeof(*pbuf), GFP_KERNEL);
++	if (!pbuf)
++		return -ENOMEM;
++
++	count = __ffa_partition_info_get(uuid0_4[0], uuid0_4[1], uuid0_4[2],
++					 uuid0_4[3], pbuf, count);
++	if (count <= 0)
++		kfree(pbuf);
++	else
++		*buffer = pbuf;
++
++	return count;
++}
++
++#define VM_ID_MASK	GENMASK(15, 0)
++static int ffa_id_get(u16 *vm_id)
++{
++	ffa_value_t id;
++
++	invoke_ffa_fn((ffa_value_t){
++		      .a0 = FFA_ID_GET,
++		      }, &id);
++
++	if (id.a0 == FFA_ERROR)
++		return ffa_to_linux_errno((int)id.a2);
++
++	*vm_id = FIELD_GET(VM_ID_MASK, (id.a2));
++
++	return 0;
++}
++
++static int ffa_msg_send_direct_req(u16 src_id, u16 dst_id, bool mode_32bit,
++				   struct ffa_send_direct_data *data)
++{
++	u32 req_id, resp_id, src_dst_ids = PACK_TARGET_INFO(src_id, dst_id);
++	ffa_value_t ret;
++
++	if (mode_32bit) {
++		req_id = FFA_MSG_SEND_DIRECT_REQ;
++		resp_id = FFA_MSG_SEND_DIRECT_RESP;
++	} else {
++		req_id = FFA_FN_NATIVE(MSG_SEND_DIRECT_REQ);
++		resp_id = FFA_FN_NATIVE(MSG_SEND_DIRECT_RESP);
++	}
++
++	invoke_ffa_fn((ffa_value_t){
++		      .a0 = req_id, .a1 = src_dst_ids, .a2 = 0,
++		      .a3 = data->data0, .a4 = data->data1, .a5 = data->data2,
++		      .a6 = data->data3, .a7 = data->data4,
++		      }, &ret);
++
++	while (ret.a0 == FFA_INTERRUPT)
++		invoke_ffa_fn((ffa_value_t){
++			      .a0 = FFA_RUN, .a1 = ret.a1,
++			      }, &ret);
++
++	if (ret.a0 == FFA_ERROR)
++		return ffa_to_linux_errno((int)ret.a2);
++
++	if (ret.a0 == resp_id) {
++		data->data0 = ret.a3;
++		data->data1 = ret.a4;
++		data->data2 = ret.a5;
++		data->data3 = ret.a6;
++		data->data4 = ret.a7;
++		return 0;
++	}
++
++	return -EINVAL;
++}
++
++static int ffa_mem_first_frag(u32 func_id, phys_addr_t buf, u32 buf_sz,
++			      u32 frag_len, u32 len, u64 *handle)
++{
++	ffa_value_t ret;
++
++	invoke_ffa_fn((ffa_value_t){
++		      .a0 = func_id, .a1 = len, .a2 = frag_len,
++		      .a3 = buf, .a4 = buf_sz,
++		      }, &ret);
++
++	while (ret.a0 == FFA_MEM_OP_PAUSE)
++		invoke_ffa_fn((ffa_value_t){
++			      .a0 = FFA_MEM_OP_RESUME,
++			      .a1 = ret.a1, .a2 = ret.a2,
++			      }, &ret);
++
++	if (ret.a0 == FFA_ERROR)
++		return ffa_to_linux_errno((int)ret.a2);
++
++	if (ret.a0 != FFA_SUCCESS)
++		return -EOPNOTSUPP;
++
++	if (handle)
++		*handle = PACK_HANDLE(ret.a2, ret.a3);
++
++	return frag_len;
++}
++
++static int ffa_mem_next_frag(u64 handle, u32 frag_len)
++{
++	ffa_value_t ret;
++
++	invoke_ffa_fn((ffa_value_t){
++		      .a0 = FFA_MEM_FRAG_TX,
++		      .a1 = HANDLE_LOW(handle), .a2 = HANDLE_HIGH(handle),
++		      .a3 = frag_len,
++		      }, &ret);
++
++	while (ret.a0 == FFA_MEM_OP_PAUSE)
++		invoke_ffa_fn((ffa_value_t){
++			      .a0 = FFA_MEM_OP_RESUME,
++			      .a1 = ret.a1, .a2 = ret.a2,
++			      }, &ret);
++
++	if (ret.a0 == FFA_ERROR)
++		return ffa_to_linux_errno((int)ret.a2);
++
++	if (ret.a0 != FFA_MEM_FRAG_RX)
++		return -EOPNOTSUPP;
++
++	return ret.a3;
++}
++
++static int
++ffa_transmit_fragment(u32 func_id, phys_addr_t buf, u32 buf_sz, u32 frag_len,
++		      u32 len, u64 *handle, bool first)
++{
++	if (!first)
++		return ffa_mem_next_frag(*handle, frag_len);
++
++	return ffa_mem_first_frag(func_id, buf, buf_sz, frag_len, len, handle);
++}
++
++static u32 ffa_get_num_pages_sg(struct scatterlist *sg)
++{
++	u32 num_pages = 0;
++
++	do {
++		num_pages += sg->length / FFA_PAGE_SIZE;
++	} while ((sg = sg_next(sg)));
++
++	return num_pages;
++}
++
++static int
++ffa_setup_and_transmit(u32 func_id, void *buffer, u32 max_fragsize,
++		       struct ffa_mem_ops_args *args)
++{
++	int rc = 0;
++	bool first = true;
++	phys_addr_t addr = 0;
++	struct ffa_composite_mem_region *composite;
++	struct ffa_mem_region_addr_range *constituents;
++	struct ffa_mem_region_attributes *ep_mem_access;
++	struct ffa_mem_region *mem_region = buffer;
++	u32 idx, frag_len, length, buf_sz = 0, num_entries = sg_nents(args->sg);
++
++	mem_region->tag = args->tag;
++	mem_region->flags = args->flags;
++	mem_region->sender_id = drv_info->vm_id;
++	mem_region->attributes = FFA_MEM_NORMAL | FFA_MEM_WRITE_BACK |
++				 FFA_MEM_INNER_SHAREABLE;
++	ep_mem_access = &mem_region->ep_mem_access[0];
++
++	for (idx = 0; idx < args->nattrs; idx++, ep_mem_access++) {
++		ep_mem_access->receiver = args->attrs[idx].receiver;
++		ep_mem_access->attrs = args->attrs[idx].attrs;
++		ep_mem_access->composite_off = COMPOSITE_OFFSET(args->nattrs);
++	}
++	mem_region->ep_count = args->nattrs;
++
++	composite = buffer + COMPOSITE_OFFSET(args->nattrs);
++	composite->total_pg_cnt = ffa_get_num_pages_sg(args->sg);
++	composite->addr_range_cnt = num_entries;
++
++	length = COMPOSITE_CONSTITUENTS_OFFSET(args->nattrs, num_entries);
++	frag_len = COMPOSITE_CONSTITUENTS_OFFSET(args->nattrs, 0);
++	if (frag_len > max_fragsize)
++		return -ENXIO;
++
++	if (!args->use_txbuf) {
++		addr = virt_to_phys(buffer);
++		buf_sz = max_fragsize / FFA_PAGE_SIZE;
++	}
++
++	constituents = buffer + frag_len;
++	idx = 0;
++	do {
++		if (frag_len == max_fragsize) {
++			rc = ffa_transmit_fragment(func_id, addr, buf_sz,
++						   frag_len, length,
++						   &args->g_handle, first);
++			if (rc < 0)
++				return -ENXIO;
++
++			first = false;
++			idx = 0;
++			frag_len = 0;
++			constituents = buffer;
++		}
++
++		if ((void *)constituents - buffer > max_fragsize) {
++			pr_err("Memory Region Fragment > Tx Buffer size\n");
++			return -EFAULT;
++		}
++
++		constituents->address = sg_phys(args->sg);
++		constituents->pg_cnt = args->sg->length / FFA_PAGE_SIZE;
++		constituents++;
++		frag_len += sizeof(struct ffa_mem_region_addr_range);
++	} while ((args->sg = sg_next(args->sg)));
++
++	return ffa_transmit_fragment(func_id, addr, buf_sz, frag_len,
++				     length, &args->g_handle, first);
++}
++
++static int ffa_memory_ops(u32 func_id, struct ffa_mem_ops_args *args)
++{
++	int ret;
++	void *buffer;
++
++	if (!args->use_txbuf) {
++		buffer = alloc_pages_exact(RXTX_BUFFER_SIZE, GFP_KERNEL);
++		if (!buffer)
++			return -ENOMEM;
++	} else {
++		buffer = drv_info->tx_buffer;
++		mutex_lock(&drv_info->tx_lock);
++	}
++
++	ret = ffa_setup_and_transmit(func_id, buffer, RXTX_BUFFER_SIZE, args);
++
++	if (args->use_txbuf)
++		mutex_unlock(&drv_info->tx_lock);
++	else
++		free_pages_exact(buffer, RXTX_BUFFER_SIZE);
++
++	return ret < 0 ? ret : 0;
++}
++
++static int ffa_memory_reclaim(u64 g_handle, u32 flags)
++{
++	ffa_value_t ret;
++
++	invoke_ffa_fn((ffa_value_t){
++		      .a0 = FFA_MEM_RECLAIM,
++		      .a1 = HANDLE_LOW(g_handle), .a2 = HANDLE_HIGH(g_handle),
++		      .a3 = flags,
++		      }, &ret);
++
++	if (ret.a0 == FFA_ERROR)
++		return ffa_to_linux_errno((int)ret.a2);
++
++	return 0;
++}
++
++static u32 ffa_api_version_get(void)
++{
++	return drv_info->version;
++}
++
++static int ffa_partition_info_get(const char *uuid_str,
++				  struct ffa_partition_info *buffer)
++{
++	int count;
++	uuid_t uuid;
++	struct ffa_partition_info *pbuf;
++
++	if (uuid_parse(uuid_str, &uuid)) {
++		pr_err("invalid uuid (%s)\n", uuid_str);
++		return -ENODEV;
++	}
++
++	count = ffa_partition_probe(&uuid_null, &pbuf);
++	if (count <= 0)
++		return -ENOENT;
++
++	memcpy(buffer, pbuf, sizeof(*pbuf) * count);
++	kfree(pbuf);
++	return 0;
++}
++
++static void ffa_mode_32bit_set(struct ffa_device *dev)
++{
++	dev->mode_32bit = true;
++}
++
++static int ffa_sync_send_receive(struct ffa_device *dev,
++				 struct ffa_send_direct_data *data)
++{
++	return ffa_msg_send_direct_req(drv_info->vm_id, dev->vm_id,
++				       dev->mode_32bit, data);
++}
++
++static int
++ffa_memory_share(struct ffa_device *dev, struct ffa_mem_ops_args *args)
++{
++	if (dev->mode_32bit)
++		return ffa_memory_ops(FFA_MEM_SHARE, args);
++
++	return ffa_memory_ops(FFA_FN_NATIVE(MEM_SHARE), args);
++}
++
++static int
++ffa_memory_lend(struct ffa_device *dev, struct ffa_mem_ops_args *args)
++{
++	/* Note that upon a successful MEM_LEND request the caller
++	 * must ensure that the memory region specified is not accessed
++	 * until a successful MEM_RECALIM call has been made.
++	 * On systems with a hypervisor present this will been enforced,
++	 * however on systems without a hypervisor the responsibility
++	 * falls to the calling kernel driver to prevent access.
++	 */
++	if (dev->mode_32bit)
++		return ffa_memory_ops(FFA_MEM_LEND, args);
++
++	return ffa_memory_ops(FFA_FN_NATIVE(MEM_LEND), args);
++}
++
++static const struct ffa_dev_ops ffa_ops = {
++	.api_version_get = ffa_api_version_get,
++	.partition_info_get = ffa_partition_info_get,
++	.mode_32bit_set = ffa_mode_32bit_set,
++	.sync_send_receive = ffa_sync_send_receive,
++	.memory_reclaim = ffa_memory_reclaim,
++	.memory_share = ffa_memory_share,
++	.memory_lend = ffa_memory_lend,
++};
++
++const struct ffa_dev_ops *ffa_dev_ops_get(struct ffa_device *dev)
++{
++	if (ffa_device_is_valid(dev))
++		return &ffa_ops;
++
++	return NULL;
++}
++EXPORT_SYMBOL_GPL(ffa_dev_ops_get);
++
++void ffa_device_match_uuid(struct ffa_device *ffa_dev, const uuid_t *uuid)
++{
++	int count, idx;
++	struct ffa_partition_info *pbuf, *tpbuf;
++
++	count = ffa_partition_probe(uuid, &pbuf);
++	if (count <= 0)
++		return;
++
++	for (idx = 0, tpbuf = pbuf; idx < count; idx++, tpbuf++)
++		if (tpbuf->id == ffa_dev->vm_id)
++			uuid_copy(&ffa_dev->uuid, uuid);
++	kfree(pbuf);
++}
++
++static void ffa_setup_partitions(void)
++{
++	int count, idx;
++	struct ffa_device *ffa_dev;
++	struct ffa_partition_info *pbuf, *tpbuf;
++
++	count = ffa_partition_probe(&uuid_null, &pbuf);
++	if (count <= 0) {
++		pr_info("%s: No partitions found, error %d\n", __func__, count);
++		return;
++	}
++
++	for (idx = 0, tpbuf = pbuf; idx < count; idx++, tpbuf++) {
++		/* Note that the &uuid_null parameter will require
++		 * ffa_device_match() to find the UUID of this partition id
++		 * with help of ffa_device_match_uuid(). Once the FF-A spec
++		 * is updated to provide correct UUID here for each partition
++		 * as part of the discovery API, we need to pass the
++		 * discovered UUID here instead.
++		 */
++		ffa_dev = ffa_device_register(&uuid_null, tpbuf->id);
++		if (!ffa_dev) {
++			pr_err("%s: failed to register partition ID 0x%x\n",
++			       __func__, tpbuf->id);
++			continue;
++		}
++
++		ffa_dev_set_drvdata(ffa_dev, drv_info);
++	}
++	kfree(pbuf);
++}
++
++static int __init ffa_init(void)
++{
++	int ret;
++
++	ret = ffa_transport_init(&invoke_ffa_fn);
++	if (ret)
++		return ret;
++
++	ret = arm_ffa_bus_init();
++	if (ret)
++		return ret;
++
++	drv_info = kzalloc(sizeof(*drv_info), GFP_KERNEL);
++	if (!drv_info) {
++		ret = -ENOMEM;
++		goto ffa_bus_exit;
++	}
++
++	ret = ffa_version_check(&drv_info->version);
++	if (ret)
++		goto free_drv_info;
++
++	if (ffa_id_get(&drv_info->vm_id)) {
++		pr_err("failed to obtain VM id for self\n");
++		ret = -ENODEV;
++		goto free_drv_info;
++	}
++
++	drv_info->rx_buffer = alloc_pages_exact(RXTX_BUFFER_SIZE, GFP_KERNEL);
++	if (!drv_info->rx_buffer) {
++		ret = -ENOMEM;
++		goto free_pages;
++	}
++
++	drv_info->tx_buffer = alloc_pages_exact(RXTX_BUFFER_SIZE, GFP_KERNEL);
++	if (!drv_info->tx_buffer) {
++		ret = -ENOMEM;
++		goto free_pages;
++	}
++
++	ret = ffa_rxtx_map(virt_to_phys(drv_info->tx_buffer),
++			   virt_to_phys(drv_info->rx_buffer),
++			   RXTX_BUFFER_SIZE / FFA_PAGE_SIZE);
++	if (ret) {
++		pr_err("failed to register FFA RxTx buffers\n");
++		goto free_pages;
++	}
++
++	mutex_init(&drv_info->rx_lock);
++	mutex_init(&drv_info->tx_lock);
++
++	ffa_setup_partitions();
++
++	return 0;
++free_pages:
++	if (drv_info->tx_buffer)
++		free_pages_exact(drv_info->tx_buffer, RXTX_BUFFER_SIZE);
++	free_pages_exact(drv_info->rx_buffer, RXTX_BUFFER_SIZE);
++free_drv_info:
++	kfree(drv_info);
++ffa_bus_exit:
++	arm_ffa_bus_exit();
++	return ret;
++}
++subsys_initcall(ffa_init);
++
++static void __exit ffa_exit(void)
++{
++	ffa_rxtx_unmap(drv_info->vm_id);
++	free_pages_exact(drv_info->tx_buffer, RXTX_BUFFER_SIZE);
++	free_pages_exact(drv_info->rx_buffer, RXTX_BUFFER_SIZE);
++	kfree(drv_info);
++	arm_ffa_bus_exit();
++}
++module_exit(ffa_exit);
++
++MODULE_ALIAS("arm-ffa");
++MODULE_AUTHOR("Sudeep Holla <sudeep.holla@arm.com>");
++MODULE_DESCRIPTION("Arm FF-A interface driver");
++MODULE_LICENSE("GPL v2");
+diff --git a/drivers/firmware/arm_ffa/smccc.c b/drivers/firmware/arm_ffa/smccc.c
+new file mode 100644
+index 000000000000..4d85bfff0a4e
+--- /dev/null
++++ b/drivers/firmware/arm_ffa/smccc.c
+@@ -0,0 +1,39 @@
++// SPDX-License-Identifier: GPL-2.0-only
++/*
++ * Copyright (C) 2021 ARM Ltd.
++ */
++
++#include <linux/printk.h>
++
++#include "common.h"
++
++static void __arm_ffa_fn_smc(ffa_value_t args, ffa_value_t *res)
++{
++	arm_smccc_1_2_smc(&args, res);
++}
++
++static void __arm_ffa_fn_hvc(ffa_value_t args, ffa_value_t *res)
++{
++	arm_smccc_1_2_hvc(&args, res);
++}
++
++int __init ffa_transport_init(ffa_fn **invoke_ffa_fn)
++{
++	enum arm_smccc_conduit conduit;
++
++	if (arm_smccc_get_version() < ARM_SMCCC_VERSION_1_2)
++		return -EOPNOTSUPP;
++
++	conduit = arm_smccc_1_1_get_conduit();
++	if (conduit == SMCCC_CONDUIT_NONE) {
++		pr_err("%s: invalid SMCCC conduit\n", __func__);
++		return -EOPNOTSUPP;
++	}
++
++	if (conduit == SMCCC_CONDUIT_SMC)
++		*invoke_ffa_fn = __arm_ffa_fn_smc;
++	else
++		*invoke_ffa_fn = __arm_ffa_fn_hvc;
++
++	return 0;
++}
+diff --git a/include/linux/arm-smccc.h b/include/linux/arm-smccc.h
+index 62c54234576c..c8eb24af3c62 100644
+--- a/include/linux/arm-smccc.h
++++ b/include/linux/arm-smccc.h
+@@ -186,6 +186,61 @@ struct arm_smccc_res {
+ 	unsigned long a3;
+ };
+ 
++#ifdef CONFIG_ARM64
++/**
++ * struct arm_smccc_1_2_regs - Arguments for or Results from SMC/HVC call
++ * @a0-a17 argument values from registers 0 to 17
++ */
++struct arm_smccc_1_2_regs {
++	unsigned long a0;
++	unsigned long a1;
++	unsigned long a2;
++	unsigned long a3;
++	unsigned long a4;
++	unsigned long a5;
++	unsigned long a6;
++	unsigned long a7;
++	unsigned long a8;
++	unsigned long a9;
++	unsigned long a10;
++	unsigned long a11;
++	unsigned long a12;
++	unsigned long a13;
++	unsigned long a14;
++	unsigned long a15;
++	unsigned long a16;
++	unsigned long a17;
++};
++
++/**
++ * arm_smccc_1_2_hvc() - make HVC calls
++ * @args: arguments passed via struct arm_smccc_1_2_regs
++ * @res: result values via struct arm_smccc_1_2_regs
++ *
++ * This function is used to make HVC calls following SMC Calling Convention
++ * v1.2 or above. The content of the supplied param are copied from the
++ * structure to registers prior to the HVC instruction. The return values
++ * are updated with the content from registers on return from the HVC
++ * instruction.
++ */
++asmlinkage void arm_smccc_1_2_hvc(const struct arm_smccc_1_2_regs *args,
++				  struct arm_smccc_1_2_regs *res);
++
++/**
++ * arm_smccc_1_2_smc() - make SMC calls
++ * @args: arguments passed via struct arm_smccc_1_2_regs
++ * @res: result values via struct arm_smccc_1_2_regs
++ *
++ * This function is used to make SMC calls following SMC Calling Convention
++ * v1.2 or above. The content of the supplied param are copied from the
++ * structure to registers prior to the SMC instruction. The return values
++ * are updated with the content from registers on return from the SMC
++ * instruction.
++ */
++asmlinkage void arm_smccc_1_2_smc(const struct arm_smccc_1_2_regs *args,
++				  struct arm_smccc_1_2_regs *res);
++#endif
++
+ /**
+  * struct arm_smccc_quirk - Contains quirk information
+  * @id: quirk identification
+diff --git a/include/linux/arm_ffa.h b/include/linux/arm_ffa.h
+new file mode 100644
+index 000000000000..85651e41ded8
+--- /dev/null
++++ b/include/linux/arm_ffa.h
+@@ -0,0 +1,269 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Copyright (C) 2021 ARM Ltd.
++ */
++
++#ifndef _LINUX_ARM_FFA_H
++#define _LINUX_ARM_FFA_H
++
++#include <linux/device.h>
++#include <linux/module.h>
++#include <linux/types.h>
++#include <linux/uuid.h>
++
++/* FFA Bus/Device/Driver related */
++struct ffa_device {
++	int vm_id;
++	bool mode_32bit;
++	uuid_t uuid;
++	struct device dev;
++};
++
++#define to_ffa_dev(d) container_of(d, struct ffa_device, dev)
++
++struct ffa_device_id {
++	uuid_t uuid;
++};
++
++struct ffa_driver {
++	const char *name;
++	int (*probe)(struct ffa_device *sdev);
++	void (*remove)(struct ffa_device *sdev);
++	const struct ffa_device_id *id_table;
++
++	struct device_driver driver;
++};
++
++#define to_ffa_driver(d) container_of(d, struct ffa_driver, driver)
++
++static inline void ffa_dev_set_drvdata(struct ffa_device *fdev, void *data)
++{
++	fdev->dev.driver_data = data;
++}
++
++#if IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT)
++struct ffa_device *ffa_device_register(const uuid_t *uuid, int vm_id);
++void ffa_device_unregister(struct ffa_device *ffa_dev);
++int ffa_driver_register(struct ffa_driver *driver, struct module *owner,
++			const char *mod_name);
++void ffa_driver_unregister(struct ffa_driver *driver);
++bool ffa_device_is_valid(struct ffa_device *ffa_dev);
++const struct ffa_dev_ops *ffa_dev_ops_get(struct ffa_device *dev);
++
++#else
++static inline
++struct ffa_device *ffa_device_register(const uuid_t *uuid, int vm_id)
++{
++	return NULL;
++}
++
++static inline void ffa_device_unregister(struct ffa_device *dev) {}
++
++static inline int
++ffa_driver_register(struct ffa_driver *driver, struct module *owner,
++		    const char *mod_name)
++{
++	return -EINVAL;
++}
++
++static inline void ffa_driver_unregister(struct ffa_driver *driver) {}
++
++static inline
++bool ffa_device_is_valid(struct ffa_device *ffa_dev) { return false; }
++
++static inline
++const struct ffa_dev_ops *ffa_dev_ops_get(struct ffa_device *dev)
++{
++	return NULL;
++}
++#endif /* CONFIG_ARM_FFA_TRANSPORT */
++
++#define ffa_register(driver) \
++	ffa_driver_register(driver, THIS_MODULE, KBUILD_MODNAME)
++#define ffa_unregister(driver) \
++	ffa_driver_unregister(driver)
++
++/**
++ * module_ffa_driver() - Helper macro for registering a psa_ffa driver
++ * @__ffa_driver: ffa_driver structure
++ *
++ * Helper macro for psa_ffa drivers to set up proper module init / exit
++ * functions.  Replaces module_init() and module_exit() and keeps people from
++ * printing pointless things to the kernel log when their driver is loaded.
++ */
++#define module_ffa_driver(__ffa_driver)	\
++	module_driver(__ffa_driver, ffa_register, ffa_unregister)
++
++/* FFA transport related */
++struct 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;
++};
++
++/* For use with FFA_MSG_SEND_DIRECT_{REQ,RESP} which pass data via registers */
++struct ffa_send_direct_data {
++	unsigned long data0; /* w3/x3 */
++	unsigned long data1; /* w4/x4 */
++	unsigned long data2; /* w5/x5 */
++	unsigned long data3; /* w6/x6 */
++	unsigned long data4; /* w7/x7 */
++};
++
++struct ffa_mem_region_addr_range {
++	/* The base IPA of the constituent memory region, aligned to 4 kiB */
++	u64 address;
++	/* The number of 4 kiB pages in the constituent memory region. */
++	u32 pg_cnt;
++	u32 reserved;
++};
++
++struct ffa_composite_mem_region {
++	/*
++	 * The total number of 4 kiB pages included in this memory region. This
++	 * must be equal to the sum of page counts specified in each
++	 * `struct ffa_mem_region_addr_range`.
++	 */
++	u32 total_pg_cnt;
++	/* The number of constituents included in this memory region range */
++	u32 addr_range_cnt;
++	u64 reserved;
++	/** An array of `addr_range_cnt` memory region constituents. */
++	struct ffa_mem_region_addr_range constituents[];
++};
++
++struct ffa_mem_region_attributes {
++	/* The ID of the VM to which the memory is being given or shared. */
++	u16 receiver;
++	/*
++	 * The permissions with which the memory region should be mapped in the
++	 * receiver's page table.
++	 */
++#define FFA_MEM_EXEC		BIT(3)
++#define FFA_MEM_NO_EXEC		BIT(2)
++#define FFA_MEM_RW		BIT(1)
++#define FFA_MEM_RO		BIT(0)
++	u8 attrs;
++	/*
++	 * Flags used during FFA_MEM_RETRIEVE_REQ and FFA_MEM_RETRIEVE_RESP
++	 * for memory regions with multiple borrowers.
++	 */
++#define FFA_MEM_RETRIEVE_SELF_BORROWER	BIT(0)
++	u8 flag;
++	u32 composite_off;
++	/*
++	 * Offset in bytes from the start of the outer `ffa_memory_region` to
++	 * an `struct ffa_mem_region_addr_range`.
++	 */
++	u64 reserved;
++};
++
++struct ffa_mem_region {
++	/* The ID of the VM/owner which originally sent the memory region */
++	u16 sender_id;
++#define FFA_MEM_NORMAL		BIT(5)
++#define FFA_MEM_DEVICE		BIT(4)
++
++#define FFA_MEM_WRITE_BACK	(3 << 2)
++#define FFA_MEM_NON_CACHEABLE	(1 << 2)
++
++#define FFA_DEV_nGnRnE		(0 << 2)
++#define FFA_DEV_nGnRE		(1 << 2)
++#define FFA_DEV_nGRE		(2 << 2)
++#define FFA_DEV_GRE		(3 << 2)
++
++#define FFA_MEM_NON_SHAREABLE	(0)
++#define FFA_MEM_OUTER_SHAREABLE	(2)
++#define FFA_MEM_INNER_SHAREABLE	(3)
++	u8 attributes;
++	u8 reserved_0;
++/*
++ * Clear memory region contents after unmapping it from the sender and
++ * before mapping it for any receiver.
++ */
++#define FFA_MEM_CLEAR			BIT(0)
++/*
++ * Whether the hypervisor may time slice the memory sharing or retrieval
++ * operation.
++ */
++#define FFA_TIME_SLICE_ENABLE		BIT(1)
++
++#define FFA_MEM_RETRIEVE_TYPE_IN_RESP	(0 << 3)
++#define FFA_MEM_RETRIEVE_TYPE_SHARE	(1 << 3)
++#define FFA_MEM_RETRIEVE_TYPE_LEND	(2 << 3)
++#define FFA_MEM_RETRIEVE_TYPE_DONATE	(3 << 3)
++
++#define FFA_MEM_RETRIEVE_ADDR_ALIGN_HINT	BIT(9)
++#define FFA_MEM_RETRIEVE_ADDR_ALIGN(x)		((x) << 5)
++	/* Flags to control behaviour of the transaction. */
++	u32 flags;
++#define HANDLE_LOW_MASK		GENMASK_ULL(31, 0)
++#define HANDLE_HIGH_MASK	GENMASK_ULL(63, 32)
++#define HANDLE_LOW(x)		((u32)(FIELD_GET(HANDLE_LOW_MASK, (x))))
++#define	HANDLE_HIGH(x)		((u32)(FIELD_GET(HANDLE_HIGH_MASK, (x))))
++
++#define PACK_HANDLE(l, h)		\
++	(FIELD_PREP(HANDLE_LOW_MASK, (l)) | FIELD_PREP(HANDLE_HIGH_MASK, (h)))
++	/*
++	 * A globally-unique ID assigned by the hypervisor for a region
++	 * of memory being sent between VMs.
++	 */
++	u64 handle;
++	/*
++	 * An implementation defined value associated with the receiver and the
++	 * memory region.
++	 */
++	u64 tag;
++	u32 reserved_1;
++	/*
++	 * The number of `ffa_mem_region_attributes` entries included in this
++	 * transaction.
++	 */
++	u32 ep_count;
++	/*
++	 * An array of endpoint memory access descriptors.
++	 * Each one specifies a memory region offset, an endpoint and the
++	 * attributes with which this memory region should be mapped in that
++	 * endpoint's page table.
++	 */
++	struct ffa_mem_region_attributes ep_mem_access[];
++};
++
++#define	COMPOSITE_OFFSET(x)	\
++	(offsetof(struct ffa_mem_region, ep_mem_access[x]))
++#define CONSTITUENTS_OFFSET(x)	\
++	(offsetof(struct ffa_composite_mem_region, constituents[x]))
++#define COMPOSITE_CONSTITUENTS_OFFSET(x, y)	\
++	(COMPOSITE_OFFSET(x) + CONSTITUENTS_OFFSET(y))
++
++struct ffa_mem_ops_args {
++	bool use_txbuf;
++	u32 nattrs;
++	u32 flags;
++	u64 tag;
++	u64 g_handle;
++	struct scatterlist *sg;
++	struct ffa_mem_region_attributes *attrs;
++};
++
++struct ffa_dev_ops {
++	u32 (*api_version_get)(void);
++	int (*partition_info_get)(const char *uuid_str,
++				  struct ffa_partition_info *buffer);
++	void (*mode_32bit_set)(struct ffa_device *dev);
++	int (*sync_send_receive)(struct ffa_device *dev,
++				 struct ffa_send_direct_data *data);
++	int (*memory_reclaim)(u64 g_handle, u32 flags);
++	int (*memory_share)(struct ffa_device *dev,
++			    struct ffa_mem_ops_args *args);
++	int (*memory_lend)(struct ffa_device *dev,
++			   struct ffa_mem_ops_args *args);
++};
++
++#endif /* _LINUX_ARM_FFA_H */
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0009-tee-add-sec_world_id-to-struct-tee_shm.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0009-tee-add-sec_world_id-to-struct-tee_shm.patch
new file mode 100644
index 0000000..eab6527
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0009-tee-add-sec_world_id-to-struct-tee_shm.patch
@@ -0,0 +1,44 @@
+From 812d2a649a9cc2a0004cbde2b3e411b46ec84af4 Mon Sep 17 00:00:00 2001
+From: Jens Wiklander <jens.wiklander@linaro.org>
+Date: Thu, 25 Mar 2021 15:08:44 +0100
+Subject: [PATCH 14/22] tee: add sec_world_id to struct tee_shm
+
+Adds sec_world_id to struct tee_shm which describes a shared memory
+object. sec_world_id can be used by a driver to store an id assigned by
+secure world.
+
+Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ include/linux/tee_drv.h | 7 ++++++-
+ 1 file changed, 6 insertions(+), 1 deletion(-)
+
+diff --git a/include/linux/tee_drv.h b/include/linux/tee_drv.h
+index cdd049a724b1..93d836fded8b 100644
+--- a/include/linux/tee_drv.h
++++ b/include/linux/tee_drv.h
+@@ -196,7 +196,11 @@ int tee_session_calc_client_uuid(uuid_t *uuid, u32 connection_method,
+  * @num_pages:	number of locked pages
+  * @dmabuf:	dmabuf used to for exporting to user space
+  * @flags:	defined by TEE_SHM_* in tee_drv.h
+- * @id:		unique id of a shared memory object on this device
++ * @id:		unique id of a shared memory object on this device, shared
++ *		with user space
++ * @sec_world_id:
++ *		secure world assigned id of this shared memory object, not
++ *		used by all drivers
+  *
+  * This pool is only supposed to be accessed directly from the TEE
+  * subsystem and from drivers that implements their own shm pool manager.
+@@ -212,6 +216,7 @@ struct tee_shm {
+ 	struct dma_buf *dmabuf;
+ 	u32 flags;
+ 	int id;
++	u64 sec_world_id;
+ };
+ 
+ /**
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0010-optee-simplify-optee_release.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0010-optee-simplify-optee_release.patch
new file mode 100644
index 0000000..94973c7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0010-optee-simplify-optee_release.patch
@@ -0,0 +1,179 @@
+From cb4f6a55b9c61a82a65edcd4b18c505d92480710 Mon Sep 17 00:00:00 2001
+From: Jens Wiklander <jens.wiklander@linaro.org>
+Date: Thu, 25 Mar 2021 15:08:46 +0100
+Subject: [PATCH 15/22] optee: simplify optee_release()
+
+Simplifies optee_release() with a new helper function,
+optee_close_session_helper() which has been factored out from
+optee_close_session().
+
+A separate optee_release_supp() is added for the supplicant device.
+
+Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/tee/optee/call.c          | 31 ++++++++++-------
+ drivers/tee/optee/core.c          | 55 +++++++++++--------------------
+ drivers/tee/optee/optee_private.h |  1 +
+ 3 files changed, 39 insertions(+), 48 deletions(-)
+
+diff --git a/drivers/tee/optee/call.c b/drivers/tee/optee/call.c
+index 0790de29f0ca..1b339b743ff5 100644
+--- a/drivers/tee/optee/call.c
++++ b/drivers/tee/optee/call.c
+@@ -285,12 +285,28 @@ int optee_open_session(struct tee_context *ctx,
+ 	return rc;
+ }
+ 
+-int optee_close_session(struct tee_context *ctx, u32 session)
++int optee_close_session_helper(struct tee_context *ctx, u32 session)
+ {
+-	struct optee_context_data *ctxdata = ctx->data;
+ 	struct tee_shm *shm;
+ 	struct optee_msg_arg *msg_arg;
+ 	phys_addr_t msg_parg;
++
++	shm = get_msg_arg(ctx, 0, &msg_arg, &msg_parg);
++	if (IS_ERR(shm))
++		return PTR_ERR(shm);
++
++	msg_arg->cmd = OPTEE_MSG_CMD_CLOSE_SESSION;
++	msg_arg->session = session;
++	optee_do_call_with_arg(ctx, msg_parg);
++
++	tee_shm_free(shm);
++
++	return 0;
++}
++
++int optee_close_session(struct tee_context *ctx, u32 session)
++{
++	struct optee_context_data *ctxdata = ctx->data;
+ 	struct optee_session *sess;
+ 
+ 	/* Check that the session is valid and remove it from the list */
+@@ -303,16 +319,7 @@ int optee_close_session(struct tee_context *ctx, u32 session)
+ 		return -EINVAL;
+ 	kfree(sess);
+ 
+-	shm = get_msg_arg(ctx, 0, &msg_arg, &msg_parg);
+-	if (IS_ERR(shm))
+-		return PTR_ERR(shm);
+-
+-	msg_arg->cmd = OPTEE_MSG_CMD_CLOSE_SESSION;
+-	msg_arg->session = session;
+-	optee_do_call_with_arg(ctx, msg_parg);
+-
+-	tee_shm_free(shm);
+-	return 0;
++	return optee_close_session_helper(ctx, session);
+ }
+ 
+ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
+diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
+index 63542c1cc291..e39c6d290d83 100644
+--- a/drivers/tee/optee/core.c
++++ b/drivers/tee/optee/core.c
+@@ -263,59 +263,42 @@ static int optee_open(struct tee_context *ctx)
+ 	return 0;
+ }
+ 
+-static void optee_release(struct tee_context *ctx)
++static void optee_release_helper(struct tee_context *ctx,
++				 int (*close_session)(struct tee_context *ctx,
++						      u32 session))
+ {
+ 	struct optee_context_data *ctxdata = ctx->data;
+-	struct tee_device *teedev = ctx->teedev;
+-	struct optee *optee = tee_get_drvdata(teedev);
+-	struct tee_shm *shm;
+-	struct optee_msg_arg *arg = NULL;
+-	phys_addr_t parg;
+ 	struct optee_session *sess;
+ 	struct optee_session *sess_tmp;
+ 
+ 	if (!ctxdata)
+ 		return;
+ 
+-	shm = tee_shm_alloc(ctx, sizeof(struct optee_msg_arg), TEE_SHM_MAPPED);
+-	if (!IS_ERR(shm)) {
+-		arg = tee_shm_get_va(shm, 0);
+-		/*
+-		 * If va2pa fails for some reason, we can't call into
+-		 * secure world, only free the memory. Secure OS will leak
+-		 * sessions and finally refuse more sessions, but we will
+-		 * at least let normal world reclaim its memory.
+-		 */
+-		if (!IS_ERR(arg))
+-			if (tee_shm_va2pa(shm, arg, &parg))
+-				arg = NULL; /* prevent usage of parg below */
+-	}
+-
+ 	list_for_each_entry_safe(sess, sess_tmp, &ctxdata->sess_list,
+ 				 list_node) {
+ 		list_del(&sess->list_node);
+-		if (!IS_ERR_OR_NULL(arg)) {
+-			memset(arg, 0, sizeof(*arg));
+-			arg->cmd = OPTEE_MSG_CMD_CLOSE_SESSION;
+-			arg->session = sess->session_id;
+-			optee_do_call_with_arg(ctx, parg);
+-		}
++		close_session(ctx, sess->session_id);
+ 		kfree(sess);
+ 	}
+ 	kfree(ctxdata);
++	ctx->data = NULL;
++}
+ 
+-	if (!IS_ERR(shm))
+-		tee_shm_free(shm);
++static void optee_release(struct tee_context *ctx)
++{
++	optee_release_helper(ctx, optee_close_session_helper);
++}
+ 
+-	ctx->data = NULL;
++static void optee_release_supp(struct tee_context *ctx)
++{
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
+ 
+-	if (teedev == optee->supp_teedev) {
+-		if (optee->scan_bus_wq) {
+-			destroy_workqueue(optee->scan_bus_wq);
+-			optee->scan_bus_wq = NULL;
+-		}
+-		optee_supp_release(&optee->supp);
++	optee_release_helper(ctx, optee_close_session_helper);
++	if (optee->scan_bus_wq) {
++		destroy_workqueue(optee->scan_bus_wq);
++		optee->scan_bus_wq = NULL;
+ 	}
++	optee_supp_release(&optee->supp);
+ }
+ 
+ static const struct tee_driver_ops optee_ops = {
+@@ -339,7 +322,7 @@ static const struct tee_desc optee_desc = {
+ static const struct tee_driver_ops optee_supp_ops = {
+ 	.get_version = optee_get_version,
+ 	.open = optee_open,
+-	.release = optee_release,
++	.release = optee_release_supp,
+ 	.supp_recv = optee_supp_recv,
+ 	.supp_send = optee_supp_send,
+ 	.shm_register = optee_shm_register_supp,
+diff --git a/drivers/tee/optee/optee_private.h b/drivers/tee/optee/optee_private.h
+index e25b216a14ef..2b63b796645e 100644
+--- a/drivers/tee/optee/optee_private.h
++++ b/drivers/tee/optee/optee_private.h
+@@ -152,6 +152,7 @@ u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg);
+ int optee_open_session(struct tee_context *ctx,
+ 		       struct tee_ioctl_open_session_arg *arg,
+ 		       struct tee_param *param);
++int optee_close_session_helper(struct tee_context *ctx, u32 session);
+ int optee_close_session(struct tee_context *ctx, u32 session);
+ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
+ 		      struct tee_param *param);
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0011-optee-sync-OP-TEE-headers.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0011-optee-sync-OP-TEE-headers.patch
new file mode 100644
index 0000000..5e3e868
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0011-optee-sync-OP-TEE-headers.patch
@@ -0,0 +1,644 @@
+From 1e43bd55c951da0610230c4f28a8ebdd13b30733 Mon Sep 17 00:00:00 2001
+From: Jens Wiklander <jens.wiklander@linaro.org>
+Date: Wed, 20 Jan 2021 11:14:12 +0100
+Subject: [PATCH 16/22] optee: sync OP-TEE headers
+
+Pulls in updates in the internal headers from OP-TEE OS [1]. A few
+defines has been shortened, hence the changes in rpc.c. Defines not used
+by the driver in tee_rpc_cmd.h has been filtered out.
+
+Note that this does not change the ABI.
+
+Link: [1] https://github.com/OP-TEE/optee_os
+Reviewed-by: Sumit Garg <sumit.garg@linaro.org>
+Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>
+Change-Id: I5d20a22a3f38bfc9d232279d5f00505c4d3ba965
+
+Upstream-Status: Backport [https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/commit/?h=v5.13-rc7&id=617d8e8b347edcee6da38df0aeb671fc9c9ba19c]
+---
+ drivers/tee/optee/optee_msg.h     | 156 ++----------------------------
+ drivers/tee/optee/optee_rpc_cmd.h | 103 ++++++++++++++++++++
+ drivers/tee/optee/optee_smc.h     |  70 +++++++++-----
+ drivers/tee/optee/rpc.c           |  39 ++++----
+ 4 files changed, 179 insertions(+), 189 deletions(-)
+ create mode 100644 drivers/tee/optee/optee_rpc_cmd.h
+
+diff --git a/drivers/tee/optee/optee_msg.h b/drivers/tee/optee/optee_msg.h
+index c7ac7d02d6cc..5bef6a0165db 100644
+--- a/drivers/tee/optee/optee_msg.h
++++ b/drivers/tee/optee/optee_msg.h
+@@ -1,6 +1,6 @@
+ /* SPDX-License-Identifier: (GPL-2.0 OR BSD-2-Clause) */
+ /*
+- * Copyright (c) 2015-2019, Linaro Limited
++ * Copyright (c) 2015-2021, Linaro Limited
+  */
+ #ifndef _OPTEE_MSG_H
+ #define _OPTEE_MSG_H
+@@ -12,11 +12,9 @@
+  * This file defines the OP-TEE message protocol (ABI) used to communicate
+  * with an instance of OP-TEE running in secure world.
+  *
+- * This file is divided into three sections.
++ * This file is divided into two sections.
+  * 1. Formatting of messages.
+  * 2. Requests from normal world
+- * 3. Requests from secure world, Remote Procedure Call (RPC), handled by
+- *    tee-supplicant.
+  */
+ 
+ /*****************************************************************************
+@@ -54,8 +52,8 @@
+  * Every entry in buffer should point to a 4k page beginning (12 least
+  * significant bits must be equal to zero).
+  *
+- * 12 least significant bints of optee_msg_param.u.tmem.buf_ptr should hold page
+- * offset of the user buffer.
++ * 12 least significant bits of optee_msg_param.u.tmem.buf_ptr should hold
++ * page offset of user buffer.
+  *
+  * So, entries should be placed like members of this structure:
+  *
+@@ -178,17 +176,9 @@ struct optee_msg_param {
+  * @params: the parameters supplied to the OS Command
+  *
+  * All normal calls to Trusted OS uses this struct. If cmd requires further
+- * information than what these field holds it can be passed as a parameter
++ * information than what these fields hold it can be passed as a parameter
+  * tagged as meta (setting the OPTEE_MSG_ATTR_META bit in corresponding
+- * attrs field). All parameters tagged as meta has to come first.
+- *
+- * Temp memref parameters can be fragmented if supported by the Trusted OS
+- * (when optee_smc.h is bearer of this protocol this is indicated with
+- * OPTEE_SMC_SEC_CAP_UNREGISTERED_SHM). If a logical memref parameter is
+- * fragmented then has all but the last fragment the
+- * OPTEE_MSG_ATTR_FRAGMENT bit set in attrs. Even if a memref is fragmented
+- * it will still be presented as a single logical memref to the Trusted
+- * Application.
++ * attrs field). All parameters tagged as meta have to come first.
+  */
+ struct optee_msg_arg {
+ 	u32 cmd;
+@@ -292,15 +282,12 @@ struct optee_msg_arg {
+  * OPTEE_MSG_CMD_REGISTER_SHM registers a shared memory reference. The
+  * information is passed as:
+  * [in] param[0].attr			OPTEE_MSG_ATTR_TYPE_TMEM_INPUT
+- *					[| OPTEE_MSG_ATTR_FRAGMENT]
++ *					[| OPTEE_MSG_ATTR_NONCONTIG]
+  * [in] param[0].u.tmem.buf_ptr		physical address (of first fragment)
+  * [in] param[0].u.tmem.size		size (of first fragment)
+  * [in] param[0].u.tmem.shm_ref		holds shared memory reference
+- * ...
+- * The shared memory can optionally be fragmented, temp memrefs can follow
+- * each other with all but the last with the OPTEE_MSG_ATTR_FRAGMENT bit set.
+  *
+- * OPTEE_MSG_CMD_UNREGISTER_SHM unregisteres a previously registered shared
++ * OPTEE_MSG_CMD_UNREGISTER_SHM unregisters a previously registered shared
+  * memory reference. The information is passed as:
+  * [in] param[0].attr			OPTEE_MSG_ATTR_TYPE_RMEM_INPUT
+  * [in] param[0].u.rmem.shm_ref		holds shared memory reference
+@@ -315,131 +302,4 @@ struct optee_msg_arg {
+ #define OPTEE_MSG_CMD_UNREGISTER_SHM	5
+ #define OPTEE_MSG_FUNCID_CALL_WITH_ARG	0x0004
+ 
+-/*****************************************************************************
+- * Part 3 - Requests from secure world, RPC
+- *****************************************************************************/
+-
+-/*
+- * All RPC is done with a struct optee_msg_arg as bearer of information,
+- * struct optee_msg_arg::arg holds values defined by OPTEE_MSG_RPC_CMD_* below
+- *
+- * RPC communication with tee-supplicant is reversed compared to normal
+- * client communication desribed above. The supplicant receives requests
+- * and sends responses.
+- */
+-
+-/*
+- * Load a TA into memory, defined in tee-supplicant
+- */
+-#define OPTEE_MSG_RPC_CMD_LOAD_TA	0
+-
+-/*
+- * Reserved
+- */
+-#define OPTEE_MSG_RPC_CMD_RPMB		1
+-
+-/*
+- * File system access, defined in tee-supplicant
+- */
+-#define OPTEE_MSG_RPC_CMD_FS		2
+-
+-/*
+- * Get time
+- *
+- * Returns number of seconds and nano seconds since the Epoch,
+- * 1970-01-01 00:00:00 +0000 (UTC).
+- *
+- * [out] param[0].u.value.a	Number of seconds
+- * [out] param[0].u.value.b	Number of nano seconds.
+- */
+-#define OPTEE_MSG_RPC_CMD_GET_TIME	3
+-
+-/*
+- * Wait queue primitive, helper for secure world to implement a wait queue.
+- *
+- * If secure world need to wait for a secure world mutex it issues a sleep
+- * request instead of spinning in secure world. Conversely is a wakeup
+- * request issued when a secure world mutex with a thread waiting thread is
+- * unlocked.
+- *
+- * Waiting on a key
+- * [in] param[0].u.value.a OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP
+- * [in] param[0].u.value.b wait key
+- *
+- * Waking up a key
+- * [in] param[0].u.value.a OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP
+- * [in] param[0].u.value.b wakeup key
+- */
+-#define OPTEE_MSG_RPC_CMD_WAIT_QUEUE	4
+-#define OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP	0
+-#define OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP	1
+-
+-/*
+- * Suspend execution
+- *
+- * [in] param[0].value	.a number of milliseconds to suspend
+- */
+-#define OPTEE_MSG_RPC_CMD_SUSPEND	5
+-
+-/*
+- * Allocate a piece of shared memory
+- *
+- * Shared memory can optionally be fragmented, to support that additional
+- * spare param entries are allocated to make room for eventual fragments.
+- * The spare param entries has .attr = OPTEE_MSG_ATTR_TYPE_NONE when
+- * unused. All returned temp memrefs except the last should have the
+- * OPTEE_MSG_ATTR_FRAGMENT bit set in the attr field.
+- *
+- * [in]  param[0].u.value.a		type of memory one of
+- *					OPTEE_MSG_RPC_SHM_TYPE_* below
+- * [in]  param[0].u.value.b		requested size
+- * [in]  param[0].u.value.c		required alignment
+- *
+- * [out] param[0].u.tmem.buf_ptr	physical address (of first fragment)
+- * [out] param[0].u.tmem.size		size (of first fragment)
+- * [out] param[0].u.tmem.shm_ref	shared memory reference
+- * ...
+- * [out] param[n].u.tmem.buf_ptr	physical address
+- * [out] param[n].u.tmem.size		size
+- * [out] param[n].u.tmem.shm_ref	shared memory reference (same value
+- *					as in param[n-1].u.tmem.shm_ref)
+- */
+-#define OPTEE_MSG_RPC_CMD_SHM_ALLOC	6
+-/* Memory that can be shared with a non-secure user space application */
+-#define OPTEE_MSG_RPC_SHM_TYPE_APPL	0
+-/* Memory only shared with non-secure kernel */
+-#define OPTEE_MSG_RPC_SHM_TYPE_KERNEL	1
+-
+-/*
+- * Free shared memory previously allocated with OPTEE_MSG_RPC_CMD_SHM_ALLOC
+- *
+- * [in]  param[0].u.value.a		type of memory one of
+- *					OPTEE_MSG_RPC_SHM_TYPE_* above
+- * [in]  param[0].u.value.b		value of shared memory reference
+- *					returned in param[0].u.tmem.shm_ref
+- *					above
+- */
+-#define OPTEE_MSG_RPC_CMD_SHM_FREE	7
+-
+-/*
+- * Access a device on an i2c bus
+- *
+- * [in]  param[0].u.value.a		mode: RD(0), WR(1)
+- * [in]  param[0].u.value.b		i2c adapter
+- * [in]  param[0].u.value.c		i2c chip
+- *
+- * [in]  param[1].u.value.a		i2c control flags
+- *
+- * [in/out] memref[2]			buffer to exchange the transfer data
+- *					with the secure world
+- *
+- * [out]  param[3].u.value.a		bytes transferred by the driver
+- */
+-#define OPTEE_MSG_RPC_CMD_I2C_TRANSFER 21
+-/* I2C master transfer modes */
+-#define OPTEE_MSG_RPC_CMD_I2C_TRANSFER_RD 0
+-#define OPTEE_MSG_RPC_CMD_I2C_TRANSFER_WR 1
+-/* I2C master control flags */
+-#define OPTEE_MSG_RPC_CMD_I2C_FLAGS_TEN_BIT  BIT(0)
+-
+ #endif /* _OPTEE_MSG_H */
+diff --git a/drivers/tee/optee/optee_rpc_cmd.h b/drivers/tee/optee/optee_rpc_cmd.h
+new file mode 100644
+index 000000000000..b8275140cef8
+--- /dev/null
++++ b/drivers/tee/optee/optee_rpc_cmd.h
+@@ -0,0 +1,103 @@
++/* SPDX-License-Identifier: BSD-2-Clause */
++/*
++ * Copyright (c) 2016-2021, Linaro Limited
++ */
++
++#ifndef __OPTEE_RPC_CMD_H
++#define __OPTEE_RPC_CMD_H
++
++/*
++ * All RPC is done with a struct optee_msg_arg as bearer of information,
++ * struct optee_msg_arg::arg holds values defined by OPTEE_RPC_CMD_* below.
++ * Only the commands handled by the kernel driver are defined here.
++ *
++ * RPC communication with tee-supplicant is reversed compared to normal
++ * client communication described above. The supplicant receives requests
++ * and sends responses.
++ */
++
++/*
++ * Get time
++ *
++ * Returns number of seconds and nano seconds since the Epoch,
++ * 1970-01-01 00:00:00 +0000 (UTC).
++ *
++ * [out]    value[0].a	    Number of seconds
++ * [out]    value[0].b	    Number of nano seconds.
++ */
++#define OPTEE_RPC_CMD_GET_TIME		3
++
++/*
++ * Wait queue primitive, helper for secure world to implement a wait queue.
++ *
++ * If secure world needs to wait for a secure world mutex it issues a sleep
++ * request instead of spinning in secure world. Conversely is a wakeup
++ * request issued when a secure world mutex with a thread waiting thread is
++ * unlocked.
++ *
++ * Waiting on a key
++ * [in]    value[0].a	    OPTEE_RPC_WAIT_QUEUE_SLEEP
++ * [in]    value[0].b	    Wait key
++ *
++ * Waking up a key
++ * [in]    value[0].a	    OPTEE_RPC_WAIT_QUEUE_WAKEUP
++ * [in]    value[0].b	    Wakeup key
++ */
++#define OPTEE_RPC_CMD_WAIT_QUEUE	4
++#define OPTEE_RPC_WAIT_QUEUE_SLEEP	0
++#define OPTEE_RPC_WAIT_QUEUE_WAKEUP	1
++
++/*
++ * Suspend execution
++ *
++ * [in]    value[0].a	Number of milliseconds to suspend
++ */
++#define OPTEE_RPC_CMD_SUSPEND		5
++
++/*
++ * Allocate a piece of shared memory
++ *
++ * [in]    value[0].a	    Type of memory one of
++ *			    OPTEE_RPC_SHM_TYPE_* below
++ * [in]    value[0].b	    Requested size
++ * [in]    value[0].c	    Required alignment
++ * [out]   memref[0]	    Buffer
++ */
++#define OPTEE_RPC_CMD_SHM_ALLOC		6
++/* Memory that can be shared with a non-secure user space application */
++#define OPTEE_RPC_SHM_TYPE_APPL		0
++/* Memory only shared with non-secure kernel */
++#define OPTEE_RPC_SHM_TYPE_KERNEL	1
++
++/*
++ * Free shared memory previously allocated with OPTEE_RPC_CMD_SHM_ALLOC
++ *
++ * [in]     value[0].a	    Type of memory one of
++ *			    OPTEE_RPC_SHM_TYPE_* above
++ * [in]     value[0].b	    Value of shared memory reference or cookie
++ */
++#define OPTEE_RPC_CMD_SHM_FREE		7
++
++/*
++ * Issue master requests (read and write operations) to an I2C chip.
++ *
++ * [in]     value[0].a	    Transfer mode (OPTEE_RPC_I2C_TRANSFER_*)
++ * [in]     value[0].b	    The I2C bus (a.k.a adapter).
++ *				16 bit field.
++ * [in]     value[0].c	    The I2C chip (a.k.a address).
++ *				16 bit field (either 7 or 10 bit effective).
++ * [in]     value[1].a	    The I2C master control flags (ie, 10 bit address).
++ *				16 bit field.
++ * [in/out] memref[2]	    Buffer used for data transfers.
++ * [out]    value[3].a	    Number of bytes transferred by the REE.
++ */
++#define OPTEE_RPC_CMD_I2C_TRANSFER	21
++
++/* I2C master transfer modes */
++#define OPTEE_RPC_I2C_TRANSFER_RD	0
++#define OPTEE_RPC_I2C_TRANSFER_WR	1
++
++/* I2C master control flags */
++#define OPTEE_RPC_I2C_FLAGS_TEN_BIT	BIT(0)
++
++#endif /*__OPTEE_RPC_CMD_H*/
+diff --git a/drivers/tee/optee/optee_smc.h b/drivers/tee/optee/optee_smc.h
+index 777ad54d4c2c..821e1c30c150 100644
+--- a/drivers/tee/optee/optee_smc.h
++++ b/drivers/tee/optee/optee_smc.h
+@@ -1,6 +1,6 @@
+ /* SPDX-License-Identifier: (GPL-2.0 OR BSD-2-Clause) */
+ /*
+- * Copyright (c) 2015-2019, Linaro Limited
++ * Copyright (c) 2015-2021, Linaro Limited
+  */
+ #ifndef OPTEE_SMC_H
+ #define OPTEE_SMC_H
+@@ -39,10 +39,10 @@
+ /*
+  * Function specified by SMC Calling convention
+  *
+- * Return one of the following UIDs if using API specified in this file
+- * without further extentions:
+- * 65cb6b93-af0c-4617-8ed6-644a8d1140f8
+- * see also OPTEE_SMC_UID_* in optee_msg.h
++ * Return the following UID if using API specified in this file
++ * without further extensions:
++ * 384fb3e0-e7f8-11e3-af63-0002a5d5c51b.
++ * see also OPTEE_MSG_UID_* in optee_msg.h
+  */
+ #define OPTEE_SMC_FUNCID_CALLS_UID OPTEE_MSG_FUNCID_CALLS_UID
+ #define OPTEE_SMC_CALLS_UID \
+@@ -53,7 +53,7 @@
+ /*
+  * Function specified by SMC Calling convention
+  *
+- * Returns 2.0 if using API specified in this file without further extentions.
++ * Returns 2.0 if using API specified in this file without further extensions.
+  * see also OPTEE_MSG_REVISION_* in optee_msg.h
+  */
+ #define OPTEE_SMC_FUNCID_CALLS_REVISION OPTEE_MSG_FUNCID_CALLS_REVISION
+@@ -109,8 +109,8 @@ struct optee_smc_call_get_os_revision_result {
+  *
+  * Call register usage:
+  * a0	SMC Function ID, OPTEE_SMC*CALL_WITH_ARG
+- * a1	Upper 32bit of a 64bit physical pointer to a struct optee_msg_arg
+- * a2	Lower 32bit of a 64bit physical pointer to a struct optee_msg_arg
++ * a1	Upper 32 bits of a 64-bit physical pointer to a struct optee_msg_arg
++ * a2	Lower 32 bits of a 64-bit physical pointer to a struct optee_msg_arg
+  * a3	Cache settings, not used if physical pointer is in a predefined shared
+  *	memory area else per OPTEE_SMC_SHM_*
+  * a4-6	Not used
+@@ -214,8 +214,9 @@ struct optee_smc_get_shm_config_result {
+  * secure world accepts command buffers located in any parts of non-secure RAM
+  */
+ #define OPTEE_SMC_SEC_CAP_DYNAMIC_SHM		BIT(2)
+-
+-/* Secure world supports Shared Memory with a NULL buffer reference */
++/* Secure world is built with virtualization support */
++#define OPTEE_SMC_SEC_CAP_VIRTUALIZATION	BIT(3)
++/* Secure world supports Shared Memory with a NULL reference */
+ #define OPTEE_SMC_SEC_CAP_MEMREF_NULL		BIT(4)
+ 
+ #define OPTEE_SMC_FUNCID_EXCHANGE_CAPABILITIES	9
+@@ -245,8 +246,8 @@ struct optee_smc_exchange_capabilities_result {
+  *
+  * Normal return register usage:
+  * a0	OPTEE_SMC_RETURN_OK
+- * a1	Upper 32bit of a 64bit Shared memory cookie
+- * a2	Lower 32bit of a 64bit Shared memory cookie
++ * a1	Upper 32 bits of a 64-bit Shared memory cookie
++ * a2	Lower 32 bits of a 64-bit Shared memory cookie
+  * a3-7	Preserved
+  *
+  * Cache empty return register usage:
+@@ -293,6 +294,31 @@ struct optee_smc_disable_shm_cache_result {
+ #define OPTEE_SMC_ENABLE_SHM_CACHE \
+ 	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_ENABLE_SHM_CACHE)
+ 
++/*
++ * Query OP-TEE about number of supported threads
++ *
++ * Normal World OS or Hypervisor issues this call to find out how many
++ * threads OP-TEE supports. That is how many standard calls can be issued
++ * in parallel before OP-TEE will return OPTEE_SMC_RETURN_ETHREAD_LIMIT.
++ *
++ * Call requests usage:
++ * a0	SMC Function ID, OPTEE_SMC_GET_THREAD_COUNT
++ * a1-6 Not used
++ * a7	Hypervisor Client ID register
++ *
++ * Normal return register usage:
++ * a0	OPTEE_SMC_RETURN_OK
++ * a1	Number of threads
++ * a2-7 Preserved
++ *
++ * Error return:
++ * a0	OPTEE_SMC_RETURN_UNKNOWN_FUNCTION   Requested call is not implemented
++ * a1-7	Preserved
++ */
++#define OPTEE_SMC_FUNCID_GET_THREAD_COUNT	15
++#define OPTEE_SMC_GET_THREAD_COUNT \
++	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_GET_THREAD_COUNT)
++
+ /*
+  * Resume from RPC (for example after processing a foreign interrupt)
+  *
+@@ -341,16 +367,16 @@ struct optee_smc_disable_shm_cache_result {
+  *
+  * "Return" register usage:
+  * a0	SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC.
+- * a1	Upper 32bits of 64bit physical pointer to allocated
++ * a1	Upper 32 bits of 64-bit physical pointer to allocated
+  *	memory, (a1 == 0 && a2 == 0) if size was 0 or if memory can't
+  *	be allocated.
+- * a2	Lower 32bits of 64bit physical pointer to allocated
++ * a2	Lower 32 bits of 64-bit physical pointer to allocated
+  *	memory, (a1 == 0 && a2 == 0) if size was 0 or if memory can't
+  *	be allocated
+  * a3	Preserved
+- * a4	Upper 32bits of 64bit Shared memory cookie used when freeing
++ * a4	Upper 32 bits of 64-bit Shared memory cookie used when freeing
+  *	the memory or doing an RPC
+- * a5	Lower 32bits of 64bit Shared memory cookie used when freeing
++ * a5	Lower 32 bits of 64-bit Shared memory cookie used when freeing
+  *	the memory or doing an RPC
+  * a6-7	Preserved
+  */
+@@ -363,9 +389,9 @@ struct optee_smc_disable_shm_cache_result {
+  *
+  * "Call" register usage:
+  * a0	This value, OPTEE_SMC_RETURN_RPC_FREE
+- * a1	Upper 32bits of 64bit shared memory cookie belonging to this
++ * a1	Upper 32 bits of 64-bit shared memory cookie belonging to this
+  *	argument memory
+- * a2	Lower 32bits of 64bit shared memory cookie belonging to this
++ * a2	Lower 32 bits of 64-bit shared memory cookie belonging to this
+  *	argument memory
+  * a3-7	Resume information, must be preserved
+  *
+@@ -379,7 +405,7 @@ struct optee_smc_disable_shm_cache_result {
+ 	OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_FREE)
+ 
+ /*
+- * Deliver foreign interrupt to normal world.
++ * Deliver a foreign interrupt in normal world.
+  *
+  * "Call" register usage:
+  * a0	OPTEE_SMC_RETURN_RPC_FOREIGN_INTR
+@@ -389,7 +415,7 @@ struct optee_smc_disable_shm_cache_result {
+  * a0	SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC.
+  * a1-7	Preserved
+  */
+-#define OPTEE_SMC_RPC_FUNC_FOREIGN_INTR		4
++#define OPTEE_SMC_RPC_FUNC_FOREIGN_INTR	4
+ #define OPTEE_SMC_RETURN_RPC_FOREIGN_INTR \
+ 	OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_FOREIGN_INTR)
+ 
+@@ -405,10 +431,10 @@ struct optee_smc_disable_shm_cache_result {
+  *
+  * "Call" register usage:
+  * a0	OPTEE_SMC_RETURN_RPC_CMD
+- * a1	Upper 32bit of a 64bit Shared memory cookie holding a
++ * a1	Upper 32 bits of a 64-bit Shared memory cookie holding a
+  *	struct optee_msg_arg, must be preserved, only the data should
+  *	be updated
+- * a2	Lower 32bit of a 64bit Shared memory cookie holding a
++ * a2	Lower 32 bits of a 64-bit Shared memory cookie holding a
+  *	struct optee_msg_arg, must be preserved, only the data should
+  *	be updated
+  * a3-7	Resume information, must be preserved
+diff --git a/drivers/tee/optee/rpc.c b/drivers/tee/optee/rpc.c
+index 6cbb3643c6c4..1849180b0278 100644
+--- a/drivers/tee/optee/rpc.c
++++ b/drivers/tee/optee/rpc.c
+@@ -12,6 +12,7 @@
+ #include <linux/tee_drv.h>
+ #include "optee_private.h"
+ #include "optee_smc.h"
++#include "optee_rpc_cmd.h"
+ 
+ struct wq_entry {
+ 	struct list_head link;
+@@ -90,7 +91,7 @@ static void handle_rpc_func_cmd_i2c_transfer(struct tee_context *ctx,
+ 	if (!adapter)
+ 		goto bad;
+ 
+-	if (params[1].u.value.a & OPTEE_MSG_RPC_CMD_I2C_FLAGS_TEN_BIT) {
++	if (params[1].u.value.a & OPTEE_RPC_I2C_FLAGS_TEN_BIT) {
+ 		if (!i2c_check_functionality(adapter,
+ 					     I2C_FUNC_10BIT_ADDR)) {
+ 			i2c_put_adapter(adapter);
+@@ -105,10 +106,10 @@ static void handle_rpc_func_cmd_i2c_transfer(struct tee_context *ctx,
+ 	msg.len  = params[2].u.memref.size;
+ 
+ 	switch (params[0].u.value.a) {
+-	case OPTEE_MSG_RPC_CMD_I2C_TRANSFER_RD:
++	case OPTEE_RPC_I2C_TRANSFER_RD:
+ 		msg.flags |= I2C_M_RD;
+ 		break;
+-	case OPTEE_MSG_RPC_CMD_I2C_TRANSFER_WR:
++	case OPTEE_RPC_I2C_TRANSFER_WR:
+ 		break;
+ 	default:
+ 		i2c_put_adapter(adapter);
+@@ -195,10 +196,10 @@ static void handle_rpc_func_cmd_wq(struct optee *optee,
+ 		goto bad;
+ 
+ 	switch (arg->params[0].u.value.a) {
+-	case OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP:
++	case OPTEE_RPC_WAIT_QUEUE_SLEEP:
+ 		wq_sleep(&optee->wait_queue, arg->params[0].u.value.b);
+ 		break;
+-	case OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP:
++	case OPTEE_RPC_WAIT_QUEUE_WAKEUP:
+ 		wq_wakeup(&optee->wait_queue, arg->params[0].u.value.b);
+ 		break;
+ 	default:
+@@ -268,11 +269,11 @@ static struct tee_shm *cmd_alloc_suppl(struct tee_context *ctx, size_t sz)
+ 	struct tee_shm *shm;
+ 
+ 	param.attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT;
+-	param.u.value.a = OPTEE_MSG_RPC_SHM_TYPE_APPL;
++	param.u.value.a = OPTEE_RPC_SHM_TYPE_APPL;
+ 	param.u.value.b = sz;
+ 	param.u.value.c = 0;
+ 
+-	ret = optee_supp_thrd_req(ctx, OPTEE_MSG_RPC_CMD_SHM_ALLOC, 1, &param);
++	ret = optee_supp_thrd_req(ctx, OPTEE_RPC_CMD_SHM_ALLOC, 1, &param);
+ 	if (ret)
+ 		return ERR_PTR(-ENOMEM);
+ 
+@@ -309,10 +310,10 @@ static void handle_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
+ 
+ 	sz = arg->params[0].u.value.b;
+ 	switch (arg->params[0].u.value.a) {
+-	case OPTEE_MSG_RPC_SHM_TYPE_APPL:
++	case OPTEE_RPC_SHM_TYPE_APPL:
+ 		shm = cmd_alloc_suppl(ctx, sz);
+ 		break;
+-	case OPTEE_MSG_RPC_SHM_TYPE_KERNEL:
++	case OPTEE_RPC_SHM_TYPE_KERNEL:
+ 		shm = tee_shm_alloc(ctx, sz, TEE_SHM_MAPPED);
+ 		break;
+ 	default:
+@@ -384,7 +385,7 @@ static void cmd_free_suppl(struct tee_context *ctx, struct tee_shm *shm)
+ 	struct tee_param param;
+ 
+ 	param.attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT;
+-	param.u.value.a = OPTEE_MSG_RPC_SHM_TYPE_APPL;
++	param.u.value.a = OPTEE_RPC_SHM_TYPE_APPL;
+ 	param.u.value.b = tee_shm_get_id(shm);
+ 	param.u.value.c = 0;
+ 
+@@ -401,7 +402,7 @@ static void cmd_free_suppl(struct tee_context *ctx, struct tee_shm *shm)
+ 	 */
+ 	tee_shm_put(shm);
+ 
+-	optee_supp_thrd_req(ctx, OPTEE_MSG_RPC_CMD_SHM_FREE, 1, &param);
++	optee_supp_thrd_req(ctx, OPTEE_RPC_CMD_SHM_FREE, 1, &param);
+ }
+ 
+ static void handle_rpc_func_cmd_shm_free(struct tee_context *ctx,
+@@ -419,10 +420,10 @@ static void handle_rpc_func_cmd_shm_free(struct tee_context *ctx,
+ 
+ 	shm = (struct tee_shm *)(unsigned long)arg->params[0].u.value.b;
+ 	switch (arg->params[0].u.value.a) {
+-	case OPTEE_MSG_RPC_SHM_TYPE_APPL:
++	case OPTEE_RPC_SHM_TYPE_APPL:
+ 		cmd_free_suppl(ctx, shm);
+ 		break;
+-	case OPTEE_MSG_RPC_SHM_TYPE_KERNEL:
++	case OPTEE_RPC_SHM_TYPE_KERNEL:
+ 		tee_shm_free(shm);
+ 		break;
+ 	default:
+@@ -459,23 +460,23 @@ static void handle_rpc_func_cmd(struct tee_context *ctx, struct optee *optee,
+ 	}
+ 
+ 	switch (arg->cmd) {
+-	case OPTEE_MSG_RPC_CMD_GET_TIME:
++	case OPTEE_RPC_CMD_GET_TIME:
+ 		handle_rpc_func_cmd_get_time(arg);
+ 		break;
+-	case OPTEE_MSG_RPC_CMD_WAIT_QUEUE:
++	case OPTEE_RPC_CMD_WAIT_QUEUE:
+ 		handle_rpc_func_cmd_wq(optee, arg);
+ 		break;
+-	case OPTEE_MSG_RPC_CMD_SUSPEND:
++	case OPTEE_RPC_CMD_SUSPEND:
+ 		handle_rpc_func_cmd_wait(arg);
+ 		break;
+-	case OPTEE_MSG_RPC_CMD_SHM_ALLOC:
++	case OPTEE_RPC_CMD_SHM_ALLOC:
+ 		free_pages_list(call_ctx);
+ 		handle_rpc_func_cmd_shm_alloc(ctx, arg, call_ctx);
+ 		break;
+-	case OPTEE_MSG_RPC_CMD_SHM_FREE:
++	case OPTEE_RPC_CMD_SHM_FREE:
+ 		handle_rpc_func_cmd_shm_free(ctx, arg);
+ 		break;
+-	case OPTEE_MSG_RPC_CMD_I2C_TRANSFER:
++	case OPTEE_RPC_CMD_I2C_TRANSFER:
+ 		handle_rpc_func_cmd_i2c_transfer(ctx, arg);
+ 		break;
+ 	default:
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0012-optee-refactor-driver-with-internal-callbacks.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0012-optee-refactor-driver-with-internal-callbacks.patch
new file mode 100644
index 0000000..083843d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0012-optee-refactor-driver-with-internal-callbacks.patch
@@ -0,0 +1,721 @@
+From abda5d14075802b84fe9e38f77bfdc371606172c Mon Sep 17 00:00:00 2001
+From: Jens Wiklander <jens.wiklander@linaro.org>
+Date: Thu, 25 Mar 2021 15:08:50 +0100
+Subject: [PATCH 17/22] optee: refactor driver with internal callbacks
+
+The OP-TEE driver is refactored with three internal callbacks replacing
+direct calls to optee_from_msg_param(), optee_to_msg_param() and
+optee_do_call_with_arg().
+
+These functions a central to communicating with OP-TEE in secure world
+by using the SMC Calling Convention directly.
+
+This refactoring makes room for using other primitives to communicate
+with OP-TEE in secure world while being able to reuse as much as
+possible from the present driver.
+
+Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/tee/optee/call.c          |  86 +++++++++--------
+ drivers/tee/optee/core.c          | 148 ++++++++++++++++++++----------
+ drivers/tee/optee/optee_private.h |  35 +++++--
+ drivers/tee/optee/rpc.c           |  19 ++--
+ 4 files changed, 182 insertions(+), 106 deletions(-)
+
+diff --git a/drivers/tee/optee/call.c b/drivers/tee/optee/call.c
+index 1b339b743ff5..e7b93153252c 100644
+--- a/drivers/tee/optee/call.c
++++ b/drivers/tee/optee/call.c
+@@ -1,6 +1,6 @@
+ // SPDX-License-Identifier: GPL-2.0-only
+ /*
+- * Copyright (c) 2015, Linaro Limited
++ * Copyright (c) 2015-2021, Linaro Limited
+  */
+ #include <linux/arm-smccc.h>
+ #include <linux/device.h>
+@@ -116,20 +116,25 @@ static struct optee_session *find_session(struct optee_context_data *ctxdata,
+ /**
+  * optee_do_call_with_arg() - Do an SMC to OP-TEE in secure world
+  * @ctx:	calling context
+- * @parg:	physical address of message to pass to secure world
++ * @arg:	shared memory holding the message to pass to secure world
+  *
+  * Does and SMC to OP-TEE in secure world and handles eventual resulting
+  * Remote Procedure Calls (RPC) from OP-TEE.
+  *
+  * Returns return code from secure world, 0 is OK
+  */
+-u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg)
++int optee_do_call_with_arg(struct tee_context *ctx, struct tee_shm *arg)
+ {
+ 	struct optee *optee = tee_get_drvdata(ctx->teedev);
+ 	struct optee_call_waiter w;
+ 	struct optee_rpc_param param = { };
+ 	struct optee_call_ctx call_ctx = { };
+-	u32 ret;
++	phys_addr_t parg;
++	int rc;
++
++	rc = tee_shm_get_pa(arg, 0, &parg);
++	if (rc)
++		return rc;
+ 
+ 	param.a0 = OPTEE_SMC_CALL_WITH_ARG;
+ 	reg_pair_from_64(&param.a1, &param.a2, parg);
+@@ -157,7 +162,7 @@ u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg)
+ 			param.a3 = res.a3;
+ 			optee_handle_rpc(ctx, &param, &call_ctx);
+ 		} else {
+-			ret = res.a0;
++			rc = res.a0;
+ 			break;
+ 		}
+ 	}
+@@ -169,14 +174,12 @@ u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg)
+ 	 */
+ 	optee_cq_wait_final(&optee->call_queue, &w);
+ 
+-	return ret;
++	return rc;
+ }
+ 
+ static struct tee_shm *get_msg_arg(struct tee_context *ctx, size_t num_params,
+-				   struct optee_msg_arg **msg_arg,
+-				   phys_addr_t *msg_parg)
++				   struct optee_msg_arg **msg_arg)
+ {
+-	int rc;
+ 	struct tee_shm *shm;
+ 	struct optee_msg_arg *ma;
+ 
+@@ -187,22 +190,13 @@ static struct tee_shm *get_msg_arg(struct tee_context *ctx, size_t num_params,
+ 
+ 	ma = tee_shm_get_va(shm, 0);
+ 	if (IS_ERR(ma)) {
+-		rc = PTR_ERR(ma);
+-		goto out;
++		tee_shm_free(shm);
++		return (void *)ma;
+ 	}
+ 
+-	rc = tee_shm_get_pa(shm, 0, msg_parg);
+-	if (rc)
+-		goto out;
+-
+ 	memset(ma, 0, OPTEE_MSG_GET_ARG_SIZE(num_params));
+ 	ma->num_params = num_params;
+ 	*msg_arg = ma;
+-out:
+-	if (rc) {
+-		tee_shm_free(shm);
+-		return ERR_PTR(rc);
+-	}
+ 
+ 	return shm;
+ }
+@@ -211,16 +205,16 @@ int optee_open_session(struct tee_context *ctx,
+ 		       struct tee_ioctl_open_session_arg *arg,
+ 		       struct tee_param *param)
+ {
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
+ 	struct optee_context_data *ctxdata = ctx->data;
+ 	int rc;
+ 	struct tee_shm *shm;
+ 	struct optee_msg_arg *msg_arg;
+-	phys_addr_t msg_parg;
+ 	struct optee_session *sess = NULL;
+ 	uuid_t client_uuid;
+ 
+ 	/* +2 for the meta parameters added below */
+-	shm = get_msg_arg(ctx, arg->num_params + 2, &msg_arg, &msg_parg);
++	shm = get_msg_arg(ctx, arg->num_params + 2, &msg_arg);
+ 	if (IS_ERR(shm))
+ 		return PTR_ERR(shm);
+ 
+@@ -244,7 +238,8 @@ int optee_open_session(struct tee_context *ctx,
+ 		goto out;
+ 	export_uuid(msg_arg->params[1].u.octets, &client_uuid);
+ 
+-	rc = optee_to_msg_param(msg_arg->params + 2, arg->num_params, param);
++	rc = optee->ops->to_msg_param(optee, msg_arg->params + 2,
++				      arg->num_params, param);
+ 	if (rc)
+ 		goto out;
+ 
+@@ -254,7 +249,7 @@ int optee_open_session(struct tee_context *ctx,
+ 		goto out;
+ 	}
+ 
+-	if (optee_do_call_with_arg(ctx, msg_parg)) {
++	if (optee->ops->do_call_with_arg(ctx, shm)) {
+ 		msg_arg->ret = TEEC_ERROR_COMMUNICATION;
+ 		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
+ 	}
+@@ -269,7 +264,8 @@ int optee_open_session(struct tee_context *ctx,
+ 		kfree(sess);
+ 	}
+ 
+-	if (optee_from_msg_param(param, arg->num_params, msg_arg->params + 2)) {
++	if (optee->ops->from_msg_param(optee, param, arg->num_params,
++				       msg_arg->params + 2)) {
+ 		arg->ret = TEEC_ERROR_COMMUNICATION;
+ 		arg->ret_origin = TEEC_ORIGIN_COMMS;
+ 		/* Close session again to avoid leakage */
+@@ -288,16 +284,16 @@ int optee_open_session(struct tee_context *ctx,
+ int optee_close_session_helper(struct tee_context *ctx, u32 session)
+ {
+ 	struct tee_shm *shm;
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
+ 	struct optee_msg_arg *msg_arg;
+-	phys_addr_t msg_parg;
+ 
+-	shm = get_msg_arg(ctx, 0, &msg_arg, &msg_parg);
++	shm = get_msg_arg(ctx, 0, &msg_arg);
+ 	if (IS_ERR(shm))
+ 		return PTR_ERR(shm);
+ 
+ 	msg_arg->cmd = OPTEE_MSG_CMD_CLOSE_SESSION;
+ 	msg_arg->session = session;
+-	optee_do_call_with_arg(ctx, msg_parg);
++	optee->ops->do_call_with_arg(ctx, shm);
+ 
+ 	tee_shm_free(shm);
+ 
+@@ -325,10 +321,10 @@ int optee_close_session(struct tee_context *ctx, u32 session)
+ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
+ 		      struct tee_param *param)
+ {
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
+ 	struct optee_context_data *ctxdata = ctx->data;
+ 	struct tee_shm *shm;
+ 	struct optee_msg_arg *msg_arg;
+-	phys_addr_t msg_parg;
+ 	struct optee_session *sess;
+ 	int rc;
+ 
+@@ -339,7 +335,7 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
+ 	if (!sess)
+ 		return -EINVAL;
+ 
+-	shm = get_msg_arg(ctx, arg->num_params, &msg_arg, &msg_parg);
++	shm = get_msg_arg(ctx, arg->num_params, &msg_arg);
+ 	if (IS_ERR(shm))
+ 		return PTR_ERR(shm);
+ 	msg_arg->cmd = OPTEE_MSG_CMD_INVOKE_COMMAND;
+@@ -347,16 +343,18 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
+ 	msg_arg->session = arg->session;
+ 	msg_arg->cancel_id = arg->cancel_id;
+ 
+-	rc = optee_to_msg_param(msg_arg->params, arg->num_params, param);
++	rc = optee->ops->to_msg_param(optee, msg_arg->params, arg->num_params,
++				      param);
+ 	if (rc)
+ 		goto out;
+ 
+-	if (optee_do_call_with_arg(ctx, msg_parg)) {
++	if (optee->ops->do_call_with_arg(ctx, shm)) {
+ 		msg_arg->ret = TEEC_ERROR_COMMUNICATION;
+ 		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
+ 	}
+ 
+-	if (optee_from_msg_param(param, arg->num_params, msg_arg->params)) {
++	if (optee->ops->from_msg_param(optee, param, arg->num_params,
++				       msg_arg->params)) {
+ 		msg_arg->ret = TEEC_ERROR_COMMUNICATION;
+ 		msg_arg->ret_origin = TEEC_ORIGIN_COMMS;
+ 	}
+@@ -370,10 +368,10 @@ int optee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
+ 
+ int optee_cancel_req(struct tee_context *ctx, u32 cancel_id, u32 session)
+ {
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
+ 	struct optee_context_data *ctxdata = ctx->data;
+ 	struct tee_shm *shm;
+ 	struct optee_msg_arg *msg_arg;
+-	phys_addr_t msg_parg;
+ 	struct optee_session *sess;
+ 
+ 	/* Check that the session is valid */
+@@ -383,14 +381,14 @@ int optee_cancel_req(struct tee_context *ctx, u32 cancel_id, u32 session)
+ 	if (!sess)
+ 		return -EINVAL;
+ 
+-	shm = get_msg_arg(ctx, 0, &msg_arg, &msg_parg);
++	shm = get_msg_arg(ctx, 0, &msg_arg);
+ 	if (IS_ERR(shm))
+ 		return PTR_ERR(shm);
+ 
+ 	msg_arg->cmd = OPTEE_MSG_CMD_CANCEL;
+ 	msg_arg->session = session;
+ 	msg_arg->cancel_id = cancel_id;
+-	optee_do_call_with_arg(ctx, msg_parg);
++	optee->ops->do_call_with_arg(ctx, shm);
+ 
+ 	tee_shm_free(shm);
+ 	return 0;
+@@ -589,10 +587,10 @@ int optee_shm_register(struct tee_context *ctx, struct tee_shm *shm,
+ 		       struct page **pages, size_t num_pages,
+ 		       unsigned long start)
+ {
+-	struct tee_shm *shm_arg = NULL;
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
+ 	struct optee_msg_arg *msg_arg;
++	struct tee_shm *shm_arg;
+ 	u64 *pages_list;
+-	phys_addr_t msg_parg;
+ 	int rc;
+ 
+ 	if (!num_pages)
+@@ -606,7 +604,7 @@ int optee_shm_register(struct tee_context *ctx, struct tee_shm *shm,
+ 	if (!pages_list)
+ 		return -ENOMEM;
+ 
+-	shm_arg = get_msg_arg(ctx, 1, &msg_arg, &msg_parg);
++	shm_arg = get_msg_arg(ctx, 1, &msg_arg);
+ 	if (IS_ERR(shm_arg)) {
+ 		rc = PTR_ERR(shm_arg);
+ 		goto out;
+@@ -627,7 +625,7 @@ int optee_shm_register(struct tee_context *ctx, struct tee_shm *shm,
+ 	msg_arg->params->u.tmem.buf_ptr = virt_to_phys(pages_list) |
+ 	  (tee_shm_get_page_offset(shm) & (OPTEE_MSG_NONCONTIG_PAGE_SIZE - 1));
+ 
+-	if (optee_do_call_with_arg(ctx, msg_parg) ||
++	if (optee->ops->do_call_with_arg(ctx, shm) ||
+ 	    msg_arg->ret != TEEC_SUCCESS)
+ 		rc = -EINVAL;
+ 
+@@ -639,12 +637,12 @@ int optee_shm_register(struct tee_context *ctx, struct tee_shm *shm,
+ 
+ int optee_shm_unregister(struct tee_context *ctx, struct tee_shm *shm)
+ {
+-	struct tee_shm *shm_arg;
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
+ 	struct optee_msg_arg *msg_arg;
+-	phys_addr_t msg_parg;
++	struct tee_shm *shm_arg;
+ 	int rc = 0;
+ 
+-	shm_arg = get_msg_arg(ctx, 1, &msg_arg, &msg_parg);
++	shm_arg = get_msg_arg(ctx, 1, &msg_arg);
+ 	if (IS_ERR(shm_arg))
+ 		return PTR_ERR(shm_arg);
+ 
+@@ -653,7 +651,7 @@ int optee_shm_unregister(struct tee_context *ctx, struct tee_shm *shm)
+ 	msg_arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_RMEM_INPUT;
+ 	msg_arg->params[0].u.rmem.shm_ref = (unsigned long)shm;
+ 
+-	if (optee_do_call_with_arg(ctx, msg_parg) ||
++	if (optee->ops->do_call_with_arg(ctx, shm) ||
+ 	    msg_arg->ret != TEEC_SUCCESS)
+ 		rc = -EINVAL;
+ 	tee_shm_free(shm_arg);
+diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
+index e39c6d290d83..ab602bb8e14a 100644
+--- a/drivers/tee/optee/core.c
++++ b/drivers/tee/optee/core.c
+@@ -1,6 +1,6 @@
+ // SPDX-License-Identifier: GPL-2.0-only
+ /*
+- * Copyright (c) 2015, Linaro Limited
++ * Copyright (c) 2015-2021, Linaro Limited
+  */
+ 
+ #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+@@ -26,21 +26,87 @@
+ 
+ #define OPTEE_SHM_NUM_PRIV_PAGES	CONFIG_OPTEE_SHM_NUM_PRIV_PAGES
+ 
++static void from_msg_param_value(struct tee_param *p, u32 attr,
++				 const struct optee_msg_param *mp)
++{
++	p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT +
++		  attr - OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
++	p->u.value.a = mp->u.value.a;
++	p->u.value.b = mp->u.value.b;
++	p->u.value.c = mp->u.value.c;
++}
++
++static int from_msg_param_tmp_mem(struct tee_param *p, u32 attr,
++				  const struct optee_msg_param *mp)
++{
++	struct tee_shm *shm;
++	phys_addr_t pa;
++	int rc;
++
++	p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT +
++		  attr - OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
++	p->u.memref.size = mp->u.tmem.size;
++	shm = (struct tee_shm *)(unsigned long)mp->u.tmem.shm_ref;
++	if (!shm) {
++		p->u.memref.shm_offs = 0;
++		p->u.memref.shm = NULL;
++		return 0;
++	}
++
++	rc = tee_shm_get_pa(shm, 0, &pa);
++	if (rc)
++		return rc;
++
++	p->u.memref.shm_offs = mp->u.tmem.buf_ptr - pa;
++	p->u.memref.shm = shm;
++
++	/* Check that the memref is covered by the shm object */
++	if (p->u.memref.size) {
++		size_t o = p->u.memref.shm_offs +
++			   p->u.memref.size - 1;
++
++		rc = tee_shm_get_pa(shm, o, NULL);
++		if (rc)
++			return rc;
++	}
++
++	return 0;
++}
++
++static void from_msg_param_reg_mem(struct tee_param *p, u32 attr,
++				   const struct optee_msg_param *mp)
++{
++	struct tee_shm *shm;
++
++	p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT +
++		  attr - OPTEE_MSG_ATTR_TYPE_RMEM_INPUT;
++	p->u.memref.size = mp->u.rmem.size;
++	shm = (struct tee_shm *)(unsigned long)mp->u.rmem.shm_ref;
++
++	if (shm) {
++		p->u.memref.shm_offs = mp->u.rmem.offs;
++		p->u.memref.shm = shm;
++	} else {
++		p->u.memref.shm_offs = 0;
++		p->u.memref.shm = NULL;
++	}
++}
++
+ /**
+  * optee_from_msg_param() - convert from OPTEE_MSG parameters to
+  *			    struct tee_param
++ * @optee:	main service struct
+  * @params:	subsystem internal parameter representation
+  * @num_params:	number of elements in the parameter arrays
+  * @msg_params:	OPTEE_MSG parameters
+  * Returns 0 on success or <0 on failure
+  */
+-int optee_from_msg_param(struct tee_param *params, size_t num_params,
+-			 const struct optee_msg_param *msg_params)
++static int optee_from_msg_param(struct optee *optee, struct tee_param *params,
++				size_t num_params,
++				const struct optee_msg_param *msg_params)
+ {
+ 	int rc;
+ 	size_t n;
+-	struct tee_shm *shm;
+-	phys_addr_t pa;
+ 
+ 	for (n = 0; n < num_params; n++) {
+ 		struct tee_param *p = params + n;
+@@ -55,48 +121,19 @@ int optee_from_msg_param(struct tee_param *params, size_t num_params,
+ 		case OPTEE_MSG_ATTR_TYPE_VALUE_INPUT:
+ 		case OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT:
+ 		case OPTEE_MSG_ATTR_TYPE_VALUE_INOUT:
+-			p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT +
+-				  attr - OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+-			p->u.value.a = mp->u.value.a;
+-			p->u.value.b = mp->u.value.b;
+-			p->u.value.c = mp->u.value.c;
++			from_msg_param_value(p, attr, mp);
+ 			break;
+ 		case OPTEE_MSG_ATTR_TYPE_TMEM_INPUT:
+ 		case OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT:
+ 		case OPTEE_MSG_ATTR_TYPE_TMEM_INOUT:
+-			p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT +
+-				  attr - OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+-			p->u.memref.size = mp->u.tmem.size;
+-			shm = (struct tee_shm *)(unsigned long)
+-				mp->u.tmem.shm_ref;
+-			if (!shm) {
+-				p->u.memref.shm_offs = 0;
+-				p->u.memref.shm = NULL;
+-				break;
+-			}
+-			rc = tee_shm_get_pa(shm, 0, &pa);
++			rc = from_msg_param_tmp_mem(p, attr, mp);
+ 			if (rc)
+ 				return rc;
+-			p->u.memref.shm_offs = mp->u.tmem.buf_ptr - pa;
+-			p->u.memref.shm = shm;
+ 			break;
+ 		case OPTEE_MSG_ATTR_TYPE_RMEM_INPUT:
+ 		case OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT:
+ 		case OPTEE_MSG_ATTR_TYPE_RMEM_INOUT:
+-			p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT +
+-				  attr - OPTEE_MSG_ATTR_TYPE_RMEM_INPUT;
+-			p->u.memref.size = mp->u.rmem.size;
+-			shm = (struct tee_shm *)(unsigned long)
+-				mp->u.rmem.shm_ref;
+-
+-			if (!shm) {
+-				p->u.memref.shm_offs = 0;
+-				p->u.memref.shm = NULL;
+-				break;
+-			}
+-			p->u.memref.shm_offs = mp->u.rmem.offs;
+-			p->u.memref.shm = shm;
+-
++			from_msg_param_reg_mem(p, attr, mp);
+ 			break;
+ 
+ 		default:
+@@ -106,6 +143,16 @@ int optee_from_msg_param(struct tee_param *params, size_t num_params,
+ 	return 0;
+ }
+ 
++static void to_msg_param_value(struct optee_msg_param *mp,
++			       const struct tee_param *p)
++{
++	mp->attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT + p->attr -
++		   TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT;
++	mp->u.value.a = p->u.value.a;
++	mp->u.value.b = p->u.value.b;
++	mp->u.value.c = p->u.value.c;
++}
++
+ static int to_msg_param_tmp_mem(struct optee_msg_param *mp,
+ 				const struct tee_param *p)
+ {
+@@ -148,13 +195,15 @@ static int to_msg_param_reg_mem(struct optee_msg_param *mp,
+ 
+ /**
+  * optee_to_msg_param() - convert from struct tee_params to OPTEE_MSG parameters
++ * @optee:	main service struct
+  * @msg_params:	OPTEE_MSG parameters
+  * @num_params:	number of elements in the parameter arrays
+  * @params:	subsystem itnernal parameter representation
+  * Returns 0 on success or <0 on failure
+  */
+-int optee_to_msg_param(struct optee_msg_param *msg_params, size_t num_params,
+-		       const struct tee_param *params)
++static int optee_to_msg_param(struct optee *optee,
++			      struct optee_msg_param *msg_params,
++			      size_t num_params, const struct tee_param *params)
+ {
+ 	int rc;
+ 	size_t n;
+@@ -171,11 +220,7 @@ int optee_to_msg_param(struct optee_msg_param *msg_params, size_t num_params,
+ 		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT:
+ 		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_OUTPUT:
+ 		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT:
+-			mp->attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT + p->attr -
+-				   TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT;
+-			mp->u.value.a = p->u.value.a;
+-			mp->u.value.b = p->u.value.b;
+-			mp->u.value.c = p->u.value.c;
++			to_msg_param_value(mp, p);
+ 			break;
+ 		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT:
+ 		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_OUTPUT:
+@@ -301,7 +346,7 @@ static void optee_release_supp(struct tee_context *ctx)
+ 	optee_supp_release(&optee->supp);
+ }
+ 
+-static const struct tee_driver_ops optee_ops = {
++static const struct tee_driver_ops optee_clnt_ops = {
+ 	.get_version = optee_get_version,
+ 	.open = optee_open,
+ 	.release = optee_release,
+@@ -313,9 +358,9 @@ static const struct tee_driver_ops optee_ops = {
+ 	.shm_unregister = optee_shm_unregister,
+ };
+ 
+-static const struct tee_desc optee_desc = {
++static const struct tee_desc optee_clnt_desc = {
+ 	.name = DRIVER_NAME "-clnt",
+-	.ops = &optee_ops,
++	.ops = &optee_clnt_ops,
+ 	.owner = THIS_MODULE,
+ };
+ 
+@@ -336,6 +381,12 @@ static const struct tee_desc optee_supp_desc = {
+ 	.flags = TEE_DESC_PRIVILEGED,
+ };
+ 
++static const struct optee_ops optee_ops = {
++	.do_call_with_arg = optee_do_call_with_arg,
++	.to_msg_param = optee_to_msg_param,
++	.from_msg_param = optee_from_msg_param,
++};
++
+ static bool optee_msg_api_uid_is_optee_api(optee_invoke_fn *invoke_fn)
+ {
+ 	struct arm_smccc_res res;
+@@ -637,10 +688,11 @@ static int optee_probe(struct platform_device *pdev)
+ 		goto err;
+ 	}
+ 
++	optee->ops = &optee_ops;
+ 	optee->invoke_fn = invoke_fn;
+ 	optee->sec_caps = sec_caps;
+ 
+-	teedev = tee_device_alloc(&optee_desc, NULL, pool, optee);
++	teedev = tee_device_alloc(&optee_clnt_desc, NULL, pool, optee);
+ 	if (IS_ERR(teedev)) {
+ 		rc = PTR_ERR(teedev);
+ 		goto err;
+diff --git a/drivers/tee/optee/optee_private.h b/drivers/tee/optee/optee_private.h
+index 2b63b796645e..c5741e96e967 100644
+--- a/drivers/tee/optee/optee_private.h
++++ b/drivers/tee/optee/optee_private.h
+@@ -1,6 +1,6 @@
+ /* SPDX-License-Identifier: GPL-2.0-only */
+ /*
+- * Copyright (c) 2015, Linaro Limited
++ * Copyright (c) 2015-2021, Linaro Limited
+  */
+ 
+ #ifndef OPTEE_PRIVATE_H
+@@ -66,9 +66,34 @@ struct optee_supp {
+ 	struct completion reqs_c;
+ };
+ 
++struct optee;
++
++/**
++ * struct optee_ops - OP-TEE driver internal operations
++ * @do_call_with_arg:	enters OP-TEE in secure world
++ * @to_msg_param:	converts from struct tee_param to OPTEE_MSG parameters
++ * @from_msg_param:	converts from OPTEE_MSG parameters to struct tee_param
++ *
++ * These OPs are only supposed to be used internally in the OP-TEE driver
++ * as a way of abstracting the different methogs of entering OP-TEE in
++ * secure world.
++ */
++struct optee_ops {
++	int (*do_call_with_arg)(struct tee_context *ctx,
++				struct tee_shm *shm_arg);
++	int (*to_msg_param)(struct optee *optee,
++			    struct optee_msg_param *msg_params,
++			    size_t num_params, const struct tee_param *params);
++	int (*from_msg_param)(struct optee *optee, struct tee_param *params,
++			      size_t num_params,
++			      const struct optee_msg_param *msg_params);
++};
++
+ /**
+  * struct optee - main service struct
+  * @supp_teedev:	supplicant device
++ * @ops:		internal callbacks for different ways to reach secure
++ *			world
+  * @teedev:		client device
+  * @invoke_fn:		function to issue smc or hvc
+  * @call_queue:		queue of threads waiting to call @invoke_fn
+@@ -86,6 +111,7 @@ struct optee_supp {
+ struct optee {
+ 	struct tee_device *supp_teedev;
+ 	struct tee_device *teedev;
++	const struct optee_ops *ops;
+ 	optee_invoke_fn *invoke_fn;
+ 	struct optee_call_queue call_queue;
+ 	struct optee_wait_queue wait_queue;
+@@ -148,7 +174,7 @@ int optee_supp_recv(struct tee_context *ctx, u32 *func, u32 *num_params,
+ int optee_supp_send(struct tee_context *ctx, u32 ret, u32 num_params,
+ 		    struct tee_param *param);
+ 
+-u32 optee_do_call_with_arg(struct tee_context *ctx, phys_addr_t parg);
++int optee_do_call_with_arg(struct tee_context *ctx, struct tee_shm *arg);
+ int optee_open_session(struct tee_context *ctx,
+ 		       struct tee_ioctl_open_session_arg *arg,
+ 		       struct tee_param *param);
+@@ -171,11 +197,6 @@ int optee_shm_register_supp(struct tee_context *ctx, struct tee_shm *shm,
+ 			    unsigned long start);
+ int optee_shm_unregister_supp(struct tee_context *ctx, struct tee_shm *shm);
+ 
+-int optee_from_msg_param(struct tee_param *params, size_t num_params,
+-			 const struct optee_msg_param *msg_params);
+-int optee_to_msg_param(struct optee_msg_param *msg_params, size_t num_params,
+-		       const struct tee_param *params);
+-
+ u64 *optee_allocate_pages_list(size_t num_entries);
+ void optee_free_pages_list(void *array, size_t num_entries);
+ void optee_fill_pages_list(u64 *dst, struct page **pages, int num_pages,
+diff --git a/drivers/tee/optee/rpc.c b/drivers/tee/optee/rpc.c
+index 1849180b0278..39562fb6841e 100644
+--- a/drivers/tee/optee/rpc.c
++++ b/drivers/tee/optee/rpc.c
+@@ -1,6 +1,6 @@
+ // SPDX-License-Identifier: GPL-2.0-only
+ /*
+- * Copyright (c) 2015-2016, Linaro Limited
++ * Copyright (c) 2015-2021, Linaro Limited
+  */
+ 
+ #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+@@ -55,6 +55,7 @@ static void handle_rpc_func_cmd_get_time(struct optee_msg_arg *arg)
+ static void handle_rpc_func_cmd_i2c_transfer(struct tee_context *ctx,
+ 					     struct optee_msg_arg *arg)
+ {
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
+ 	struct tee_param *params;
+ 	struct i2c_adapter *adapter;
+ 	struct i2c_msg msg = { };
+@@ -79,7 +80,8 @@ static void handle_rpc_func_cmd_i2c_transfer(struct tee_context *ctx,
+ 		return;
+ 	}
+ 
+-	if (optee_from_msg_param(params, arg->num_params, arg->params))
++	if (optee->ops->from_msg_param(optee, params, arg->num_params,
++				       arg->params))
+ 		goto bad;
+ 
+ 	for (i = 0; i < arg->num_params; i++) {
+@@ -122,7 +124,8 @@ static void handle_rpc_func_cmd_i2c_transfer(struct tee_context *ctx,
+ 		arg->ret = TEEC_ERROR_COMMUNICATION;
+ 	} else {
+ 		params[3].u.value.a = msg.len;
+-		if (optee_to_msg_param(arg->params, arg->num_params, params))
++		if (optee->ops->to_msg_param(optee, arg->params,
++					     arg->num_params, params))
+ 			arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+ 		else
+ 			arg->ret = TEEC_SUCCESS;
+@@ -234,7 +237,7 @@ static void handle_rpc_func_cmd_wait(struct optee_msg_arg *arg)
+ 	arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+ }
+ 
+-static void handle_rpc_supp_cmd(struct tee_context *ctx,
++static void handle_rpc_supp_cmd(struct tee_context *ctx, struct optee *optee,
+ 				struct optee_msg_arg *arg)
+ {
+ 	struct tee_param *params;
+@@ -248,14 +251,16 @@ static void handle_rpc_supp_cmd(struct tee_context *ctx,
+ 		return;
+ 	}
+ 
+-	if (optee_from_msg_param(params, arg->num_params, arg->params)) {
++	if (optee->ops->from_msg_param(optee, params, arg->num_params,
++				       arg->params)) {
+ 		arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+ 		goto out;
+ 	}
+ 
+ 	arg->ret = optee_supp_thrd_req(ctx, arg->cmd, arg->num_params, params);
+ 
+-	if (optee_to_msg_param(arg->params, arg->num_params, params))
++	if (optee->ops->to_msg_param(optee, arg->params, arg->num_params,
++				     params))
+ 		arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+ out:
+ 	kfree(params);
+@@ -480,7 +485,7 @@ static void handle_rpc_func_cmd(struct tee_context *ctx, struct optee *optee,
+ 		handle_rpc_func_cmd_i2c_transfer(ctx, arg);
+ 		break;
+ 	default:
+-		handle_rpc_supp_cmd(ctx, arg);
++		handle_rpc_supp_cmd(ctx, optee, arg);
+ 	}
+ }
+ 
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0013-optee-add-a-FF-A-memory-pool.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0013-optee-add-a-FF-A-memory-pool.patch
new file mode 100644
index 0000000..6be1581
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0013-optee-add-a-FF-A-memory-pool.patch
@@ -0,0 +1,131 @@
+From eafffa586795e3cb485310fbd287322c9c7dc3bb Mon Sep 17 00:00:00 2001
+From: Jens Wiklander <jens.wiklander@linaro.org>
+Date: Thu, 25 Mar 2021 15:08:52 +0100
+Subject: [PATCH 18/22] optee: add a FF-A memory pool
+
+Adds a memory pool to be used when the driver uses FF-A [1] as transport
+layer.
+
+[1] https://developer.arm.com/documentation/den0077/latest
+Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/tee/optee/shm_pool.c | 65 +++++++++++++++++++++++++++++++++---
+ drivers/tee/optee/shm_pool.h |  1 +
+ 2 files changed, 61 insertions(+), 5 deletions(-)
+
+diff --git a/drivers/tee/optee/shm_pool.c b/drivers/tee/optee/shm_pool.c
+index d767eebf30bd..d2116cb39c8b 100644
+--- a/drivers/tee/optee/shm_pool.c
++++ b/drivers/tee/optee/shm_pool.c
+@@ -12,8 +12,14 @@
+ #include "optee_smc.h"
+ #include "shm_pool.h"
+ 
+-static int pool_op_alloc(struct tee_shm_pool_mgr *poolm,
+-			 struct tee_shm *shm, size_t size)
++static int
++pool_op_alloc_helper(struct tee_shm_pool_mgr *poolm,
++		     struct tee_shm *shm, size_t size,
++		     int (*shm_register)(struct tee_context *ctx,
++					 struct tee_shm *shm,
++					 struct page **pages,
++					 size_t num_pages,
++					 unsigned long start))
+ {
+ 	unsigned int order = get_order(size);
+ 	struct page *page;
+@@ -27,7 +33,7 @@ static int pool_op_alloc(struct tee_shm_pool_mgr *poolm,
+ 	shm->paddr = page_to_phys(page);
+ 	shm->size = PAGE_SIZE << order;
+ 
+-	if (shm->flags & TEE_SHM_DMA_BUF) {
++	if (shm_register) {
+ 		unsigned int nr_pages = 1 << order, i;
+ 		struct page **pages;
+ 
+@@ -41,14 +47,23 @@ static int pool_op_alloc(struct tee_shm_pool_mgr *poolm,
+ 		}
+ 
+ 		shm->flags |= TEE_SHM_REGISTER;
+-		rc = optee_shm_register(shm->ctx, shm, pages, nr_pages,
+-					(unsigned long)shm->kaddr);
++		rc = shm_register(shm->ctx, shm, pages, nr_pages,
++				  (unsigned long)shm->kaddr);
+ 		kfree(pages);
+ 	}
+ 
+ 	return rc;
+ }
+ 
++static int pool_op_alloc(struct tee_shm_pool_mgr *poolm,
++			 struct tee_shm *shm, size_t size)
++{
++	if (!(shm->flags & TEE_SHM_DMA_BUF))
++		return pool_op_alloc_helper(poolm, shm, size, NULL);
++
++	return pool_op_alloc_helper(poolm, shm, size, optee_shm_register);
++}
++
+ static void pool_op_free(struct tee_shm_pool_mgr *poolm,
+ 			 struct tee_shm *shm)
+ {
+@@ -87,3 +102,43 @@ struct tee_shm_pool_mgr *optee_shm_pool_alloc_pages(void)
+ 
+ 	return mgr;
+ }
++
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++static int pool_ffa_op_alloc(struct tee_shm_pool_mgr *poolm,
++			     struct tee_shm *shm, size_t size)
++{
++	return pool_op_alloc_helper(poolm, shm, size, optee_ffa_shm_register);
++}
++
++static void pool_ffa_op_free(struct tee_shm_pool_mgr *poolm,
++			     struct tee_shm *shm)
++{
++	optee_ffa_shm_unregister(shm->ctx, shm);
++	free_pages((unsigned long)shm->kaddr, get_order(shm->size));
++	shm->kaddr = NULL;
++}
++
++static const struct tee_shm_pool_mgr_ops pool_ffa_ops = {
++	.alloc = pool_ffa_op_alloc,
++	.free = pool_ffa_op_free,
++	.destroy_poolmgr = pool_op_destroy_poolmgr,
++};
++
++/**
++ * optee_ffa_shm_pool_alloc_pages() - create page-based allocator pool
++ *
++ * This pool is used with OP-TEE over FF-A. In this case command buffers
++ * and such are allocated from kernel's own memory.
++ */
++struct tee_shm_pool_mgr *optee_ffa_shm_pool_alloc_pages(void)
++{
++	struct tee_shm_pool_mgr *mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
++
++	if (!mgr)
++		return ERR_PTR(-ENOMEM);
++
++	mgr->ops = &pool_ffa_ops;
++
++	return mgr;
++}
++#endif /*CONFIG_ARM_FFA_TRANSPORT*/
+diff --git a/drivers/tee/optee/shm_pool.h b/drivers/tee/optee/shm_pool.h
+index 28109d991c4b..34c5fd74a3ff 100644
+--- a/drivers/tee/optee/shm_pool.h
++++ b/drivers/tee/optee/shm_pool.h
+@@ -10,5 +10,6 @@
+ #include <linux/tee_drv.h>
+ 
+ struct tee_shm_pool_mgr *optee_shm_pool_alloc_pages(void);
++struct tee_shm_pool_mgr *optee_ffa_shm_pool_alloc_pages(void);
+ 
+ #endif
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0014-optee-add-FF-A-support.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0014-optee-add-FF-A-support.patch
new file mode 100644
index 0000000..faf6cd0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0014-optee-add-FF-A-support.patch
@@ -0,0 +1,1270 @@
+From 5665ffd3fb8e94003abc1c0c05c9fa30d4028b67 Mon Sep 17 00:00:00 2001
+From: Jens Wiklander <jens.wiklander@linaro.org>
+Date: Thu, 25 Mar 2021 15:08:53 +0100
+Subject: [PATCH 19/22] optee: add FF-A support
+
+Adds support for using FF-A [1] as transport to the OP-TEE driver.
+
+Introduces struct optee_msg_param_fmem which carries all information
+needed when OP-TEE is calling FFA_MEM_RETRIEVE_REQ to get the shared
+memory reference mapped by the hypervisor in S-EL2. Register usage is
+also updated to include the information needed.
+
+The FF-A part of this driver is enabled if CONFIG_ARM_FFA_TRANSPORT is
+enabled.
+
+[1] https://developer.arm.com/documentation/den0077/latest
+Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/tee/optee/call.c          | 212 ++++++++++++-
+ drivers/tee/optee/core.c          | 486 +++++++++++++++++++++++++++++-
+ drivers/tee/optee/optee_ffa.h     | 153 ++++++++++
+ drivers/tee/optee/optee_msg.h     |  27 +-
+ drivers/tee/optee/optee_private.h |  52 ++++
+ drivers/tee/optee/rpc.c           | 118 ++++++++
+ 6 files changed, 1040 insertions(+), 8 deletions(-)
+ create mode 100644 drivers/tee/optee/optee_ffa.h
+
+diff --git a/drivers/tee/optee/call.c b/drivers/tee/optee/call.c
+index e7b93153252c..cf91a81a242a 100644
+--- a/drivers/tee/optee/call.c
++++ b/drivers/tee/optee/call.c
+@@ -3,15 +3,18 @@
+  * Copyright (c) 2015-2021, Linaro Limited
+  */
+ #include <linux/arm-smccc.h>
++#include <linux/arm_ffa.h>
+ #include <linux/device.h>
+ #include <linux/err.h>
+ #include <linux/errno.h>
+ #include <linux/mm.h>
+ #include <linux/sched.h>
++#include <linux/scatterlist.h>
+ #include <linux/slab.h>
+ #include <linux/tee_drv.h>
+ #include <linux/types.h>
+ #include <linux/uaccess.h>
++#include "optee_ffa.h"
+ #include "optee_private.h"
+ #include "optee_smc.h"
+ 
+@@ -180,11 +183,21 @@ int optee_do_call_with_arg(struct tee_context *ctx, struct tee_shm *arg)
+ static struct tee_shm *get_msg_arg(struct tee_context *ctx, size_t num_params,
+ 				   struct optee_msg_arg **msg_arg)
+ {
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
++	size_t sz = OPTEE_MSG_GET_ARG_SIZE(num_params);
+ 	struct tee_shm *shm;
+ 	struct optee_msg_arg *ma;
+ 
+-	shm = tee_shm_alloc(ctx, OPTEE_MSG_GET_ARG_SIZE(num_params),
+-			    TEE_SHM_MAPPED);
++	/*
++	 * rpc_arg_count is set to the number of allocated parameters in
++	 * the RPC argument struct if a second MSG arg struct is expected.
++	 * The second arg struct will then be used for RPC. So far only
++	 * enabled when using FF-A as transport layer.
++	 */
++	if (optee->rpc_arg_count)
++		sz += OPTEE_MSG_GET_ARG_SIZE(optee->rpc_arg_count);
++
++	shm = tee_shm_alloc(ctx, sz, TEE_SHM_MAPPED);
+ 	if (IS_ERR(shm))
+ 		return shm;
+ 
+@@ -673,3 +686,198 @@ int optee_shm_unregister_supp(struct tee_context *ctx, struct tee_shm *shm)
+ {
+ 	return 0;
+ }
++
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++static int optee_ffa_yielding_call(struct tee_context *ctx,
++				   struct ffa_send_direct_data *data,
++				   struct optee_msg_arg *rpc_arg)
++{
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
++	const struct ffa_dev_ops *ffa_ops = optee->ffa.ffa_ops;
++	struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
++	struct optee_call_waiter w;
++	u32 cmd = data->data0;
++	u32 w4 = data->data1;
++	u32 w5 = data->data2;
++	u32 w6 = data->data3;
++	int rc;
++
++	/* Initialize waiter */
++	optee_cq_wait_init(&optee->call_queue, &w);
++	while (true) {
++		rc = ffa_ops->sync_send_receive(ffa_dev, data);
++		if (rc)
++			goto done;
++
++		switch ((int)data->data0) {
++		case TEEC_SUCCESS:
++			break;
++		case TEEC_ERROR_BUSY:
++			if (cmd == OPTEE_FFA_YIELDING_CALL_RESUME) {
++				rc = -EIO;
++				goto done;
++			}
++
++			/*
++			 * Out of threads in secure world, wait for a thread
++			 * become available.
++			 */
++			optee_cq_wait_for_completion(&optee->call_queue, &w);
++			data->data0 = cmd;
++			data->data1 = w4;
++			data->data2 = w5;
++			data->data3 = w6;
++			continue;
++		default:
++			rc = -EIO;
++			goto done;
++		}
++
++		if (data->data1 == OPTEE_FFA_YIELDING_CALL_RETURN_DONE)
++			goto done;
++
++		/*
++		 * OP-TEE has returned with a RPC request.
++		 *
++		 * Note that data->data4 (passed in register w7) is already
++		 * filled in by ffa_ops->sync_send_receive() returning
++		 * above.
++		 */
++		cond_resched();
++		optee_handle_ffa_rpc(ctx, data->data1, rpc_arg);
++		cmd = OPTEE_FFA_YIELDING_CALL_RESUME;
++		data->data0 = cmd;
++		data->data1 = 0;
++		data->data2 = 0;
++		data->data3 = 0;
++	}
++done:
++	/*
++	 * We're done with our thread in secure world, if there's any
++	 * thread waiters wake up one.
++	 */
++	optee_cq_wait_final(&optee->call_queue, &w);
++
++	return rc;
++}
++
++/**
++ * optee_ffa_do_call_with_arg() - Do a FF-A call to enter OP-TEE in secure world
++ * @ctx:	calling context
++ * @shm:	shared memory holding the message to pass to secure world
++ *
++ * Does a FF-A call to OP-TEE in secure world and handles eventual resulting
++ * Remote Procedure Calls (RPC) from OP-TEE.
++ *
++ * Returns return code from FF-A, 0 is OK
++ */
++
++int optee_ffa_do_call_with_arg(struct tee_context *ctx, struct tee_shm *shm)
++{
++	struct ffa_send_direct_data data = {
++		.data0 = OPTEE_FFA_YIELDING_CALL_WITH_ARG,
++		.data1 = (u32)shm->sec_world_id,
++		.data2 = (u32)(shm->sec_world_id >> 32),
++		.data3 = shm->offset,
++	};
++	struct optee_msg_arg *arg = tee_shm_get_va(shm, 0);
++	unsigned int rpc_arg_offs = OPTEE_MSG_GET_ARG_SIZE(arg->num_params);
++	struct optee_msg_arg *rpc_arg = tee_shm_get_va(shm, rpc_arg_offs);
++
++	return optee_ffa_yielding_call(ctx, &data, rpc_arg);
++}
++
++int optee_ffa_shm_register(struct tee_context *ctx, struct tee_shm *shm,
++			   struct page **pages, size_t num_pages,
++			   unsigned long start)
++{
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
++	const struct ffa_dev_ops *ffa_ops = optee->ffa.ffa_ops;
++	struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
++	struct ffa_mem_region_attributes mem_attr = {
++		.receiver = ffa_dev->vm_id,
++		.attrs = FFA_MEM_RW,
++	};
++	struct ffa_mem_ops_args args = {
++		.use_txbuf = true,
++		.attrs = &mem_attr,
++		.nattrs = 1,
++	};
++	struct sg_table sgt;
++	int rc;
++
++	rc = check_mem_type(start, num_pages);
++	if (rc)
++		return rc;
++
++	rc = sg_alloc_table_from_pages(&sgt, pages, num_pages, 0,
++				       num_pages * PAGE_SIZE, GFP_KERNEL);
++	if (rc)
++		return rc;
++	args.sg = sgt.sgl;
++	rc = ffa_ops->memory_share(ffa_dev, &args);
++	sg_free_table(&sgt);
++	if (rc)
++		return rc;
++
++	rc = optee_shm_add_ffa_handle(optee, shm, args.g_handle);
++	if (rc) {
++		ffa_ops->memory_reclaim(args.g_handle, 0);
++		return rc;
++	}
++
++	shm->sec_world_id = args.g_handle;
++
++	return 0;
++}
++
++int optee_ffa_shm_unregister(struct tee_context *ctx, struct tee_shm *shm)
++{
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
++	const struct ffa_dev_ops *ffa_ops = optee->ffa.ffa_ops;
++	struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
++	u64 global_handle = shm->sec_world_id;
++	struct ffa_send_direct_data data = {
++		.data0 = OPTEE_FFA_UNREGISTER_SHM,
++		.data1 = (u32)global_handle,
++		.data2 = (u32)(global_handle >> 32)
++	};
++	int rc;
++
++	optee_shm_rem_ffa_handle(optee, global_handle);
++	shm->sec_world_id = 0;
++
++	rc = ffa_ops->sync_send_receive(ffa_dev, &data);
++	if (rc)
++		pr_err("Unregister SHM id 0x%llx rc %d\n", global_handle, rc);
++
++	rc = ffa_ops->memory_reclaim(global_handle, 0);
++	if (rc)
++		pr_err("mem_reclain: 0x%llx %d", global_handle, rc);
++
++	return rc;
++}
++
++int optee_ffa_shm_unregister_supp(struct tee_context *ctx, struct tee_shm *shm)
++{
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
++	const struct ffa_dev_ops *ffa_ops = optee->ffa.ffa_ops;
++	u64 global_handle = shm->sec_world_id;
++	int rc;
++
++	/*
++	 * We're skipping the OPTEE_FFA_YIELDING_CALL_UNREGISTER_SHM call
++	 * since this is OP-TEE freeing via RPC so it has already retired
++	 * this ID.
++	 */
++
++	optee_shm_rem_ffa_handle(optee, global_handle);
++	rc = ffa_ops->memory_reclaim(global_handle, 0);
++	if (rc)
++		pr_err("mem_reclain: 0x%llx %d", global_handle, rc);
++
++	shm->sec_world_id = 0;
++
++	return rc;
++}
++#endif /*CONFIG_ARM_FFA_TRANSPORT*/
+diff --git a/drivers/tee/optee/core.c b/drivers/tee/optee/core.c
+index ab602bb8e14a..b9719c60dc48 100644
+--- a/drivers/tee/optee/core.c
++++ b/drivers/tee/optee/core.c
+@@ -6,6 +6,7 @@
+ #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+ 
+ #include <linux/arm-smccc.h>
++#include <linux/arm_ffa.h>
+ #include <linux/errno.h>
+ #include <linux/io.h>
+ #include <linux/module.h>
+@@ -20,6 +21,7 @@
+ #include <linux/workqueue.h>
+ #include "optee_private.h"
+ #include "optee_smc.h"
++#include "optee_ffa.h"
+ #include "shm_pool.h"
+ 
+ #define DRIVER_NAME "optee"
+@@ -299,10 +301,9 @@ static int optee_open(struct tee_context *ctx)
+ 	mutex_init(&ctxdata->mutex);
+ 	INIT_LIST_HEAD(&ctxdata->sess_list);
+ 
+-	if (optee->sec_caps & OPTEE_SMC_SEC_CAP_MEMREF_NULL)
+-		ctx->cap_memref_null  = true;
+-	else
+-		ctx->cap_memref_null = false;
++	ctx->cap_memref_null = optee_is_ffa_based(optee) ||
++			       (optee->sec_caps &
++				OPTEE_SMC_SEC_CAP_MEMREF_NULL);
+ 
+ 	ctx->data = ctxdata;
+ 	return 0;
+@@ -567,6 +568,472 @@ optee_config_shm_memremap(optee_invoke_fn *invoke_fn, void **memremaped_shm)
+ 	return rc;
+ }
+ 
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++static void optee_ffa_get_version(struct tee_device *teedev,
++				  struct tee_ioctl_version_data *vers)
++{
++	struct tee_ioctl_version_data v = {
++		.impl_id = TEE_IMPL_ID_OPTEE,
++		.impl_caps = TEE_OPTEE_CAP_TZ,
++		.gen_caps = TEE_GEN_CAP_GP | TEE_GEN_CAP_REG_MEM |
++			    TEE_GEN_CAP_MEMREF_NULL,
++	};
++
++	*vers = v;
++}
++
++struct shm_rhash {
++	struct tee_shm *shm;
++	u64 global_id;
++	struct rhash_head linkage;
++};
++
++static void rh_free_fn(void *ptr, void *arg)
++{
++	kfree(ptr);
++}
++
++static const struct rhashtable_params shm_rhash_params = {
++	.head_offset = offsetof(struct shm_rhash, linkage),
++	.key_len     = sizeof(u64),
++	.key_offset  = offsetof(struct shm_rhash, global_id),
++	.automatic_shrinking = true,
++};
++
++struct tee_shm *optee_shm_from_ffa_handle(struct optee *optee, u64 global_id)
++{
++	struct tee_shm *shm = NULL;
++	struct shm_rhash *r;
++
++	mutex_lock(&optee->ffa.mutex);
++	r = rhashtable_lookup_fast(&optee->ffa.global_ids, &global_id,
++				   shm_rhash_params);
++	if (r)
++		shm = r->shm;
++	mutex_unlock(&optee->ffa.mutex);
++
++	return shm;
++}
++
++int optee_shm_add_ffa_handle(struct optee *optee, struct tee_shm *shm,
++			     u64 global_id)
++{
++	struct shm_rhash *r;
++	int rc;
++
++	r = kmalloc(sizeof(*r), GFP_KERNEL);
++	if (!r)
++		return -ENOMEM;
++	r->shm = shm;
++	r->global_id = global_id;
++
++	mutex_lock(&optee->ffa.mutex);
++	rc = rhashtable_lookup_insert_fast(&optee->ffa.global_ids, &r->linkage,
++					   shm_rhash_params);
++	mutex_unlock(&optee->ffa.mutex);
++
++	if (rc)
++		kfree(r);
++
++	return rc;
++}
++
++int optee_shm_rem_ffa_handle(struct optee *optee, u64 global_id)
++{
++	struct shm_rhash *r;
++	int rc = -ENOENT;
++
++	mutex_lock(&optee->ffa.mutex);
++	r = rhashtable_lookup_fast(&optee->ffa.global_ids, &global_id,
++				   shm_rhash_params);
++	if (r)
++		rc = rhashtable_remove_fast(&optee->ffa.global_ids,
++					    &r->linkage, shm_rhash_params);
++	mutex_unlock(&optee->ffa.mutex);
++
++	if (!rc)
++		kfree(r);
++
++	return rc;
++}
++
++static void from_msg_param_ffa_mem(struct optee *optee, struct tee_param *p,
++				   u32 attr, const struct optee_msg_param *mp)
++{
++	struct tee_shm *shm = NULL;
++	u64 offs_high = 0;
++	u64 offs_low = 0;
++
++	p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT +
++		  attr - OPTEE_MSG_ATTR_TYPE_FMEM_INPUT;
++	p->u.memref.size = mp->u.fmem.size;
++
++	if (mp->u.fmem.global_id != OPTEE_MSG_FMEM_INVALID_GLOBAL_ID)
++		shm = optee_shm_from_ffa_handle(optee, mp->u.fmem.global_id);
++	p->u.memref.shm = shm;
++
++	if (shm) {
++		offs_low = mp->u.fmem.offs_low;
++		offs_high = mp->u.fmem.offs_high;
++	}
++	p->u.memref.shm_offs = offs_low | offs_high << 32;
++}
++
++/**
++ * optee_ffa_from_msg_param() - convert from OPTEE_MSG parameters to
++ *				struct tee_param
++ * @optee:	main service struct
++ * @params:	subsystem internal parameter representation
++ * @num_params:	number of elements in the parameter arrays
++ * @msg_params:	OPTEE_MSG parameters
++ *
++ * Returns 0 on success or <0 on failure
++ */
++static int optee_ffa_from_msg_param(struct optee *optee,
++				    struct tee_param *params, size_t num_params,
++				    const struct optee_msg_param *msg_params)
++{
++	size_t n;
++
++	for (n = 0; n < num_params; n++) {
++		struct tee_param *p = params + n;
++		const struct optee_msg_param *mp = msg_params + n;
++		u32 attr = mp->attr & OPTEE_MSG_ATTR_TYPE_MASK;
++
++		switch (attr) {
++		case OPTEE_MSG_ATTR_TYPE_NONE:
++			p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_NONE;
++			memset(&p->u, 0, sizeof(p->u));
++			break;
++		case OPTEE_MSG_ATTR_TYPE_VALUE_INPUT:
++		case OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT:
++		case OPTEE_MSG_ATTR_TYPE_VALUE_INOUT:
++			from_msg_param_value(p, attr, mp);
++			break;
++		case OPTEE_MSG_ATTR_TYPE_FMEM_INPUT:
++		case OPTEE_MSG_ATTR_TYPE_FMEM_OUTPUT:
++		case OPTEE_MSG_ATTR_TYPE_FMEM_INOUT:
++			from_msg_param_ffa_mem(optee, p, attr, mp);
++			break;
++		default:
++			return -EINVAL;
++		}
++	}
++
++	return 0;
++}
++
++static int to_msg_param_ffa_mem(struct optee_msg_param *mp,
++				const struct tee_param *p)
++{
++	struct tee_shm *shm = p->u.memref.shm;
++
++	mp->attr = OPTEE_MSG_ATTR_TYPE_FMEM_INPUT + p->attr -
++		   TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT;
++
++	if (shm) {
++		u64 shm_offs = p->u.memref.shm_offs;
++
++		mp->u.fmem.internal_offs = shm->offset;
++
++		mp->u.fmem.offs_low = shm_offs;
++		mp->u.fmem.offs_high = shm_offs >> 32;
++		/* Check that the entire offset could be stored. */
++		if (mp->u.fmem.offs_high != shm_offs >> 32)
++			return -EINVAL;
++
++		mp->u.fmem.global_id = shm->sec_world_id;
++	} else {
++		memset(&mp->u, 0, sizeof(mp->u));
++		mp->u.fmem.global_id = OPTEE_MSG_FMEM_INVALID_GLOBAL_ID;
++	}
++	mp->u.fmem.size = p->u.memref.size;
++
++	return 0;
++}
++
++/**
++ * optee_ffa_to_msg_param() - convert from struct tee_params to OPTEE_MSG
++ *			      parameters
++ * @optee:	main service struct
++ * @msg_params:	OPTEE_MSG parameters
++ * @num_params:	number of elements in the parameter arrays
++ * @params:	subsystem itnernal parameter representation
++ * Returns 0 on success or <0 on failure
++ */
++static int optee_ffa_to_msg_param(struct optee *optee,
++				  struct optee_msg_param *msg_params,
++				  size_t num_params,
++				  const struct tee_param *params)
++{
++	size_t n;
++
++	for (n = 0; n < num_params; n++) {
++		const struct tee_param *p = params + n;
++		struct optee_msg_param *mp = msg_params + n;
++
++		switch (p->attr) {
++		case TEE_IOCTL_PARAM_ATTR_TYPE_NONE:
++			mp->attr = TEE_IOCTL_PARAM_ATTR_TYPE_NONE;
++			memset(&mp->u, 0, sizeof(mp->u));
++			break;
++		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT:
++		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_OUTPUT:
++		case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT:
++			to_msg_param_value(mp, p);
++			break;
++		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT:
++		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_OUTPUT:
++		case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INOUT:
++			if (to_msg_param_ffa_mem(mp, p))
++				return -EINVAL;
++			break;
++		default:
++			return -EINVAL;
++		}
++	}
++
++	return 0;
++}
++
++static bool optee_ffa_api_is_compatbile(struct ffa_device *ffa_dev,
++					const struct ffa_dev_ops *ops)
++{
++	struct ffa_send_direct_data data = { OPTEE_FFA_GET_API_VERSION };
++	int rc;
++
++	ops->mode_32bit_set(ffa_dev);
++
++	rc = ops->sync_send_receive(ffa_dev, &data);
++	if (rc) {
++		pr_err("Unexpected error %d\n", rc);
++		return false;
++	}
++	if (data.data0 != OPTEE_FFA_VERSION_MAJOR ||
++	    data.data1 < OPTEE_FFA_VERSION_MINOR) {
++		pr_err("Incompatible OP-TEE API version %lu.%lu",
++		       data.data0, data.data1);
++		return false;
++	}
++
++	data = (struct ffa_send_direct_data){ OPTEE_FFA_GET_OS_VERSION };
++	rc = ops->sync_send_receive(ffa_dev, &data);
++	if (rc) {
++		pr_err("Unexpected error %d\n", rc);
++		return false;
++	}
++	if (data.data2)
++		pr_info("revision %lu.%lu (%08lx)",
++			data.data0, data.data1, data.data2);
++	else
++		pr_info("revision %lu.%lu", data.data0, data.data1);
++
++	return true;
++}
++
++static bool optee_ffa_exchange_caps(struct ffa_device *ffa_dev,
++				    const struct ffa_dev_ops *ops,
++				    u32 *sec_caps, unsigned int *rpc_arg_count)
++{
++	struct ffa_send_direct_data data = { OPTEE_FFA_EXCHANGE_CAPABILITIES };
++	int rc;
++
++	rc = ops->sync_send_receive(ffa_dev, &data);
++	if (rc) {
++		pr_err("Unexpected error %d", rc);
++		return false;
++	}
++	if (data.data0) {
++		pr_err("Unexpected exchange error %lu", data.data0);
++		return false;
++	}
++
++	*sec_caps = 0;
++	*rpc_arg_count = (u8)data.data1;
++
++	return true;
++}
++
++static struct tee_shm_pool *optee_ffa_config_dyn_shm(void)
++{
++	struct tee_shm_pool_mgr *priv_mgr;
++	struct tee_shm_pool_mgr *dmabuf_mgr;
++	void *rc;
++
++	rc = optee_ffa_shm_pool_alloc_pages();
++	if (IS_ERR(rc))
++		return rc;
++	priv_mgr = rc;
++
++	rc = optee_ffa_shm_pool_alloc_pages();
++	if (IS_ERR(rc)) {
++		tee_shm_pool_mgr_destroy(priv_mgr);
++		return rc;
++	}
++	dmabuf_mgr = rc;
++
++	rc = tee_shm_pool_alloc(priv_mgr, dmabuf_mgr);
++	if (IS_ERR(rc)) {
++		tee_shm_pool_mgr_destroy(priv_mgr);
++		tee_shm_pool_mgr_destroy(dmabuf_mgr);
++	}
++
++	return rc;
++}
++
++static const struct tee_driver_ops optee_ffa_clnt_ops = {
++	.get_version = optee_ffa_get_version,
++	.open = optee_open,
++	.release = optee_release,
++	.open_session = optee_open_session,
++	.close_session = optee_close_session,
++	.invoke_func = optee_invoke_func,
++	.cancel_req = optee_cancel_req,
++	.shm_register = optee_ffa_shm_register,
++	.shm_unregister = optee_ffa_shm_unregister,
++};
++
++static const struct tee_desc optee_ffa_clnt_desc = {
++	.name = DRIVER_NAME "ffa-clnt",
++	.ops = &optee_ffa_clnt_ops,
++	.owner = THIS_MODULE,
++};
++
++static const struct tee_driver_ops optee_ffa_supp_ops = {
++	.get_version = optee_ffa_get_version,
++	.open = optee_open,
++	.release = optee_release_supp,
++	.supp_recv = optee_supp_recv,
++	.supp_send = optee_supp_send,
++	.shm_register = optee_ffa_shm_register, /* same as for clnt ops */
++	.shm_unregister = optee_ffa_shm_unregister_supp,
++};
++
++static const struct tee_desc optee_ffa_supp_desc = {
++	.name = DRIVER_NAME "ffa-supp",
++	.ops = &optee_ffa_supp_ops,
++	.owner = THIS_MODULE,
++	.flags = TEE_DESC_PRIVILEGED,
++};
++
++static const struct optee_ops optee_ffa_ops = {
++	.do_call_with_arg = optee_ffa_do_call_with_arg,
++	.to_msg_param = optee_ffa_to_msg_param,
++	.from_msg_param = optee_ffa_from_msg_param,
++};
++
++static void optee_ffa_remove(struct ffa_device *ffa_dev)
++{
++	(void)ffa_dev;
++}
++
++static int optee_ffa_probe(struct ffa_device *ffa_dev)
++{
++	const struct ffa_dev_ops *ffa_ops;
++	unsigned int rpc_arg_count;
++	struct tee_device *teedev;
++	struct optee *optee;
++	u32 sec_caps;
++	int rc;
++
++	ffa_ops = ffa_dev_ops_get(ffa_dev);
++	if (!ffa_ops) {
++		pr_warn("failed \"method\" init: ffa\n");
++		return -ENOENT;
++	}
++
++	if (!optee_ffa_api_is_compatbile(ffa_dev, ffa_ops))
++		return -EINVAL;
++
++	if (!optee_ffa_exchange_caps(ffa_dev, ffa_ops, &sec_caps,
++				     &rpc_arg_count))
++		return -EINVAL;
++
++	optee = kzalloc(sizeof(*optee), GFP_KERNEL);
++	if (!optee) {
++		rc = -ENOMEM;
++		goto err;
++	}
++	optee->pool = optee_ffa_config_dyn_shm();
++	if (IS_ERR(optee->pool)) {
++		rc = PTR_ERR(optee->pool);
++		optee->pool = NULL;
++		goto err;
++	}
++
++	optee->ops = &optee_ffa_ops;
++	optee->ffa.ffa_dev = ffa_dev;
++	optee->ffa.ffa_ops = ffa_ops;
++	optee->sec_caps = sec_caps;
++	optee->rpc_arg_count = rpc_arg_count;
++
++	teedev = tee_device_alloc(&optee_ffa_clnt_desc, NULL, optee->pool,
++				  optee);
++	if (IS_ERR(teedev)) {
++		rc = PTR_ERR(teedev);
++		goto err;
++	}
++	optee->teedev = teedev;
++
++	teedev = tee_device_alloc(&optee_ffa_supp_desc, NULL, optee->pool,
++				  optee);
++	if (IS_ERR(teedev)) {
++		rc = PTR_ERR(teedev);
++		goto err;
++	}
++	optee->supp_teedev = teedev;
++
++	rc = tee_device_register(optee->teedev);
++	if (rc)
++		goto err;
++
++	rc = tee_device_register(optee->supp_teedev);
++	if (rc)
++		goto err;
++
++	rc = rhashtable_init(&optee->ffa.global_ids, &shm_rhash_params);
++	if (rc)
++		goto err;
++	mutex_init(&optee->ffa.mutex);
++	mutex_init(&optee->call_queue.mutex);
++	INIT_LIST_HEAD(&optee->call_queue.waiters);
++	optee_wait_queue_init(&optee->wait_queue);
++	optee_supp_init(&optee->supp);
++	ffa_dev_set_drvdata(ffa_dev, optee);
++
++	pr_info("initialized driver\n");
++	return 0;
++err:
++	/*
++	 * tee_device_unregister() is safe to call even if the
++	 * devices hasn't been registered with
++	 * tee_device_register() yet.
++	 */
++	tee_device_unregister(optee->supp_teedev);
++	tee_device_unregister(optee->teedev);
++	if (optee->pool)
++		tee_shm_pool_free(optee->pool);
++	kfree(optee);
++	return rc;
++}
++
++static const struct ffa_device_id optee_ffa_device_id[] = {
++	/* 486178e0-e7f8-11e3-bc5e0002a5d5c51b */
++	{ UUID_INIT(0x486178e0, 0xe7f8, 0x11e3,
++		    0xbc, 0x5e, 0x00, 0x02, 0xa5, 0xd5, 0xc5, 0x1b) },
++	{}
++};
++
++static struct ffa_driver optee_ffa_driver = {
++	.name = "optee",
++	.probe = optee_ffa_probe,
++	.remove = optee_ffa_remove,
++	.id_table = optee_ffa_device_id,
++};
++
++module_ffa_driver(optee_ffa_driver);
++#endif /*CONFIG_ARM_FFA_TRANSPORT*/
++
+ /* Simple wrapper functions to be able to use a function pointer */
+ static void optee_smccc_smc(unsigned long a0, unsigned long a1,
+ 			    unsigned long a2, unsigned long a3,
+@@ -615,7 +1082,8 @@ static int optee_remove(struct platform_device *pdev)
+ 	 * reference counters and also avoid wild pointers in secure world
+ 	 * into the old shared memory range.
+ 	 */
+-	optee_disable_shm_cache(optee);
++	if (!optee_is_ffa_based(optee))
++		optee_disable_shm_cache(optee);
+ 
+ 	/*
+ 	 * The two devices have to be unregistered before we can free the
+@@ -631,6 +1099,14 @@ static int optee_remove(struct platform_device *pdev)
+ 	optee_supp_uninit(&optee->supp);
+ 	mutex_destroy(&optee->call_queue.mutex);
+ 
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++	if (optee->ffa.ffa_ops) {
++		mutex_destroy(&optee->ffa.mutex);
++		rhashtable_free_and_destroy(&optee->ffa.global_ids,
++					    rh_free_fn, NULL);
++	}
++#endif /*CONFIG_ARM_FFA_TRANSPORT*/
++
+ 	kfree(optee);
+ 
+ 	return 0;
+diff --git a/drivers/tee/optee/optee_ffa.h b/drivers/tee/optee/optee_ffa.h
+new file mode 100644
+index 000000000000..ee3a03fc392c
+--- /dev/null
++++ b/drivers/tee/optee/optee_ffa.h
+@@ -0,0 +1,153 @@
++/* SPDX-License-Identifier: BSD-2-Clause */
++/*
++ * Copyright (c) 2019-2021, Linaro Limited
++ */
++
++/*
++ * This file is exported by OP-TEE and is kept in sync between secure world
++ * and normal world drivers. We're using ARM FF-A 1.0 specification.
++ */
++
++#ifndef __OPTEE_FFA_H
++#define __OPTEE_FFA_H
++
++#include <linux/arm_ffa.h>
++
++/*
++ * Normal world sends requests with FFA_MSG_SEND_DIRECT_REQ and
++ * responses are returned with FFA_MSG_SEND_DIRECT_RESP for normal
++ * messages.
++ *
++ * All requests with FFA_MSG_SEND_DIRECT_REQ and FFA_MSG_SEND_DIRECT_RESP
++ * are using the AArch32 SMC calling convention with register usage as
++ * defined in FF-A specification:
++ * w0:    Function ID (0x8400006F or 0x84000070)
++ * w1:    Source/Destination IDs
++ * w2:    Reserved (MBZ)
++ * w3-w7: Implementation defined, free to be used below
++ */
++
++#define OPTEE_FFA_VERSION_MAJOR	1
++#define OPTEE_FFA_VERSION_MINOR	0
++
++#define OPTEE_FFA_BLOCKING_CALL(id)	(id)
++#define OPTEE_FFA_YIELDING_CALL_BIT	31
++#define OPTEE_FFA_YIELDING_CALL(id)	((id) | BIT(OPTEE_FFA_YIELDING_CALL_BIT))
++
++/*
++ * Returns the API version implemented, currently follows the FF-A version.
++ * Call register usage:
++ * w3:    Service ID, OPTEE_FFA_GET_API_VERSION
++ * w4-w7: Not used (MBZ)
++ *
++ * Return register usage:
++ * w3:    OPTEE_FFA_VERSION_MAJOR
++ * w4:    OPTEE_FFA_VERSION_MINOR
++ * w5-w7: Not used (MBZ)
++ */
++#define OPTEE_FFA_GET_API_VERSION	OPTEE_FFA_BLOCKING_CALL(0)
++
++/*
++ * Returns the revision of OP-TEE.
++ *
++ * Used by non-secure world to figure out which version of the Trusted OS
++ * is installed. Note that the returned revision is the revision of the
++ * Trusted OS, not of the API.
++ *
++ * Call register usage:
++ * w3:    Service ID, OPTEE_FFA_GET_OS_VERSION
++ * w4-w7: Unused (MBZ)
++ *
++ * Return register usage:
++ * w3:    CFG_OPTEE_REVISION_MAJOR
++ * w4:    CFG_OPTEE_REVISION_MINOR
++ * w5:    TEE_IMPL_GIT_SHA1 (or zero if not supported)
++ */
++#define OPTEE_FFA_GET_OS_VERSION	OPTEE_FFA_BLOCKING_CALL(1)
++
++/*
++ * Exchange capabilities between normal world and secure world.
++ *
++ * Currently there are no defined capabilities. When features are added new
++ * capabilities may be added.
++ *
++ * Call register usage:
++ * w3:    Service ID, OPTEE_FFA_EXCHANGE_CAPABILITIES
++ * w4-w7: Note used (MBZ)
++ *
++ * Return register usage:
++ * w3:    Error code, 0 on success
++ * w4:    Bit[7:0]:  Number of parameters needed for RPC to be supplied
++ *                   as the second MSG arg struct for
++ *                   OPTEE_FFA_YIELDING_CALL_WITH_ARG.
++ *        Bit[31:8]: Reserved (MBZ)
++ * w5-w7: Note used (MBZ)
++ */
++#define OPTEE_FFA_EXCHANGE_CAPABILITIES OPTEE_FFA_BLOCKING_CALL(2)
++
++/*
++ * Unregister shared memory
++ *
++ * Call register usage:
++ * w3:    Service ID, OPTEE_FFA_YIELDING_CALL_UNREGISTER_SHM
++ * w4:    Shared memory handle, lower bits
++ * w5:    Shared memory handle, higher bits
++ * w6-w7: Not used (MBZ)
++ *
++ * Return register usage:
++ * w3:    Error code, 0 on success
++ * w4-w7: Note used (MBZ)
++ */
++#define OPTEE_FFA_UNREGISTER_SHM	OPTEE_FFA_BLOCKING_CALL(3)
++
++/*
++ * Call with struct optee_msg_arg as argument in the supplied shared memory
++ * with a zero internal offset and normal cached memory attributes.
++ * Register usage:
++ * w3:    Service ID, OPTEE_FFA_YIELDING_CALL_WITH_ARG
++ * w4:    Lower 32 bits of a 64-bit Shared memory handle
++ * w5:    Upper 32 bits of a 64-bit Shared memory handle
++ * w6:    Offset into shared memory pointing to a struct optee_msg_arg
++ *	  right after the parameters of this struct (at offset
++ *	  OPTEE_MSG_GET_ARG_SIZE(num_params) follows a struct optee_msg_arg
++ *	  for RPC, this struct has reserved space for the number of RPC
++ *	  parameters as returned by OPTEE_FFA_EXCHANGE_CAPABILITIES.
++ * w7:    Not used (MBZ)
++ * Resume from RPC. Register usage:
++ * w3:    Service ID, OPTEE_FFA_YIELDING_CALL_RESUME
++ * w4-w6: Not used (MBZ)
++ * w7:    Resume info
++ *
++ * Normal return (yielding call is completed). Register usage:
++ * w3:    Error code, 0 on success
++ * w4:    OPTEE_FFA_YIELDING_CALL_RETURN_DONE
++ * w5-w7: Not used (MBZ)
++ *
++ * RPC interrupt return (RPC from secure world). Register usage:
++ * w3:    Error code == 0
++ * w4:    Any defined RPC code but OPTEE_FFA_YIELDING_CALL_RETURN_DONE
++ * w5-w6: Not used (MBZ)
++ * w7:    Resume info
++ *
++ * Possible error codes in register w3:
++ * 0:                       Success
++ * FFA_DENIED:              w4 isn't one of OPTEE_FFA_YIELDING_CALL_START
++ *                          OPTEE_FFA_YIELDING_CALL_RESUME
++ *
++ * Possible error codes for OPTEE_FFA_YIELDING_CALL_START,
++ * FFA_BUSY:               Number of OP-TEE OS threads exceeded,
++ *                         try again later
++ * FFA_DENIED:             RPC shared memory object not found
++ * FFA_INVALID_PARAMETER:  Bad shared memory handle or offset into the memory
++ *
++ * Possible error codes for OPTEE_FFA_YIELDING_CALL_RESUME
++ * FFA_INVALID_PARAMETER:  Bad resume info
++ */
++#define OPTEE_FFA_YIELDING_CALL_WITH_ARG	OPTEE_FFA_YIELDING_CALL(0)
++#define OPTEE_FFA_YIELDING_CALL_RESUME		OPTEE_FFA_YIELDING_CALL(1)
++
++#define OPTEE_FFA_YIELDING_CALL_RETURN_DONE		0
++#define OPTEE_FFA_YIELDING_CALL_RETURN_RPC_CMD		1
++#define OPTEE_FFA_YIELDING_CALL_RETURN_INTERRUPT	2
++
++#endif /*__OPTEE_FFA_H*/
+diff --git a/drivers/tee/optee/optee_msg.h b/drivers/tee/optee/optee_msg.h
+index 5bef6a0165db..1ee943980c68 100644
+--- a/drivers/tee/optee/optee_msg.h
++++ b/drivers/tee/optee/optee_msg.h
+@@ -28,6 +28,9 @@
+ #define OPTEE_MSG_ATTR_TYPE_RMEM_INPUT		0x5
+ #define OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT		0x6
+ #define OPTEE_MSG_ATTR_TYPE_RMEM_INOUT		0x7
++#define OPTEE_MSG_ATTR_TYPE_FMEM_INPUT		OPTEE_MSG_ATTR_TYPE_RMEM_INPUT
++#define OPTEE_MSG_ATTR_TYPE_FMEM_OUTPUT		OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT
++#define OPTEE_MSG_ATTR_TYPE_FMEM_INOUT		OPTEE_MSG_ATTR_TYPE_RMEM_INOUT
+ #define OPTEE_MSG_ATTR_TYPE_TMEM_INPUT		0x9
+ #define OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT		0xa
+ #define OPTEE_MSG_ATTR_TYPE_TMEM_INOUT		0xb
+@@ -96,6 +99,8 @@
+  */
+ #define OPTEE_MSG_NONCONTIG_PAGE_SIZE		4096
+ 
++#define OPTEE_MSG_FMEM_INVALID_GLOBAL_ID	0xffffffffffffffff
++
+ /**
+  * struct optee_msg_param_tmem - temporary memory reference parameter
+  * @buf_ptr:	Address of the buffer
+@@ -127,6 +132,23 @@ struct optee_msg_param_rmem {
+ 	u64 shm_ref;
+ };
+ 
++/**
++ * struct optee_msg_param_fmem - ffa memory reference parameter
++ * @offs_lower:	   Lower bits of offset into shared memory reference
++ * @offs_upper:	   Upper bits of offset into shared memory reference
++ * @internal_offs: Internal offset into the first page of shared memory
++ *		   reference
++ * @size:	   Size of the buffer
++ * @global_id:	   Global identifier of Shared memory
++ */
++struct optee_msg_param_fmem {
++	u32 offs_low;
++	u16 offs_high;
++	u16 internal_offs;
++	u64 size;
++	u64 global_id;
++};
++
+ /**
+  * struct optee_msg_param_value - opaque value parameter
+  *
+@@ -143,13 +165,15 @@ struct optee_msg_param_value {
+  * @attr:	attributes
+  * @tmem:	parameter by temporary memory reference
+  * @rmem:	parameter by registered memory reference
++ * @fmem:	parameter by ffa registered memory reference
+  * @value:	parameter by opaque value
+  * @octets:	parameter by octet string
+  *
+  * @attr & OPTEE_MSG_ATTR_TYPE_MASK indicates if tmem, rmem or value is used in
+  * the union. OPTEE_MSG_ATTR_TYPE_VALUE_* indicates value or octets,
+  * OPTEE_MSG_ATTR_TYPE_TMEM_* indicates @tmem and
+- * OPTEE_MSG_ATTR_TYPE_RMEM_* indicates @rmem,
++ * OPTEE_MSG_ATTR_TYPE_RMEM_* or the alias PTEE_MSG_ATTR_TYPE_FMEM_* indicates
++ * @rmem or @fmem depending on the conduit.
+  * OPTEE_MSG_ATTR_TYPE_NONE indicates that none of the members are used.
+  */
+ struct optee_msg_param {
+@@ -157,6 +181,7 @@ struct optee_msg_param {
+ 	union {
+ 		struct optee_msg_param_tmem tmem;
+ 		struct optee_msg_param_rmem rmem;
++		struct optee_msg_param_fmem fmem;
+ 		struct optee_msg_param_value value;
+ 		u8 octets[24];
+ 	} u;
+diff --git a/drivers/tee/optee/optee_private.h b/drivers/tee/optee/optee_private.h
+index c5741e96e967..1ffe74e66d15 100644
+--- a/drivers/tee/optee/optee_private.h
++++ b/drivers/tee/optee/optee_private.h
+@@ -7,6 +7,7 @@
+ #define OPTEE_PRIVATE_H
+ 
+ #include <linux/arm-smccc.h>
++#include <linux/rhashtable.h>
+ #include <linux/semaphore.h>
+ #include <linux/tee_drv.h>
+ #include <linux/types.h>
+@@ -20,6 +21,7 @@
+ #define TEEC_ERROR_NOT_SUPPORTED	0xFFFF000A
+ #define TEEC_ERROR_COMMUNICATION	0xFFFF000E
+ #define TEEC_ERROR_OUT_OF_MEMORY	0xFFFF000C
++#define TEEC_ERROR_BUSY			0xFFFF000D
+ #define TEEC_ERROR_SHORT_BUFFER		0xFFFF0010
+ 
+ #define TEEC_ORIGIN_COMMS		0x00000002
+@@ -66,6 +68,22 @@ struct optee_supp {
+ 	struct completion reqs_c;
+ };
+ 
++/**
++ * struct optee_ffa_data -  FFA communication struct
++ * @ffa_dev		FFA device, contains the destination id, the id of
++ *			OP-TEE in secure world
++ * @ffa_ops		FFA operations
++ * @mutex		Serializes access to @global_ids
++ * @global_ids		FF-A shared memory global handle translation
++ */
++struct optee_ffa {
++	struct ffa_device *ffa_dev;
++	const struct ffa_dev_ops *ffa_ops;
++	/* Serializes access to @global_ids */
++	struct mutex mutex;
++	struct rhashtable global_ids;
++};
++
+ struct optee;
+ 
+ /**
+@@ -113,11 +131,15 @@ struct optee {
+ 	struct tee_device *teedev;
+ 	const struct optee_ops *ops;
+ 	optee_invoke_fn *invoke_fn;
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++	struct optee_ffa ffa;
++#endif
+ 	struct optee_call_queue call_queue;
+ 	struct optee_wait_queue wait_queue;
+ 	struct optee_supp supp;
+ 	struct tee_shm_pool *pool;
+ 	void *memremaped_shm;
++	unsigned int rpc_arg_count;
+ 	u32 sec_caps;
+ 	bool   scan_bus_done;
+ 	struct workqueue_struct *scan_bus_wq;
+@@ -206,6 +228,36 @@ void optee_fill_pages_list(u64 *dst, struct page **pages, int num_pages,
+ #define PTA_CMD_GET_DEVICES_SUPP	0x1
+ int optee_enumerate_devices(u32 func);
+ 
++int optee_shm_add_ffa_handle(struct optee *optee, struct tee_shm *shm,
++			     u64 global_id);
++int optee_shm_rem_ffa_handle(struct optee *optee, u64 global_id);
++
++struct tee_shm *optee_shm_from_ffa_handle(struct optee *optee, u64 global_id);
++
++int optee_ffa_shm_register(struct tee_context *ctx, struct tee_shm *shm,
++			   struct page **pages, size_t num_pages,
++			   unsigned long start);
++int optee_ffa_shm_unregister(struct tee_context *ctx, struct tee_shm *shm);
++int optee_ffa_shm_register_supp(struct tee_context *ctx, struct tee_shm *shm,
++				struct page **pages, size_t num_pages,
++				unsigned long start);
++int optee_ffa_shm_unregister_supp(struct tee_context *ctx,
++				  struct tee_shm *shm);
++
++int optee_ffa_do_call_with_arg(struct tee_context *ctx, struct tee_shm *arg);
++int optee_ffa_rpc_shm_register(struct tee_context *ctx, struct tee_shm *shm);
++void optee_handle_ffa_rpc(struct tee_context *ctx, u32 cmd,
++			  struct optee_msg_arg *arg);
++
++static inline bool optee_is_ffa_based(struct optee *optee)
++{
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++	return optee->ffa.ffa_ops;
++#else
++	return false;
++#endif
++}
++
+ /*
+  * Small helpers
+  */
+diff --git a/drivers/tee/optee/rpc.c b/drivers/tee/optee/rpc.c
+index 39562fb6841e..865a9ab3cf65 100644
+--- a/drivers/tee/optee/rpc.c
++++ b/drivers/tee/optee/rpc.c
+@@ -10,6 +10,7 @@
+ #include <linux/i2c.h>
+ #include <linux/slab.h>
+ #include <linux/tee_drv.h>
++#include "optee_ffa.h"
+ #include "optee_private.h"
+ #include "optee_smc.h"
+ #include "optee_rpc_cmd.h"
+@@ -543,3 +544,120 @@ void optee_handle_rpc(struct tee_context *ctx, struct optee_rpc_param *param,
+ 
+ 	param->a0 = OPTEE_SMC_CALL_RETURN_FROM_RPC;
+ }
++
++#ifdef CONFIG_ARM_FFA_TRANSPORT
++static void handle_ffa_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
++					      struct optee_msg_arg *arg)
++{
++	struct tee_shm *shm;
++
++	if (arg->num_params != 1 ||
++	    arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT) {
++		arg->ret = TEEC_ERROR_BAD_PARAMETERS;
++		return;
++	}
++
++	switch (arg->params[0].u.value.a) {
++	case OPTEE_RPC_SHM_TYPE_APPL:
++		shm = cmd_alloc_suppl(ctx, arg->params[0].u.value.b);
++		break;
++	case OPTEE_RPC_SHM_TYPE_KERNEL:
++		shm = tee_shm_alloc(ctx, arg->params[0].u.value.b,
++				    TEE_SHM_MAPPED);
++		break;
++	default:
++		arg->ret = TEEC_ERROR_BAD_PARAMETERS;
++		return;
++	}
++
++	if (IS_ERR(shm)) {
++		arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
++		return;
++	}
++
++	arg->params[0] = (struct optee_msg_param){
++		.attr = OPTEE_MSG_ATTR_TYPE_FMEM_OUTPUT,
++		.u.fmem.size = tee_shm_get_size(shm),
++		.u.fmem.global_id = shm->sec_world_id,
++		.u.fmem.internal_offs = shm->offset,
++	};
++
++	arg->ret = TEEC_SUCCESS;
++}
++
++static void handle_ffa_rpc_func_cmd_shm_free(struct tee_context *ctx,
++					     struct optee *optee,
++					     struct optee_msg_arg *arg)
++{
++	struct tee_shm *shm;
++
++	if (arg->num_params != 1 ||
++	    arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT)
++		goto err_bad_param;
++
++	shm = optee_shm_from_ffa_handle(optee, arg->params[0].u.value.b);
++	if (!shm)
++		goto err_bad_param;
++	switch (arg->params[0].u.value.a) {
++	case OPTEE_RPC_SHM_TYPE_APPL:
++		cmd_free_suppl(ctx, shm);
++		break;
++	case OPTEE_RPC_SHM_TYPE_KERNEL:
++		tee_shm_free(shm);
++		break;
++	default:
++		goto err_bad_param;
++	}
++	arg->ret = TEEC_SUCCESS;
++	return;
++
++err_bad_param:
++	arg->ret = TEEC_ERROR_BAD_PARAMETERS;
++}
++
++static void handle_ffa_rpc_func_cmd(struct tee_context *ctx,
++				    struct optee_msg_arg *arg)
++{
++	struct optee *optee = tee_get_drvdata(ctx->teedev);
++
++	arg->ret_origin = TEEC_ORIGIN_COMMS;
++	switch (arg->cmd) {
++	case OPTEE_RPC_CMD_GET_TIME:
++		handle_rpc_func_cmd_get_time(arg);
++		break;
++	case OPTEE_RPC_CMD_WAIT_QUEUE:
++		handle_rpc_func_cmd_wq(optee, arg);
++		break;
++	case OPTEE_RPC_CMD_SUSPEND:
++		handle_rpc_func_cmd_wait(arg);
++		break;
++	case OPTEE_RPC_CMD_SHM_ALLOC:
++		handle_ffa_rpc_func_cmd_shm_alloc(ctx, arg);
++		break;
++	case OPTEE_RPC_CMD_SHM_FREE:
++		handle_ffa_rpc_func_cmd_shm_free(ctx, optee, arg);
++		break;
++	case OPTEE_RPC_CMD_I2C_TRANSFER:
++		handle_rpc_func_cmd_i2c_transfer(ctx, arg);
++		break;
++	default:
++		handle_rpc_supp_cmd(ctx, optee, arg);
++	}
++}
++
++void optee_handle_ffa_rpc(struct tee_context *ctx, u32 cmd,
++			  struct optee_msg_arg *arg)
++{
++	switch (cmd) {
++	case OPTEE_FFA_YIELDING_CALL_RETURN_RPC_CMD:
++		handle_ffa_rpc_func_cmd(ctx, arg);
++		break;
++	case OPTEE_FFA_YIELDING_CALL_RETURN_INTERRUPT:
++		/* Interrupt delivered by now */
++		break;
++	default:
++		pr_warn("Unknown RPC func 0x%x\n", cmd);
++		break;
++	}
++}
++#endif /*CONFIG_ARM_FFA_TRANSPORT*/
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0015-coresight-etm4x-Save-restore-TRFCR_EL1.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0015-coresight-etm4x-Save-restore-TRFCR_EL1.patch
new file mode 100644
index 0000000..e67658f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0015-coresight-etm4x-Save-restore-TRFCR_EL1.patch
@@ -0,0 +1,181 @@
+From 7150eac72ee0c2c7da03f53a90a871c3d6d4e538 Mon Sep 17 00:00:00 2001
+From: Suzuki K Poulose <suzuki.poulose@arm.com>
+Date: Tue, 14 Sep 2021 11:26:32 +0100
+Subject: [PATCH 1/2] coresight: etm4x: Save restore TRFCR_EL1
+
+When the CPU enters a low power mode, the TRFCR_EL1 contents could be
+reset. Thus we need to save/restore the TRFCR_EL1 along with the ETM4x
+registers to allow the tracing.
+
+The TRFCR related helpers are in a new header file, as we need to use
+them for TRBE in the later patches.
+
+Cc: Mathieu Poirier <mathieu.poirier@linaro.org>
+Cc: Anshuman Khandual <anshuman.khandual@arm.com>
+Cc: Mike Leach <mike.leach@linaro.org>
+Cc: Leo Yan <leo.yan@linaro.org>
+Reviewed-by: Anshuman Khandual <anshuman.khandual@arm.com>
+Signed-off-by: Suzuki K Poulose <suzuki.poulose@arm.com>
+Link: https://lore.kernel.org/r/20210914102641.1852544-2-suzuki.poulose@arm.com
+[Fixed cosmetic details]
+Signed-off-by: Mathieu Poirier <mathieu.poirier@linaro.org>
+
+Upstream-Status: Backport [https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/commit/?id=937d3f58cacf377cab7c32e475e1ffa91d611dce]
+Signed-off-by: Davidson K <davidson.kumaresan@arm.com>
+---
+ .../coresight/coresight-etm4x-core.c          | 43 +++++++++++++------
+ drivers/hwtracing/coresight/coresight-etm4x.h |  2 +
+ .../coresight/coresight-self-hosted-trace.h   | 24 +++++++++++
+ 3 files changed, 57 insertions(+), 12 deletions(-)
+ create mode 100644 drivers/hwtracing/coresight/coresight-self-hosted-trace.h
+
+diff --git a/drivers/hwtracing/coresight/coresight-etm4x-core.c b/drivers/hwtracing/coresight/coresight-etm4x-core.c
+index 90827077d2f9..b78080d169f8 100644
+--- a/drivers/hwtracing/coresight/coresight-etm4x-core.c
++++ b/drivers/hwtracing/coresight/coresight-etm4x-core.c
+@@ -39,6 +39,7 @@
+ 
+ #include "coresight-etm4x.h"
+ #include "coresight-etm-perf.h"
++#include "coresight-self-hosted-trace.h"
+ 
+ static int boot_enable;
+ module_param(boot_enable, int, 0444);
+@@ -990,7 +991,7 @@ static void cpu_enable_tracing(struct etmv4_drvdata *drvdata)
+ 	if (is_kernel_in_hyp_mode())
+ 		trfcr |= TRFCR_EL2_CX;
+ 
+-	write_sysreg_s(trfcr, SYS_TRFCR_EL1);
++	write_trfcr(trfcr);
+ }
+ 
+ static void etm4_init_arch_data(void *info)
+@@ -1528,7 +1529,7 @@ static void etm4_init_trace_id(struct etmv4_drvdata *drvdata)
+ 	drvdata->trcid = coresight_get_trace_id(drvdata->cpu);
+ }
+ 
+-static int etm4_cpu_save(struct etmv4_drvdata *drvdata)
++static int __etm4_cpu_save(struct etmv4_drvdata *drvdata)
+ {
+ 	int i, ret = 0;
+ 	struct etmv4_save_state *state;
+@@ -1667,7 +1668,23 @@ static int etm4_cpu_save(struct etmv4_drvdata *drvdata)
+ 	return ret;
+ }
+ 
+-static void etm4_cpu_restore(struct etmv4_drvdata *drvdata)
++static int etm4_cpu_save(struct etmv4_drvdata *drvdata)
++{
++	int ret = 0;
++
++	/* Save the TRFCR irrespective of whether the ETM is ON */
++	if (drvdata->trfc)
++		drvdata->save_trfcr = read_trfcr();
++	/*
++	 * Save and restore the ETM Trace registers only if
++	 * the ETM is active.
++	 */
++	if (local_read(&drvdata->mode) && drvdata->save_state)
++		ret = __etm4_cpu_save(drvdata);
++	return ret;
++}
++
++static void __etm4_cpu_restore(struct etmv4_drvdata *drvdata)
+ {
+ 	int i;
+ 	struct etmv4_save_state *state = drvdata->save_state;
+@@ -1763,6 +1780,14 @@ static void etm4_cpu_restore(struct etmv4_drvdata *drvdata)
+ 	etm4_cs_lock(drvdata, csa);
+ }
+ 
++static void etm4_cpu_restore(struct etmv4_drvdata *drvdata)
++{
++	if (drvdata->trfc)
++		write_trfcr(drvdata->save_trfcr);
++	if (drvdata->state_needs_restore)
++		__etm4_cpu_restore(drvdata);
++}
++
+ static int etm4_cpu_pm_notify(struct notifier_block *nb, unsigned long cmd,
+ 			      void *v)
+ {
+@@ -1774,23 +1799,17 @@ static int etm4_cpu_pm_notify(struct notifier_block *nb, unsigned long cmd,
+ 
+ 	drvdata = etmdrvdata[cpu];
+ 
+-	if (!drvdata->save_state)
+-		return NOTIFY_OK;
+-
+ 	if (WARN_ON_ONCE(drvdata->cpu != cpu))
+ 		return NOTIFY_BAD;
+ 
+ 	switch (cmd) {
+ 	case CPU_PM_ENTER:
+-		/* save the state if self-hosted coresight is in use */
+-		if (local_read(&drvdata->mode))
+-			if (etm4_cpu_save(drvdata))
+-				return NOTIFY_BAD;
++		if (etm4_cpu_save(drvdata))
++			return NOTIFY_BAD;
+ 		break;
+ 	case CPU_PM_EXIT:
+ 	case CPU_PM_ENTER_FAILED:
+-		if (drvdata->state_needs_restore)
+-			etm4_cpu_restore(drvdata);
++		etm4_cpu_restore(drvdata);
+ 		break;
+ 	default:
+ 		return NOTIFY_DONE;
+diff --git a/drivers/hwtracing/coresight/coresight-etm4x.h b/drivers/hwtracing/coresight/coresight-etm4x.h
+index e5b79bdb9851..82cba16b73a6 100644
+--- a/drivers/hwtracing/coresight/coresight-etm4x.h
++++ b/drivers/hwtracing/coresight/coresight-etm4x.h
+@@ -921,6 +921,7 @@ struct etmv4_save_state {
+  * @lpoverride:	If the implementation can support low-power state over.
+  * @trfc:	If the implementation supports Arm v8.4 trace filter controls.
+  * @config:	structure holding configuration parameters.
++ * @save_trfcr:	Saved TRFCR_EL1 register during a CPU PM event.
+  * @save_state:	State to be preserved across power loss
+  * @state_needs_restore: True when there is context to restore after PM exit
+  * @skip_power_up: Indicates if an implementation can skip powering up
+@@ -973,6 +974,7 @@ struct etmv4_drvdata {
+ 	bool				lpoverride;
+ 	bool				trfc;
+ 	struct etmv4_config		config;
++	u64				save_trfcr;
+ 	struct etmv4_save_state		*save_state;
+ 	bool				state_needs_restore;
+ 	bool				skip_power_up;
+diff --git a/drivers/hwtracing/coresight/coresight-self-hosted-trace.h b/drivers/hwtracing/coresight/coresight-self-hosted-trace.h
+new file mode 100644
+index 000000000000..303d71911870
+--- /dev/null
++++ b/drivers/hwtracing/coresight/coresight-self-hosted-trace.h
+@@ -0,0 +1,24 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Arm v8 Self-Hosted trace support.
++ *
++ * Copyright (C) 2021 ARM Ltd.
++ */
++
++#ifndef __CORESIGHT_SELF_HOSTED_TRACE_H
++#define __CORESIGHT_SELF_HOSTED_TRACE_H
++
++#include <asm/sysreg.h>
++
++static inline u64 read_trfcr(void)
++{
++	return read_sysreg_s(SYS_TRFCR_EL1);
++}
++
++static inline void write_trfcr(u64 val)
++{
++	write_sysreg_s(val, SYS_TRFCR_EL1);
++	isb();
++}
++
++#endif /*  __CORESIGHT_SELF_HOSTED_TRACE_H */
+-- 
+2.34.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0016-coresight-etm4x-Use-Trace-Filtering-controls-dynamic.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0016-coresight-etm4x-Use-Trace-Filtering-controls-dynamic.patch
new file mode 100644
index 0000000..4d5a719
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0016-coresight-etm4x-Use-Trace-Filtering-controls-dynamic.patch
@@ -0,0 +1,227 @@
+From 55228b0522bfb7d945019a8931742ab9b063b6c9 Mon Sep 17 00:00:00 2001
+From: Suzuki K Poulose <suzuki.poulose@arm.com>
+Date: Tue, 14 Sep 2021 11:26:33 +0100
+Subject: [PATCH 2/2] coresight: etm4x: Use Trace Filtering controls
+ dynamically
+
+The Trace Filtering support (FEAT_TRF) ensures that the ETM
+can be prohibited from generating any trace for a given EL.
+This is much stricter knob, than the TRCVICTLR exception level
+masks, which doesn't prevent the ETM from generating Context
+packets for an "excluded" EL. At the moment, we do a onetime
+enable trace at user and kernel and leave it untouched for the
+kernel life time. This implies that the ETM could potentially
+generate trace packets containing the kernel addresses, and
+thus leaking the kernel virtual address in the trace.
+
+This patch makes the switch dynamic, by honoring the filters
+set by the user and enforcing them in the TRFCR controls.
+We also rename the cpu_enable_tracing() appropriately to
+cpu_detect_trace_filtering() and the drvdata member
+trfc => trfcr to indicate the "value" of the TRFCR_EL1.
+
+Cc: Mathieu Poirier <mathieu.poirier@linaro.org>
+Cc: Al Grant <al.grant@arm.com>
+Cc: Mike Leach <mike.leach@linaro.org>
+Cc: Leo Yan <leo.yan@linaro.org>
+Signed-off-by: Suzuki K Poulose <suzuki.poulose@arm.com>
+Reviewed-by: Anshuman Khandual <anshuman.khandual@arm.com>
+Link: https://lore.kernel.org/r/20210914102641.1852544-3-suzuki.poulose@arm.com
+Signed-off-by: Mathieu Poirier <mathieu.poirier@linaro.org>
+
+Upstream-Status: Backport [https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/commit/?id=5f6fd1aa8cc147b111af1a833574487a87237dc0]
+Signed-off-by: Davidson K <davidson.kumaresan@arm.com>
+---
+ .../coresight/coresight-etm4x-core.c          | 63 ++++++++++++++-----
+ drivers/hwtracing/coresight/coresight-etm4x.h |  7 ++-
+ .../coresight/coresight-self-hosted-trace.h   |  7 +++
+ 3 files changed, 59 insertions(+), 18 deletions(-)
+
+diff --git a/drivers/hwtracing/coresight/coresight-etm4x-core.c b/drivers/hwtracing/coresight/coresight-etm4x-core.c
+index b78080d169f8..b804d4413b43 100644
+--- a/drivers/hwtracing/coresight/coresight-etm4x-core.c
++++ b/drivers/hwtracing/coresight/coresight-etm4x-core.c
+@@ -237,6 +237,45 @@ struct etm4_enable_arg {
+ 	int rc;
+ };
+ 
++/*
++ * etm4x_prohibit_trace - Prohibit the CPU from tracing at all ELs.
++ * When the CPU supports FEAT_TRF, we could move the ETM to a trace
++ * prohibited state by filtering the Exception levels via TRFCR_EL1.
++ */
++static void etm4x_prohibit_trace(struct etmv4_drvdata *drvdata)
++{
++	/* If the CPU doesn't support FEAT_TRF, nothing to do */
++	if (!drvdata->trfcr)
++		return;
++	cpu_prohibit_trace();
++}
++
++/*
++ * etm4x_allow_trace - Allow CPU tracing in the respective ELs,
++ * as configured by the drvdata->config.mode for the current
++ * session. Even though we have TRCVICTLR bits to filter the
++ * trace in the ELs, it doesn't prevent the ETM from generating
++ * a packet (e.g, TraceInfo) that might contain the addresses from
++ * the excluded levels. Thus we use the additional controls provided
++ * via the Trace Filtering controls (FEAT_TRF) to make sure no trace
++ * is generated for the excluded ELs.
++ */
++static void etm4x_allow_trace(struct etmv4_drvdata *drvdata)
++{
++	u64 trfcr = drvdata->trfcr;
++
++	/* If the CPU doesn't support FEAT_TRF, nothing to do */
++	if (!trfcr)
++		return;
++
++	if (drvdata->config.mode & ETM_MODE_EXCL_KERN)
++		trfcr &= ~TRFCR_ELx_ExTRE;
++	if (drvdata->config.mode & ETM_MODE_EXCL_USER)
++		trfcr &= ~TRFCR_ELx_E0TRE;
++
++	write_trfcr(trfcr);
++}
++
+ #ifdef CONFIG_ETM4X_IMPDEF_FEATURE
+ 
+ #define HISI_HIP08_AMBA_ID		0x000b6d01
+@@ -441,6 +480,7 @@ static int etm4_enable_hw(struct etmv4_drvdata *drvdata)
+ 	if (etm4x_is_ete(drvdata))
+ 		etm4x_relaxed_write32(csa, TRCRSR_TA, TRCRSR);
+ 
++	etm4x_allow_trace(drvdata);
+ 	/* Enable the trace unit */
+ 	etm4x_relaxed_write32(csa, 1, TRCPRGCTLR);
+ 
+@@ -724,7 +764,6 @@ static int etm4_enable(struct coresight_device *csdev,
+ static void etm4_disable_hw(void *info)
+ {
+ 	u32 control;
+-	u64 trfcr;
+ 	struct etmv4_drvdata *drvdata = info;
+ 	struct etmv4_config *config = &drvdata->config;
+ 	struct coresight_device *csdev = drvdata->csdev;
+@@ -751,12 +790,7 @@ static void etm4_disable_hw(void *info)
+ 	 * If the CPU supports v8.4 Trace filter Control,
+ 	 * set the ETM to trace prohibited region.
+ 	 */
+-	if (drvdata->trfc) {
+-		trfcr = read_sysreg_s(SYS_TRFCR_EL1);
+-		write_sysreg_s(trfcr & ~(TRFCR_ELx_ExTRE | TRFCR_ELx_E0TRE),
+-			       SYS_TRFCR_EL1);
+-		isb();
+-	}
++	etm4x_prohibit_trace(drvdata);
+ 	/*
+ 	 * Make sure everything completes before disabling, as recommended
+ 	 * by section 7.3.77 ("TRCVICTLR, ViewInst Main Control Register,
+@@ -772,9 +806,6 @@ static void etm4_disable_hw(void *info)
+ 	if (coresight_timeout(csa, TRCSTATR, TRCSTATR_PMSTABLE_BIT, 1))
+ 		dev_err(etm_dev,
+ 			"timeout while waiting for PM stable Trace Status\n");
+-	if (drvdata->trfc)
+-		write_sysreg_s(trfcr, SYS_TRFCR_EL1);
+-
+ 	/* read the status of the single shot comparators */
+ 	for (i = 0; i < drvdata->nr_ss_cmp; i++) {
+ 		config->ss_status[i] =
+@@ -969,15 +1000,15 @@ static bool etm4_init_csdev_access(struct etmv4_drvdata *drvdata,
+ 	return false;
+ }
+ 
+-static void cpu_enable_tracing(struct etmv4_drvdata *drvdata)
++static void cpu_detect_trace_filtering(struct etmv4_drvdata *drvdata)
+ {
+ 	u64 dfr0 = read_sysreg(id_aa64dfr0_el1);
+ 	u64 trfcr;
+ 
++	drvdata->trfcr = 0;
+ 	if (!cpuid_feature_extract_unsigned_field(dfr0, ID_AA64DFR0_TRACE_FILT_SHIFT))
+ 		return;
+ 
+-	drvdata->trfc = true;
+ 	/*
+ 	 * If the CPU supports v8.4 SelfHosted Tracing, enable
+ 	 * tracing at the kernel EL and EL0, forcing to use the
+@@ -991,7 +1022,7 @@ static void cpu_enable_tracing(struct etmv4_drvdata *drvdata)
+ 	if (is_kernel_in_hyp_mode())
+ 		trfcr |= TRFCR_EL2_CX;
+ 
+-	write_trfcr(trfcr);
++	drvdata->trfcr = trfcr;
+ }
+ 
+ static void etm4_init_arch_data(void *info)
+@@ -1177,7 +1208,7 @@ static void etm4_init_arch_data(void *info)
+ 	/* NUMCNTR, bits[30:28] number of counters available for tracing */
+ 	drvdata->nr_cntr = BMVAL(etmidr5, 28, 30);
+ 	etm4_cs_lock(drvdata, csa);
+-	cpu_enable_tracing(drvdata);
++	cpu_detect_trace_filtering(drvdata);
+ }
+ 
+ static inline u32 etm4_get_victlr_access_type(struct etmv4_config *config)
+@@ -1673,7 +1704,7 @@ static int etm4_cpu_save(struct etmv4_drvdata *drvdata)
+ 	int ret = 0;
+ 
+ 	/* Save the TRFCR irrespective of whether the ETM is ON */
+-	if (drvdata->trfc)
++	if (drvdata->trfcr)
+ 		drvdata->save_trfcr = read_trfcr();
+ 	/*
+ 	 * Save and restore the ETM Trace registers only if
+@@ -1782,7 +1813,7 @@ static void __etm4_cpu_restore(struct etmv4_drvdata *drvdata)
+ 
+ static void etm4_cpu_restore(struct etmv4_drvdata *drvdata)
+ {
+-	if (drvdata->trfc)
++	if (drvdata->trfcr)
+ 		write_trfcr(drvdata->save_trfcr);
+ 	if (drvdata->state_needs_restore)
+ 		__etm4_cpu_restore(drvdata);
+diff --git a/drivers/hwtracing/coresight/coresight-etm4x.h b/drivers/hwtracing/coresight/coresight-etm4x.h
+index 82cba16b73a6..3c4d69b096ca 100644
+--- a/drivers/hwtracing/coresight/coresight-etm4x.h
++++ b/drivers/hwtracing/coresight/coresight-etm4x.h
+@@ -919,7 +919,10 @@ struct etmv4_save_state {
+  * @nooverflow:	Indicate if overflow prevention is supported.
+  * @atbtrig:	If the implementation can support ATB triggers
+  * @lpoverride:	If the implementation can support low-power state over.
+- * @trfc:	If the implementation supports Arm v8.4 trace filter controls.
++ * @trfcr:	If the CPU supports FEAT_TRF, value of the TRFCR_ELx that
++ *		allows tracing at all ELs. We don't want to compute this
++ *		at runtime, due to the additional setting of TRFCR_CX when
++ *		in EL2. Otherwise, 0.
+  * @config:	structure holding configuration parameters.
+  * @save_trfcr:	Saved TRFCR_EL1 register during a CPU PM event.
+  * @save_state:	State to be preserved across power loss
+@@ -972,7 +975,7 @@ struct etmv4_drvdata {
+ 	bool				nooverflow;
+ 	bool				atbtrig;
+ 	bool				lpoverride;
+-	bool				trfc;
++	u64				trfcr;
+ 	struct etmv4_config		config;
+ 	u64				save_trfcr;
+ 	struct etmv4_save_state		*save_state;
+diff --git a/drivers/hwtracing/coresight/coresight-self-hosted-trace.h b/drivers/hwtracing/coresight/coresight-self-hosted-trace.h
+index 303d71911870..23f05df3f173 100644
+--- a/drivers/hwtracing/coresight/coresight-self-hosted-trace.h
++++ b/drivers/hwtracing/coresight/coresight-self-hosted-trace.h
+@@ -21,4 +21,11 @@ static inline void write_trfcr(u64 val)
+ 	isb();
+ }
+ 
++static inline void cpu_prohibit_trace(void)
++{
++	u64 trfcr = read_trfcr();
++
++	/* Prohibit tracing at EL0 & the kernel EL */
++	write_trfcr(trfcr & ~(TRFCR_ELx_ExTRE | TRFCR_ELx_E0TRE));
++}
+ #endif /*  __CORESIGHT_SELF_HOSTED_TRACE_H */
+-- 
+2.34.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0017-perf-arm-cmn-Use-irq_set_affinity.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0017-perf-arm-cmn-Use-irq_set_affinity.patch
new file mode 100644
index 0000000..e8674c3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0017-perf-arm-cmn-Use-irq_set_affinity.patch
@@ -0,0 +1,74 @@
+From ad3c5d9224ffcd7b2e083f03441c6188d2bbef67 Mon Sep 17 00:00:00 2001
+From: Thomas Gleixner <tglx@linutronix.de>
+Date: Tue, 18 May 2021 11:17:28 +0200
+Subject: [PATCH 01/14] perf/arm-cmn: Use irq_set_affinity()
+
+The driver uses irq_set_affinity_hint() to set the affinity for the PMU
+interrupts, which relies on the undocumented side effect that this function
+actually sets the affinity under the hood.
+
+Setting an hint is clearly not a guarantee and for these PMU interrupts an
+affinity hint, which is supposed to guide userspace for setting affinity,
+is beyond pointless, because the affinity of these interrupts cannot be
+modified from user space.
+
+Aside of that the error checks are bogus because the only error which is
+returned from irq_set_affinity_hint() is when there is no irq descriptor
+for the interrupt number, but not when the affinity set fails. That's on
+purpose because the hint can point to an offline CPU.
+
+Replace the mindless abuse with irq_set_affinity().
+
+Signed-off-by: Thomas Gleixner <tglx@linutronix.de>
+
+Link: https://lore.kernel.org/r/20210518093118.277228577@linutronix.de
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/20210518093118.277228577@linutronix.de]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 9 ++-------
+ 1 file changed, 2 insertions(+), 7 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index 46defb1dcf86..38fa6f89d0bc 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -1162,7 +1162,7 @@ static int arm_cmn_pmu_offline_cpu(unsigned int cpu, struct hlist_node *node)
+ 
+ 	perf_pmu_migrate_context(&cmn->pmu, cpu, target);
+ 	for (i = 0; i < cmn->num_dtcs; i++)
+-		irq_set_affinity_hint(cmn->dtc[i].irq, cpumask_of(target));
++		irq_set_affinity(cmn->dtc[i].irq, cpumask_of(target));
+ 	cmn->cpu = target;
+ 	return 0;
+ }
+@@ -1222,7 +1222,7 @@ static int arm_cmn_init_irqs(struct arm_cmn *cmn)
+ 		if (err)
+ 			return err;
+ 
+-		err = irq_set_affinity_hint(irq, cpumask_of(cmn->cpu));
++		err = irq_set_affinity(irq, cpumask_of(cmn->cpu));
+ 		if (err)
+ 			return err;
+ 	next:
+@@ -1568,16 +1568,11 @@ static int arm_cmn_probe(struct platform_device *pdev)
+ static int arm_cmn_remove(struct platform_device *pdev)
+ {
+ 	struct arm_cmn *cmn = platform_get_drvdata(pdev);
+-	int i;
+ 
+ 	writel_relaxed(0, cmn->dtc[0].base + CMN_DT_DTC_CTL);
+ 
+ 	perf_pmu_unregister(&cmn->pmu);
+ 	cpuhp_state_remove_instance(arm_cmn_hp_state, &cmn->cpuhp_node);
+-
+-	for (i = 0; i < cmn->num_dtcs; i++)
+-		irq_set_affinity_hint(cmn->dtc[i].irq, NULL);
+-
+ 	return 0;
+ }
+ 
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0018-perf-arm-cmn-Fix-CPU-hotplug-unregistration.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0018-perf-arm-cmn-Fix-CPU-hotplug-unregistration.patch
new file mode 100644
index 0000000..e06fb88
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0018-perf-arm-cmn-Fix-CPU-hotplug-unregistration.patch
@@ -0,0 +1,46 @@
+From 249304c3517a38863c8e45e63d509d01bd67dead Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:44:50 +0000
+Subject: [PATCH 02/14] perf/arm-cmn: Fix CPU hotplug unregistration
+
+Attempting to migrate the PMU context after we've unregistered the PMU
+device, or especially if we never successfully registered it in the
+first place, is a woefully bad idea. It's also fundamentally pointless
+anyway. Make sure to unregister an instance from the hotplug handler
+*without* invoking the teardown callback.
+
+Fixes: 0ba64770a2f2 ("perf: Add Arm CMN-600 PMU driver")
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/2c221d745544774e4b07583b65b5d4d94f7e0fe4.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 5 +++--
+ 1 file changed, 3 insertions(+), 2 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index 38fa6f89d0bc..fe7f3e945481 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -1561,7 +1561,8 @@ static int arm_cmn_probe(struct platform_device *pdev)
+ 
+ 	err = perf_pmu_register(&cmn->pmu, name, -1);
+ 	if (err)
+-		cpuhp_state_remove_instance(arm_cmn_hp_state, &cmn->cpuhp_node);
++		cpuhp_state_remove_instance_nocalls(arm_cmn_hp_state, &cmn->cpuhp_node);
++
+ 	return err;
+ }
+ 
+@@ -1572,7 +1573,7 @@ static int arm_cmn_remove(struct platform_device *pdev)
+ 	writel_relaxed(0, cmn->dtc[0].base + CMN_DT_DTC_CTL);
+ 
+ 	perf_pmu_unregister(&cmn->pmu);
+-	cpuhp_state_remove_instance(arm_cmn_hp_state, &cmn->cpuhp_node);
++	cpuhp_state_remove_instance_nocalls(arm_cmn_hp_state, &cmn->cpuhp_node);
+ 	return 0;
+ }
+ 
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0019-perf-arm-cmn-Account-for-NUMA-affinity.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0019-perf-arm-cmn-Account-for-NUMA-affinity.patch
new file mode 100644
index 0000000..f93bff7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0019-perf-arm-cmn-Account-for-NUMA-affinity.patch
@@ -0,0 +1,107 @@
+From c4b023618252ad8c03b7ae2cc411718af285bf66 Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:44:51 +0000
+Subject: [PATCH 03/14] perf/arm-cmn: Account for NUMA affinity
+
+On a system with multiple CMN meshes, ideally we'd want to access each
+PMU from within its own mesh, rather than with a long CML round-trip,
+wherever feasible. Since such a system is likely to be presented as
+multiple NUMA nodes, let's also hope a proximity domain is specified
+for each CMN programming interface, and use that to guide our choice
+of IRQ affinity to favour a node-local CPU where possible.
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/32438b0d016e0649d882d47d30ac2000484287b9.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/32438b0d016e0649d882d47d30ac2000484287b9.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 51 +++++++++++++++++++++++++++++++-----------
+ 1 file changed, 38 insertions(+), 13 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index fe7f3e945481..2146d1c0103f 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -1147,23 +1147,47 @@ static int arm_cmn_commit_txn(struct pmu *pmu)
+ 	return 0;
+ }
+ 
+-static int arm_cmn_pmu_offline_cpu(unsigned int cpu, struct hlist_node *node)
++static void arm_cmn_migrate(struct arm_cmn *cmn, unsigned int cpu)
++{
++	unsigned int i;
++
++	perf_pmu_migrate_context(&cmn->pmu, cmn->cpu, cpu);
++	for (i = 0; i < cmn->num_dtcs; i++)
++		irq_set_affinity(cmn->dtc[i].irq, cpumask_of(cpu));
++	cmn->cpu = cpu;
++}
++
++static int arm_cmn_pmu_online_cpu(unsigned int cpu, struct hlist_node *cpuhp_node)
+ {
+ 	struct arm_cmn *cmn;
+-	unsigned int i, target;
++	int node;
+ 
+-	cmn = hlist_entry_safe(node, struct arm_cmn, cpuhp_node);
+-	if (cpu != cmn->cpu)
+-		return 0;
++	cmn = hlist_entry_safe(cpuhp_node, struct arm_cmn, cpuhp_node);
++	node = dev_to_node(cmn->dev);
++	if (node != NUMA_NO_NODE && cpu_to_node(cmn->cpu) != node && cpu_to_node(cpu) == node)
++		arm_cmn_migrate(cmn, cpu);
++	return 0;
++}
++
++static int arm_cmn_pmu_offline_cpu(unsigned int cpu, struct hlist_node *cpuhp_node)
++{
++	struct arm_cmn *cmn;
++	unsigned int target;
++	int node;
++	cpumask_t mask;
+ 
+-	target = cpumask_any_but(cpu_online_mask, cpu);
+-	if (target >= nr_cpu_ids)
++	cmn = hlist_entry_safe(cpuhp_node, struct arm_cmn, cpuhp_node);
++	if (cpu != cmn->cpu)
+ 		return 0;
+ 
+-	perf_pmu_migrate_context(&cmn->pmu, cpu, target);
+-	for (i = 0; i < cmn->num_dtcs; i++)
+-		irq_set_affinity(cmn->dtc[i].irq, cpumask_of(target));
+-	cmn->cpu = target;
++	node = dev_to_node(cmn->dev);
++	if (cpumask_and(&mask, cpumask_of_node(node), cpu_online_mask) &&
++	    cpumask_andnot(&mask, &mask, cpumask_of(cpu)))
++		target = cpumask_any(&mask);
++	else
++		target = cpumask_any_but(cpu_online_mask, cpu);
++	if (target < nr_cpu_ids)
++		arm_cmn_migrate(cmn, target);
+ 	return 0;
+ }
+ 
+@@ -1532,7 +1556,7 @@ static int arm_cmn_probe(struct platform_device *pdev)
+ 	if (err)
+ 		return err;
+ 
+-	cmn->cpu = raw_smp_processor_id();
++	cmn->cpu = cpumask_local_spread(0, dev_to_node(cmn->dev));
+ 	cmn->pmu = (struct pmu) {
+ 		.module = THIS_MODULE,
+ 		.attr_groups = arm_cmn_attr_groups,
+@@ -1608,7 +1632,8 @@ static int __init arm_cmn_init(void)
+ 	int ret;
+ 
+ 	ret = cpuhp_setup_state_multi(CPUHP_AP_ONLINE_DYN,
+-				      "perf/arm/cmn:online", NULL,
++				      "perf/arm/cmn:online",
++				      arm_cmn_pmu_online_cpu,
+ 				      arm_cmn_pmu_offline_cpu);
+ 	if (ret < 0)
+ 		return ret;
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0020-perf-arm-cmn-Drop-compile-test-restriction.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0020-perf-arm-cmn-Drop-compile-test-restriction.patch
new file mode 100644
index 0000000..1a3e2d9
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0020-perf-arm-cmn-Drop-compile-test-restriction.patch
@@ -0,0 +1,89 @@
+From c3b11ad7a7e3e154a17f36c6768deab9227e28de Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:44:52 +0000
+Subject: [PATCH 04/14] perf/arm-cmn: Drop compile-test restriction
+
+Although CMN is currently (and overwhelmingly likely to remain) deployed
+in arm64-only (modulo userspace) systems, the 64-bit "dependency" for
+compile-testing was just laziness due to heavy reliance on readq/writeq
+accessors. Since we only need one extra include for robustness in that
+regard, let's pull that in, widen the compile-test coverage, and fix up
+the smattering of type laziness that that brings to light.
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/baee9ee0d0bdad8aaeb70f5a4b98d8fd4b1f5786.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/baee9ee0d0bdad8aaeb70f5a4b98d8fd4b1f5786.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/Kconfig   |  2 +-
+ drivers/perf/arm-cmn.c | 25 +++++++++++++------------
+ 2 files changed, 14 insertions(+), 13 deletions(-)
+
+diff --git a/drivers/perf/Kconfig b/drivers/perf/Kconfig
+index 130327ff0b0e..828a042d6a07 100644
+--- a/drivers/perf/Kconfig
++++ b/drivers/perf/Kconfig
+@@ -43,7 +43,7 @@ config ARM_CCN
+ 
+ config ARM_CMN
+ 	tristate "Arm CMN-600 PMU support"
+-	depends on ARM64 || (COMPILE_TEST && 64BIT)
++	depends on ARM64 || COMPILE_TEST
+ 	help
+ 	  Support for PMU events monitoring on the Arm CMN-600 Coherent Mesh
+ 	  Network interconnect.
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index 2146d1c0103f..e9af79b5f3de 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -7,6 +7,7 @@
+ #include <linux/bitops.h>
+ #include <linux/interrupt.h>
+ #include <linux/io.h>
++#include <linux/io-64-nonatomic-lo-hi.h>
+ #include <linux/kernel.h>
+ #include <linux/list.h>
+ #include <linux/module.h>
+@@ -122,11 +123,11 @@
+ 
+ 
+ /* Event attributes */
+-#define CMN_CONFIG_TYPE			GENMASK(15, 0)
+-#define CMN_CONFIG_EVENTID		GENMASK(23, 16)
+-#define CMN_CONFIG_OCCUPID		GENMASK(27, 24)
+-#define CMN_CONFIG_BYNODEID		BIT(31)
+-#define CMN_CONFIG_NODEID		GENMASK(47, 32)
++#define CMN_CONFIG_TYPE			GENMASK_ULL(15, 0)
++#define CMN_CONFIG_EVENTID		GENMASK_ULL(23, 16)
++#define CMN_CONFIG_OCCUPID		GENMASK_ULL(27, 24)
++#define CMN_CONFIG_BYNODEID		BIT_ULL(31)
++#define CMN_CONFIG_NODEID		GENMASK_ULL(47, 32)
+ 
+ #define CMN_EVENT_TYPE(event)		FIELD_GET(CMN_CONFIG_TYPE, (event)->attr.config)
+ #define CMN_EVENT_EVENTID(event)	FIELD_GET(CMN_CONFIG_EVENTID, (event)->attr.config)
+@@ -134,13 +135,13 @@
+ #define CMN_EVENT_BYNODEID(event)	FIELD_GET(CMN_CONFIG_BYNODEID, (event)->attr.config)
+ #define CMN_EVENT_NODEID(event)		FIELD_GET(CMN_CONFIG_NODEID, (event)->attr.config)
+ 
+-#define CMN_CONFIG_WP_COMBINE		GENMASK(27, 24)
+-#define CMN_CONFIG_WP_DEV_SEL		BIT(48)
+-#define CMN_CONFIG_WP_CHN_SEL		GENMASK(50, 49)
+-#define CMN_CONFIG_WP_GRP		BIT(52)
+-#define CMN_CONFIG_WP_EXCLUSIVE		BIT(53)
+-#define CMN_CONFIG1_WP_VAL		GENMASK(63, 0)
+-#define CMN_CONFIG2_WP_MASK		GENMASK(63, 0)
++#define CMN_CONFIG_WP_COMBINE		GENMASK_ULL(27, 24)
++#define CMN_CONFIG_WP_DEV_SEL		BIT_ULL(48)
++#define CMN_CONFIG_WP_CHN_SEL		GENMASK_ULL(50, 49)
++#define CMN_CONFIG_WP_GRP		BIT_ULL(52)
++#define CMN_CONFIG_WP_EXCLUSIVE		BIT_ULL(53)
++#define CMN_CONFIG1_WP_VAL		GENMASK_ULL(63, 0)
++#define CMN_CONFIG2_WP_MASK		GENMASK_ULL(63, 0)
+ 
+ #define CMN_EVENT_WP_COMBINE(event)	FIELD_GET(CMN_CONFIG_WP_COMBINE, (event)->attr.config)
+ #define CMN_EVENT_WP_DEV_SEL(event)	FIELD_GET(CMN_CONFIG_WP_DEV_SEL, (event)->attr.config)
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0021-perf-arm-cmn-Refactor-node-ID-handling.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0021-perf-arm-cmn-Refactor-node-ID-handling.patch
new file mode 100644
index 0000000..ce4c2e5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0021-perf-arm-cmn-Refactor-node-ID-handling.patch
@@ -0,0 +1,155 @@
+From 4f4a4cd7c79396fa72870ff712d15e82ebff80cf Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:44:53 +0000
+Subject: [PATCH 05/14] perf/arm-cmn: Refactor node ID handling
+
+Add a bit more abstraction for the places where we decompose node IDs.
+This will help keep things nice and manageable when we come to add yet
+more variables which affect the node ID format. Also use the opportunity
+to move the rest of the low-level node management helpers back up to the
+logical place they were meant to be - how they ended up buried right in
+the middle of the event-related definitions is somewhat of a mystery...
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/a2242a8c3c96056c13a04ae87bf2047e5e64d2d9.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/a2242a8c3c96056c13a04ae87bf2047e5e64d2d9.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 94 +++++++++++++++++++++++++-----------------
+ 1 file changed, 56 insertions(+), 38 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index e9af79b5f3de..cee301fe0f7e 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -255,6 +255,58 @@ struct arm_cmn {
+ 
+ static int arm_cmn_hp_state;
+ 
++struct arm_cmn_nodeid {
++	u8 x;
++	u8 y;
++	u8 port;
++	u8 dev;
++};
++
++static int arm_cmn_xyidbits(const struct arm_cmn *cmn)
++{
++	int dim = max(cmn->mesh_x, cmn->mesh_y);
++
++	return dim > 4 ? 3 : 2;
++}
++
++static struct arm_cmn_nodeid arm_cmn_nid(const struct arm_cmn *cmn, u16 id)
++{
++	struct arm_cmn_nodeid nid;
++	int bits = arm_cmn_xyidbits(cmn);
++
++	nid.x = CMN_NODEID_X(id, bits);
++	nid.y = CMN_NODEID_Y(id, bits);
++	nid.port = CMN_NODEID_PID(id);
++	nid.dev = CMN_NODEID_DEVID(id);
++
++	return nid;
++}
++
++static void arm_cmn_init_node_to_xp(const struct arm_cmn *cmn,
++				    struct arm_cmn_node *dn)
++{
++	struct arm_cmn_nodeid nid = arm_cmn_nid(cmn, dn->id);
++	int xp_idx = cmn->mesh_x * nid.y + nid.x;
++
++	dn->to_xp = (cmn->xps + xp_idx) - dn;
++}
++
++static struct arm_cmn_node *arm_cmn_node_to_xp(struct arm_cmn_node *dn)
++{
++	return dn->type == CMN_TYPE_XP ? dn : dn + dn->to_xp;
++}
++
++static struct arm_cmn_node *arm_cmn_node(const struct arm_cmn *cmn,
++					 enum cmn_node_type type)
++{
++	int i;
++
++	for (i = 0; i < cmn->num_dns; i++)
++		if (cmn->dns[i].type == type)
++			return &cmn->dns[i];
++	return NULL;
++}
++
+ struct arm_cmn_hw_event {
+ 	struct arm_cmn_node *dn;
+ 	u64 dtm_idx[2];
+@@ -295,38 +347,6 @@ struct arm_cmn_format_attr {
+ 	int config;
+ };
+ 
+-static int arm_cmn_xyidbits(const struct arm_cmn *cmn)
+-{
+-	return cmn->mesh_x > 4 || cmn->mesh_y > 4 ? 3 : 2;
+-}
+-
+-static void arm_cmn_init_node_to_xp(const struct arm_cmn *cmn,
+-				    struct arm_cmn_node *dn)
+-{
+-	int bits = arm_cmn_xyidbits(cmn);
+-	int x = CMN_NODEID_X(dn->id, bits);
+-	int y = CMN_NODEID_Y(dn->id, bits);
+-	int xp_idx = cmn->mesh_x * y + x;
+-
+-	dn->to_xp = (cmn->xps + xp_idx) - dn;
+-}
+-
+-static struct arm_cmn_node *arm_cmn_node_to_xp(struct arm_cmn_node *dn)
+-{
+-	return dn->type == CMN_TYPE_XP ? dn : dn + dn->to_xp;
+-}
+-
+-static struct arm_cmn_node *arm_cmn_node(const struct arm_cmn *cmn,
+-					 enum cmn_node_type type)
+-{
+-	int i;
+-
+-	for (i = 0; i < cmn->num_dns; i++)
+-		if (cmn->dns[i].type == type)
+-			return &cmn->dns[i];
+-	return NULL;
+-}
+-
+ #define CMN_EVENT_ATTR(_name, _type, _eventid, _occupid)		\
+ 	(&((struct arm_cmn_event_attr[]) {{				\
+ 		.attr = __ATTR(_name, 0444, arm_cmn_event_show, NULL),	\
+@@ -966,11 +986,10 @@ static int arm_cmn_event_init(struct perf_event *event)
+ 	}
+ 
+ 	if (!hw->num_dns) {
+-		int bits = arm_cmn_xyidbits(cmn);
++		struct arm_cmn_nodeid nid = arm_cmn_nid(cmn, nodeid);
+ 
+ 		dev_dbg(cmn->dev, "invalid node 0x%x (%d,%d,%d,%d) type 0x%x\n",
+-			nodeid, CMN_NODEID_X(nodeid, bits), CMN_NODEID_Y(nodeid, bits),
+-			CMN_NODEID_PID(nodeid), CMN_NODEID_DEVID(nodeid), type);
++			nodeid, nid.x, nid.y, nid.port, nid.dev, type);
+ 		return -EINVAL;
+ 	}
+ 	/*
+@@ -1068,11 +1087,10 @@ static int arm_cmn_event_add(struct perf_event *event, int flags)
+ 			dn->wp_event[wp_idx] = dtc_idx;
+ 			writel_relaxed(cfg, dn->pmu_base + CMN_DTM_WPn_CONFIG(wp_idx));
+ 		} else {
+-			unsigned int port = CMN_NODEID_PID(dn->id);
+-			unsigned int dev = CMN_NODEID_DEVID(dn->id);
++			struct arm_cmn_nodeid nid = arm_cmn_nid(cmn, dn->id);
+ 
+ 			input_sel = CMN__PMEVCNT0_INPUT_SEL_DEV + dtm_idx +
+-				    (port << 4) + (dev << 2);
++				    (nid.port << 4) + (nid.dev << 2);
+ 
+ 			if (arm_cmn_is_occup_event(type, CMN_EVENT_EVENTID(event))) {
+ 				int occupid = CMN_EVENT_OCCUPID(event);
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0022-perf-arm-cmn-Streamline-node-iteration.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0022-perf-arm-cmn-Streamline-node-iteration.patch
new file mode 100644
index 0000000..fbe49b2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0022-perf-arm-cmn-Streamline-node-iteration.patch
@@ -0,0 +1,118 @@
+From 1b8e1ce0ebaa02c4cb7fa615b28c6905b0884e41 Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:44:54 +0000
+Subject: [PATCH 06/14] perf/arm-cmn: Streamline node iteration
+
+Refactor the places where we scan through the set of nodes to switch
+from explicit array indexing to pointer-based iteration. This leads to
+slightly simpler object code, but also makes the source less dense and
+more pleasant for further development. It also unearths an almost-bug
+in arm_cmn_event_init() where we've been depending on the "array index"
+of NULL relative to cmn->dns being a sufficiently large number, yuck.
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/ee0c9eda9a643f46001ac43aadf3f0b1fd5660dd.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/ee0c9eda9a643f46001ac43aadf3f0b1fd5660dd.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 33 ++++++++++++++++++++-------------
+ 1 file changed, 20 insertions(+), 13 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index cee301fe0f7e..77ebed7fae08 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -299,11 +299,11 @@ static struct arm_cmn_node *arm_cmn_node_to_xp(struct arm_cmn_node *dn)
+ static struct arm_cmn_node *arm_cmn_node(const struct arm_cmn *cmn,
+ 					 enum cmn_node_type type)
+ {
+-	int i;
++	struct arm_cmn_node *dn;
+ 
+-	for (i = 0; i < cmn->num_dns; i++)
+-		if (cmn->dns[i].type == type)
+-			return &cmn->dns[i];
++	for (dn = cmn->dns; dn->type; dn++)
++		if (dn->type == type)
++			return dn;
+ 	return NULL;
+ }
+ 
+@@ -941,8 +941,8 @@ static int arm_cmn_event_init(struct perf_event *event)
+ {
+ 	struct arm_cmn *cmn = to_cmn(event->pmu);
+ 	struct arm_cmn_hw_event *hw = to_cmn_hw(event);
++	struct arm_cmn_node *dn;
+ 	enum cmn_node_type type;
+-	unsigned int i;
+ 	bool bynodeid;
+ 	u16 nodeid, eventid;
+ 
+@@ -974,10 +974,12 @@ static int arm_cmn_event_init(struct perf_event *event)
+ 	nodeid = CMN_EVENT_NODEID(event);
+ 
+ 	hw->dn = arm_cmn_node(cmn, type);
+-	for (i = hw->dn - cmn->dns; i < cmn->num_dns && cmn->dns[i].type == type; i++) {
++	if (!hw->dn)
++		return -EINVAL;
++	for (dn = hw->dn; dn->type == type; dn++) {
+ 		if (!bynodeid) {
+ 			hw->num_dns++;
+-		} else if (cmn->dns[i].id != nodeid) {
++		} else if (dn->id != nodeid) {
+ 			hw->dn++;
+ 		} else {
+ 			hw->num_dns = 1;
+@@ -1332,7 +1334,7 @@ static int arm_cmn_init_dtcs(struct arm_cmn *cmn)
+ 
+ 	cmn->xps = arm_cmn_node(cmn, CMN_TYPE_XP);
+ 
+-	for (dn = cmn->dns; dn < cmn->dns + cmn->num_dns; dn++) {
++	for (dn = cmn->dns; dn->type; dn++) {
+ 		if (dn->type != CMN_TYPE_XP)
+ 			arm_cmn_init_node_to_xp(cmn, dn);
+ 		else if (cmn->num_dtcs == 1)
+@@ -1382,6 +1384,7 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 	u32 xp_offset[CMN_MAX_XPS];
+ 	u64 reg;
+ 	int i, j;
++	size_t sz;
+ 
+ 	cfg_region = cmn->base + rgn_offset;
+ 	reg = readl_relaxed(cfg_region + CMN_CFGM_PERIPH_ID_2);
+@@ -1408,14 +1411,13 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 		cmn->num_dns += FIELD_GET(CMN_CI_CHILD_COUNT, reg);
+ 	}
+ 
+-	/* Cheeky +1 to help terminate pointer-based iteration */
+-	cmn->dns = devm_kcalloc(cmn->dev, cmn->num_dns + 1,
+-				sizeof(*cmn->dns), GFP_KERNEL);
+-	if (!cmn->dns)
++	/* Cheeky +1 to help terminate pointer-based iteration later */
++	dn = devm_kcalloc(cmn->dev, cmn->num_dns + 1, sizeof(*dn), GFP_KERNEL);
++	if (!dn)
+ 		return -ENOMEM;
+ 
+ 	/* Pass 2: now we can actually populate the nodes */
+-	dn = cmn->dns;
++	cmn->dns = dn;
+ 	for (i = 0; i < cmn->num_xps; i++) {
+ 		void __iomem *xp_region = cmn->base + xp_offset[i];
+ 		struct arm_cmn_node *xp = dn++;
+@@ -1484,6 +1486,11 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 	/* Correct for any nodes we skipped */
+ 	cmn->num_dns = dn - cmn->dns;
+ 
++	sz = (void *)(dn + 1) - (void *)cmn->dns;
++	dn = devm_krealloc(cmn->dev, cmn->dns, sz, GFP_KERNEL);
++	if (dn)
++		cmn->dns = dn;
++
+ 	/*
+ 	 * If mesh_x wasn't set during discovery then we never saw
+ 	 * an XP at (0,1), thus we must have an Nx1 configuration.
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0023-drivers-perf-arm-cmn-Add-space-after.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0023-drivers-perf-arm-cmn-Add-space-after.patch
new file mode 100644
index 0000000..3b11192
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0023-drivers-perf-arm-cmn-Add-space-after.patch
@@ -0,0 +1,34 @@
+From c3e137a2231f434f623593b6951c7575d22e1cdb Mon Sep 17 00:00:00 2001
+From: Junhao He <hejunhao2@hisilicon.com>
+Date: Tue, 11 May 2021 20:27:33 +0800
+Subject: [PATCH 07/14] drivers/perf: arm-cmn: Add space after ','
+
+Fix a warning from checkpatch.pl.
+
+ERROR: space required after that ',' (ctx:VxV)
+
+Signed-off-by: Junhao He <hejunhao2@hisilicon.com>
+Signed-off-by: Jay Fang <f.fangjian@huawei.com>
+
+Upstream-Status: Backport [https://lore.kernel.org/all/1620736054-58412-4-git-send-email-f.fangjian@huawei.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index 77ebed7fae08..e9f27f7776a2 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -32,7 +32,7 @@
+ #define CMN_CI_CHILD_COUNT		GENMASK_ULL(15, 0)
+ #define CMN_CI_CHILD_PTR_OFFSET		GENMASK_ULL(31, 16)
+ 
+-#define CMN_CHILD_NODE_ADDR		GENMASK(27,0)
++#define CMN_CHILD_NODE_ADDR		GENMASK(27, 0)
+ #define CMN_CHILD_NODE_EXTERNAL		BIT(31)
+ 
+ #define CMN_ADDR_NODE_PTR		GENMASK(27, 14)
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0024-perf-arm-cmn-Refactor-DTM-handling.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0024-perf-arm-cmn-Refactor-DTM-handling.patch
new file mode 100644
index 0000000..6a68a79
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0024-perf-arm-cmn-Refactor-DTM-handling.patch
@@ -0,0 +1,406 @@
+From 79bbc3eeee54b2e039dd4822e4a643e71adfbb04 Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:44:55 +0000
+Subject: [PATCH 08/14] perf/arm-cmn: Refactor DTM handling
+
+Untangle DTMs from XPs into a dedicated abstraction. This helps make
+things a little more obvious and robust, but primarily paves the way
+for further development where new IPs can grow extra DTMs per XP.
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/9cca18b1b98f482df7f1aaf3d3213e7f39500423.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/9cca18b1b98f482df7f1aaf3d3213e7f39500423.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 169 +++++++++++++++++++++--------------------
+ 1 file changed, 87 insertions(+), 82 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index e9f27f7776a2..2ae3e92690a7 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -35,14 +35,9 @@
+ #define CMN_CHILD_NODE_ADDR		GENMASK(27, 0)
+ #define CMN_CHILD_NODE_EXTERNAL		BIT(31)
+ 
+-#define CMN_ADDR_NODE_PTR		GENMASK(27, 14)
+-
+-#define CMN_NODE_PTR_DEVID(ptr)		(((ptr) >> 2) & 3)
+-#define CMN_NODE_PTR_PID(ptr)		((ptr) & 1)
+-#define CMN_NODE_PTR_X(ptr, bits)	((ptr) >> (6 + (bits)))
+-#define CMN_NODE_PTR_Y(ptr, bits)	(((ptr) >> 6) & ((1U << (bits)) - 1))
+-
+-#define CMN_MAX_XPS			(8 * 8)
++#define CMN_MAX_DIMENSION		8
++#define CMN_MAX_XPS			(CMN_MAX_DIMENSION * CMN_MAX_DIMENSION)
++#define CMN_MAX_DTMS			CMN_MAX_XPS
+ 
+ /* The CFG node has one other useful purpose */
+ #define CMN_CFGM_PERIPH_ID_2		0x0010
+@@ -190,32 +185,32 @@ struct arm_cmn_node {
+ 	u16 id, logid;
+ 	enum cmn_node_type type;
+ 
++	int dtm;
+ 	union {
+-		/* Device node */
++		/* DN/HN-F/CXHA */
+ 		struct {
+-			int to_xp;
+-			/* DN/HN-F/CXHA */
+-			unsigned int occupid_val;
+-			unsigned int occupid_count;
++			u8 occupid_val;
++			u8 occupid_count;
+ 		};
+ 		/* XP */
+-		struct {
+-			int dtc;
+-			u32 pmu_config_low;
+-			union {
+-				u8 input_sel[4];
+-				__le32 pmu_config_high;
+-			};
+-			s8 wp_event[4];
+-		};
++		int dtc;
+ 	};
+-
+ 	union {
+ 		u8 event[4];
+ 		__le32 event_sel;
+ 	};
+ };
+ 
++struct arm_cmn_dtm {
++	void __iomem *base;
++	u32 pmu_config_low;
++	union {
++		u8 input_sel[4];
++		__le32 pmu_config_high;
++	};
++	s8 wp_event[4];
++};
++
+ struct arm_cmn_dtc {
+ 	void __iomem *base;
+ 	int irq;
+@@ -241,6 +236,7 @@ struct arm_cmn {
+ 	struct arm_cmn_node *xps;
+ 	struct arm_cmn_node *dns;
+ 
++	struct arm_cmn_dtm *dtms;
+ 	struct arm_cmn_dtc *dtc;
+ 	unsigned int num_dtcs;
+ 
+@@ -282,20 +278,14 @@ static struct arm_cmn_nodeid arm_cmn_nid(const struct arm_cmn *cmn, u16 id)
+ 	return nid;
+ }
+ 
+-static void arm_cmn_init_node_to_xp(const struct arm_cmn *cmn,
+-				    struct arm_cmn_node *dn)
++static struct arm_cmn_node *arm_cmn_node_to_xp(const struct arm_cmn *cmn,
++					       const struct arm_cmn_node *dn)
+ {
+ 	struct arm_cmn_nodeid nid = arm_cmn_nid(cmn, dn->id);
+ 	int xp_idx = cmn->mesh_x * nid.y + nid.x;
+ 
+-	dn->to_xp = (cmn->xps + xp_idx) - dn;
+-}
+-
+-static struct arm_cmn_node *arm_cmn_node_to_xp(struct arm_cmn_node *dn)
+-{
+-	return dn->type == CMN_TYPE_XP ? dn : dn + dn->to_xp;
++	return cmn->xps + xp_idx;
+ }
+-
+ static struct arm_cmn_node *arm_cmn_node(const struct arm_cmn *cmn,
+ 					 enum cmn_node_type type)
+ {
+@@ -706,9 +696,9 @@ static u64 arm_cmn_read_dtm(struct arm_cmn *cmn, struct arm_cmn_hw_event *hw,
+ 
+ 	offset = snapshot ? CMN_DTM_PMEVCNTSR : CMN_DTM_PMEVCNT;
+ 	for_each_hw_dn(hw, dn, i) {
+-		struct arm_cmn_node *xp = arm_cmn_node_to_xp(dn);
++		struct arm_cmn_dtm *dtm = &cmn->dtms[dn->dtm];
+ 		int dtm_idx = arm_cmn_get_index(hw->dtm_idx, i);
+-		u64 reg = readq_relaxed(xp->pmu_base + offset);
++		u64 reg = readq_relaxed(dtm->base + offset);
+ 		u16 dtm_count = reg >> (dtm_idx * 16);
+ 
+ 		count += dtm_count;
+@@ -835,9 +825,9 @@ static void arm_cmn_event_stop(struct perf_event *event, int flags)
+ }
+ 
+ struct arm_cmn_val {
+-	u8 dtm_count[CMN_MAX_XPS];
+-	u8 occupid[CMN_MAX_XPS];
+-	u8 wp[CMN_MAX_XPS][4];
++	u8 dtm_count[CMN_MAX_DTMS];
++	u8 occupid[CMN_MAX_DTMS];
++	u8 wp[CMN_MAX_DTMS][4];
+ 	int dtc_count;
+ 	bool cycles;
+ };
+@@ -866,16 +856,16 @@ static void arm_cmn_val_add_event(struct arm_cmn_val *val, struct perf_event *ev
+ 		occupid = 0;
+ 
+ 	for_each_hw_dn(hw, dn, i) {
+-		int wp_idx, xp = arm_cmn_node_to_xp(dn)->logid;
++		int wp_idx, dtm = dn->dtm;
+ 
+-		val->dtm_count[xp]++;
+-		val->occupid[xp] = occupid;
++		val->dtm_count[dtm]++;
++		val->occupid[dtm] = occupid;
+ 
+ 		if (type != CMN_TYPE_WP)
+ 			continue;
+ 
+ 		wp_idx = arm_cmn_wp_idx(event);
+-		val->wp[xp][wp_idx] = CMN_EVENT_WP_COMBINE(event) + 1;
++		val->wp[dtm][wp_idx] = CMN_EVENT_WP_COMBINE(event) + 1;
+ 	}
+ }
+ 
+@@ -914,22 +904,22 @@ static int arm_cmn_validate_group(struct perf_event *event)
+ 		occupid = 0;
+ 
+ 	for_each_hw_dn(hw, dn, i) {
+-		int wp_idx, wp_cmb, xp = arm_cmn_node_to_xp(dn)->logid;
++		int wp_idx, wp_cmb, dtm = dn->dtm;
+ 
+-		if (val.dtm_count[xp] == CMN_DTM_NUM_COUNTERS)
++		if (val.dtm_count[dtm] == CMN_DTM_NUM_COUNTERS)
+ 			return -EINVAL;
+ 
+-		if (occupid && val.occupid[xp] && occupid != val.occupid[xp])
++		if (occupid && val.occupid[dtm] && occupid != val.occupid[dtm])
+ 			return -EINVAL;
+ 
+ 		if (type != CMN_TYPE_WP)
+ 			continue;
+ 
+ 		wp_idx = arm_cmn_wp_idx(event);
+-		if (val.wp[xp][wp_idx])
++		if (val.wp[dtm][wp_idx])
+ 			return -EINVAL;
+ 
+-		wp_cmb = val.wp[xp][wp_idx ^ 1];
++		wp_cmb = val.wp[dtm][wp_idx ^ 1];
+ 		if (wp_cmb && wp_cmb != CMN_EVENT_WP_COMBINE(event) + 1)
+ 			return -EINVAL;
+ 	}
+@@ -1010,17 +1000,17 @@ static void arm_cmn_event_clear(struct arm_cmn *cmn, struct perf_event *event,
+ 	enum cmn_node_type type = CMN_EVENT_TYPE(event);
+ 
+ 	while (i--) {
+-		struct arm_cmn_node *xp = arm_cmn_node_to_xp(hw->dn + i);
++		struct arm_cmn_dtm *dtm = &cmn->dtms[hw->dn[i].dtm];
+ 		unsigned int dtm_idx = arm_cmn_get_index(hw->dtm_idx, i);
+ 
+ 		if (type == CMN_TYPE_WP)
+-			hw->dn[i].wp_event[arm_cmn_wp_idx(event)] = -1;
++			dtm->wp_event[arm_cmn_wp_idx(event)] = -1;
+ 
+ 		if (arm_cmn_is_occup_event(type, CMN_EVENT_EVENTID(event)))
+ 			hw->dn[i].occupid_count--;
+ 
+-		xp->pmu_config_low &= ~CMN__PMEVCNT_PAIRED(dtm_idx);
+-		writel_relaxed(xp->pmu_config_low, xp->pmu_base + CMN_DTM_PMU_CONFIG);
++		dtm->pmu_config_low &= ~CMN__PMEVCNT_PAIRED(dtm_idx);
++		writel_relaxed(dtm->pmu_config_low, dtm->base + CMN_DTM_PMU_CONFIG);
+ 	}
+ 	memset(hw->dtm_idx, 0, sizeof(hw->dtm_idx));
+ 
+@@ -1062,12 +1052,12 @@ static int arm_cmn_event_add(struct perf_event *event, int flags)
+ 
+ 	/* ...then the local counters to feed it. */
+ 	for_each_hw_dn(hw, dn, i) {
+-		struct arm_cmn_node *xp = arm_cmn_node_to_xp(dn);
++		struct arm_cmn_dtm *dtm = &cmn->dtms[dn->dtm];
+ 		unsigned int dtm_idx, shift;
+ 		u64 reg;
+ 
+ 		dtm_idx = 0;
+-		while (xp->pmu_config_low & CMN__PMEVCNT_PAIRED(dtm_idx))
++		while (dtm->pmu_config_low & CMN__PMEVCNT_PAIRED(dtm_idx))
+ 			if (++dtm_idx == CMN_DTM_NUM_COUNTERS)
+ 				goto free_dtms;
+ 
+@@ -1077,17 +1067,17 @@ static int arm_cmn_event_add(struct perf_event *event, int flags)
+ 			int tmp, wp_idx = arm_cmn_wp_idx(event);
+ 			u32 cfg = arm_cmn_wp_config(event);
+ 
+-			if (dn->wp_event[wp_idx] >= 0)
++			if (dtm->wp_event[wp_idx] >= 0)
+ 				goto free_dtms;
+ 
+-			tmp = dn->wp_event[wp_idx ^ 1];
++			tmp = dtm->wp_event[wp_idx ^ 1];
+ 			if (tmp >= 0 && CMN_EVENT_WP_COMBINE(event) !=
+ 					CMN_EVENT_WP_COMBINE(dtc->counters[tmp]))
+ 				goto free_dtms;
+ 
+ 			input_sel = CMN__PMEVCNT0_INPUT_SEL_WP + wp_idx;
+-			dn->wp_event[wp_idx] = dtc_idx;
+-			writel_relaxed(cfg, dn->pmu_base + CMN_DTM_WPn_CONFIG(wp_idx));
++			dtm->wp_event[wp_idx] = dtc_idx;
++			writel_relaxed(cfg, dtm->base + CMN_DTM_WPn_CONFIG(wp_idx));
+ 		} else {
+ 			struct arm_cmn_nodeid nid = arm_cmn_nid(cmn, dn->id);
+ 
+@@ -1095,7 +1085,7 @@ static int arm_cmn_event_add(struct perf_event *event, int flags)
+ 				    (nid.port << 4) + (nid.dev << 2);
+ 
+ 			if (arm_cmn_is_occup_event(type, CMN_EVENT_EVENTID(event))) {
+-				int occupid = CMN_EVENT_OCCUPID(event);
++				u8 occupid = CMN_EVENT_OCCUPID(event);
+ 
+ 				if (dn->occupid_count == 0) {
+ 					dn->occupid_val = occupid;
+@@ -1110,13 +1100,13 @@ static int arm_cmn_event_add(struct perf_event *event, int flags)
+ 
+ 		arm_cmn_set_index(hw->dtm_idx, i, dtm_idx);
+ 
+-		xp->input_sel[dtm_idx] = input_sel;
++		dtm->input_sel[dtm_idx] = input_sel;
+ 		shift = CMN__PMEVCNTn_GLOBAL_NUM_SHIFT(dtm_idx);
+-		xp->pmu_config_low &= ~(CMN__PMEVCNT0_GLOBAL_NUM << shift);
+-		xp->pmu_config_low |= FIELD_PREP(CMN__PMEVCNT0_GLOBAL_NUM, dtc_idx) << shift;
+-		xp->pmu_config_low |= CMN__PMEVCNT_PAIRED(dtm_idx);
+-		reg = (u64)le32_to_cpu(xp->pmu_config_high) << 32 | xp->pmu_config_low;
+-		writeq_relaxed(reg, xp->pmu_base + CMN_DTM_PMU_CONFIG);
++		dtm->pmu_config_low &= ~(CMN__PMEVCNT0_GLOBAL_NUM << shift);
++		dtm->pmu_config_low |= FIELD_PREP(CMN__PMEVCNT0_GLOBAL_NUM, dtc_idx) << shift;
++		dtm->pmu_config_low |= CMN__PMEVCNT_PAIRED(dtm_idx);
++		reg = (u64)le32_to_cpu(dtm->pmu_config_high) << 32 | dtm->pmu_config_low;
++		writeq_relaxed(reg, dtm->base + CMN_DTM_PMU_CONFIG);
+ 	}
+ 
+ 	/* Go go go! */
+@@ -1276,23 +1266,22 @@ static int arm_cmn_init_irqs(struct arm_cmn *cmn)
+ 	return 0;
+ }
+ 
+-static void arm_cmn_init_dtm(struct arm_cmn_node *xp)
++static void arm_cmn_init_dtm(struct arm_cmn_dtm *dtm, struct arm_cmn_node *xp)
+ {
+ 	int i;
+ 
++	dtm->base = xp->pmu_base;
++	dtm->pmu_config_low = CMN_DTM_PMU_CONFIG_PMU_EN;
+ 	for (i = 0; i < 4; i++) {
+-		xp->wp_event[i] = -1;
+-		writeq_relaxed(0, xp->pmu_base + CMN_DTM_WPn_MASK(i));
+-		writeq_relaxed(~0ULL, xp->pmu_base + CMN_DTM_WPn_VAL(i));
++		dtm->wp_event[i] = -1;
++		writeq_relaxed(0, dtm->base + CMN_DTM_WPn_MASK(i));
++		writeq_relaxed(~0ULL, dtm->base + CMN_DTM_WPn_VAL(i));
+ 	}
+-	xp->pmu_config_low = CMN_DTM_PMU_CONFIG_PMU_EN;
+-	xp->dtc = -1;
+ }
+ 
+ static int arm_cmn_init_dtc(struct arm_cmn *cmn, struct arm_cmn_node *dn, int idx)
+ {
+ 	struct arm_cmn_dtc *dtc = cmn->dtc + idx;
+-	struct arm_cmn_node *xp;
+ 
+ 	dtc->base = dn->pmu_base - CMN_PMU_OFFSET;
+ 	dtc->irq = platform_get_irq(to_platform_device(cmn->dev), idx);
+@@ -1303,10 +1292,6 @@ static int arm_cmn_init_dtc(struct arm_cmn *cmn, struct arm_cmn_node *dn, int id
+ 	writel_relaxed(0x1ff, dtc->base + CMN_DT_PMOVSR_CLR);
+ 	writel_relaxed(CMN_DT_PMCR_OVFL_INTR_EN, dtc->base + CMN_DT_PMCR);
+ 
+-	/* We do at least know that a DTC's XP must be in that DTC's domain */
+-	xp = arm_cmn_node_to_xp(dn);
+-	xp->dtc = idx;
+-
+ 	return 0;
+ }
+ 
+@@ -1323,7 +1308,7 @@ static int arm_cmn_node_cmp(const void *a, const void *b)
+ 
+ static int arm_cmn_init_dtcs(struct arm_cmn *cmn)
+ {
+-	struct arm_cmn_node *dn;
++	struct arm_cmn_node *dn, *xp;
+ 	int dtc_idx = 0;
+ 
+ 	cmn->dtc = devm_kcalloc(cmn->dev, cmn->num_dtcs, sizeof(cmn->dtc[0]), GFP_KERNEL);
+@@ -1335,13 +1320,24 @@ static int arm_cmn_init_dtcs(struct arm_cmn *cmn)
+ 	cmn->xps = arm_cmn_node(cmn, CMN_TYPE_XP);
+ 
+ 	for (dn = cmn->dns; dn->type; dn++) {
+-		if (dn->type != CMN_TYPE_XP)
+-			arm_cmn_init_node_to_xp(cmn, dn);
+-		else if (cmn->num_dtcs == 1)
+-			dn->dtc = 0;
++		if (dn->type == CMN_TYPE_XP) {
++			if (dn->dtc < 0 && cmn->num_dtcs == 1)
++				dn->dtc = 0;
++			continue;
++		}
+ 
+-		if (dn->type == CMN_TYPE_DTC)
+-			arm_cmn_init_dtc(cmn, dn, dtc_idx++);
++		xp = arm_cmn_node_to_xp(cmn, dn);
++		dn->dtm = xp->dtm;
++
++		if (dn->type == CMN_TYPE_DTC) {
++			int err;
++			/* We do at least know that a DTC's XP must be in that DTC's domain */
++			if (xp->dtc < 0)
++				xp->dtc = dtc_idx;
++			err = arm_cmn_init_dtc(cmn, dn, dtc_idx++);
++			if (err)
++				return err;
++		}
+ 
+ 		/* To the PMU, RN-Ds don't add anything over RN-Is, so smoosh them together */
+ 		if (dn->type == CMN_TYPE_RND)
+@@ -1380,6 +1376,7 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ {
+ 	void __iomem *cfg_region;
+ 	struct arm_cmn_node cfg, *dn;
++	struct arm_cmn_dtm *dtm;
+ 	u16 child_count, child_poff;
+ 	u32 xp_offset[CMN_MAX_XPS];
+ 	u64 reg;
+@@ -1416,14 +1413,18 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 	if (!dn)
+ 		return -ENOMEM;
+ 
++	dtm = devm_kcalloc(cmn->dev, cmn->num_xps, sizeof(*dtm), GFP_KERNEL);
++	if (!dtm)
++		return -ENOMEM;
++
+ 	/* Pass 2: now we can actually populate the nodes */
+ 	cmn->dns = dn;
++	cmn->dtms = dtm;
+ 	for (i = 0; i < cmn->num_xps; i++) {
+ 		void __iomem *xp_region = cmn->base + xp_offset[i];
+ 		struct arm_cmn_node *xp = dn++;
+ 
+ 		arm_cmn_init_node_info(cmn, xp_offset[i], xp);
+-		arm_cmn_init_dtm(xp);
+ 		/*
+ 		 * Thanks to the order in which XP logical IDs seem to be
+ 		 * assigned, we can handily infer the mesh X dimension by
+@@ -1433,6 +1434,10 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 		if (xp->id == (1 << 3))
+ 			cmn->mesh_x = xp->logid;
+ 
++		xp->dtc = -1;
++		xp->dtm = dtm - cmn->dtms;
++		arm_cmn_init_dtm(dtm++, xp);
++
+ 		reg = readq_relaxed(xp_region + CMN_CHILD_INFO);
+ 		child_count = FIELD_GET(CMN_CI_CHILD_COUNT, reg);
+ 		child_poff = FIELD_GET(CMN_CI_CHILD_PTR_OFFSET, reg);
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0025-perf-arm-cmn-Optimise-DTM-counter-reads.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0025-perf-arm-cmn-Optimise-DTM-counter-reads.patch
new file mode 100644
index 0000000..af33468
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0025-perf-arm-cmn-Optimise-DTM-counter-reads.patch
@@ -0,0 +1,56 @@
+From a63878c01597e21451c2b3f239cbf0a2fbdeeadf Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:44:56 +0000
+Subject: [PATCH 09/14] perf/arm-cmn: Optimise DTM counter reads
+
+When multiple nodes of the same type are connected to the same XP
+(particularly in CAL configurations), it seems that they are likely
+to be consecutive in logical ID. Therefore, we're likely to gain a
+small benefit from an easy tweak to optimise out consecutive reads
+of the same set of DTM counters for an aggregated event.
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/7777d77c2df17693cd3dabb6e268906e15238d82.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/7777d77c2df17693cd3dabb6e268906e15238d82.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 17 +++++++++--------
+ 1 file changed, 9 insertions(+), 8 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index 2ae3e92690a7..5fa31ebc1fce 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -690,18 +690,19 @@ static void arm_cmn_pmu_disable(struct pmu *pmu)
+ static u64 arm_cmn_read_dtm(struct arm_cmn *cmn, struct arm_cmn_hw_event *hw,
+ 			    bool snapshot)
+ {
++	struct arm_cmn_dtm *dtm = NULL;
+ 	struct arm_cmn_node *dn;
+-	unsigned int i, offset;
+-	u64 count = 0;
++	unsigned int i, offset, dtm_idx;
++	u64 reg, count = 0;
+ 
+ 	offset = snapshot ? CMN_DTM_PMEVCNTSR : CMN_DTM_PMEVCNT;
+ 	for_each_hw_dn(hw, dn, i) {
+-		struct arm_cmn_dtm *dtm = &cmn->dtms[dn->dtm];
+-		int dtm_idx = arm_cmn_get_index(hw->dtm_idx, i);
+-		u64 reg = readq_relaxed(dtm->base + offset);
+-		u16 dtm_count = reg >> (dtm_idx * 16);
+-
+-		count += dtm_count;
++		if (dtm != &cmn->dtms[dn->dtm]) {
++			dtm = &cmn->dtms[dn->dtm];
++			reg = readq_relaxed(dtm->base + offset);
++		}
++		dtm_idx = arm_cmn_get_index(hw->dtm_idx, i);
++		count += (u16)(reg >> (dtm_idx * 16));
+ 	}
+ 	return count;
+ }
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0026-perf-arm-cmn-Optimise-DTC-counter-accesses.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0026-perf-arm-cmn-Optimise-DTC-counter-accesses.patch
new file mode 100644
index 0000000..56334e8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0026-perf-arm-cmn-Optimise-DTC-counter-accesses.patch
@@ -0,0 +1,111 @@
+From 782b7cd98e6a6b8c5fcd9e20f5c534617b1f04d3 Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:44:57 +0000
+Subject: [PATCH 10/14] perf/arm-cmn: Optimise DTC counter accesses
+
+In cases where we do know which DTC domain a node belongs to, we can
+skip initialising or reading the global count in DTCs where we know
+it won't change. The machinery to achieve that is mostly in place
+already, so finish hooking it up by converting the vestigial domain
+tracking to propagate suitable bitmaps all the way through to events.
+
+Note that this does not allow allocating such an unused counter to a
+different event on that DTC, because that is a flippin' nightmare.
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/51d930fd945ef51c81f5889ccca055c302b0a1d0.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/51d930fd945ef51c81f5889ccca055c302b0a1d0.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 29 ++++++++++++-----------------
+ 1 file changed, 12 insertions(+), 17 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index 5fa31ebc1fce..2204d6500814 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -193,7 +193,7 @@ struct arm_cmn_node {
+ 			u8 occupid_count;
+ 		};
+ 		/* XP */
+-		int dtc;
++		u8 dtc;
+ 	};
+ 	union {
+ 		u8 event[4];
+@@ -968,14 +968,14 @@ static int arm_cmn_event_init(struct perf_event *event)
+ 	if (!hw->dn)
+ 		return -EINVAL;
+ 	for (dn = hw->dn; dn->type == type; dn++) {
+-		if (!bynodeid) {
+-			hw->num_dns++;
+-		} else if (dn->id != nodeid) {
++		if (bynodeid && dn->id != nodeid) {
+ 			hw->dn++;
+-		} else {
+-			hw->num_dns = 1;
+-			break;
++			continue;
+ 		}
++		hw->dtcs_used |= arm_cmn_node_to_xp(cmn, dn)->dtc;
++		hw->num_dns++;
++		if (bynodeid)
++			break;
+ 	}
+ 
+ 	if (!hw->num_dns) {
+@@ -985,11 +985,6 @@ static int arm_cmn_event_init(struct perf_event *event)
+ 			nodeid, nid.x, nid.y, nid.port, nid.dev, type);
+ 		return -EINVAL;
+ 	}
+-	/*
+-	 * By assuming events count in all DTC domains, we cunningly avoid
+-	 * needing to know anything about how XPs are assigned to domains.
+-	 */
+-	hw->dtcs_used = (1U << cmn->num_dtcs) - 1;
+ 
+ 	return arm_cmn_validate_group(event);
+ }
+@@ -1311,6 +1306,7 @@ static int arm_cmn_init_dtcs(struct arm_cmn *cmn)
+ {
+ 	struct arm_cmn_node *dn, *xp;
+ 	int dtc_idx = 0;
++	u8 dtcs_present = (1 << cmn->num_dtcs) - 1;
+ 
+ 	cmn->dtc = devm_kcalloc(cmn->dev, cmn->num_dtcs, sizeof(cmn->dtc[0]), GFP_KERNEL);
+ 	if (!cmn->dtc)
+@@ -1322,8 +1318,7 @@ static int arm_cmn_init_dtcs(struct arm_cmn *cmn)
+ 
+ 	for (dn = cmn->dns; dn->type; dn++) {
+ 		if (dn->type == CMN_TYPE_XP) {
+-			if (dn->dtc < 0 && cmn->num_dtcs == 1)
+-				dn->dtc = 0;
++			dn->dtc &= dtcs_present;
+ 			continue;
+ 		}
+ 
+@@ -1333,8 +1328,8 @@ static int arm_cmn_init_dtcs(struct arm_cmn *cmn)
+ 		if (dn->type == CMN_TYPE_DTC) {
+ 			int err;
+ 			/* We do at least know that a DTC's XP must be in that DTC's domain */
+-			if (xp->dtc < 0)
+-				xp->dtc = dtc_idx;
++			if (xp->dtc == 0xf)
++				xp->dtc = 1 << dtc_idx;
+ 			err = arm_cmn_init_dtc(cmn, dn, dtc_idx++);
+ 			if (err)
+ 				return err;
+@@ -1435,7 +1430,7 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 		if (xp->id == (1 << 3))
+ 			cmn->mesh_x = xp->logid;
+ 
+-		xp->dtc = -1;
++		xp->dtc = 0xf;
+ 		xp->dtm = dtm - cmn->dtms;
+ 		arm_cmn_init_dtm(dtm++, xp);
+ 
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0027-perf-arm-cmn-Move-group-validation-data-off-stack.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0027-perf-arm-cmn-Move-group-validation-data-off-stack.patch
new file mode 100644
index 0000000..06adc56
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0027-perf-arm-cmn-Move-group-validation-data-off-stack.patch
@@ -0,0 +1,108 @@
+From d919e8bcbb790018e097cb8a01e7c840dcdb82aa Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:44:58 +0000
+Subject: [PATCH 11/14] perf/arm-cmn: Move group validation data off-stack
+
+With the value of CMN_MAX_DTMS increasing significantly, our validation
+data structure is set to get quite big. Technically we could pack it at
+least twice as densely, since we only need around 19 bits of information
+per DTM, but that makes the code even more mind-bogglingly impenetrable,
+and even half of "quite big" may still be uncomfortably large for a
+stack frame (~1KB). Just move it to an off-stack allocation instead.
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/0cabff2e5839ddc0979e757c55515966f65359e4.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/0cabff2e5839ddc0979e757c55515966f65359e4.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 43 ++++++++++++++++++++++++------------------
+ 1 file changed, 25 insertions(+), 18 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index 2204d6500814..b89a081d26ff 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -876,8 +876,8 @@ static int arm_cmn_validate_group(struct perf_event *event)
+ 	struct arm_cmn_node *dn;
+ 	struct perf_event *sibling, *leader = event->group_leader;
+ 	enum cmn_node_type type;
+-	struct arm_cmn_val val;
+-	int i;
++	struct arm_cmn_val *val;
++	int i, ret = -EINVAL;
+ 	u8 occupid;
+ 
+ 	if (leader == event)
+@@ -886,18 +886,22 @@ static int arm_cmn_validate_group(struct perf_event *event)
+ 	if (event->pmu != leader->pmu && !is_software_event(leader))
+ 		return -EINVAL;
+ 
+-	memset(&val, 0, sizeof(val));
++	val = kzalloc(sizeof(*val), GFP_KERNEL);
++	if (!val)
++		return -ENOMEM;
+ 
+-	arm_cmn_val_add_event(&val, leader);
++	arm_cmn_val_add_event(val, leader);
+ 	for_each_sibling_event(sibling, leader)
+-		arm_cmn_val_add_event(&val, sibling);
++		arm_cmn_val_add_event(val, sibling);
+ 
+ 	type = CMN_EVENT_TYPE(event);
+-	if (type == CMN_TYPE_DTC)
+-		return val.cycles ? -EINVAL : 0;
++	if (type == CMN_TYPE_DTC) {
++		ret = val->cycles ? -EINVAL : 0;
++		goto done;
++	}
+ 
+-	if (val.dtc_count == CMN_DT_NUM_COUNTERS)
+-		return -EINVAL;
++	if (val->dtc_count == CMN_DT_NUM_COUNTERS)
++		goto done;
+ 
+ 	if (arm_cmn_is_occup_event(type, CMN_EVENT_EVENTID(event)))
+ 		occupid = CMN_EVENT_OCCUPID(event) + 1;
+@@ -907,25 +911,28 @@ static int arm_cmn_validate_group(struct perf_event *event)
+ 	for_each_hw_dn(hw, dn, i) {
+ 		int wp_idx, wp_cmb, dtm = dn->dtm;
+ 
+-		if (val.dtm_count[dtm] == CMN_DTM_NUM_COUNTERS)
+-			return -EINVAL;
++		if (val->dtm_count[dtm] == CMN_DTM_NUM_COUNTERS)
++			goto done;
+ 
+-		if (occupid && val.occupid[dtm] && occupid != val.occupid[dtm])
+-			return -EINVAL;
++		if (occupid && val->occupid[dtm] && occupid != val->occupid[dtm])
++			goto done;
+ 
+ 		if (type != CMN_TYPE_WP)
+ 			continue;
+ 
+ 		wp_idx = arm_cmn_wp_idx(event);
+-		if (val.wp[dtm][wp_idx])
+-			return -EINVAL;
++		if (val->wp[dtm][wp_idx])
++			goto done;
+ 
+-		wp_cmb = val.wp[dtm][wp_idx ^ 1];
++		wp_cmb = val->wp[dtm][wp_idx ^ 1];
+ 		if (wp_cmb && wp_cmb != CMN_EVENT_WP_COMBINE(event) + 1)
+-			return -EINVAL;
++			goto done;
+ 	}
+ 
+-	return 0;
++	ret = 0;
++done:
++	kfree(val);
++	return ret;
+ }
+ 
+ static int arm_cmn_event_init(struct perf_event *event)
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0028-perf-arm-cmn-Demarcate-CMN-600-specifics.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0028-perf-arm-cmn-Demarcate-CMN-600-specifics.patch
new file mode 100644
index 0000000..0fbc5cb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0028-perf-arm-cmn-Demarcate-CMN-600-specifics.patch
@@ -0,0 +1,466 @@
+From 7400784247be42beb996f7538547c56acd6cfa0c Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:44:59 +0000
+Subject: [PATCH 12/14] perf/arm-cmn: Demarcate CMN-600 specifics
+
+In preparation for supporting newer CMN products, let's introduce a
+means to differentiate the features and events which are specific to a
+particular IP from those which remain common to the whole family. The
+newer designs have also smoothed off some of the rough edges in terms
+of discoverability, so separate out the parts of the flow which have
+effectively now become CMN-600 quirks.
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/9f6368cdca4c821d801138939508a5bba54ccabb.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/9f6368cdca4c821d801138939508a5bba54ccabb.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 313 +++++++++++++++++++++--------------------
+ 1 file changed, 162 insertions(+), 151 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index b89a081d26ff..92ff273fbe58 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -151,7 +151,12 @@
+ #define CMN_WP_DOWN			2
+ 
+ 
+-/* r0px probably don't exist in silicon, thankfully */
++enum cmn_model {
++	CMN_ANY = -1,
++	CMN600 = 1,
++};
++
++/* CMN-600 r0px shouldn't exist in silicon, thankfully */
+ enum cmn_revision {
+ 	CMN600_R1P0,
+ 	CMN600_R1P1,
+@@ -159,6 +164,7 @@ enum cmn_revision {
+ 	CMN600_R1P3,
+ 	CMN600_R2P0,
+ 	CMN600_R3P0,
++	CMN600_R3P1,
+ };
+ 
+ enum cmn_node_type {
+@@ -229,6 +235,7 @@ struct arm_cmn {
+ 	void __iomem *base;
+ 
+ 	enum cmn_revision rev;
++	enum cmn_model model;
+ 	u8 mesh_x;
+ 	u8 mesh_y;
+ 	u16 num_xps;
+@@ -326,6 +333,7 @@ static unsigned int arm_cmn_get_index(u64 x[], unsigned int pos)
+ 
+ struct arm_cmn_event_attr {
+ 	struct device_attribute attr;
++	enum cmn_model model;
+ 	enum cmn_node_type type;
+ 	u8 eventid;
+ 	u8 occupid;
+@@ -337,9 +345,10 @@ struct arm_cmn_format_attr {
+ 	int config;
+ };
+ 
+-#define CMN_EVENT_ATTR(_name, _type, _eventid, _occupid)		\
++#define CMN_EVENT_ATTR(_model, _name, _type, _eventid, _occupid)	\
+ 	(&((struct arm_cmn_event_attr[]) {{				\
+ 		.attr = __ATTR(_name, 0444, arm_cmn_event_show, NULL),	\
++		.model = _model,					\
+ 		.type = _type,						\
+ 		.eventid = _eventid,					\
+ 		.occupid = _occupid,					\
+@@ -386,12 +395,15 @@ static umode_t arm_cmn_event_attr_is_visible(struct kobject *kobj,
+ 	eattr = container_of(attr, typeof(*eattr), attr.attr);
+ 	type = eattr->type;
+ 
++	if (!(eattr->model & cmn->model))
++		return 0;
++
+ 	/* Watchpoints aren't nodes */
+ 	if (type == CMN_TYPE_WP)
+ 		type = CMN_TYPE_XP;
+ 
+ 	/* Revision-specific differences */
+-	if (cmn->rev < CMN600_R1P2) {
++	if (cmn->model == CMN600 && cmn->rev < CMN600_R1P2) {
+ 		if (type == CMN_TYPE_HNF && eattr->eventid == 0x1b)
+ 			return 0;
+ 	}
+@@ -402,25 +414,27 @@ static umode_t arm_cmn_event_attr_is_visible(struct kobject *kobj,
+ 	return attr->mode;
+ }
+ 
+-#define _CMN_EVENT_DVM(_name, _event, _occup)			\
+-	CMN_EVENT_ATTR(dn_##_name, CMN_TYPE_DVM, _event, _occup)
++#define _CMN_EVENT_DVM(_model, _name, _event, _occup)		\
++	CMN_EVENT_ATTR(_model, dn_##_name, CMN_TYPE_DVM, _event, _occup)
+ #define CMN_EVENT_DTC(_name)					\
+-	CMN_EVENT_ATTR(dtc_##_name, CMN_TYPE_DTC, 0, 0)
+-#define _CMN_EVENT_HNF(_name, _event, _occup)			\
+-	CMN_EVENT_ATTR(hnf_##_name, CMN_TYPE_HNF, _event, _occup)
++	CMN_EVENT_ATTR(CMN_ANY, dtc_##_name, CMN_TYPE_DTC, 0, 0)
++#define _CMN_EVENT_HNF(_model, _name, _event, _occup)		\
++	CMN_EVENT_ATTR(_model, hnf_##_name, CMN_TYPE_HNF, _event, _occup)
+ #define CMN_EVENT_HNI(_name, _event)				\
+-	CMN_EVENT_ATTR(hni_##_name, CMN_TYPE_HNI, _event, 0)
++	CMN_EVENT_ATTR(CMN_ANY, hni_##_name, CMN_TYPE_HNI, _event, 0)
+ #define __CMN_EVENT_XP(_name, _event)				\
+-	CMN_EVENT_ATTR(mxp_##_name, CMN_TYPE_XP, _event, 0)
+-#define CMN_EVENT_SBSX(_name, _event)				\
+-	CMN_EVENT_ATTR(sbsx_##_name, CMN_TYPE_SBSX, _event, 0)
+-#define CMN_EVENT_RNID(_name, _event)				\
+-	CMN_EVENT_ATTR(rnid_##_name, CMN_TYPE_RNI, _event, 0)
+-
+-#define CMN_EVENT_DVM(_name, _event)				\
+-	_CMN_EVENT_DVM(_name, _event, 0)
+-#define CMN_EVENT_HNF(_name, _event)				\
+-	_CMN_EVENT_HNF(_name, _event, 0)
++	CMN_EVENT_ATTR(CMN_ANY, mxp_##_name, CMN_TYPE_XP, _event, 0)
++#define CMN_EVENT_SBSX(_model, _name, _event)			\
++	CMN_EVENT_ATTR(_model, sbsx_##_name, CMN_TYPE_SBSX, _event, 0)
++#define CMN_EVENT_RNID(_model, _name, _event)			\
++	CMN_EVENT_ATTR(_model, rnid_##_name, CMN_TYPE_RNI, _event, 0)
++#define CMN_EVENT_MTSX(_name, _event)				\
++	CMN_EVENT_ATTR(CMN_ANY, mtsx_##_name, CMN_TYPE_MTSX, _event, 0)
++
++#define CMN_EVENT_DVM(_model, _name, _event)			\
++	_CMN_EVENT_DVM(_model, _name, _event, 0)
++#define CMN_EVENT_HNF(_model, _name, _event)			\
++	_CMN_EVENT_HNF(_model, _name, _event, 0)
+ #define _CMN_EVENT_XP(_name, _event)				\
+ 	__CMN_EVENT_XP(e_##_name, (_event) | (0 << 2)),		\
+ 	__CMN_EVENT_XP(w_##_name, (_event) | (1 << 2)),		\
+@@ -445,115 +459,115 @@ static struct attribute *arm_cmn_event_attrs[] = {
+ 	 * slot, but our lazy short-cut of using the DTM counter index for
+ 	 * the PMU index as well happens to avoid that by construction.
+ 	 */
+-	CMN_EVENT_DVM(rxreq_dvmop,	0x01),
+-	CMN_EVENT_DVM(rxreq_dvmsync,	0x02),
+-	CMN_EVENT_DVM(rxreq_dvmop_vmid_filtered, 0x03),
+-	CMN_EVENT_DVM(rxreq_retried,	0x04),
+-	_CMN_EVENT_DVM(rxreq_trk_occupancy_all, 0x05, 0),
+-	_CMN_EVENT_DVM(rxreq_trk_occupancy_dvmop, 0x05, 1),
+-	_CMN_EVENT_DVM(rxreq_trk_occupancy_dvmsync, 0x05, 2),
+-
+-	CMN_EVENT_HNF(cache_miss,	0x01),
+-	CMN_EVENT_HNF(slc_sf_cache_access, 0x02),
+-	CMN_EVENT_HNF(cache_fill,	0x03),
+-	CMN_EVENT_HNF(pocq_retry,	0x04),
+-	CMN_EVENT_HNF(pocq_reqs_recvd,	0x05),
+-	CMN_EVENT_HNF(sf_hit,		0x06),
+-	CMN_EVENT_HNF(sf_evictions,	0x07),
+-	CMN_EVENT_HNF(dir_snoops_sent,	0x08),
+-	CMN_EVENT_HNF(brd_snoops_sent,	0x09),
+-	CMN_EVENT_HNF(slc_eviction,	0x0a),
+-	CMN_EVENT_HNF(slc_fill_invalid_way, 0x0b),
+-	CMN_EVENT_HNF(mc_retries,	0x0c),
+-	CMN_EVENT_HNF(mc_reqs,		0x0d),
+-	CMN_EVENT_HNF(qos_hh_retry,	0x0e),
+-	_CMN_EVENT_HNF(qos_pocq_occupancy_all, 0x0f, 0),
+-	_CMN_EVENT_HNF(qos_pocq_occupancy_read, 0x0f, 1),
+-	_CMN_EVENT_HNF(qos_pocq_occupancy_write, 0x0f, 2),
+-	_CMN_EVENT_HNF(qos_pocq_occupancy_atomic, 0x0f, 3),
+-	_CMN_EVENT_HNF(qos_pocq_occupancy_stash, 0x0f, 4),
+-	CMN_EVENT_HNF(pocq_addrhaz,	0x10),
+-	CMN_EVENT_HNF(pocq_atomic_addrhaz, 0x11),
+-	CMN_EVENT_HNF(ld_st_swp_adq_full, 0x12),
+-	CMN_EVENT_HNF(cmp_adq_full,	0x13),
+-	CMN_EVENT_HNF(txdat_stall,	0x14),
+-	CMN_EVENT_HNF(txrsp_stall,	0x15),
+-	CMN_EVENT_HNF(seq_full,		0x16),
+-	CMN_EVENT_HNF(seq_hit,		0x17),
+-	CMN_EVENT_HNF(snp_sent,		0x18),
+-	CMN_EVENT_HNF(sfbi_dir_snp_sent, 0x19),
+-	CMN_EVENT_HNF(sfbi_brd_snp_sent, 0x1a),
+-	CMN_EVENT_HNF(snp_sent_untrk,	0x1b),
+-	CMN_EVENT_HNF(intv_dirty,	0x1c),
+-	CMN_EVENT_HNF(stash_snp_sent,	0x1d),
+-	CMN_EVENT_HNF(stash_data_pull,	0x1e),
+-	CMN_EVENT_HNF(snp_fwded,	0x1f),
+-
+-	CMN_EVENT_HNI(rrt_rd_occ_cnt_ovfl, 0x20),
+-	CMN_EVENT_HNI(rrt_wr_occ_cnt_ovfl, 0x21),
+-	CMN_EVENT_HNI(rdt_rd_occ_cnt_ovfl, 0x22),
+-	CMN_EVENT_HNI(rdt_wr_occ_cnt_ovfl, 0x23),
+-	CMN_EVENT_HNI(wdb_occ_cnt_ovfl,	0x24),
+-	CMN_EVENT_HNI(rrt_rd_alloc,	0x25),
+-	CMN_EVENT_HNI(rrt_wr_alloc,	0x26),
+-	CMN_EVENT_HNI(rdt_rd_alloc,	0x27),
+-	CMN_EVENT_HNI(rdt_wr_alloc,	0x28),
+-	CMN_EVENT_HNI(wdb_alloc,	0x29),
+-	CMN_EVENT_HNI(txrsp_retryack,	0x2a),
+-	CMN_EVENT_HNI(arvalid_no_arready, 0x2b),
+-	CMN_EVENT_HNI(arready_no_arvalid, 0x2c),
+-	CMN_EVENT_HNI(awvalid_no_awready, 0x2d),
+-	CMN_EVENT_HNI(awready_no_awvalid, 0x2e),
+-	CMN_EVENT_HNI(wvalid_no_wready,	0x2f),
+-	CMN_EVENT_HNI(txdat_stall,	0x30),
+-	CMN_EVENT_HNI(nonpcie_serialization, 0x31),
+-	CMN_EVENT_HNI(pcie_serialization, 0x32),
+-
+-	CMN_EVENT_XP(txflit_valid,	0x01),
+-	CMN_EVENT_XP(txflit_stall,	0x02),
+-	CMN_EVENT_XP(partial_dat_flit,	0x03),
++	CMN_EVENT_DVM(CMN600, rxreq_dvmop,		0x01),
++	CMN_EVENT_DVM(CMN600, rxreq_dvmsync,		0x02),
++	CMN_EVENT_DVM(CMN600, rxreq_dvmop_vmid_filtered, 0x03),
++	CMN_EVENT_DVM(CMN600, rxreq_retried,		0x04),
++	_CMN_EVENT_DVM(CMN600, rxreq_trk_occupancy_all, 0x05, 0),
++	_CMN_EVENT_DVM(CMN600, rxreq_trk_occupancy_dvmop, 0x05, 1),
++	_CMN_EVENT_DVM(CMN600, rxreq_trk_occupancy_dvmsync, 0x05, 2),
++
++	CMN_EVENT_HNF(CMN_ANY, cache_miss,		0x01),
++	CMN_EVENT_HNF(CMN_ANY, slc_sf_cache_access,	0x02),
++	CMN_EVENT_HNF(CMN_ANY, cache_fill,		0x03),
++	CMN_EVENT_HNF(CMN_ANY, pocq_retry,		0x04),
++	CMN_EVENT_HNF(CMN_ANY, pocq_reqs_recvd,		0x05),
++	CMN_EVENT_HNF(CMN_ANY, sf_hit,			0x06),
++	CMN_EVENT_HNF(CMN_ANY, sf_evictions,		0x07),
++	CMN_EVENT_HNF(CMN_ANY, dir_snoops_sent,		0x08),
++	CMN_EVENT_HNF(CMN_ANY, brd_snoops_sent,		0x09),
++	CMN_EVENT_HNF(CMN_ANY, slc_eviction,		0x0a),
++	CMN_EVENT_HNF(CMN_ANY, slc_fill_invalid_way,	0x0b),
++	CMN_EVENT_HNF(CMN_ANY, mc_retries,		0x0c),
++	CMN_EVENT_HNF(CMN_ANY, mc_reqs,			0x0d),
++	CMN_EVENT_HNF(CMN_ANY, qos_hh_retry,		0x0e),
++	_CMN_EVENT_HNF(CMN_ANY, qos_pocq_occupancy_all,	0x0f, 0),
++	_CMN_EVENT_HNF(CMN_ANY, qos_pocq_occupancy_read, 0x0f, 1),
++	_CMN_EVENT_HNF(CMN_ANY, qos_pocq_occupancy_write, 0x0f, 2),
++	_CMN_EVENT_HNF(CMN_ANY, qos_pocq_occupancy_atomic, 0x0f, 3),
++	_CMN_EVENT_HNF(CMN_ANY, qos_pocq_occupancy_stash, 0x0f, 4),
++	CMN_EVENT_HNF(CMN_ANY, pocq_addrhaz,		0x10),
++	CMN_EVENT_HNF(CMN_ANY, pocq_atomic_addrhaz,	0x11),
++	CMN_EVENT_HNF(CMN_ANY, ld_st_swp_adq_full,	0x12),
++	CMN_EVENT_HNF(CMN_ANY, cmp_adq_full,		0x13),
++	CMN_EVENT_HNF(CMN_ANY, txdat_stall,		0x14),
++	CMN_EVENT_HNF(CMN_ANY, txrsp_stall,		0x15),
++	CMN_EVENT_HNF(CMN_ANY, seq_full,		0x16),
++	CMN_EVENT_HNF(CMN_ANY, seq_hit,			0x17),
++	CMN_EVENT_HNF(CMN_ANY, snp_sent,		0x18),
++	CMN_EVENT_HNF(CMN_ANY, sfbi_dir_snp_sent,	0x19),
++	CMN_EVENT_HNF(CMN_ANY, sfbi_brd_snp_sent,	0x1a),
++	CMN_EVENT_HNF(CMN_ANY, snp_sent_untrk,		0x1b),
++	CMN_EVENT_HNF(CMN_ANY, intv_dirty,		0x1c),
++	CMN_EVENT_HNF(CMN_ANY, stash_snp_sent,		0x1d),
++	CMN_EVENT_HNF(CMN_ANY, stash_data_pull,		0x1e),
++	CMN_EVENT_HNF(CMN_ANY, snp_fwded,		0x1f),
++
++	CMN_EVENT_HNI(rrt_rd_occ_cnt_ovfl,		0x20),
++	CMN_EVENT_HNI(rrt_wr_occ_cnt_ovfl,		0x21),
++	CMN_EVENT_HNI(rdt_rd_occ_cnt_ovfl,		0x22),
++	CMN_EVENT_HNI(rdt_wr_occ_cnt_ovfl,		0x23),
++	CMN_EVENT_HNI(wdb_occ_cnt_ovfl,			0x24),
++	CMN_EVENT_HNI(rrt_rd_alloc,			0x25),
++	CMN_EVENT_HNI(rrt_wr_alloc,			0x26),
++	CMN_EVENT_HNI(rdt_rd_alloc,			0x27),
++	CMN_EVENT_HNI(rdt_wr_alloc,			0x28),
++	CMN_EVENT_HNI(wdb_alloc,			0x29),
++	CMN_EVENT_HNI(txrsp_retryack,			0x2a),
++	CMN_EVENT_HNI(arvalid_no_arready,		0x2b),
++	CMN_EVENT_HNI(arready_no_arvalid,		0x2c),
++	CMN_EVENT_HNI(awvalid_no_awready,		0x2d),
++	CMN_EVENT_HNI(awready_no_awvalid,		0x2e),
++	CMN_EVENT_HNI(wvalid_no_wready,			0x2f),
++	CMN_EVENT_HNI(txdat_stall,			0x30),
++	CMN_EVENT_HNI(nonpcie_serialization,		0x31),
++	CMN_EVENT_HNI(pcie_serialization,		0x32),
++
++	CMN_EVENT_XP(txflit_valid,			0x01),
++	CMN_EVENT_XP(txflit_stall,			0x02),
++	CMN_EVENT_XP(partial_dat_flit,			0x03),
+ 	/* We treat watchpoints as a special made-up class of XP events */
+-	CMN_EVENT_ATTR(watchpoint_up, CMN_TYPE_WP, 0, 0),
+-	CMN_EVENT_ATTR(watchpoint_down, CMN_TYPE_WP, 2, 0),
+-
+-	CMN_EVENT_SBSX(rd_req,		0x01),
+-	CMN_EVENT_SBSX(wr_req,		0x02),
+-	CMN_EVENT_SBSX(cmo_req,		0x03),
+-	CMN_EVENT_SBSX(txrsp_retryack,	0x04),
+-	CMN_EVENT_SBSX(txdat_flitv,	0x05),
+-	CMN_EVENT_SBSX(txrsp_flitv,	0x06),
+-	CMN_EVENT_SBSX(rd_req_trkr_occ_cnt_ovfl, 0x11),
+-	CMN_EVENT_SBSX(wr_req_trkr_occ_cnt_ovfl, 0x12),
+-	CMN_EVENT_SBSX(cmo_req_trkr_occ_cnt_ovfl, 0x13),
+-	CMN_EVENT_SBSX(wdb_occ_cnt_ovfl, 0x14),
+-	CMN_EVENT_SBSX(rd_axi_trkr_occ_cnt_ovfl, 0x15),
+-	CMN_EVENT_SBSX(cmo_axi_trkr_occ_cnt_ovfl, 0x16),
+-	CMN_EVENT_SBSX(arvalid_no_arready, 0x21),
+-	CMN_EVENT_SBSX(awvalid_no_awready, 0x22),
+-	CMN_EVENT_SBSX(wvalid_no_wready, 0x23),
+-	CMN_EVENT_SBSX(txdat_stall,	0x24),
+-	CMN_EVENT_SBSX(txrsp_stall,	0x25),
+-
+-	CMN_EVENT_RNID(s0_rdata_beats,	0x01),
+-	CMN_EVENT_RNID(s1_rdata_beats,	0x02),
+-	CMN_EVENT_RNID(s2_rdata_beats,	0x03),
+-	CMN_EVENT_RNID(rxdat_flits,	0x04),
+-	CMN_EVENT_RNID(txdat_flits,	0x05),
+-	CMN_EVENT_RNID(txreq_flits_total, 0x06),
+-	CMN_EVENT_RNID(txreq_flits_retried, 0x07),
+-	CMN_EVENT_RNID(rrt_occ_ovfl,	0x08),
+-	CMN_EVENT_RNID(wrt_occ_ovfl,	0x09),
+-	CMN_EVENT_RNID(txreq_flits_replayed, 0x0a),
+-	CMN_EVENT_RNID(wrcancel_sent,	0x0b),
+-	CMN_EVENT_RNID(s0_wdata_beats,	0x0c),
+-	CMN_EVENT_RNID(s1_wdata_beats,	0x0d),
+-	CMN_EVENT_RNID(s2_wdata_beats,	0x0e),
+-	CMN_EVENT_RNID(rrt_alloc,	0x0f),
+-	CMN_EVENT_RNID(wrt_alloc,	0x10),
+-	CMN_EVENT_RNID(rdb_unord,	0x11),
+-	CMN_EVENT_RNID(rdb_replay,	0x12),
+-	CMN_EVENT_RNID(rdb_hybrid,	0x13),
+-	CMN_EVENT_RNID(rdb_ord,		0x14),
++	CMN_EVENT_ATTR(CMN_ANY, watchpoint_up, CMN_TYPE_WP, CMN_WP_UP, 0),
++	CMN_EVENT_ATTR(CMN_ANY, watchpoint_down, CMN_TYPE_WP, CMN_WP_DOWN, 0),
++
++	CMN_EVENT_SBSX(CMN_ANY, rd_req,			0x01),
++	CMN_EVENT_SBSX(CMN_ANY, wr_req,			0x02),
++	CMN_EVENT_SBSX(CMN_ANY, cmo_req,		0x03),
++	CMN_EVENT_SBSX(CMN_ANY, txrsp_retryack,		0x04),
++	CMN_EVENT_SBSX(CMN_ANY, txdat_flitv,		0x05),
++	CMN_EVENT_SBSX(CMN_ANY, txrsp_flitv,		0x06),
++	CMN_EVENT_SBSX(CMN_ANY, rd_req_trkr_occ_cnt_ovfl, 0x11),
++	CMN_EVENT_SBSX(CMN_ANY, wr_req_trkr_occ_cnt_ovfl, 0x12),
++	CMN_EVENT_SBSX(CMN_ANY, cmo_req_trkr_occ_cnt_ovfl, 0x13),
++	CMN_EVENT_SBSX(CMN_ANY, wdb_occ_cnt_ovfl,	0x14),
++	CMN_EVENT_SBSX(CMN_ANY, rd_axi_trkr_occ_cnt_ovfl, 0x15),
++	CMN_EVENT_SBSX(CMN_ANY, cmo_axi_trkr_occ_cnt_ovfl, 0x16),
++	CMN_EVENT_SBSX(CMN_ANY, arvalid_no_arready,	0x21),
++	CMN_EVENT_SBSX(CMN_ANY, awvalid_no_awready,	0x22),
++	CMN_EVENT_SBSX(CMN_ANY, wvalid_no_wready,	0x23),
++	CMN_EVENT_SBSX(CMN_ANY, txdat_stall,		0x24),
++	CMN_EVENT_SBSX(CMN_ANY, txrsp_stall,		0x25),
++
++	CMN_EVENT_RNID(CMN_ANY, s0_rdata_beats,		0x01),
++	CMN_EVENT_RNID(CMN_ANY, s1_rdata_beats,		0x02),
++	CMN_EVENT_RNID(CMN_ANY, s2_rdata_beats,		0x03),
++	CMN_EVENT_RNID(CMN_ANY, rxdat_flits,		0x04),
++	CMN_EVENT_RNID(CMN_ANY, txdat_flits,		0x05),
++	CMN_EVENT_RNID(CMN_ANY, txreq_flits_total,	0x06),
++	CMN_EVENT_RNID(CMN_ANY, txreq_flits_retried,	0x07),
++	CMN_EVENT_RNID(CMN_ANY, rrt_occ_ovfl,		0x08),
++	CMN_EVENT_RNID(CMN_ANY, wrt_occ_ovfl,		0x09),
++	CMN_EVENT_RNID(CMN_ANY, txreq_flits_replayed,	0x0a),
++	CMN_EVENT_RNID(CMN_ANY, wrcancel_sent,		0x0b),
++	CMN_EVENT_RNID(CMN_ANY, s0_wdata_beats,		0x0c),
++	CMN_EVENT_RNID(CMN_ANY, s1_wdata_beats,		0x0d),
++	CMN_EVENT_RNID(CMN_ANY, s2_wdata_beats,		0x0e),
++	CMN_EVENT_RNID(CMN_ANY, rrt_alloc,		0x0f),
++	CMN_EVENT_RNID(CMN_ANY, wrt_alloc,		0x10),
++	CMN_EVENT_RNID(CMN600, rdb_unord,		0x11),
++	CMN_EVENT_RNID(CMN600, rdb_replay,		0x12),
++	CMN_EVENT_RNID(CMN600, rdb_hybrid,		0x13),
++	CMN_EVENT_RNID(CMN600, rdb_ord,			0x14),
+ 
+ 	NULL
+ };
+@@ -1386,15 +1400,14 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 	int i, j;
+ 	size_t sz;
+ 
+-	cfg_region = cmn->base + rgn_offset;
+-	reg = readl_relaxed(cfg_region + CMN_CFGM_PERIPH_ID_2);
+-	cmn->rev = FIELD_GET(CMN_CFGM_PID2_REVISION, reg);
+-	dev_dbg(cmn->dev, "periph_id_2 revision: %d\n", cmn->rev);
+-
+ 	arm_cmn_init_node_info(cmn, rgn_offset, &cfg);
+ 	if (cfg.type != CMN_TYPE_CFG)
+ 		return -ENODEV;
+ 
++	cfg_region = cmn->base + rgn_offset;
++	reg = readl_relaxed(cfg_region + CMN_CFGM_PERIPH_ID_2);
++	cmn->rev = FIELD_GET(CMN_CFGM_PID2_REVISION, reg);
++
+ 	reg = readq_relaxed(cfg_region + CMN_CHILD_INFO);
+ 	child_count = FIELD_GET(CMN_CI_CHILD_COUNT, reg);
+ 	child_poff = FIELD_GET(CMN_CI_CHILD_PTR_OFFSET, reg);
+@@ -1507,13 +1520,14 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 		cmn->mesh_x = cmn->num_xps;
+ 	cmn->mesh_y = cmn->num_xps / cmn->mesh_x;
+ 
++	dev_dbg(cmn->dev, "model %d, periph_id_2 revision %d\n", cmn->model, cmn->rev);
+ 	dev_dbg(cmn->dev, "mesh %dx%d, ID width %d\n",
+ 		cmn->mesh_x, cmn->mesh_y, arm_cmn_xyidbits(cmn));
+ 
+ 	return 0;
+ }
+ 
+-static int arm_cmn_acpi_probe(struct platform_device *pdev, struct arm_cmn *cmn)
++static int arm_cmn600_acpi_probe(struct platform_device *pdev, struct arm_cmn *cmn)
+ {
+ 	struct resource *cfg, *root;
+ 
+@@ -1540,21 +1554,11 @@ static int arm_cmn_acpi_probe(struct platform_device *pdev, struct arm_cmn *cmn)
+ 	return root->start - cfg->start;
+ }
+ 
+-static int arm_cmn_of_probe(struct platform_device *pdev, struct arm_cmn *cmn)
++static int arm_cmn600_of_probe(struct device_node *np)
+ {
+-	struct device_node *np = pdev->dev.of_node;
+ 	u32 rootnode;
+-	int ret;
+ 
+-	cmn->base = devm_platform_ioremap_resource(pdev, 0);
+-	if (IS_ERR(cmn->base))
+-		return PTR_ERR(cmn->base);
+-
+-	ret = of_property_read_u32(np, "arm,root-node", &rootnode);
+-	if (ret)
+-		return ret;
+-
+-	return rootnode;
++	return of_property_read_u32(np, "arm,root-node", &rootnode) ?: rootnode;
+ }
+ 
+ static int arm_cmn_probe(struct platform_device *pdev)
+@@ -1569,12 +1573,19 @@ static int arm_cmn_probe(struct platform_device *pdev)
+ 		return -ENOMEM;
+ 
+ 	cmn->dev = &pdev->dev;
++	cmn->model = (unsigned long)device_get_match_data(cmn->dev);
+ 	platform_set_drvdata(pdev, cmn);
+ 
+-	if (has_acpi_companion(cmn->dev))
+-		rootnode = arm_cmn_acpi_probe(pdev, cmn);
+-	else
+-		rootnode = arm_cmn_of_probe(pdev, cmn);
++	if (cmn->model == CMN600 && has_acpi_companion(cmn->dev)) {
++		rootnode = arm_cmn600_acpi_probe(pdev, cmn);
++	} else {
++		rootnode = 0;
++		cmn->base = devm_platform_ioremap_resource(pdev, 0);
++		if (IS_ERR(cmn->base))
++			return PTR_ERR(cmn->base);
++		if (cmn->model == CMN600)
++			rootnode = arm_cmn600_of_probe(pdev->dev.of_node);
++	}
+ 	if (rootnode < 0)
+ 		return rootnode;
+ 
+@@ -1637,7 +1648,7 @@ static int arm_cmn_remove(struct platform_device *pdev)
+ 
+ #ifdef CONFIG_OF
+ static const struct of_device_id arm_cmn_of_match[] = {
+-	{ .compatible = "arm,cmn-600", },
++	{ .compatible = "arm,cmn-600", .data = (void *)CMN600 },
+ 	{}
+ };
+ MODULE_DEVICE_TABLE(of, arm_cmn_of_match);
+@@ -1645,7 +1656,7 @@ MODULE_DEVICE_TABLE(of, arm_cmn_of_match);
+ 
+ #ifdef CONFIG_ACPI
+ static const struct acpi_device_id arm_cmn_acpi_match[] = {
+-	{ "ARMHC600", },
++	{ "ARMHC600", CMN600 },
+ 	{}
+ };
+ MODULE_DEVICE_TABLE(acpi, arm_cmn_acpi_match);
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0029-perf-arm-cmn-Support-new-IP-features.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0029-perf-arm-cmn-Support-new-IP-features.patch
new file mode 100644
index 0000000..fa0c5d9
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0029-perf-arm-cmn-Support-new-IP-features.patch
@@ -0,0 +1,549 @@
+From a10c446ba1f7516c16dd6400c9a7f5e203779a5d Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:45:00 +0000
+Subject: [PATCH 13/14] perf/arm-cmn: Support new IP features
+
+The second generation of CMN IPs add new node types and significantly
+expand the configuration space with options for extra device ports on
+edge XPs, either plumbed into the regular DTM or with extra dedicated
+DTMs to monitor them, plus larger (and smaller) mesh sizes. Add basic
+support for pulling this new information out of the hardware, piping
+it around as necessary, and handling (most of) the new choices.
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/e58b495bcc7deec3882be4bac910ed0bf6979674.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 222 ++++++++++++++++++++++++++++++++---------
+ 1 file changed, 173 insertions(+), 49 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index 92ff273fbe58..871c86687379 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -24,7 +24,10 @@
+ #define CMN_NI_LOGICAL_ID		GENMASK_ULL(47, 32)
+ 
+ #define CMN_NODEID_DEVID(reg)		((reg) & 3)
++#define CMN_NODEID_EXT_DEVID(reg)	((reg) & 1)
+ #define CMN_NODEID_PID(reg)		(((reg) >> 2) & 1)
++#define CMN_NODEID_EXT_PID(reg)		(((reg) >> 1) & 3)
++#define CMN_NODEID_1x1_PID(reg)		(((reg) >> 2) & 7)
+ #define CMN_NODEID_X(reg, bits)		((reg) >> (3 + (bits)))
+ #define CMN_NODEID_Y(reg, bits)		(((reg) >> 3) & ((1U << (bits)) - 1))
+ 
+@@ -37,13 +40,26 @@
+ 
+ #define CMN_MAX_DIMENSION		8
+ #define CMN_MAX_XPS			(CMN_MAX_DIMENSION * CMN_MAX_DIMENSION)
+-#define CMN_MAX_DTMS			CMN_MAX_XPS
++#define CMN_MAX_DTMS			(CMN_MAX_XPS + (CMN_MAX_DIMENSION - 1) * 4)
+ 
+-/* The CFG node has one other useful purpose */
++/* The CFG node has various info besides the discovery tree */
+ #define CMN_CFGM_PERIPH_ID_2		0x0010
+ #define CMN_CFGM_PID2_REVISION		GENMASK(7, 4)
+ 
+-/* PMU registers occupy the 3rd 4KB page of each node's 16KB space */
++#define CMN_CFGM_INFO_GLOBAL		0x900
++#define CMN_INFO_MULTIPLE_DTM_EN	BIT_ULL(63)
++#define CMN_INFO_RSP_VC_NUM		GENMASK_ULL(53, 52)
++#define CMN_INFO_DAT_VC_NUM		GENMASK_ULL(51, 50)
++
++/* XPs also have some local topology info which has uses too */
++#define CMN_MXP__CONNECT_INFO_P0	0x0008
++#define CMN_MXP__CONNECT_INFO_P1	0x0010
++#define CMN_MXP__CONNECT_INFO_P2	0x0028
++#define CMN_MXP__CONNECT_INFO_P3	0x0030
++#define CMN_MXP__CONNECT_INFO_P4	0x0038
++#define CMN_MXP__CONNECT_INFO_P5	0x0040
++
++/* PMU registers occupy the 3rd 4KB page of each node's region */
+ #define CMN_PMU_OFFSET			0x2000
+ 
+ /* For most nodes, this is all there is */
+@@ -53,6 +69,7 @@
+ /* DTMs live in the PMU space of XP registers */
+ #define CMN_DTM_WPn(n)			(0x1A0 + (n) * 0x18)
+ #define CMN_DTM_WPn_CONFIG(n)		(CMN_DTM_WPn(n) + 0x00)
++#define CMN_DTM_WPn_CONFIG_WP_DEV_SEL2	GENMASK_ULL(18,17)
+ #define CMN_DTM_WPn_CONFIG_WP_COMBINE	BIT(6)
+ #define CMN_DTM_WPn_CONFIG_WP_EXCLUSIVE	BIT(5)
+ #define CMN_DTM_WPn_CONFIG_WP_GRP	BIT(4)
+@@ -77,7 +94,11 @@
+ 
+ #define CMN_DTM_PMEVCNTSR		0x240
+ 
++#define CMN_DTM_UNIT_INFO		0x0910
++
+ #define CMN_DTM_NUM_COUNTERS		4
++/* Want more local counters? Why not replicate the whole DTM! Ugh... */
++#define CMN_DTM_OFFSET(n)		((n) * 0x200)
+ 
+ /* The DTC node is where the magic happens */
+ #define CMN_DT_DTC_CTL			0x0a00
+@@ -131,10 +152,10 @@
+ #define CMN_EVENT_NODEID(event)		FIELD_GET(CMN_CONFIG_NODEID, (event)->attr.config)
+ 
+ #define CMN_CONFIG_WP_COMBINE		GENMASK_ULL(27, 24)
+-#define CMN_CONFIG_WP_DEV_SEL		BIT_ULL(48)
+-#define CMN_CONFIG_WP_CHN_SEL		GENMASK_ULL(50, 49)
+-#define CMN_CONFIG_WP_GRP		BIT_ULL(52)
+-#define CMN_CONFIG_WP_EXCLUSIVE		BIT_ULL(53)
++#define CMN_CONFIG_WP_DEV_SEL		GENMASK_ULL(50, 48)
++#define CMN_CONFIG_WP_CHN_SEL		GENMASK_ULL(55, 51)
++#define CMN_CONFIG_WP_GRP		BIT_ULL(56)
++#define CMN_CONFIG_WP_EXCLUSIVE		BIT_ULL(57)
+ #define CMN_CONFIG1_WP_VAL		GENMASK_ULL(63, 0)
+ #define CMN_CONFIG2_WP_MASK		GENMASK_ULL(63, 0)
+ 
+@@ -176,9 +197,12 @@ enum cmn_node_type {
+ 	CMN_TYPE_HNF,
+ 	CMN_TYPE_XP,
+ 	CMN_TYPE_SBSX,
+-	CMN_TYPE_RNI = 0xa,
++	CMN_TYPE_MPAM_S,
++	CMN_TYPE_MPAM_NS,
++	CMN_TYPE_RNI,
+ 	CMN_TYPE_RND = 0xd,
+ 	CMN_TYPE_RNSAM = 0xf,
++	CMN_TYPE_MTSX,
+ 	CMN_TYPE_CXRA = 0x100,
+ 	CMN_TYPE_CXHA = 0x101,
+ 	CMN_TYPE_CXLA = 0x102,
+@@ -233,6 +257,7 @@ struct arm_cmn_dtc {
+ struct arm_cmn {
+ 	struct device *dev;
+ 	void __iomem *base;
++	unsigned int state;
+ 
+ 	enum cmn_revision rev;
+ 	enum cmn_model model;
+@@ -240,6 +265,13 @@ struct arm_cmn {
+ 	u8 mesh_y;
+ 	u16 num_xps;
+ 	u16 num_dns;
++	bool multi_dtm;
++	u8 ports_used;
++	struct {
++		unsigned int rsp_vc_num : 2;
++		unsigned int dat_vc_num : 2;
++	};
++
+ 	struct arm_cmn_node *xps;
+ 	struct arm_cmn_node *dns;
+ 
+@@ -250,7 +282,6 @@ struct arm_cmn {
+ 	int cpu;
+ 	struct hlist_node cpuhp_node;
+ 
+-	unsigned int state;
+ 	struct pmu pmu;
+ };
+ 
+@@ -275,13 +306,25 @@ static int arm_cmn_xyidbits(const struct arm_cmn *cmn)
+ static struct arm_cmn_nodeid arm_cmn_nid(const struct arm_cmn *cmn, u16 id)
+ {
+ 	struct arm_cmn_nodeid nid;
+-	int bits = arm_cmn_xyidbits(cmn);
+ 
+-	nid.x = CMN_NODEID_X(id, bits);
+-	nid.y = CMN_NODEID_Y(id, bits);
+-	nid.port = CMN_NODEID_PID(id);
+-	nid.dev = CMN_NODEID_DEVID(id);
++	if (cmn->num_xps == 1) {
++		nid.x = 0;
++		nid.y = 0;
++		nid.port = CMN_NODEID_1x1_PID(id);
++		nid.dev = CMN_NODEID_DEVID(id);
++	} else {
++		int bits = arm_cmn_xyidbits(cmn);
+ 
++		nid.x = CMN_NODEID_X(id, bits);
++		nid.y = CMN_NODEID_Y(id, bits);
++		if (cmn->ports_used & 0xc) {
++			nid.port = CMN_NODEID_EXT_PID(id);
++			nid.dev = CMN_NODEID_EXT_DEVID(id);
++		} else {
++			nid.port = CMN_NODEID_PID(id);
++			nid.dev = CMN_NODEID_DEVID(id);
++		}
++	}
+ 	return nid;
+ }
+ 
+@@ -310,6 +353,7 @@ struct arm_cmn_hw_event {
+ 	unsigned int dtc_idx;
+ 	u8 dtcs_used;
+ 	u8 num_dns;
++	u8 dtm_offset;
+ };
+ 
+ #define for_each_hw_dn(hw, dn, i) \
+@@ -354,7 +398,8 @@ struct arm_cmn_format_attr {
+ 		.occupid = _occupid,					\
+ 	}})[0].attr.attr)
+ 
+-static bool arm_cmn_is_occup_event(enum cmn_node_type type, unsigned int id)
++static bool arm_cmn_is_occup_event(enum cmn_model model,
++				   enum cmn_node_type type, unsigned int id)
+ {
+ 	return (type == CMN_TYPE_DVM && id == 0x05) ||
+ 	       (type == CMN_TYPE_HNF && id == 0x0f);
+@@ -375,9 +420,9 @@ static ssize_t arm_cmn_event_show(struct device *dev,
+ 				"type=0x%x,eventid=0x%x,wp_dev_sel=?,wp_chn_sel=?,wp_grp=?,wp_val=?,wp_mask=?\n",
+ 				eattr->type, eattr->eventid);
+ 
+-	if (arm_cmn_is_occup_event(eattr->type, eattr->eventid))
+-		return snprintf(buf, PAGE_SIZE, "type=0x%x,eventid=0x%x,occupid=0x%x\n",
+-				eattr->type, eattr->eventid, eattr->occupid);
++	if (arm_cmn_is_occup_event(eattr->model, eattr->type, eattr->eventid))
++		return sysfs_emit(buf, "type=0x%x,eventid=0x%x,occupid=0x%x\n",
++				  eattr->type, eattr->eventid, eattr->occupid);
+ 
+ 	return snprintf(buf, PAGE_SIZE, "type=0x%x,eventid=0x%x\n",
+ 			eattr->type, eattr->eventid);
+@@ -390,25 +435,36 @@ static umode_t arm_cmn_event_attr_is_visible(struct kobject *kobj,
+ 	struct device *dev = kobj_to_dev(kobj);
+ 	struct arm_cmn *cmn = to_cmn(dev_get_drvdata(dev));
+ 	struct arm_cmn_event_attr *eattr;
+-	enum cmn_node_type type;
+ 
+ 	eattr = container_of(attr, typeof(*eattr), attr.attr);
+-	type = eattr->type;
+ 
+ 	if (!(eattr->model & cmn->model))
+ 		return 0;
+ 
+-	/* Watchpoints aren't nodes */
+-	if (type == CMN_TYPE_WP)
+-		type = CMN_TYPE_XP;
++	/* Watchpoints aren't nodes, so avoid confusion */
++	if (eattr->type == CMN_TYPE_WP)
++		return attr->mode;
++
++	/* Hide XP events for unused interfaces/channels */
++	if (eattr->type == CMN_TYPE_XP) {
++		unsigned int intf = (eattr->eventid >> 2) & 7;
++		unsigned int chan = eattr->eventid >> 5;
++
++		if ((intf & 4) && !(cmn->ports_used & BIT(intf & 3)))
++			return 0;
++
++		if ((chan == 5 && cmn->rsp_vc_num < 2) ||
++		    (chan == 6 && cmn->dat_vc_num < 2))
++			return 0;
++	}
+ 
+ 	/* Revision-specific differences */
+ 	if (cmn->model == CMN600 && cmn->rev < CMN600_R1P2) {
+-		if (type == CMN_TYPE_HNF && eattr->eventid == 0x1b)
++		if (eattr->type == CMN_TYPE_HNF && eattr->eventid == 0x1b)
+ 			return 0;
+ 	}
+ 
+-	if (!arm_cmn_node(cmn, type))
++	if (!arm_cmn_node(cmn, eattr->type))
+ 		return 0;
+ 
+ 	return attr->mode;
+@@ -669,7 +725,8 @@ static u32 arm_cmn_wp_config(struct perf_event *event)
+ 	config = FIELD_PREP(CMN_DTM_WPn_CONFIG_WP_DEV_SEL, dev) |
+ 		 FIELD_PREP(CMN_DTM_WPn_CONFIG_WP_CHN_SEL, chn) |
+ 		 FIELD_PREP(CMN_DTM_WPn_CONFIG_WP_GRP, grp) |
+-		 FIELD_PREP(CMN_DTM_WPn_CONFIG_WP_EXCLUSIVE, exc);
++		 FIELD_PREP(CMN_DTM_WPn_CONFIG_WP_EXCLUSIVE, exc) |
++		 FIELD_PREP(CMN_DTM_WPn_CONFIG_WP_DEV_SEL2, dev >> 1);
+ 	if (combine && !grp)
+ 		config |= CMN_DTM_WPn_CONFIG_WP_COMBINE;
+ 
+@@ -712,7 +769,7 @@ static u64 arm_cmn_read_dtm(struct arm_cmn *cmn, struct arm_cmn_hw_event *hw,
+ 	offset = snapshot ? CMN_DTM_PMEVCNTSR : CMN_DTM_PMEVCNT;
+ 	for_each_hw_dn(hw, dn, i) {
+ 		if (dtm != &cmn->dtms[dn->dtm]) {
+-			dtm = &cmn->dtms[dn->dtm];
++			dtm = &cmn->dtms[dn->dtm] + hw->dtm_offset;
+ 			reg = readq_relaxed(dtm->base + offset);
+ 		}
+ 		dtm_idx = arm_cmn_get_index(hw->dtm_idx, i);
+@@ -800,8 +857,10 @@ static void arm_cmn_event_start(struct perf_event *event, int flags)
+ 		u64 mask = CMN_EVENT_WP_MASK(event);
+ 
+ 		for_each_hw_dn(hw, dn, i) {
+-			writeq_relaxed(val, dn->pmu_base + CMN_DTM_WPn_VAL(wp_idx));
+-			writeq_relaxed(mask, dn->pmu_base + CMN_DTM_WPn_MASK(wp_idx));
++			void __iomem *base = dn->pmu_base + CMN_DTM_OFFSET(hw->dtm_offset);
++
++			writeq_relaxed(val, base + CMN_DTM_WPn_VAL(wp_idx));
++			writeq_relaxed(mask, base + CMN_DTM_WPn_MASK(wp_idx));
+ 		}
+ 	} else for_each_hw_dn(hw, dn, i) {
+ 		int dtm_idx = arm_cmn_get_index(hw->dtm_idx, i);
+@@ -826,8 +885,10 @@ static void arm_cmn_event_stop(struct perf_event *event, int flags)
+ 		int wp_idx = arm_cmn_wp_idx(event);
+ 
+ 		for_each_hw_dn(hw, dn, i) {
+-			writeq_relaxed(0, dn->pmu_base + CMN_DTM_WPn_MASK(wp_idx));
+-			writeq_relaxed(~0ULL, dn->pmu_base + CMN_DTM_WPn_VAL(wp_idx));
++			void __iomem *base = dn->pmu_base + CMN_DTM_OFFSET(hw->dtm_offset);
++
++			writeq_relaxed(0, base + CMN_DTM_WPn_MASK(wp_idx));
++			writeq_relaxed(~0ULL, base + CMN_DTM_WPn_VAL(wp_idx));
+ 		}
+ 	} else for_each_hw_dn(hw, dn, i) {
+ 		int dtm_idx = arm_cmn_get_index(hw->dtm_idx, i);
+@@ -847,7 +908,8 @@ struct arm_cmn_val {
+ 	bool cycles;
+ };
+ 
+-static void arm_cmn_val_add_event(struct arm_cmn_val *val, struct perf_event *event)
++static void arm_cmn_val_add_event(struct arm_cmn *cmn, struct arm_cmn_val *val,
++				  struct perf_event *event)
+ {
+ 	struct arm_cmn_hw_event *hw = to_cmn_hw(event);
+ 	struct arm_cmn_node *dn;
+@@ -865,7 +927,7 @@ static void arm_cmn_val_add_event(struct arm_cmn_val *val, struct perf_event *ev
+ 	}
+ 
+ 	val->dtc_count++;
+-	if (arm_cmn_is_occup_event(type, CMN_EVENT_EVENTID(event)))
++	if (arm_cmn_is_occup_event(cmn->model, type, CMN_EVENT_EVENTID(event)))
+ 		occupid = CMN_EVENT_OCCUPID(event) + 1;
+ 	else
+ 		occupid = 0;
+@@ -884,7 +946,7 @@ static void arm_cmn_val_add_event(struct arm_cmn_val *val, struct perf_event *ev
+ 	}
+ }
+ 
+-static int arm_cmn_validate_group(struct perf_event *event)
++static int arm_cmn_validate_group(struct arm_cmn *cmn, struct perf_event *event)
+ {
+ 	struct arm_cmn_hw_event *hw = to_cmn_hw(event);
+ 	struct arm_cmn_node *dn;
+@@ -904,9 +966,9 @@ static int arm_cmn_validate_group(struct perf_event *event)
+ 	if (!val)
+ 		return -ENOMEM;
+ 
+-	arm_cmn_val_add_event(val, leader);
++	arm_cmn_val_add_event(cmn, val, leader);
+ 	for_each_sibling_event(sibling, leader)
+-		arm_cmn_val_add_event(val, sibling);
++		arm_cmn_val_add_event(cmn, val, sibling);
+ 
+ 	type = CMN_EVENT_TYPE(event);
+ 	if (type == CMN_TYPE_DTC) {
+@@ -917,7 +979,7 @@ static int arm_cmn_validate_group(struct perf_event *event)
+ 	if (val->dtc_count == CMN_DT_NUM_COUNTERS)
+ 		goto done;
+ 
+-	if (arm_cmn_is_occup_event(type, CMN_EVENT_EVENTID(event)))
++	if (arm_cmn_is_occup_event(cmn->model, type, CMN_EVENT_EVENTID(event)))
+ 		occupid = CMN_EVENT_OCCUPID(event) + 1;
+ 	else
+ 		occupid = 0;
+@@ -980,6 +1042,9 @@ static int arm_cmn_event_init(struct perf_event *event)
+ 		eventid = CMN_EVENT_EVENTID(event);
+ 		if (eventid != CMN_WP_UP && eventid != CMN_WP_DOWN)
+ 			return -EINVAL;
++		/* ...but the DTM may depend on which port we're watching */
++		if (cmn->multi_dtm)
++			hw->dtm_offset = CMN_EVENT_WP_DEV_SEL(event) / 2;
+ 	}
+ 
+ 	bynodeid = CMN_EVENT_BYNODEID(event);
+@@ -1007,7 +1072,7 @@ static int arm_cmn_event_init(struct perf_event *event)
+ 		return -EINVAL;
+ 	}
+ 
+-	return arm_cmn_validate_group(event);
++	return arm_cmn_validate_group(cmn, event);
+ }
+ 
+ static void arm_cmn_event_clear(struct arm_cmn *cmn, struct perf_event *event,
+@@ -1017,13 +1082,13 @@ static void arm_cmn_event_clear(struct arm_cmn *cmn, struct perf_event *event,
+ 	enum cmn_node_type type = CMN_EVENT_TYPE(event);
+ 
+ 	while (i--) {
+-		struct arm_cmn_dtm *dtm = &cmn->dtms[hw->dn[i].dtm];
++		struct arm_cmn_dtm *dtm = &cmn->dtms[hw->dn[i].dtm] + hw->dtm_offset;
+ 		unsigned int dtm_idx = arm_cmn_get_index(hw->dtm_idx, i);
+ 
+ 		if (type == CMN_TYPE_WP)
+ 			dtm->wp_event[arm_cmn_wp_idx(event)] = -1;
+ 
+-		if (arm_cmn_is_occup_event(type, CMN_EVENT_EVENTID(event)))
++		if (arm_cmn_is_occup_event(cmn->model, type, CMN_EVENT_EVENTID(event)))
+ 			hw->dn[i].occupid_count--;
+ 
+ 		dtm->pmu_config_low &= ~CMN__PMEVCNT_PAIRED(dtm_idx);
+@@ -1069,7 +1134,7 @@ static int arm_cmn_event_add(struct perf_event *event, int flags)
+ 
+ 	/* ...then the local counters to feed it. */
+ 	for_each_hw_dn(hw, dn, i) {
+-		struct arm_cmn_dtm *dtm = &cmn->dtms[dn->dtm];
++		struct arm_cmn_dtm *dtm = &cmn->dtms[dn->dtm] + hw->dtm_offset;
+ 		unsigned int dtm_idx, shift;
+ 		u64 reg;
+ 
+@@ -1098,10 +1163,13 @@ static int arm_cmn_event_add(struct perf_event *event, int flags)
+ 		} else {
+ 			struct arm_cmn_nodeid nid = arm_cmn_nid(cmn, dn->id);
+ 
++			if (cmn->multi_dtm)
++				nid.port %= 2;
++
+ 			input_sel = CMN__PMEVCNT0_INPUT_SEL_DEV + dtm_idx +
+ 				    (nid.port << 4) + (nid.dev << 2);
+ 
+-			if (arm_cmn_is_occup_event(type, CMN_EVENT_EVENTID(event))) {
++			if (arm_cmn_is_occup_event(cmn->model, type, CMN_EVENT_EVENTID(event))) {
+ 				u8 occupid = CMN_EVENT_OCCUPID(event);
+ 
+ 				if (dn->occupid_count == 0) {
+@@ -1283,11 +1351,11 @@ static int arm_cmn_init_irqs(struct arm_cmn *cmn)
+ 	return 0;
+ }
+ 
+-static void arm_cmn_init_dtm(struct arm_cmn_dtm *dtm, struct arm_cmn_node *xp)
++static void arm_cmn_init_dtm(struct arm_cmn_dtm *dtm, struct arm_cmn_node *xp, int idx)
+ {
+ 	int i;
+ 
+-	dtm->base = xp->pmu_base;
++	dtm->base = xp->pmu_base + CMN_DTM_OFFSET(idx);
+ 	dtm->pmu_config_low = CMN_DTM_PMU_CONFIG_PMU_EN;
+ 	for (i = 0; i < 4; i++) {
+ 		dtm->wp_event[i] = -1;
+@@ -1345,6 +1413,8 @@ static int arm_cmn_init_dtcs(struct arm_cmn *cmn)
+ 
+ 		xp = arm_cmn_node_to_xp(cmn, dn);
+ 		dn->dtm = xp->dtm;
++		if (cmn->multi_dtm)
++			dn->dtm += arm_cmn_nid(cmn, dn->id).port / 2;
+ 
+ 		if (dn->type == CMN_TYPE_DTC) {
+ 			int err;
+@@ -1408,6 +1478,11 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 	reg = readl_relaxed(cfg_region + CMN_CFGM_PERIPH_ID_2);
+ 	cmn->rev = FIELD_GET(CMN_CFGM_PID2_REVISION, reg);
+ 
++	reg = readq_relaxed(cfg_region + CMN_CFGM_INFO_GLOBAL);
++	cmn->multi_dtm = reg & CMN_INFO_MULTIPLE_DTM_EN;
++	cmn->rsp_vc_num = FIELD_GET(CMN_INFO_RSP_VC_NUM, reg);
++	cmn->dat_vc_num = FIELD_GET(CMN_INFO_DAT_VC_NUM, reg);
++
+ 	reg = readq_relaxed(cfg_region + CMN_CHILD_INFO);
+ 	child_count = FIELD_GET(CMN_CI_CHILD_COUNT, reg);
+ 	child_poff = FIELD_GET(CMN_CI_CHILD_PTR_OFFSET, reg);
+@@ -1429,7 +1504,11 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 	if (!dn)
+ 		return -ENOMEM;
+ 
+-	dtm = devm_kcalloc(cmn->dev, cmn->num_xps, sizeof(*dtm), GFP_KERNEL);
++	/* Initial safe upper bound on DTMs for any possible mesh layout */
++	i = cmn->num_xps;
++	if (cmn->multi_dtm)
++		i += cmn->num_xps + 1;
++	dtm = devm_kcalloc(cmn->dev, i, sizeof(*dtm), GFP_KERNEL);
+ 	if (!dtm)
+ 		return -ENOMEM;
+ 
+@@ -1439,6 +1518,7 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 	for (i = 0; i < cmn->num_xps; i++) {
+ 		void __iomem *xp_region = cmn->base + xp_offset[i];
+ 		struct arm_cmn_node *xp = dn++;
++		unsigned int xp_ports = 0;
+ 
+ 		arm_cmn_init_node_info(cmn, xp_offset[i], xp);
+ 		/*
+@@ -1450,9 +1530,39 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 		if (xp->id == (1 << 3))
+ 			cmn->mesh_x = xp->logid;
+ 
+-		xp->dtc = 0xf;
++		if (cmn->model == CMN600)
++			xp->dtc = 0xf;
++		else
++			xp->dtc = 1 << readl_relaxed(xp_region + CMN_DTM_UNIT_INFO);
++
+ 		xp->dtm = dtm - cmn->dtms;
+-		arm_cmn_init_dtm(dtm++, xp);
++		arm_cmn_init_dtm(dtm++, xp, 0);
++		/*
++		 * Keeping track of connected ports will let us filter out
++		 * unnecessary XP events easily. We can also reliably infer the
++		 * "extra device ports" configuration for the node ID format
++		 * from this, since in that case we will see at least one XP
++		 * with port 2 connected, for the HN-D.
++		 */
++		if (readq_relaxed(xp_region + CMN_MXP__CONNECT_INFO_P0))
++			xp_ports |= BIT(0);
++		if (readq_relaxed(xp_region + CMN_MXP__CONNECT_INFO_P1))
++			xp_ports |= BIT(1);
++		if (readq_relaxed(xp_region + CMN_MXP__CONNECT_INFO_P2))
++			xp_ports |= BIT(2);
++		if (readq_relaxed(xp_region + CMN_MXP__CONNECT_INFO_P3))
++			xp_ports |= BIT(3);
++		if (readq_relaxed(xp_region + CMN_MXP__CONNECT_INFO_P4))
++			xp_ports |= BIT(4);
++		if (readq_relaxed(xp_region + CMN_MXP__CONNECT_INFO_P5))
++			xp_ports |= BIT(5);
++
++		if (cmn->multi_dtm && (xp_ports & 0xc))
++			arm_cmn_init_dtm(dtm++, xp, 1);
++		if (cmn->multi_dtm && (xp_ports & 0x30))
++			arm_cmn_init_dtm(dtm++, xp, 2);
++
++		cmn->ports_used |= xp_ports;
+ 
+ 		reg = readq_relaxed(xp_region + CMN_CHILD_INFO);
+ 		child_count = FIELD_GET(CMN_CI_CHILD_COUNT, reg);
+@@ -1488,11 +1598,14 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 			case CMN_TYPE_SBSX:
+ 			case CMN_TYPE_RNI:
+ 			case CMN_TYPE_RND:
++			case CMN_TYPE_MTSX:
+ 			case CMN_TYPE_CXRA:
+ 			case CMN_TYPE_CXHA:
+ 				dn++;
+ 				break;
+ 			/* Nothing to see here */
++			case CMN_TYPE_MPAM_S:
++			case CMN_TYPE_MPAM_NS:
+ 			case CMN_TYPE_RNSAM:
+ 			case CMN_TYPE_CXLA:
+ 				break;
+@@ -1512,6 +1625,11 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 	if (dn)
+ 		cmn->dns = dn;
+ 
++	sz = (void *)dtm - (void *)cmn->dtms;
++	dtm = devm_krealloc(cmn->dev, cmn->dtms, sz, GFP_KERNEL);
++	if (dtm)
++		cmn->dtms = dtm;
++
+ 	/*
+ 	 * If mesh_x wasn't set during discovery then we never saw
+ 	 * an XP at (0,1), thus we must have an Nx1 configuration.
+@@ -1520,9 +1638,15 @@ static int arm_cmn_discover(struct arm_cmn *cmn, unsigned int rgn_offset)
+ 		cmn->mesh_x = cmn->num_xps;
+ 	cmn->mesh_y = cmn->num_xps / cmn->mesh_x;
+ 
++	/* 1x1 config plays havoc with XP event encodings */
++	if (cmn->num_xps == 1)
++		dev_warn(cmn->dev, "1x1 config not fully supported, translate XP events manually\n");
++
+ 	dev_dbg(cmn->dev, "model %d, periph_id_2 revision %d\n", cmn->model, cmn->rev);
+-	dev_dbg(cmn->dev, "mesh %dx%d, ID width %d\n",
+-		cmn->mesh_x, cmn->mesh_y, arm_cmn_xyidbits(cmn));
++	reg = cmn->ports_used;
++	dev_dbg(cmn->dev, "mesh %dx%d, ID width %d, ports %6pbl%s\n",
++		cmn->mesh_x, cmn->mesh_y, arm_cmn_xyidbits(cmn), &reg,
++		cmn->multi_dtm ? ", multi-DTM" : "");
+ 
+ 	return 0;
+ }
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0030-perf-arm-cmn-Add-CI-700-Support.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0030-perf-arm-cmn-Add-CI-700-Support.patch
new file mode 100644
index 0000000..a12911a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0030-perf-arm-cmn-Add-CI-700-Support.patch
@@ -0,0 +1,150 @@
+From f5d0979c2385c3ef43d6f2af07c14ee897835e2c Mon Sep 17 00:00:00 2001
+From: Robin Murphy <robin.murphy@arm.com>
+Date: Fri, 3 Dec 2021 11:45:02 +0000
+Subject: [PATCH 14/14] perf/arm-cmn: Add CI-700 Support
+
+Add the identifiers and events for the CI-700 coherent interconnect.
+
+Signed-off-by: Robin Murphy <robin.murphy@arm.com>
+Link: https://lore.kernel.org/r/28f566ab23a83733c6c9ef9414c010b760b4549c.1638530442.git.robin.murphy@arm.com
+Signed-off-by: Will Deacon <will@kernel.org>
+
+Upstream-Status: Backport [https://lore.kernel.org/r/28f566ab23a83733c6c9ef9414c010b760b4549c.1638530442.git.robin.murphy@arm.com]
+Signed-off-by: Rupinderjit Singh <rupinderjit.singh@arm.com>
+---
+ drivers/perf/arm-cmn.c | 57 +++++++++++++++++++++++++++++++++++++++---
+ 1 file changed, 53 insertions(+), 4 deletions(-)
+
+diff --git a/drivers/perf/arm-cmn.c b/drivers/perf/arm-cmn.c
+index 871c86687379..e0f78b6c643c 100644
+--- a/drivers/perf/arm-cmn.c
++++ b/drivers/perf/arm-cmn.c
+@@ -175,6 +175,7 @@
+ enum cmn_model {
+ 	CMN_ANY = -1,
+ 	CMN600 = 1,
++	CI700 = 2,
+ };
+ 
+ /* CMN-600 r0px shouldn't exist in silicon, thankfully */
+@@ -186,6 +187,9 @@ enum cmn_revision {
+ 	CMN600_R2P0,
+ 	CMN600_R3P0,
+ 	CMN600_R3P1,
++	CI700_R0P0 = 0,
++	CI700_R1P0,
++	CI700_R2P0,
+ };
+ 
+ enum cmn_node_type {
+@@ -401,8 +405,10 @@ struct arm_cmn_format_attr {
+ static bool arm_cmn_is_occup_event(enum cmn_model model,
+ 				   enum cmn_node_type type, unsigned int id)
+ {
+-	return (type == CMN_TYPE_DVM && id == 0x05) ||
+-	       (type == CMN_TYPE_HNF && id == 0x0f);
++	if (type == CMN_TYPE_DVM)
++		return (model == CMN600 && id == 0x05) ||
++		       (model == CI700 && id == 0x0c);
++	return type == CMN_TYPE_HNF && id == 0x0f;
+ }
+ 
+ static ssize_t arm_cmn_event_show(struct device *dev,
+@@ -497,14 +503,19 @@ static umode_t arm_cmn_event_attr_is_visible(struct kobject *kobj,
+ 	__CMN_EVENT_XP(n_##_name, (_event) | (2 << 2)),		\
+ 	__CMN_EVENT_XP(s_##_name, (_event) | (3 << 2)),		\
+ 	__CMN_EVENT_XP(p0_##_name, (_event) | (4 << 2)),	\
+-	__CMN_EVENT_XP(p1_##_name, (_event) | (5 << 2))
++	__CMN_EVENT_XP(p1_##_name, (_event) | (5 << 2)),	\
++	__CMN_EVENT_XP(p2_##_name, (_event) | (6 << 2)),	\
++	__CMN_EVENT_XP(p3_##_name, (_event) | (7 << 2))
+ 
+ /* Good thing there are only 3 fundamental XP events... */
+ #define CMN_EVENT_XP(_name, _event)				\
+ 	_CMN_EVENT_XP(req_##_name, (_event) | (0 << 5)),	\
+ 	_CMN_EVENT_XP(rsp_##_name, (_event) | (1 << 5)),	\
+ 	_CMN_EVENT_XP(snp_##_name, (_event) | (2 << 5)),	\
+-	_CMN_EVENT_XP(dat_##_name, (_event) | (3 << 5))
++	_CMN_EVENT_XP(dat_##_name, (_event) | (3 << 5)),	\
++	_CMN_EVENT_XP(pub_##_name, (_event) | (4 << 5)),	\
++	_CMN_EVENT_XP(rsp2_##_name, (_event) | (5 << 5)),	\
++	_CMN_EVENT_XP(dat2_##_name, (_event) | (6 << 5))
+ 
+ 
+ static struct attribute *arm_cmn_event_attrs[] = {
+@@ -522,6 +533,20 @@ static struct attribute *arm_cmn_event_attrs[] = {
+ 	_CMN_EVENT_DVM(CMN600, rxreq_trk_occupancy_all, 0x05, 0),
+ 	_CMN_EVENT_DVM(CMN600, rxreq_trk_occupancy_dvmop, 0x05, 1),
+ 	_CMN_EVENT_DVM(CMN600, rxreq_trk_occupancy_dvmsync, 0x05, 2),
++	CMN_EVENT_DVM(CI700, dvmop_tlbi,		0x01),
++	CMN_EVENT_DVM(CI700, dvmop_bpi,			0x02),
++	CMN_EVENT_DVM(CI700, dvmop_pici,		0x03),
++	CMN_EVENT_DVM(CI700, dvmop_vici,		0x04),
++	CMN_EVENT_DVM(CI700, dvmsync,			0x05),
++	CMN_EVENT_DVM(CI700, vmid_filtered,		0x06),
++	CMN_EVENT_DVM(CI700, rndop_filtered,		0x07),
++	CMN_EVENT_DVM(CI700, retry,			0x08),
++	CMN_EVENT_DVM(CI700, txsnp_flitv,		0x09),
++	CMN_EVENT_DVM(CI700, txsnp_stall,		0x0a),
++	CMN_EVENT_DVM(CI700, trkfull,			0x0b),
++	_CMN_EVENT_DVM(CI700, trk_occupancy_all,	0x0c, 0),
++	_CMN_EVENT_DVM(CI700, trk_occupancy_dvmop,	0x0c, 1),
++	_CMN_EVENT_DVM(CI700, trk_occupancy_dvmsync,	0x0c, 2),
+ 
+ 	CMN_EVENT_HNF(CMN_ANY, cache_miss,		0x01),
+ 	CMN_EVENT_HNF(CMN_ANY, slc_sf_cache_access,	0x02),
+@@ -558,6 +583,9 @@ static struct attribute *arm_cmn_event_attrs[] = {
+ 	CMN_EVENT_HNF(CMN_ANY, stash_snp_sent,		0x1d),
+ 	CMN_EVENT_HNF(CMN_ANY, stash_data_pull,		0x1e),
+ 	CMN_EVENT_HNF(CMN_ANY, snp_fwded,		0x1f),
++	CMN_EVENT_HNF(CI700, atomic_fwd,		0x20),
++	CMN_EVENT_HNF(CI700, mpam_hardlim,		0x21),
++	CMN_EVENT_HNF(CI700, mpam_softlim,		0x22),
+ 
+ 	CMN_EVENT_HNI(rrt_rd_occ_cnt_ovfl,		0x20),
+ 	CMN_EVENT_HNI(rrt_wr_occ_cnt_ovfl,		0x21),
+@@ -598,6 +626,7 @@ static struct attribute *arm_cmn_event_attrs[] = {
+ 	CMN_EVENT_SBSX(CMN_ANY, wdb_occ_cnt_ovfl,	0x14),
+ 	CMN_EVENT_SBSX(CMN_ANY, rd_axi_trkr_occ_cnt_ovfl, 0x15),
+ 	CMN_EVENT_SBSX(CMN_ANY, cmo_axi_trkr_occ_cnt_ovfl, 0x16),
++	CMN_EVENT_SBSX(CI700, rdb_occ_cnt_ovfl,		0x17),
+ 	CMN_EVENT_SBSX(CMN_ANY, arvalid_no_arready,	0x21),
+ 	CMN_EVENT_SBSX(CMN_ANY, awvalid_no_awready,	0x22),
+ 	CMN_EVENT_SBSX(CMN_ANY, wvalid_no_wready,	0x23),
+@@ -624,6 +653,25 @@ static struct attribute *arm_cmn_event_attrs[] = {
+ 	CMN_EVENT_RNID(CMN600, rdb_replay,		0x12),
+ 	CMN_EVENT_RNID(CMN600, rdb_hybrid,		0x13),
+ 	CMN_EVENT_RNID(CMN600, rdb_ord,			0x14),
++	CMN_EVENT_RNID(CI700, padb_occ_ovfl,		0x11),
++	CMN_EVENT_RNID(CI700, rpdb_occ_ovfl,		0x12),
++	CMN_EVENT_RNID(CI700, rrt_occup_ovfl_slice1,	0x13),
++	CMN_EVENT_RNID(CI700, rrt_occup_ovfl_slice2,	0x14),
++	CMN_EVENT_RNID(CI700, rrt_occup_ovfl_slice3,	0x15),
++	CMN_EVENT_RNID(CI700, wrt_throttled,		0x16),
++
++	CMN_EVENT_MTSX(tc_lookup,			0x01),
++	CMN_EVENT_MTSX(tc_fill,				0x02),
++	CMN_EVENT_MTSX(tc_miss,				0x03),
++	CMN_EVENT_MTSX(tdb_forward,			0x04),
++	CMN_EVENT_MTSX(tcq_hazard,			0x05),
++	CMN_EVENT_MTSX(tcq_rd_alloc,			0x06),
++	CMN_EVENT_MTSX(tcq_wr_alloc,			0x07),
++	CMN_EVENT_MTSX(tcq_cmo_alloc,			0x08),
++	CMN_EVENT_MTSX(axi_rd_req,			0x09),
++	CMN_EVENT_MTSX(axi_wr_req,			0x0a),
++	CMN_EVENT_MTSX(tcq_occ_cnt_ovfl,		0x0b),
++	CMN_EVENT_MTSX(tdb_occ_cnt_ovfl,		0x0c),
+ 
+ 	NULL
+ };
+@@ -1773,6 +1821,7 @@ static int arm_cmn_remove(struct platform_device *pdev)
+ #ifdef CONFIG_OF
+ static const struct of_device_id arm_cmn_of_match[] = {
+ 	{ .compatible = "arm,cmn-600", .data = (void *)CMN600 },
++	{ .compatible = "arm,ci-700", .data = (void *)CI700 },
+ 	{}
+ };
+ MODULE_DEVICE_TABLE(of, arm_cmn_of_match);
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0031-firmware-arm_ffa-Fix-uuid-argument-passed-to-ffa_par.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0031-firmware-arm_ffa-Fix-uuid-argument-passed-to-ffa_par.patch
new file mode 100644
index 0000000..35b4f10
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0031-firmware-arm_ffa-Fix-uuid-argument-passed-to-ffa_par.patch
@@ -0,0 +1,29 @@
+From 4d0a8147477699d40a02f121e7c72b21547273cf Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Thu, 13 Jan 2022 20:14:25 +0000
+Subject: [PATCH 19/32] firmware: arm_ffa: Fix uuid argument passed to
+ ffa_partition_probe
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Ib2749ec3e02da5bb6d835f7dbf2d608c41fad1f2
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/firmware/arm_ffa/driver.c | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/drivers/firmware/arm_ffa/driver.c b/drivers/firmware/arm_ffa/driver.c
+index 14f900047ac0..8fa1785afd42 100644
+--- a/drivers/firmware/arm_ffa/driver.c
++++ b/drivers/firmware/arm_ffa/driver.c
+@@ -582,7 +582,7 @@ static int ffa_partition_info_get(const char *uuid_str,
+ 		return -ENODEV;
+ 	}
+ 
+-	count = ffa_partition_probe(&uuid_null, &pbuf);
++	count = ffa_partition_probe(&uuid, &pbuf);
+ 	if (count <= 0)
+ 		return -ENOENT;
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0032-firmware-arm_ffa-Add-ffa_dev_get_drvdata.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0032-firmware-arm_ffa-Add-ffa_dev_get_drvdata.patch
new file mode 100644
index 0000000..52cf71b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0032-firmware-arm_ffa-Add-ffa_dev_get_drvdata.patch
@@ -0,0 +1,33 @@
+From 9acd4425667e240603ec196d8b64b2b25879805e Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Thu, 13 Jan 2022 22:22:28 +0000
+Subject: [PATCH 20/32] firmware: arm_ffa: Add ffa_dev_get_drvdata
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Icd09d686cab9922563b1deda5276307ea5d94923
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ include/linux/arm_ffa.h | 7 ++++++-
+ 1 file changed, 6 insertions(+), 1 deletion(-)
+
+diff --git a/include/linux/arm_ffa.h b/include/linux/arm_ffa.h
+index 85651e41ded8..e5c76c1ef9ed 100644
+--- a/include/linux/arm_ffa.h
++++ b/include/linux/arm_ffa.h
+@@ -38,7 +38,12 @@ struct ffa_driver {
+ 
+ static inline void ffa_dev_set_drvdata(struct ffa_device *fdev, void *data)
+ {
+-	fdev->dev.driver_data = data;
++	dev_set_drvdata(&fdev->dev, data);
++}
++
++static inline void *ffa_dev_get_drvdata(struct ffa_device *fdev)
++{
++	return dev_get_drvdata(&fdev->dev);
+ }
+ 
+ #if IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT)
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0033-firmware-arm_ffa-extern-ffa_bus_type.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0033-firmware-arm_ffa-extern-ffa_bus_type.patch
new file mode 100644
index 0000000..bbbc178
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0033-firmware-arm_ffa-extern-ffa_bus_type.patch
@@ -0,0 +1,30 @@
+From 7a9298916fe892ddac5fe4e0a13a566b1636f542 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Thu, 13 Jan 2022 22:23:52 +0000
+Subject: [PATCH 21/32] firmware: arm_ffa: extern ffa_bus_type
+
+extern ffa_bus_type so that SP driver can use it in bus_find_device call.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Ib7a6a563aa35627a545f82c796816a5f72c80d70
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ include/linux/arm_ffa.h | 2 ++
+ 1 file changed, 2 insertions(+)
+
+diff --git a/include/linux/arm_ffa.h b/include/linux/arm_ffa.h
+index e5c76c1ef9ed..4eb7e03ca560 100644
+--- a/include/linux/arm_ffa.h
++++ b/include/linux/arm_ffa.h
+@@ -88,6 +88,8 @@ const struct ffa_dev_ops *ffa_dev_ops_get(struct ffa_device *dev)
+ #define ffa_unregister(driver) \
+ 	ffa_driver_unregister(driver)
+ 
++extern struct bus_type ffa_bus_type;
++
+ /**
+  * module_ffa_driver() - Helper macro for registering a psa_ffa driver
+  * @__ffa_driver: ffa_driver structure
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0034-firmware-arm_ffa-Fix-FFA_MEM_SHARE-and-FFA_MEM_FRAG_.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0034-firmware-arm_ffa-Fix-FFA_MEM_SHARE-and-FFA_MEM_FRAG_.patch
new file mode 100644
index 0000000..977b550
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0034-firmware-arm_ffa-Fix-FFA_MEM_SHARE-and-FFA_MEM_FRAG_.patch
@@ -0,0 +1,54 @@
+From e0b9971db819fb9ed9b08a5d3f6f2a4565e92a1a Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 14 Jan 2022 12:23:04 +0000
+Subject: [PATCH 22/32] firmware: arm_ffa: Fix FFA_MEM_SHARE and
+ FFA_MEM_FRAG_TX
+
+FFA memory share on success might return FFA_MEM_FRAG_RX. In that case
+set handle from w1/w2 from FFA return value.
+
+FFA_MEM_FRAG_TX call will return FFA_SUCCESS for the last fragment, so
+check for this return code.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I7ef44742d53a9e75d8587d1213be98a1352f16d4
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/firmware/arm_ffa/driver.c | 12 ++++++++----
+ 1 file changed, 8 insertions(+), 4 deletions(-)
+
+diff --git a/drivers/firmware/arm_ffa/driver.c b/drivers/firmware/arm_ffa/driver.c
+index 8fa1785afd42..a3b1df6d7f3c 100644
+--- a/drivers/firmware/arm_ffa/driver.c
++++ b/drivers/firmware/arm_ffa/driver.c
+@@ -398,11 +398,15 @@ static int ffa_mem_first_frag(u32 func_id, phys_addr_t buf, u32 buf_sz,
+ 	if (ret.a0 == FFA_ERROR)
+ 		return ffa_to_linux_errno((int)ret.a2);
+ 
+-	if (ret.a0 != FFA_SUCCESS)
++	if (ret.a0 != FFA_SUCCESS && ret.a0 != FFA_MEM_FRAG_RX)
+ 		return -EOPNOTSUPP;
+ 
+-	if (handle)
+-		*handle = PACK_HANDLE(ret.a2, ret.a3);
++	if (handle) {
++		if (ret.a0 == FFA_MEM_FRAG_RX)
++			*handle = PACK_HANDLE(ret.a1, ret.a2);
++		else
++			*handle = PACK_HANDLE(ret.a2, ret.a3);
++	}
+ 
+ 	return frag_len;
+ }
+@@ -426,7 +430,7 @@ static int ffa_mem_next_frag(u64 handle, u32 frag_len)
+ 	if (ret.a0 == FFA_ERROR)
+ 		return ffa_to_linux_errno((int)ret.a2);
+ 
+-	if (ret.a0 != FFA_MEM_FRAG_RX)
++	if (ret.a0 != FFA_SUCCESS && ret.a0 != FFA_MEM_FRAG_RX)
+ 		return -EOPNOTSUPP;
+ 
+ 	return ret.a3;
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0035-ANDROID-trusty-Backport-of-trusty-driver.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0035-ANDROID-trusty-Backport-of-trusty-driver.patch
new file mode 100644
index 0000000..290de51
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0035-ANDROID-trusty-Backport-of-trusty-driver.patch
@@ -0,0 +1,8099 @@
+From 3e1e61f54538e8ce4bcbb5a9a213624eafcae514 Mon Sep 17 00:00:00 2001
+From: =?UTF-8?q?Arve=20Hj=C3=B8nnev=C3=A5g?= <arve@android.com>
+Date: Mon, 18 Nov 2013 20:46:48 -0800
+Subject: [PATCH 18/32] ANDROID: trusty: Backport of trusty driver
+
+This adds Trusty driver from android-trusty-5.10
+
+Original commits:
+b60d55f33484 ANDROID: trusty-ipc: Allow registering multiple handles
+629a4d3318cc ANDROID: trusty: Support setting trusty_shared_mem_id_t
+94a36a1374e7 ANDROID: trusty-log: Don't copy Trusty logs to linux kernel log
+efc21cced8af ANDROID: trusty-log: rework buffer allocation
+8cb1a07ca814 ANDROID: trusty-ipc: Fix lock protection of shared_handles
+52cdd137fae0 ANDROID: trusty-log: support poll()
+24c3649dceb9 ANDROID: trusty-irq: enqueue work in trusty_irq_cpu_up
+05a05bdd921e ANDROID: trusty: Add config TRUSTY_CRASH_IS_PANIC
+b5fbdba2ec72 ANDROID: trusty-ipc: Fix crash when running out of txbuffers
+46da5b95605e ANDROID: trusty: Allow TRUSTY_LEND of buffers
+2ebfb16645af ANDROID: trusty-virtio: remove unnecessary include of dma-mapping.h
+bf9d994a65a2 ANDROID: trusty-log: Complement logging sink with unthrottled virtual file
+d5cb51d0365d ANDROID: trusty-log: Refactor logging state to support concurrent sinks
+b421a5ad3eb3 ANDROID: trusty-log: Sanitize u32 overflow of the log ring buffer write index
+58e9681c57af ANDROID: trusty-log: On trusty panic, unthrottle sink to the kernel log
+ba12be0f203a ANDROID: trusty-log: Update trusty log buffer size to hold a complete  Trusty crash logs
+a8a3f83e52b6 ANDROID: trusty_qemu_defconfig: Enable dma-buf and ion system heaps
+988b52b392a1 ANDROID: trusty: Support setting FF-A Tag
+f544e96489aa ANDROID: Add trusty_qemu_defconfig
+8a9b09317f29 ANDROID: trusty-ipc: Switch from memfd to dma_buf
+5460418ec9a4 ANDROID: trusty-irq: document new way of specifying IPIs
+da3c30b943c2 ANDROID: trusty-irq: specify IPIs in new way
+5b5bb7f74856 ANDROID: trusty: Add trusty-test driver
+e80d87f422fd ANDROID: trusty: Add trusty-ipc driver
+03c248cbf693 ANDROID: trusty: Add trusty-virtio driver
+1047661edb97 ANDROID: trusty: Add trusty-log driver
+18fd5c59b423 ANDROID: trusty: Add trusty-irq driver
+479c39a683f8 ANDROID: trusty: Add trusty-core driver
+
+Upstream-Status: Backport
+Change-Id: I91f71b891a1091383a298e7fb2f9030382a19ca5
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ .../devicetree/bindings/trusty/trusty-irq.txt |   67 +
+ .../devicetree/bindings/trusty/trusty-smc.txt |    6 +
+ arch/arm/configs/trusty_qemu_defconfig        |  291 +++
+ .../configs/trusty_qemu_defconfig.fragment    |   26 +
+ drivers/Kconfig                               |    2 +
+ drivers/Makefile                              |    1 +
+ drivers/trusty/Kconfig                        |  116 +
+ drivers/trusty/Makefile                       |   14 +
+ drivers/trusty/trusty-ipc.c                   | 2256 +++++++++++++++++
+ drivers/trusty/trusty-irq.c                   |  645 +++++
+ drivers/trusty/trusty-log.c                   |  830 ++++++
+ drivers/trusty/trusty-log.h                   |   28 +
+ drivers/trusty/trusty-mem.c                   |  139 +
+ drivers/trusty/trusty-smc-arm.S               |   41 +
+ drivers/trusty/trusty-smc-arm64.S             |   35 +
+ drivers/trusty/trusty-smc.h                   |   26 +
+ drivers/trusty/trusty-test.c                  |  440 ++++
+ drivers/trusty/trusty-test.h                  |   13 +
+ drivers/trusty/trusty-virtio.c                |  840 ++++++
+ drivers/trusty/trusty.c                       |  981 +++++++
+ include/linux/trusty/arm_ffa.h                |  590 +++++
+ include/linux/trusty/sm_err.h                 |   28 +
+ include/linux/trusty/smcall.h                 |  124 +
+ include/linux/trusty/trusty.h                 |  131 +
+ include/linux/trusty/trusty_ipc.h             |   89 +
+ include/uapi/linux/trusty/ipc.h               |   65 +
+ include/uapi/linux/virtio_ids.h               |    1 +
+ 27 files changed, 7825 insertions(+)
+ create mode 100644 Documentation/devicetree/bindings/trusty/trusty-irq.txt
+ create mode 100644 Documentation/devicetree/bindings/trusty/trusty-smc.txt
+ create mode 100644 arch/arm/configs/trusty_qemu_defconfig
+ create mode 100644 arch/arm64/configs/trusty_qemu_defconfig.fragment
+ create mode 100644 drivers/trusty/Kconfig
+ create mode 100644 drivers/trusty/Makefile
+ create mode 100644 drivers/trusty/trusty-ipc.c
+ create mode 100644 drivers/trusty/trusty-irq.c
+ create mode 100644 drivers/trusty/trusty-log.c
+ create mode 100644 drivers/trusty/trusty-log.h
+ create mode 100644 drivers/trusty/trusty-mem.c
+ create mode 100644 drivers/trusty/trusty-smc-arm.S
+ create mode 100644 drivers/trusty/trusty-smc-arm64.S
+ create mode 100644 drivers/trusty/trusty-smc.h
+ create mode 100644 drivers/trusty/trusty-test.c
+ create mode 100644 drivers/trusty/trusty-test.h
+ create mode 100644 drivers/trusty/trusty-virtio.c
+ create mode 100644 drivers/trusty/trusty.c
+ create mode 100644 include/linux/trusty/arm_ffa.h
+ create mode 100644 include/linux/trusty/sm_err.h
+ create mode 100644 include/linux/trusty/smcall.h
+ create mode 100644 include/linux/trusty/trusty.h
+ create mode 100644 include/linux/trusty/trusty_ipc.h
+ create mode 100644 include/uapi/linux/trusty/ipc.h
+
+diff --git a/Documentation/devicetree/bindings/trusty/trusty-irq.txt b/Documentation/devicetree/bindings/trusty/trusty-irq.txt
+new file mode 100644
+index 000000000000..cbb545ad452b
+--- /dev/null
++++ b/Documentation/devicetree/bindings/trusty/trusty-irq.txt
+@@ -0,0 +1,67 @@
++Trusty irq interface
++
++Trusty requires non-secure irqs to be forwarded to the secure OS.
++
++Required properties:
++- compatible: "android,trusty-irq-v1"
++
++Optional properties:
++
++- interrupt-templates: is an optional property that works together
++  with "interrupt-ranges" to specify secure side to kernel IRQs mapping.
++
++  It is a list of entries, each one of which defines a group of interrupts
++  having common properties, and has the following format:
++    < phandle irq_id_pos [templ_data]>
++      phandle - phandle of interrupt controller this template is for
++      irq_id_pos - the position of irq id in interrupt specifier array
++                   for interrupt controller referenced by phandle.
++      templ_data - is an array of u32 values (could be empty) in the same
++                   format as interrupt specifier for interrupt controller
++                   referenced by phandle but with omitted irq id field.
++
++- interrupt-ranges: list of entries that specifies secure side to kernel
++  IRQs mapping.
++
++  Each entry in the "interrupt-ranges" list has the following format:
++    <beg end templ_idx>
++      beg - first entry in this range
++      end - last entry in this range
++      templ_idx  - index of entry in "interrupt-templates" property
++                   that must be used as a template for all interrupts
++                   in this range
++
++- ipi-range: optional mapping of a linear range of trusty IRQs to a linear range
++  of IPIs (inter-processor interrupts).  This has the following format:
++    <beg end ipi_base>
++      beg - first trusty IRQ number that is an IPI
++      end - last trusty IRQ number that is an IPI
++      ipi_base - IPI number of 'beg'
++
++Example:
++{
++	gic: interrupt-controller@50041000 {
++		compatible = "arm,gic-400";
++		#interrupt-cells = <3>;
++		interrupt-controller;
++		...
++	};
++	...
++	trusty {
++		compatible = "android,trusty-smc-v1";
++		ranges;
++		#address-cells = <2>;
++		#size-cells = <2>;
++
++		irq {
++			compatible = "android,trusty-irq-v1";
++			interrupt-templates = <&gic 1 GIC_PPI 0>,
++					      <&gic 1 GIC_SPI 0>;
++			interrupt-ranges = <16  31 0>,
++					   <32 223 1>;
++			ipi-range = <8 15 8>;
++		};
++	}
++}
++
++Must be a child of the node that provides the trusty std/fast call interface.
+diff --git a/Documentation/devicetree/bindings/trusty/trusty-smc.txt b/Documentation/devicetree/bindings/trusty/trusty-smc.txt
+new file mode 100644
+index 000000000000..1b39ad317c67
+--- /dev/null
++++ b/Documentation/devicetree/bindings/trusty/trusty-smc.txt
+@@ -0,0 +1,6 @@
++Trusty smc interface
++
++Trusty is running in secure mode on the same (arm) cpu(s) as the current os.
++
++Required properties:
++- compatible: "android,trusty-smc-v1"
+diff --git a/arch/arm/configs/trusty_qemu_defconfig b/arch/arm/configs/trusty_qemu_defconfig
+new file mode 100644
+index 000000000000..46ad9504c23d
+--- /dev/null
++++ b/arch/arm/configs/trusty_qemu_defconfig
+@@ -0,0 +1,291 @@
++# CONFIG_LOCALVERSION_AUTO is not set
++# CONFIG_SWAP is not set
++CONFIG_POSIX_MQUEUE=y
++CONFIG_AUDIT=y
++CONFIG_NO_HZ=y
++CONFIG_HIGH_RES_TIMERS=y
++CONFIG_PREEMPT=y
++CONFIG_BSD_PROCESS_ACCT=y
++CONFIG_BSD_PROCESS_ACCT_V3=y
++CONFIG_TASKSTATS=y
++CONFIG_TASK_DELAY_ACCT=y
++CONFIG_TASK_XACCT=y
++CONFIG_TASK_IO_ACCOUNTING=y
++CONFIG_IKCONFIG=y
++CONFIG_IKCONFIG_PROC=y
++CONFIG_LOG_BUF_SHIFT=14
++CONFIG_RT_GROUP_SCHED=y
++CONFIG_CGROUP_FREEZER=y
++CONFIG_CGROUP_CPUACCT=y
++CONFIG_CGROUP_DEBUG=y
++CONFIG_SCHED_AUTOGROUP=y
++CONFIG_BLK_DEV_INITRD=y
++CONFIG_KALLSYMS_ALL=y
++CONFIG_EMBEDDED=y
++# CONFIG_COMPAT_BRK is not set
++CONFIG_PROFILING=y
++CONFIG_ARCH_VIRT=y
++CONFIG_PCI=y
++CONFIG_PCI_HOST_GENERIC=y
++CONFIG_SMP=y
++CONFIG_HIGHMEM=y
++CONFIG_SECCOMP=y
++CONFIG_CMDLINE="console=ttyAMA0"
++CONFIG_PM_AUTOSLEEP=y
++CONFIG_PM_WAKELOCKS=y
++CONFIG_PM_WAKELOCKS_LIMIT=0
++# CONFIG_PM_WAKELOCKS_GC is not set
++CONFIG_PM_DEBUG=y
++# CONFIG_BLK_DEV_BSG is not set
++# CONFIG_IOSCHED_DEADLINE is not set
++# CONFIG_CORE_DUMP_DEFAULT_ELF_HEADERS is not set
++CONFIG_KSM=y
++CONFIG_NET=y
++CONFIG_PACKET=y
++CONFIG_UNIX=y
++CONFIG_XFRM_USER=y
++CONFIG_NET_KEY=y
++CONFIG_INET=y
++CONFIG_IP_MULTICAST=y
++CONFIG_IP_ADVANCED_ROUTER=y
++CONFIG_IP_MULTIPLE_TABLES=y
++CONFIG_IP_PNP=y
++CONFIG_IP_PNP_DHCP=y
++CONFIG_IP_PNP_BOOTP=y
++CONFIG_INET_ESP=y
++CONFIG_INET_DIAG_DESTROY=y
++CONFIG_IPV6_ROUTER_PREF=y
++CONFIG_IPV6_ROUTE_INFO=y
++CONFIG_IPV6_OPTIMISTIC_DAD=y
++CONFIG_INET6_AH=y
++CONFIG_INET6_ESP=y
++CONFIG_INET6_IPCOMP=y
++CONFIG_IPV6_MIP6=y
++CONFIG_IPV6_MULTIPLE_TABLES=y
++CONFIG_NETFILTER=y
++CONFIG_NF_CONNTRACK=y
++CONFIG_NF_CONNTRACK_SECMARK=y
++CONFIG_NF_CONNTRACK_EVENTS=y
++CONFIG_NF_CONNTRACK_AMANDA=y
++CONFIG_NF_CONNTRACK_FTP=y
++CONFIG_NF_CONNTRACK_H323=y
++CONFIG_NF_CONNTRACK_IRC=y
++CONFIG_NF_CONNTRACK_NETBIOS_NS=y
++CONFIG_NF_CONNTRACK_PPTP=y
++CONFIG_NF_CONNTRACK_SANE=y
++CONFIG_NF_CONNTRACK_TFTP=y
++CONFIG_NF_CT_NETLINK=y
++CONFIG_NETFILTER_XT_TARGET_CLASSIFY=y
++CONFIG_NETFILTER_XT_TARGET_CONNMARK=y
++CONFIG_NETFILTER_XT_TARGET_CONNSECMARK=y
++CONFIG_NETFILTER_XT_TARGET_IDLETIMER=y
++CONFIG_NETFILTER_XT_TARGET_MARK=y
++CONFIG_NETFILTER_XT_TARGET_NFLOG=y
++CONFIG_NETFILTER_XT_TARGET_NFQUEUE=y
++CONFIG_NETFILTER_XT_TARGET_TPROXY=y
++CONFIG_NETFILTER_XT_TARGET_TRACE=y
++CONFIG_NETFILTER_XT_TARGET_SECMARK=y
++CONFIG_NETFILTER_XT_TARGET_TCPMSS=y
++CONFIG_NETFILTER_XT_MATCH_COMMENT=y
++CONFIG_NETFILTER_XT_MATCH_CONNLIMIT=y
++CONFIG_NETFILTER_XT_MATCH_CONNMARK=y
++CONFIG_NETFILTER_XT_MATCH_CONNTRACK=y
++CONFIG_NETFILTER_XT_MATCH_HASHLIMIT=y
++CONFIG_NETFILTER_XT_MATCH_HELPER=y
++CONFIG_NETFILTER_XT_MATCH_IPRANGE=y
++CONFIG_NETFILTER_XT_MATCH_LENGTH=y
++CONFIG_NETFILTER_XT_MATCH_LIMIT=y
++CONFIG_NETFILTER_XT_MATCH_MAC=y
++CONFIG_NETFILTER_XT_MATCH_MARK=y
++CONFIG_NETFILTER_XT_MATCH_POLICY=y
++CONFIG_NETFILTER_XT_MATCH_PKTTYPE=y
++CONFIG_NETFILTER_XT_MATCH_QUOTA=y
++CONFIG_NETFILTER_XT_MATCH_QUOTA2=y
++CONFIG_NETFILTER_XT_MATCH_SOCKET=y
++CONFIG_NETFILTER_XT_MATCH_STATE=y
++CONFIG_NETFILTER_XT_MATCH_STATISTIC=y
++CONFIG_NETFILTER_XT_MATCH_STRING=y
++CONFIG_NETFILTER_XT_MATCH_TIME=y
++CONFIG_NETFILTER_XT_MATCH_U32=y
++CONFIG_IP_NF_IPTABLES=y
++CONFIG_IP_NF_MATCH_AH=y
++CONFIG_IP_NF_MATCH_ECN=y
++CONFIG_IP_NF_MATCH_RPFILTER=y
++CONFIG_IP_NF_MATCH_TTL=y
++CONFIG_IP_NF_FILTER=y
++CONFIG_IP_NF_TARGET_REJECT=y
++CONFIG_IP_NF_MANGLE=y
++CONFIG_IP_NF_TARGET_ECN=y
++CONFIG_IP_NF_TARGET_TTL=y
++CONFIG_IP_NF_RAW=y
++CONFIG_IP_NF_SECURITY=y
++CONFIG_IP_NF_ARPTABLES=y
++CONFIG_IP_NF_ARPFILTER=y
++CONFIG_IP_NF_ARP_MANGLE=y
++CONFIG_IP6_NF_IPTABLES=y
++CONFIG_IP6_NF_MATCH_AH=y
++CONFIG_IP6_NF_MATCH_EUI64=y
++CONFIG_IP6_NF_MATCH_FRAG=y
++CONFIG_IP6_NF_MATCH_OPTS=y
++CONFIG_IP6_NF_MATCH_HL=y
++CONFIG_IP6_NF_MATCH_IPV6HEADER=y
++CONFIG_IP6_NF_MATCH_MH=y
++CONFIG_IP6_NF_MATCH_RT=y
++CONFIG_IP6_NF_TARGET_HL=y
++CONFIG_IP6_NF_FILTER=y
++CONFIG_IP6_NF_TARGET_REJECT=y
++CONFIG_IP6_NF_MANGLE=y
++CONFIG_IP6_NF_RAW=y
++CONFIG_BRIDGE=y
++CONFIG_NET_SCHED=y
++CONFIG_NET_SCH_HTB=y
++CONFIG_NET_CLS_U32=y
++CONFIG_NET_EMATCH=y
++CONFIG_NET_EMATCH_U32=y
++CONFIG_NET_CLS_ACT=y
++# CONFIG_WIRELESS is not set
++CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
++CONFIG_BLK_DEV_LOOP=y
++CONFIG_BLK_DEV_RAM=y
++CONFIG_BLK_DEV_RAM_SIZE=8192
++CONFIG_VIRTIO_BLK=y
++CONFIG_SCSI=y
++# CONFIG_SCSI_PROC_FS is not set
++CONFIG_BLK_DEV_SD=y
++# CONFIG_SCSI_LOWLEVEL is not set
++CONFIG_MD=y
++CONFIG_BLK_DEV_DM=y
++CONFIG_DM_CRYPT=y
++CONFIG_DM_UEVENT=y
++CONFIG_DM_VERITY=y
++CONFIG_DM_VERITY_FEC=y
++CONFIG_NETDEVICES=y
++CONFIG_TUN=y
++CONFIG_VIRTIO_NET=y
++CONFIG_E1000=y
++CONFIG_E1000E=y
++CONFIG_PPP=y
++CONFIG_PPP_BSDCOMP=y
++CONFIG_PPP_DEFLATE=y
++CONFIG_PPP_MPPE=y
++# CONFIG_WLAN is not set
++CONFIG_INPUT_EVDEV=y
++CONFIG_KEYBOARD_GOLDFISH_EVENTS=y
++# CONFIG_INPUT_MOUSE is not set
++CONFIG_INPUT_JOYSTICK=y
++CONFIG_INPUT_TABLET=y
++CONFIG_INPUT_MISC=y
++CONFIG_INPUT_UINPUT=y
++# CONFIG_SERIO_SERPORT is not set
++# CONFIG_VT is not set
++# CONFIG_LEGACY_PTYS is not set
++# CONFIG_DEVMEM is not set
++CONFIG_SERIAL_AMBA_PL011=y
++CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
++CONFIG_VIRTIO_CONSOLE=y
++# CONFIG_HW_RANDOM is not set
++CONFIG_BATTERY_GOLDFISH=y
++# CONFIG_HWMON is not set
++CONFIG_TRUSTY=y
++CONFIG_MEDIA_SUPPORT=y
++CONFIG_FB=y
++CONFIG_FB_GOLDFISH=y
++CONFIG_FB_SIMPLE=y
++CONFIG_BACKLIGHT_LCD_SUPPORT=y
++CONFIG_LOGO=y
++# CONFIG_LOGO_LINUX_MONO is not set
++# CONFIG_LOGO_LINUX_VGA16 is not set
++CONFIG_SOUND=y
++CONFIG_SND=y
++CONFIG_HIDRAW=y
++CONFIG_UHID=y
++CONFIG_HID_A4TECH=y
++CONFIG_HID_ACRUX=y
++CONFIG_HID_ACRUX_FF=y
++CONFIG_HID_APPLE=y
++CONFIG_HID_BELKIN=y
++CONFIG_HID_CHERRY=y
++CONFIG_HID_CHICONY=y
++CONFIG_HID_PRODIKEYS=y
++CONFIG_HID_CYPRESS=y
++CONFIG_HID_DRAGONRISE=y
++CONFIG_DRAGONRISE_FF=y
++CONFIG_HID_EMS_FF=y
++CONFIG_HID_ELECOM=y
++CONFIG_HID_EZKEY=y
++CONFIG_HID_KEYTOUCH=y
++CONFIG_HID_KYE=y
++CONFIG_HID_WALTOP=y
++CONFIG_HID_GYRATION=y
++CONFIG_HID_TWINHAN=y
++CONFIG_HID_KENSINGTON=y
++CONFIG_HID_LCPOWER=y
++CONFIG_HID_LOGITECH=y
++CONFIG_HID_LOGITECH_DJ=y
++CONFIG_LOGITECH_FF=y
++CONFIG_LOGIRUMBLEPAD2_FF=y
++CONFIG_LOGIG940_FF=y
++CONFIG_HID_MAGICMOUSE=y
++CONFIG_HID_MICROSOFT=y
++CONFIG_HID_MONTEREY=y
++CONFIG_HID_MULTITOUCH=y
++CONFIG_HID_ORTEK=y
++CONFIG_HID_PANTHERLORD=y
++CONFIG_PANTHERLORD_FF=y
++CONFIG_HID_PETALYNX=y
++CONFIG_HID_PICOLCD=y
++CONFIG_HID_PRIMAX=y
++CONFIG_HID_SAITEK=y
++CONFIG_HID_SAMSUNG=y
++CONFIG_HID_SPEEDLINK=y
++CONFIG_HID_SUNPLUS=y
++CONFIG_HID_GREENASIA=y
++CONFIG_GREENASIA_FF=y
++CONFIG_HID_SMARTJOYPLUS=y
++CONFIG_SMARTJOYPLUS_FF=y
++CONFIG_HID_TIVO=y
++CONFIG_HID_TOPSEED=y
++CONFIG_HID_THRUSTMASTER=y
++CONFIG_HID_ZEROPLUS=y
++CONFIG_HID_ZYDACRON=y
++# CONFIG_USB_SUPPORT is not set
++CONFIG_RTC_CLASS=y
++CONFIG_VIRTIO_PCI=y
++CONFIG_VIRTIO_MMIO=y
++CONFIG_STAGING=y
++CONFIG_ASHMEM=y
++CONFIG_ION=y
++CONFIG_GOLDFISH_AUDIO=y
++CONFIG_GOLDFISH=y
++CONFIG_GOLDFISH_PIPE=y
++# CONFIG_IOMMU_SUPPORT is not set
++CONFIG_ANDROID=y
++CONFIG_ANDROID_BINDER_IPC=y
++CONFIG_EXT2_FS=y
++CONFIG_EXT4_FS=y
++CONFIG_EXT4_FS_SECURITY=y
++CONFIG_QUOTA=y
++CONFIG_FUSE_FS=y
++CONFIG_CUSE=y
++CONFIG_MSDOS_FS=y
++CONFIG_VFAT_FS=y
++CONFIG_TMPFS=y
++CONFIG_TMPFS_POSIX_ACL=y
++# CONFIG_MISC_FILESYSTEMS is not set
++CONFIG_NFS_FS=y
++CONFIG_ROOT_NFS=y
++CONFIG_NLS_CODEPAGE_437=y
++CONFIG_NLS_ISO8859_1=y
++CONFIG_SECURITY=y
++CONFIG_SECURITY_NETWORK=y
++CONFIG_SECURITY_SELINUX=y
++CONFIG_DYNAMIC_DEBUG=y
++CONFIG_DEBUG_INFO=y
++CONFIG_DEBUG_FS=y
++CONFIG_MAGIC_SYSRQ=y
++CONFIG_PANIC_TIMEOUT=5
++# CONFIG_SCHED_DEBUG is not set
++CONFIG_SCHEDSTATS=y
++# CONFIG_FTRACE is not set
++CONFIG_DMA_API_DEBUG=y
++CONFIG_ATOMIC64_SELFTEST=y
+diff --git a/arch/arm64/configs/trusty_qemu_defconfig.fragment b/arch/arm64/configs/trusty_qemu_defconfig.fragment
+new file mode 100644
+index 000000000000..166eef1797fd
+--- /dev/null
++++ b/arch/arm64/configs/trusty_qemu_defconfig.fragment
+@@ -0,0 +1,26 @@
++# From goldfish
++CONFIG_VIRTIO_BLK=y
++CONFIG_VIRTIO_CONSOLE=y
++CONFIG_VIRTIO_INPUT=y
++CONFIG_VIRTIO_MMIO=y
++CONFIG_VIRTIO_MMIO_CMDLINE_DEVICES=y
++CONFIG_VIRTIO_NET=y
++CONFIG_VIRTIO_PCI=y
++CONFIG_VIRTIO_PMEM=y
++# From Trusty
++CONFIG_TRUSTY=y
++CONFIG_DMA_API_DEBUG=y
++CONFIG_DYNAMIC_DEBUG=y
++CONFIG_PROVE_LOCKING=y
++CONFIG_DEBUG_ATOMIC_SLEEP=y
++CONFIG_SEMIHOSTING_EXIT=y
++CONFIG_E1000=y
++CONFIG_E1000E=y
++CONFIG_REBOOT_EMULATOR_EXIT=y
++CONFIG_DMABUF_HEAPS_SYSTEM=y
++# securefb test uses ION
++CONFIG_ION=y
++CONFIG_ION_SYSTEM_HEAP=y
++# LTO slows down build times considerably. Disable it.
++# CONFIG_LTO_CLANG is not set
++# CONFIG_LTO_CLANG_FULL is not set
+diff --git a/drivers/Kconfig b/drivers/Kconfig
+index dcecc9f6e33f..2e9abcc98126 100644
+--- a/drivers/Kconfig
++++ b/drivers/Kconfig
+@@ -86,6 +86,8 @@ source "drivers/hwmon/Kconfig"
+ 
+ source "drivers/thermal/Kconfig"
+ 
++source "drivers/trusty/Kconfig"
++
+ source "drivers/watchdog/Kconfig"
+ 
+ source "drivers/ssb/Kconfig"
+diff --git a/drivers/Makefile b/drivers/Makefile
+index 576228037718..7d15799dbe77 100644
+--- a/drivers/Makefile
++++ b/drivers/Makefile
+@@ -118,6 +118,7 @@ obj-$(CONFIG_W1)		+= w1/
+ obj-y				+= power/
+ obj-$(CONFIG_HWMON)		+= hwmon/
+ obj-$(CONFIG_THERMAL)		+= thermal/
++obj-$(CONFIG_TRUSTY)		+= trusty/
+ obj-$(CONFIG_WATCHDOG)		+= watchdog/
+ obj-$(CONFIG_MD)		+= md/
+ obj-$(CONFIG_BT)		+= bluetooth/
+diff --git a/drivers/trusty/Kconfig b/drivers/trusty/Kconfig
+new file mode 100644
+index 000000000000..fcde7f097acf
+--- /dev/null
++++ b/drivers/trusty/Kconfig
+@@ -0,0 +1,116 @@
++# SPDX-License-Identifier: GPL-2.0-only
++#
++# Trusty driver
++#
++
++menu "Trusty driver"
++
++config TRUSTY
++	tristate "Trusty core driver"
++	depends on ARM || ARM64
++	help
++	  Trusty is a secure OS that provides a Trusted Execution Environment
++	  (TEE) for Android.  Trusty runs on the same processor as Linux but is
++	  isolated from the rest of the system by both hardware and software.
++
++	  This option enables the core part of the Linux kernel driver for
++	  Trusty.  This doesn't do much by itself; you'll need to enable some of
++	  the sub-modules too.
++
++	  If you build this as a module, it will be called trusty-core.
++
++if TRUSTY
++
++config TRUSTY_IRQ
++	tristate "Trusty IRQ support"
++	default y
++	help
++	  Enable forwarding of IRQs from Linux to Trusty.  This module retrieves
++	  from Trusty a list of IRQs that Trusty uses, and it registers handlers
++	  for them which notify Trusty that the IRQ has been received.
++
++	  If you build this as a module, it will be called trusty-irq.
++
++	  Usually this is needed for Trusty to work, so say 'y' or 'm'.
++
++config TRUSTY_LOG
++	tristate "Trusty log support"
++	default y
++	help
++	  Print log messages generated by the secure OS to the Linux kernel log.
++
++	  While this module is loaded, messages are retrieved and printed after
++	  each call into Trusty, and also during Linux kernel panics.
++
++	  If you build this as a module, it will be called trusty-log.
++
++config TRUSTY_TEST
++	tristate "Trusty stdcall test"
++	default y
++	help
++	  Allow running tests of the Trusty stdcall interface.  Running these
++	  tests is initiated by userspace writing to a sysfs file.
++
++	  This depends on having a test sevice running on the Trusty side.
++
++	  If you build this as a module, it will be called trusty-test.
++
++config TRUSTY_VIRTIO
++	tristate "Trusty virtio support"
++	select VIRTIO
++	default y
++	help
++	  Enable the Trusty virtio driver, which is responsible for management
++	  and interaction with virtio devices exposed by Trusty.  This driver
++	  requests the virtio device descriptors from Trusty, then parses them
++	  and adds the corresponding virtio devices.
++
++	  If you build this as a module, it will be called trusty-virtio.
++
++config TRUSTY_VIRTIO_IPC
++	tristate "Trusty Virtio IPC driver"
++	depends on TRUSTY_VIRTIO
++	default y
++	help
++	  Enable support for communicating with Trusty services.
++
++	  If you build this as a module, it will be called trusty-ipc.
++
++config TRUSTY_DMA_BUF_FFA_TAG
++	bool "Availability of trusty_dma_buf_get_ffa_tag"
++	default n
++	help
++	  Whether trusty_dma_buf_get_ffa_tag is provided on this platform.
++	  Providing this function will allow the platform to select what tag
++	  should be passed to the SPM when attempting to transfer the buffer
++	  to secure world. The value passed here is implementation defined and
++	  may depend on your SPM.
++
++	  If set to N, a default implementation which returns 0 will be used.
++
++config TRUSTY_DMA_BUF_SHARED_MEM_ID
++	bool "Availability of trusty_dma_buf_get_shared_mem_id"
++	default n
++	help
++	  Whether trusty_dma_buf_get_shared_mem_id is provided on this platform.
++	  Providing this function allows the platform to manage memory
++	  transaction life cycle of DMA bufs independently of Trusty IPC driver.
++	  The latter can query trusty_shared_mem_id_t value allocated for a
++	  given DMA buf using trusty_dma_buf_get_shared_mem_id interface.
++
++	  If set to N, a default implementation which does not allocate any IDs
++	  will be used.
++
++config TRUSTY_CRASH_IS_PANIC
++	bool "When trusty panics, then panic the kernel"
++	help
++	 This option will treat Trusty panics as fatal.  This is useful if
++	 your system cannot recover from Trusty panic/halt and you require
++	 the system to reboot to recover.
++
++	 If N, it will contine to run the kernel, but trusty operations will
++	 return errors.
++
++endif # TRUSTY
++
++endmenu
+diff --git a/drivers/trusty/Makefile b/drivers/trusty/Makefile
+new file mode 100644
+index 000000000000..2cf1cfccf97b
+--- /dev/null
++++ b/drivers/trusty/Makefile
+@@ -0,0 +1,14 @@
++# SPDX-License-Identifier: GPL-2.0-only
++#
++# Makefile for trusty components
++#
++
++obj-$(CONFIG_TRUSTY)		+= trusty-core.o
++trusty-core-objs		+= trusty.o trusty-mem.o
++trusty-core-$(CONFIG_ARM)	+= trusty-smc-arm.o
++trusty-core-$(CONFIG_ARM64)	+= trusty-smc-arm64.o
++obj-$(CONFIG_TRUSTY_IRQ)	+= trusty-irq.o
++obj-$(CONFIG_TRUSTY_LOG)	+= trusty-log.o
++obj-$(CONFIG_TRUSTY_TEST)	+= trusty-test.o
++obj-$(CONFIG_TRUSTY_VIRTIO)	+= trusty-virtio.o
++obj-$(CONFIG_TRUSTY_VIRTIO_IPC)	+= trusty-ipc.o
+diff --git a/drivers/trusty/trusty-ipc.c b/drivers/trusty/trusty-ipc.c
+new file mode 100644
+index 000000000000..82d6ddeb41f4
+--- /dev/null
++++ b/drivers/trusty/trusty-ipc.c
+@@ -0,0 +1,2256 @@
++// SPDX-License-Identifier: GPL-2.0-only
++/*
++ * Copyright (C) 2020 Google, Inc.
++ */
++
++#include <linux/aio.h>
++#include <linux/kernel.h>
++#include <linux/module.h>
++#include <linux/cdev.h>
++#include <linux/slab.h>
++#include <linux/fs.h>
++#include <linux/poll.h>
++#include <linux/idr.h>
++#include <linux/completion.h>
++#include <linux/dma-buf.h>
++#include <linux/sched.h>
++#include <linux/sched/signal.h>
++#include <linux/compat.h>
++#include <linux/uio.h>
++#include <linux/file.h>
++
++#include <linux/virtio.h>
++#include <linux/virtio_ids.h>
++#include <linux/virtio_config.h>
++
++#include <linux/trusty/trusty.h>
++#include <linux/trusty/trusty_ipc.h>
++
++#include <uapi/linux/trusty/ipc.h>
++
++#define MAX_DEVICES			4
++
++#define REPLY_TIMEOUT			5000
++#define TXBUF_TIMEOUT			15000
++
++#define MAX_SRV_NAME_LEN		256
++#define MAX_DEV_NAME_LEN		32
++
++#define DEFAULT_MSG_BUF_SIZE		PAGE_SIZE
++#define DEFAULT_MSG_BUF_ALIGN		PAGE_SIZE
++
++#define TIPC_CTRL_ADDR			53
++#define TIPC_ANY_ADDR			0xFFFFFFFF
++
++#define TIPC_MIN_LOCAL_ADDR		1024
++
++#ifdef CONFIG_COMPAT
++#define TIPC_IOC32_CONNECT	_IOW(TIPC_IOC_MAGIC, 0x80, compat_uptr_t)
++#endif
++
++struct tipc_virtio_dev;
++
++struct tipc_dev_config {
++	u32 msg_buf_max_size;
++	u32 msg_buf_alignment;
++	char dev_name[MAX_DEV_NAME_LEN];
++} __packed;
++
++struct tipc_shm {
++	trusty_shared_mem_id_t obj_id;
++	u64 size;
++	u64 tag;
++};
++
++struct tipc_msg_hdr {
++	u32 src;
++	u32 dst;
++	u16 reserved;
++	u16 shm_cnt;
++	u16 len;
++	u16 flags;
++	u8 data[];
++} __packed;
++
++enum tipc_ctrl_msg_types {
++	TIPC_CTRL_MSGTYPE_GO_ONLINE = 1,
++	TIPC_CTRL_MSGTYPE_GO_OFFLINE,
++	TIPC_CTRL_MSGTYPE_CONN_REQ,
++	TIPC_CTRL_MSGTYPE_CONN_RSP,
++	TIPC_CTRL_MSGTYPE_DISC_REQ,
++	TIPC_CTRL_MSGTYPE_RELEASE,
++};
++
++struct tipc_ctrl_msg {
++	u32 type;
++	u32 body_len;
++	u8  body[];
++} __packed;
++
++struct tipc_conn_req_body {
++	char name[MAX_SRV_NAME_LEN];
++} __packed;
++
++struct tipc_conn_rsp_body {
++	u32 target;
++	u32 status;
++	u32 remote;
++	u32 max_msg_size;
++	u32 max_msg_cnt;
++} __packed;
++
++struct tipc_disc_req_body {
++	u32 target;
++} __packed;
++
++struct tipc_release_body {
++	trusty_shared_mem_id_t id;
++} __packed;
++
++struct tipc_cdev_node {
++	struct cdev cdev;
++	struct device *dev;
++	unsigned int minor;
++};
++
++enum tipc_device_state {
++	VDS_OFFLINE = 0,
++	VDS_ONLINE,
++	VDS_DEAD,
++};
++
++struct tipc_virtio_dev {
++	struct kref refcount;
++	struct mutex lock; /* protects access to this device */
++	struct virtio_device *vdev;
++	struct virtqueue *rxvq;
++	struct virtqueue *txvq;
++	unsigned int msg_buf_cnt;
++	unsigned int msg_buf_max_cnt;
++	size_t msg_buf_max_sz;
++	unsigned int free_msg_buf_cnt;
++	struct list_head free_buf_list;
++	wait_queue_head_t sendq;
++	struct idr addr_idr;
++	enum tipc_device_state state;
++	struct tipc_cdev_node cdev_node;
++	/* protects shared_handles, dev lock never acquired while held */
++	struct mutex shared_handles_lock;
++	struct rb_root shared_handles;
++	char   cdev_name[MAX_DEV_NAME_LEN];
++};
++
++enum tipc_chan_state {
++	TIPC_DISCONNECTED = 0,
++	TIPC_CONNECTING,
++	TIPC_CONNECTED,
++	TIPC_STALE,
++};
++
++struct tipc_chan {
++	struct mutex lock; /* protects channel state  */
++	struct kref refcount;
++	enum tipc_chan_state state;
++	struct tipc_virtio_dev *vds;
++	const struct tipc_chan_ops *ops;
++	void *ops_arg;
++	u32 remote;
++	u32 local;
++	u32 max_msg_size;
++	u32 max_msg_cnt;
++	char srv_name[MAX_SRV_NAME_LEN];
++};
++
++struct tipc_shared_handle {
++	struct rb_node node;
++	struct tipc_shm tipc;
++	struct tipc_virtio_dev *vds;
++	struct dma_buf *dma_buf;
++	bool shared;
++	/*
++	 * Following fields are only used if dma_buf does not own a
++	 * trusty_shared_mem_id_t.
++	 */
++	struct dma_buf_attachment *attach;
++	struct sg_table *sgt;
++};
++
++static struct class *tipc_class;
++static unsigned int tipc_major;
++
++static struct virtio_device *default_vdev;
++
++static DEFINE_IDR(tipc_devices);
++static DEFINE_MUTEX(tipc_devices_lock);
++
++static int _match_any(int id, void *p, void *data)
++{
++	return id;
++}
++
++static int _match_data(int id, void *p, void *data)
++{
++	return (p == data);
++}
++
++static void *_alloc_shareable_mem(size_t sz, gfp_t gfp)
++{
++	return alloc_pages_exact(sz, gfp);
++}
++
++static void _free_shareable_mem(size_t sz, void *va)
++{
++	free_pages_exact(va, sz);
++}
++
++static struct tipc_msg_buf *vds_alloc_msg_buf(struct tipc_virtio_dev *vds,
++					      bool share_write)
++{
++	int ret;
++	struct tipc_msg_buf *mb;
++	size_t sz = vds->msg_buf_max_sz;
++	pgprot_t pgprot = share_write ? PAGE_KERNEL : PAGE_KERNEL_RO;
++
++	/* allocate tracking structure */
++	mb = kzalloc(sizeof(struct tipc_msg_buf), GFP_KERNEL);
++	if (!mb)
++		return NULL;
++
++	/* allocate buffer that can be shared with secure world */
++	mb->buf_va = _alloc_shareable_mem(sz, GFP_KERNEL);
++	if (!mb->buf_va)
++		goto err_alloc;
++
++	sg_init_one(&mb->sg, mb->buf_va, sz);
++	ret = trusty_share_memory_compat(vds->vdev->dev.parent->parent,
++					 &mb->buf_id, &mb->sg, 1, pgprot);
++	if (ret) {
++		dev_err(&vds->vdev->dev, "trusty_share_memory failed: %d\n",
++			ret);
++		goto err_share;
++	}
++
++	mb->buf_sz = sz;
++	mb->shm_cnt = 0;
++
++	return mb;
++
++err_share:
++	_free_shareable_mem(sz, mb->buf_va);
++err_alloc:
++	kfree(mb);
++	return NULL;
++}
++
++static void vds_free_msg_buf(struct tipc_virtio_dev *vds,
++			     struct tipc_msg_buf *mb)
++{
++	int ret;
++
++	ret = trusty_reclaim_memory(vds->vdev->dev.parent->parent, mb->buf_id,
++				    &mb->sg, 1);
++	if (WARN_ON(ret)) {
++		dev_err(&vds->vdev->dev,
++			"trusty_revoke_memory failed: %d txbuf %lld\n",
++			ret, mb->buf_id);
++
++		/*
++		 * It is not safe to free this memory if trusty_revoke_memory
++		 * fails. Leak it in that case.
++		 */
++	} else {
++		_free_shareable_mem(mb->buf_sz, mb->buf_va);
++	}
++	kfree(mb);
++}
++
++static void vds_free_msg_buf_list(struct tipc_virtio_dev *vds,
++				  struct list_head *list)
++{
++	struct tipc_msg_buf *mb = NULL;
++
++	mb = list_first_entry_or_null(list, struct tipc_msg_buf, node);
++	while (mb) {
++		list_del(&mb->node);
++		vds_free_msg_buf(vds, mb);
++		mb = list_first_entry_or_null(list, struct tipc_msg_buf, node);
++	}
++}
++
++static inline void mb_reset(struct tipc_msg_buf *mb)
++{
++	mb->wpos = 0;
++	mb->rpos = 0;
++}
++
++static inline void mb_reset_read(struct tipc_msg_buf *mb)
++{
++	mb->rpos = 0;
++}
++
++static void _free_vds(struct kref *kref)
++{
++	struct tipc_virtio_dev *vds =
++		container_of(kref, struct tipc_virtio_dev, refcount);
++	/*
++	 * If this WARN triggers, we're leaking remote memory references.
++	 *
++	 * No need to lock shared_handles_lock. All references to this lock
++	 * should already be gone by this point, since we are freeing it in this
++	 * function.
++	 */
++	WARN_ON(!RB_EMPTY_ROOT(&vds->shared_handles));
++	kfree(vds);
++}
++
++static void _free_chan(struct kref *kref)
++{
++	struct tipc_chan *ch = container_of(kref, struct tipc_chan, refcount);
++
++	if (ch->ops && ch->ops->handle_release)
++		ch->ops->handle_release(ch->ops_arg);
++
++	kref_put(&ch->vds->refcount, _free_vds);
++	kfree(ch);
++}
++
++static bool _put_txbuf_locked(struct tipc_virtio_dev *vds,
++			      struct tipc_msg_buf *mb)
++{
++	list_add_tail(&mb->node, &vds->free_buf_list);
++	return vds->free_msg_buf_cnt++ == 0;
++}
++
++static struct tipc_msg_buf *_get_txbuf_locked(struct tipc_virtio_dev *vds)
++{
++	struct tipc_msg_buf *mb;
++
++	if (vds->state != VDS_ONLINE)
++		return  ERR_PTR(-ENODEV);
++
++	if (vds->free_msg_buf_cnt) {
++		/* take it out of free list */
++		mb = list_first_entry(&vds->free_buf_list,
++				      struct tipc_msg_buf, node);
++		list_del(&mb->node);
++		mb->shm_cnt = 0;
++		vds->free_msg_buf_cnt--;
++	} else {
++		if (vds->msg_buf_cnt >= vds->msg_buf_max_cnt)
++			return ERR_PTR(-EAGAIN);
++
++		/* try to allocate it */
++		mb = vds_alloc_msg_buf(vds, false);
++		if (!mb)
++			return ERR_PTR(-ENOMEM);
++
++		vds->msg_buf_cnt++;
++	}
++	return mb;
++}
++
++static struct tipc_msg_buf *_vds_get_txbuf(struct tipc_virtio_dev *vds)
++{
++	struct tipc_msg_buf *mb;
++
++	mutex_lock(&vds->lock);
++	mb = _get_txbuf_locked(vds);
++	mutex_unlock(&vds->lock);
++
++	return mb;
++}
++
++static void vds_put_txbuf(struct tipc_virtio_dev *vds, struct tipc_msg_buf *mb)
++{
++	mutex_lock(&vds->lock);
++	_put_txbuf_locked(vds, mb);
++	wake_up_interruptible(&vds->sendq);
++	mutex_unlock(&vds->lock);
++}
++
++static struct tipc_msg_buf *vds_get_txbuf(struct tipc_virtio_dev *vds,
++					  long timeout)
++{
++	struct tipc_msg_buf *mb;
++
++	mb = _vds_get_txbuf(vds);
++
++	if ((PTR_ERR(mb) == -EAGAIN) && timeout) {
++		DEFINE_WAIT_FUNC(wait, woken_wake_function);
++
++		timeout = msecs_to_jiffies(timeout);
++		add_wait_queue(&vds->sendq, &wait);
++		for (;;) {
++			timeout = wait_woken(&wait, TASK_INTERRUPTIBLE,
++					     timeout);
++			if (!timeout) {
++				mb = ERR_PTR(-ETIMEDOUT);
++				break;
++			}
++
++			if (signal_pending(current)) {
++				mb = ERR_PTR(-ERESTARTSYS);
++				break;
++			}
++
++			mb = _vds_get_txbuf(vds);
++			if (PTR_ERR(mb) != -EAGAIN)
++				break;
++		}
++		remove_wait_queue(&vds->sendq, &wait);
++	}
++
++	if (IS_ERR(mb))
++		return mb;
++
++	if (WARN_ON(!mb))
++		return ERR_PTR(-EINVAL);
++
++	/* reset and reserve space for message header */
++	mb_reset(mb);
++	mb_put_data(mb, sizeof(struct tipc_msg_hdr));
++
++	return mb;
++}
++
++static int vds_queue_txbuf(struct tipc_virtio_dev *vds,
++			   struct tipc_msg_buf *mb)
++{
++	int err;
++	struct scatterlist sg;
++	bool need_notify = false;
++
++	mutex_lock(&vds->lock);
++	if (vds->state == VDS_ONLINE) {
++		sg_init_one(&sg, mb, mb->wpos);
++		err = virtqueue_add_outbuf(vds->txvq, &sg, 1, mb, GFP_KERNEL);
++		need_notify = virtqueue_kick_prepare(vds->txvq);
++	} else {
++		err = -ENODEV;
++	}
++	mutex_unlock(&vds->lock);
++
++	if (need_notify)
++		virtqueue_notify(vds->txvq);
++
++	return err;
++}
++
++static int vds_add_channel(struct tipc_virtio_dev *vds,
++			   struct tipc_chan *chan)
++{
++	int ret;
++
++	mutex_lock(&vds->lock);
++	if (vds->state == VDS_ONLINE) {
++		ret = idr_alloc(&vds->addr_idr, chan,
++				TIPC_MIN_LOCAL_ADDR, TIPC_ANY_ADDR - 1,
++				GFP_KERNEL);
++		if (ret > 0) {
++			chan->local = ret;
++			kref_get(&chan->refcount);
++			ret = 0;
++		}
++	} else {
++		ret = -EINVAL;
++	}
++	mutex_unlock(&vds->lock);
++
++	return ret;
++}
++
++static void vds_del_channel(struct tipc_virtio_dev *vds,
++			    struct tipc_chan *chan)
++{
++	mutex_lock(&vds->lock);
++	if (chan->local) {
++		idr_remove(&vds->addr_idr, chan->local);
++		chan->local = 0;
++		chan->remote = 0;
++		kref_put(&chan->refcount, _free_chan);
++	}
++	mutex_unlock(&vds->lock);
++}
++
++static struct tipc_chan *vds_lookup_channel(struct tipc_virtio_dev *vds,
++					    u32 addr)
++{
++	int id;
++	struct tipc_chan *chan = NULL;
++
++	mutex_lock(&vds->lock);
++	if (addr == TIPC_ANY_ADDR) {
++		id = idr_for_each(&vds->addr_idr, _match_any, NULL);
++		if (id > 0)
++			chan = idr_find(&vds->addr_idr, id);
++	} else {
++		chan = idr_find(&vds->addr_idr, addr);
++	}
++	if (chan)
++		kref_get(&chan->refcount);
++	mutex_unlock(&vds->lock);
++
++	return chan;
++}
++
++static struct tipc_chan *vds_create_channel(struct tipc_virtio_dev *vds,
++					    const struct tipc_chan_ops *ops,
++					    void *ops_arg)
++{
++	int ret;
++	struct tipc_chan *chan = NULL;
++
++	if (!vds)
++		return ERR_PTR(-ENOENT);
++
++	if (!ops)
++		return ERR_PTR(-EINVAL);
++
++	chan = kzalloc(sizeof(*chan), GFP_KERNEL);
++	if (!chan)
++		return ERR_PTR(-ENOMEM);
++
++	kref_get(&vds->refcount);
++	chan->vds = vds;
++	chan->ops = ops;
++	chan->ops_arg = ops_arg;
++	mutex_init(&chan->lock);
++	kref_init(&chan->refcount);
++	chan->state = TIPC_DISCONNECTED;
++
++	ret = vds_add_channel(vds, chan);
++	if (ret) {
++		kfree(chan);
++		kref_put(&vds->refcount, _free_vds);
++		return ERR_PTR(ret);
++	}
++
++	return chan;
++}
++
++static void fill_msg_hdr(struct tipc_msg_buf *mb, u32 src, u32 dst)
++{
++	struct tipc_msg_hdr *hdr = mb_get_data(mb, sizeof(*hdr));
++
++	hdr->src = src;
++	hdr->dst = dst;
++	hdr->len = mb_avail_data(mb);
++	hdr->flags = 0;
++	hdr->shm_cnt = mb->shm_cnt;
++	hdr->reserved = 0;
++}
++
++static int tipc_shared_handle_new(struct tipc_shared_handle **shared_handle,
++				  struct tipc_virtio_dev *vds)
++{
++	struct tipc_shared_handle *out = kzalloc(sizeof(*out), GFP_KERNEL);
++
++	if (!out)
++		return -ENOMEM;
++
++	out->vds = vds;
++	*shared_handle = out;
++
++	return 0;
++}
++
++static struct device *tipc_shared_handle_dev(struct tipc_shared_handle
++					     *shared_handle)
++{
++	return shared_handle->vds->vdev->dev.parent->parent;
++}
++
++static bool is_same_memory_region(struct tipc_shared_handle *h1,
++				  struct tipc_shared_handle *h2)
++{
++	return h1->tipc.obj_id == h2->tipc.obj_id &&
++			h1->tipc.size == h2->tipc.size &&
++			h1->tipc.tag == h2->tipc.tag &&
++			h1->dma_buf == h2->dma_buf &&
++			h1->shared == h2->shared;
++}
++
++static bool dma_buf_owns_shared_mem_id(struct tipc_shared_handle *h)
++{
++	/* h->shared is true only if dma_buf did not own an shared memory ID */
++	return !h->shared;
++}
++
++static void tipc_shared_handle_register(struct tipc_shared_handle
++					*new_handle)
++{
++	struct tipc_virtio_dev *vds = new_handle->vds;
++	struct rb_node **new;
++	struct rb_node *parent = NULL;
++
++	mutex_lock(&vds->shared_handles_lock);
++
++	new = &vds->shared_handles.rb_node;
++	while (*new) {
++		struct tipc_shared_handle *handle =
++			rb_entry(*new, struct tipc_shared_handle, node);
++		parent = *new;
++		/*
++		 * An obj_id can be registered multiple times if it's owned by a
++		 * dma_buf, because in this case we use the same obj_id across
++		 * multiple memory transfer operations.
++		 */
++		if (handle->tipc.obj_id == new_handle->tipc.obj_id) {
++			if (dma_buf_owns_shared_mem_id(new_handle)) {
++				WARN_ON(!is_same_memory_region(handle,
++							       new_handle));
++			} else {
++				WARN(1, "This handle is already registered");
++				goto already_registered;
++			}
++		}
++
++		if (handle->tipc.obj_id > new_handle->tipc.obj_id)
++			new = &((*new)->rb_left);
++		else
++			new = &((*new)->rb_right);
++	}
++
++	rb_link_node(&new_handle->node, parent, new);
++	rb_insert_color(&new_handle->node, &vds->shared_handles);
++
++already_registered:
++	mutex_unlock(&vds->shared_handles_lock);
++}
++
++static struct tipc_shared_handle *tipc_shared_handle_take(struct tipc_virtio_dev
++							  *vds,
++							  trusty_shared_mem_id_t
++							  obj_id)
++{
++	struct rb_node *node;
++	struct tipc_shared_handle *out = NULL;
++
++	mutex_lock(&vds->shared_handles_lock);
++
++	node = vds->shared_handles.rb_node;
++	while (node) {
++		struct tipc_shared_handle *handle =
++			rb_entry(node, struct tipc_shared_handle, node);
++		if (obj_id == handle->tipc.obj_id) {
++			rb_erase(node, &vds->shared_handles);
++			out = handle;
++			break;
++		} else if (obj_id < handle->tipc.obj_id) {
++			node = node->rb_left;
++		} else {
++			node = node->rb_right;
++		}
++	}
++
++	mutex_unlock(&vds->shared_handles_lock);
++
++	return out;
++}
++
++static int tipc_shared_handle_drop(struct tipc_shared_handle *shared_handle)
++{
++	int ret;
++	struct tipc_virtio_dev *vds = shared_handle->vds;
++	struct device *dev = tipc_shared_handle_dev(shared_handle);
++
++	if (shared_handle->shared) {
++		/*
++		 * If this warning fires, it means this shared handle was still
++		 * in the set of active handles. This shouldn't happen (calling
++		 * code should ensure it is out if the tree) but this serves as
++		 * an extra check before it is released.
++		 *
++		 * However, the take itself should clean this incorrect state up
++		 * by removing the handle from the tree.
++		 *
++		 * This warning is only applicable when registering a handle
++		 * multiple times is not allowed, i.e. when dma_buf doesn't own
++		 * the handle.
++		 */
++		WARN_ON(tipc_shared_handle_take(vds,
++						shared_handle->tipc.obj_id));
++
++		ret = trusty_reclaim_memory(dev,
++					    shared_handle->tipc.obj_id,
++					    shared_handle->sgt->sgl,
++					    shared_handle->sgt->orig_nents);
++		if (ret) {
++			/*
++			 * We can't safely release this, it may still be in
++			 * use outside Linux.
++			 */
++			dev_warn(dev, "Failed to drop handle, leaking...\n");
++			return ret;
++		}
++	}
++
++	if (shared_handle->sgt)
++		dma_buf_unmap_attachment(shared_handle->attach,
++					 shared_handle->sgt, DMA_BIDIRECTIONAL);
++	if (shared_handle->attach)
++		dma_buf_detach(shared_handle->dma_buf, shared_handle->attach);
++	if (shared_handle->dma_buf)
++		dma_buf_put(shared_handle->dma_buf);
++
++	kfree(shared_handle);
++
++	return 0;
++}
++
++/*****************************************************************************/
++
++struct tipc_chan *tipc_create_channel(struct device *dev,
++				      const struct tipc_chan_ops *ops,
++				      void *ops_arg)
++{
++	struct virtio_device *vd;
++	struct tipc_chan *chan;
++	struct tipc_virtio_dev *vds;
++
++	mutex_lock(&tipc_devices_lock);
++	if (dev) {
++		vd = container_of(dev, struct virtio_device, dev);
++	} else {
++		vd = default_vdev;
++		if (!vd) {
++			mutex_unlock(&tipc_devices_lock);
++			return ERR_PTR(-ENOENT);
++		}
++	}
++	vds = vd->priv;
++	kref_get(&vds->refcount);
++	mutex_unlock(&tipc_devices_lock);
++
++	chan = vds_create_channel(vds, ops, ops_arg);
++	kref_put(&vds->refcount, _free_vds);
++	return chan;
++}
++EXPORT_SYMBOL(tipc_create_channel);
++
++struct tipc_msg_buf *tipc_chan_get_rxbuf(struct tipc_chan *chan)
++{
++	return vds_alloc_msg_buf(chan->vds, true);
++}
++EXPORT_SYMBOL(tipc_chan_get_rxbuf);
++
++void tipc_chan_put_rxbuf(struct tipc_chan *chan, struct tipc_msg_buf *mb)
++{
++	vds_free_msg_buf(chan->vds, mb);
++}
++EXPORT_SYMBOL(tipc_chan_put_rxbuf);
++
++struct tipc_msg_buf *tipc_chan_get_txbuf_timeout(struct tipc_chan *chan,
++						 long timeout)
++{
++	return vds_get_txbuf(chan->vds, timeout);
++}
++EXPORT_SYMBOL(tipc_chan_get_txbuf_timeout);
++
++void tipc_chan_put_txbuf(struct tipc_chan *chan, struct tipc_msg_buf *mb)
++{
++	vds_put_txbuf(chan->vds, mb);
++}
++EXPORT_SYMBOL(tipc_chan_put_txbuf);
++
++int tipc_chan_queue_msg(struct tipc_chan *chan, struct tipc_msg_buf *mb)
++{
++	int err;
++
++	mutex_lock(&chan->lock);
++	switch (chan->state) {
++	case TIPC_CONNECTED:
++		fill_msg_hdr(mb, chan->local, chan->remote);
++		err = vds_queue_txbuf(chan->vds, mb);
++		if (err) {
++			/* this should never happen */
++			dev_err(&chan->vds->vdev->dev,
++				"%s: failed to queue tx buffer (%d)\n",
++			       __func__, err);
++		}
++		break;
++	case TIPC_DISCONNECTED:
++	case TIPC_CONNECTING:
++		err = -ENOTCONN;
++		break;
++	case TIPC_STALE:
++		err = -ESHUTDOWN;
++		break;
++	default:
++		err = -EBADFD;
++		dev_err(&chan->vds->vdev->dev,
++			"%s: unexpected channel state %d\n",
++			__func__, chan->state);
++	}
++	mutex_unlock(&chan->lock);
++	return err;
++}
++EXPORT_SYMBOL(tipc_chan_queue_msg);
++
++
++int tipc_chan_connect(struct tipc_chan *chan, const char *name)
++{
++	int err;
++	struct tipc_ctrl_msg *msg;
++	struct tipc_conn_req_body *body;
++	struct tipc_msg_buf *txbuf;
++
++	txbuf = vds_get_txbuf(chan->vds, TXBUF_TIMEOUT);
++	if (IS_ERR(txbuf))
++		return PTR_ERR(txbuf);
++
++	/* reserve space for connection request control message */
++	msg = mb_put_data(txbuf, sizeof(*msg) + sizeof(*body));
++	body = (struct tipc_conn_req_body *)msg->body;
++
++	/* fill message */
++	msg->type = TIPC_CTRL_MSGTYPE_CONN_REQ;
++	msg->body_len  = sizeof(*body);
++
++	strncpy(body->name, name, sizeof(body->name));
++	body->name[sizeof(body->name)-1] = '\0';
++
++	mutex_lock(&chan->lock);
++	switch (chan->state) {
++	case TIPC_DISCONNECTED:
++		/* save service name we are connecting to */
++		strcpy(chan->srv_name, body->name);
++
++		fill_msg_hdr(txbuf, chan->local, TIPC_CTRL_ADDR);
++		err = vds_queue_txbuf(chan->vds, txbuf);
++		if (err) {
++			/* this should never happen */
++			dev_err(&chan->vds->vdev->dev,
++				"%s: failed to queue tx buffer (%d)\n",
++				__func__, err);
++		} else {
++			chan->state = TIPC_CONNECTING;
++			txbuf = NULL; /* prevents discarding buffer */
++		}
++		break;
++	case TIPC_CONNECTED:
++	case TIPC_CONNECTING:
++		/* check if we are trying to connect to the same service */
++		if (strcmp(chan->srv_name, body->name) == 0)
++			err = 0;
++		else
++			if (chan->state == TIPC_CONNECTING)
++				err = -EALREADY; /* in progress */
++			else
++				err = -EISCONN;  /* already connected */
++		break;
++
++	case TIPC_STALE:
++		err = -ESHUTDOWN;
++		break;
++	default:
++		err = -EBADFD;
++		dev_err(&chan->vds->vdev->dev,
++			"%s: unexpected channel state %d\n",
++			__func__, chan->state);
++		break;
++	}
++	mutex_unlock(&chan->lock);
++
++	if (txbuf)
++		tipc_chan_put_txbuf(chan, txbuf); /* discard it */
++
++	return err;
++}
++EXPORT_SYMBOL(tipc_chan_connect);
++
++int tipc_chan_shutdown(struct tipc_chan *chan)
++{
++	int err;
++	struct tipc_ctrl_msg *msg;
++	struct tipc_disc_req_body *body;
++	struct tipc_msg_buf *txbuf = NULL;
++
++	/* get tx buffer */
++	txbuf = vds_get_txbuf(chan->vds, TXBUF_TIMEOUT);
++	if (IS_ERR(txbuf))
++		return PTR_ERR(txbuf);
++
++	mutex_lock(&chan->lock);
++	if (chan->state == TIPC_CONNECTED || chan->state == TIPC_CONNECTING) {
++		/* reserve space for disconnect request control message */
++		msg = mb_put_data(txbuf, sizeof(*msg) + sizeof(*body));
++		body = (struct tipc_disc_req_body *)msg->body;
++
++		msg->type = TIPC_CTRL_MSGTYPE_DISC_REQ;
++		msg->body_len = sizeof(*body);
++		body->target = chan->remote;
++
++		fill_msg_hdr(txbuf, chan->local, TIPC_CTRL_ADDR);
++		err = vds_queue_txbuf(chan->vds, txbuf);
++		if (err) {
++			/* this should never happen */
++			dev_err(&chan->vds->vdev->dev,
++				"%s: failed to queue tx buffer (%d)\n",
++				__func__, err);
++		}
++	} else {
++		err = -ENOTCONN;
++	}
++	chan->state = TIPC_STALE;
++	mutex_unlock(&chan->lock);
++
++	if (err) {
++		/* release buffer */
++		tipc_chan_put_txbuf(chan, txbuf);
++	}
++
++	return err;
++}
++EXPORT_SYMBOL(tipc_chan_shutdown);
++
++void tipc_chan_destroy(struct tipc_chan *chan)
++{
++	vds_del_channel(chan->vds, chan);
++	kref_put(&chan->refcount, _free_chan);
++}
++EXPORT_SYMBOL(tipc_chan_destroy);
++
++/***************************************************************************/
++
++struct tipc_dn_chan {
++	int state;
++	struct mutex lock; /* protects rx_msg_queue list and channel state */
++	struct tipc_chan *chan;
++	wait_queue_head_t readq;
++	struct completion reply_comp;
++	struct list_head rx_msg_queue;
++};
++
++static int dn_wait_for_reply(struct tipc_dn_chan *dn, int timeout)
++{
++	int ret;
++
++	ret = wait_for_completion_interruptible_timeout(&dn->reply_comp,
++					msecs_to_jiffies(timeout));
++	if (ret < 0)
++		return ret;
++
++	mutex_lock(&dn->lock);
++	if (!ret) {
++		/* no reply from remote */
++		dn->state = TIPC_STALE;
++		ret = -ETIMEDOUT;
++	} else {
++		/* got reply */
++		if (dn->state == TIPC_CONNECTED)
++			ret = 0;
++		else if (dn->state == TIPC_DISCONNECTED)
++			if (!list_empty(&dn->rx_msg_queue))
++				ret = 0;
++			else
++				ret = -ENOTCONN;
++		else
++			ret = -EIO;
++	}
++	mutex_unlock(&dn->lock);
++
++	return ret;
++}
++
++static struct tipc_msg_buf *dn_handle_msg(void *data,
++					  struct tipc_msg_buf *rxbuf)
++{
++	struct tipc_dn_chan *dn = data;
++	struct tipc_msg_buf *newbuf = rxbuf;
++
++	mutex_lock(&dn->lock);
++	if (dn->state == TIPC_CONNECTED) {
++		/* get new buffer */
++		newbuf = tipc_chan_get_rxbuf(dn->chan);
++		if (newbuf) {
++			/* queue an old buffer and return a new one */
++			list_add_tail(&rxbuf->node, &dn->rx_msg_queue);
++			wake_up_interruptible(&dn->readq);
++		} else {
++			/*
++			 * return an old buffer effectively discarding
++			 * incoming message
++			 */
++			dev_err(&dn->chan->vds->vdev->dev,
++				"%s: discard incoming message\n", __func__);
++			newbuf = rxbuf;
++		}
++	}
++	mutex_unlock(&dn->lock);
++
++	return newbuf;
++}
++
++static void dn_connected(struct tipc_dn_chan *dn)
++{
++	mutex_lock(&dn->lock);
++	dn->state = TIPC_CONNECTED;
++
++	/* complete all pending  */
++	complete(&dn->reply_comp);
++
++	mutex_unlock(&dn->lock);
++}
++
++static void dn_disconnected(struct tipc_dn_chan *dn)
++{
++	mutex_lock(&dn->lock);
++	dn->state = TIPC_DISCONNECTED;
++
++	/* complete all pending  */
++	complete(&dn->reply_comp);
++
++	/* wakeup all readers */
++	wake_up_interruptible_all(&dn->readq);
++
++	mutex_unlock(&dn->lock);
++}
++
++static void dn_shutdown(struct tipc_dn_chan *dn)
++{
++	mutex_lock(&dn->lock);
++
++	/* set state to STALE */
++	dn->state = TIPC_STALE;
++
++	/* complete all pending  */
++	complete(&dn->reply_comp);
++
++	/* wakeup all readers */
++	wake_up_interruptible_all(&dn->readq);
++
++	mutex_unlock(&dn->lock);
++}
++
++static void dn_handle_event(void *data, int event)
++{
++	struct tipc_dn_chan *dn = data;
++
++	switch (event) {
++	case TIPC_CHANNEL_SHUTDOWN:
++		dn_shutdown(dn);
++		break;
++
++	case TIPC_CHANNEL_DISCONNECTED:
++		dn_disconnected(dn);
++		break;
++
++	case TIPC_CHANNEL_CONNECTED:
++		dn_connected(dn);
++		break;
++
++	default:
++		dev_err(&dn->chan->vds->vdev->dev,
++			"%s: unhandled event %d\n", __func__, event);
++		break;
++	}
++}
++
++static void dn_handle_release(void *data)
++{
++	kfree(data);
++}
++
++static const struct tipc_chan_ops _dn_ops = {
++	.handle_msg = dn_handle_msg,
++	.handle_event = dn_handle_event,
++	.handle_release = dn_handle_release,
++};
++
++#define cdev_to_cdn(c) container_of((c), struct tipc_cdev_node, cdev)
++#define cdn_to_vds(cdn) container_of((cdn), struct tipc_virtio_dev, cdev_node)
++
++static struct tipc_virtio_dev *_dn_lookup_vds(struct tipc_cdev_node *cdn)
++{
++	int ret;
++	struct tipc_virtio_dev *vds = NULL;
++
++	mutex_lock(&tipc_devices_lock);
++	ret = idr_for_each(&tipc_devices, _match_data, cdn);
++	if (ret) {
++		vds = cdn_to_vds(cdn);
++		kref_get(&vds->refcount);
++	}
++	mutex_unlock(&tipc_devices_lock);
++	return vds;
++}
++
++static int tipc_open(struct inode *inode, struct file *filp)
++{
++	int ret;
++	struct tipc_virtio_dev *vds;
++	struct tipc_dn_chan *dn;
++	struct tipc_cdev_node *cdn = cdev_to_cdn(inode->i_cdev);
++
++	vds = _dn_lookup_vds(cdn);
++	if (!vds) {
++		ret = -ENOENT;
++		goto err_vds_lookup;
++	}
++
++	dn = kzalloc(sizeof(*dn), GFP_KERNEL);
++	if (!dn) {
++		ret = -ENOMEM;
++		goto err_alloc_chan;
++	}
++
++	mutex_init(&dn->lock);
++	init_waitqueue_head(&dn->readq);
++	init_completion(&dn->reply_comp);
++	INIT_LIST_HEAD(&dn->rx_msg_queue);
++
++	dn->state = TIPC_DISCONNECTED;
++
++	dn->chan = vds_create_channel(vds, &_dn_ops, dn);
++	if (IS_ERR(dn->chan)) {
++		ret = PTR_ERR(dn->chan);
++		goto err_create_chan;
++	}
++
++	filp->private_data = dn;
++	kref_put(&vds->refcount, _free_vds);
++	return 0;
++
++err_create_chan:
++	kfree(dn);
++err_alloc_chan:
++	kref_put(&vds->refcount, _free_vds);
++err_vds_lookup:
++	return ret;
++}
++
++
++static int dn_connect_ioctl(struct tipc_dn_chan *dn, char __user *usr_name)
++{
++	int ret;
++	char name[MAX_SRV_NAME_LEN];
++
++	/* copy in service name from user space */
++	ret = strncpy_from_user(name, usr_name, sizeof(name));
++	if (ret < 0)
++		return ret;
++	if (ret == sizeof(name))
++		return -ENAMETOOLONG;
++
++	/* send connect request */
++	ret = tipc_chan_connect(dn->chan, name);
++	if (ret)
++		return ret;
++
++	/* and wait for reply */
++	return dn_wait_for_reply(dn, REPLY_TIMEOUT);
++}
++
++static int dn_share_fd(struct tipc_dn_chan *dn, int fd,
++		       enum transfer_kind transfer_kind,
++		       struct tipc_shared_handle **out)
++{
++	int ret = 0;
++	struct tipc_shared_handle *shared_handle = NULL;
++	struct file *file = NULL;
++	struct device *dev = &dn->chan->vds->vdev->dev;
++	bool writable = false;
++	pgprot_t prot;
++	u64 tag = 0;
++	trusty_shared_mem_id_t mem_id;
++	bool lend;
++
++	if (dn->state != TIPC_CONNECTED) {
++		dev_dbg(dev, "Tried to share fd while not connected\n");
++		return -ENOTCONN;
++	}
++
++	file = fget(fd);
++	if (!file) {
++		dev_dbg(dev, "Invalid fd (%d)\n", fd);
++		return -EBADF;
++	}
++
++	if (!(file->f_mode & FMODE_READ)) {
++		dev_dbg(dev, "Cannot create write-only mapping\n");
++		fput(file);
++		return -EACCES;
++	}
++
++	writable = file->f_mode & FMODE_WRITE;
++	prot = writable ? PAGE_KERNEL : PAGE_KERNEL_RO;
++	fput(file);
++	file = NULL;
++
++	ret = tipc_shared_handle_new(&shared_handle, dn->chan->vds);
++	if (ret)
++		return ret;
++
++	shared_handle->dma_buf = dma_buf_get(fd);
++	if (IS_ERR(shared_handle->dma_buf)) {
++		ret = PTR_ERR(shared_handle->dma_buf);
++		shared_handle->dma_buf = NULL;
++		dev_dbg(dev, "Unable to get dma buf from fd (%d)\n", ret);
++		goto cleanup_handle;
++	}
++
++	tag = trusty_dma_buf_get_ffa_tag(shared_handle->dma_buf);
++	ret = trusty_dma_buf_get_shared_mem_id(shared_handle->dma_buf, &mem_id);
++	/*
++	 * Buffers with a preallocated mem_id should only be sent to Trusty
++	 * using TRUSTY_SEND_SECURE. And conversely, TRUSTY_SEND_SECURE should
++	 * only be used to send buffers with preallcoated mem_id.
++	 */
++	if (!ret) {
++		/* Use shared memory ID owned by dma_buf */
++		/* TODO: Enforce transfer_kind == TRUSTY_SEND_SECURE */
++		WARN_ONCE(transfer_kind != TRUSTY_SEND_SECURE,
++			  "Use TRUSTY_SEND_SECURE instead");
++		goto mem_id_allocated;
++	}
++
++	if (ret != -ENODATA) {
++		dev_err(dev, "dma_buf can't be transferred (%d)\n", ret);
++		goto cleanup_handle;
++	}
++
++	if (transfer_kind == TRUSTY_SEND_SECURE) {
++		dev_err(dev, "No mem ID for TRUSTY_SEND_SECURE\n");
++		goto cleanup_handle;
++	}
++	lend = (transfer_kind == TRUSTY_LEND);
++
++	shared_handle->attach = dma_buf_attach(shared_handle->dma_buf, dev);
++	if (IS_ERR(shared_handle->attach)) {
++		ret = PTR_ERR(shared_handle->attach);
++		shared_handle->attach = NULL;
++		dev_dbg(dev, "Unable to attach to dma_buf (%d)\n", ret);
++		goto cleanup_handle;
++	}
++
++	shared_handle->sgt = dma_buf_map_attachment(shared_handle->attach,
++						    DMA_BIDIRECTIONAL);
++	if (IS_ERR(shared_handle->sgt)) {
++		ret = PTR_ERR(shared_handle->sgt);
++		shared_handle->sgt = NULL;
++		dev_dbg(dev, "Failed to match attachment (%d)\n", ret);
++		goto cleanup_handle;
++	}
++
++	ret = trusty_transfer_memory(tipc_shared_handle_dev(shared_handle),
++				     &mem_id, shared_handle->sgt->sgl,
++				     shared_handle->sgt->orig_nents, prot, tag,
++				     lend);
++
++	if (ret < 0) {
++		dev_dbg(dev, "Transferring memory failed: %d\n", ret);
++		/*
++		 * The handle now has a sgt containing the pages, so we no
++		 * longer need to clean up the pages directly.
++		 */
++		goto cleanup_handle;
++	}
++	shared_handle->shared = true;
++
++mem_id_allocated:
++	shared_handle->tipc.obj_id = mem_id;
++	shared_handle->tipc.size = shared_handle->dma_buf->size;
++	shared_handle->tipc.tag = tag;
++	*out = shared_handle;
++	return 0;
++
++cleanup_handle:
++	tipc_shared_handle_drop(shared_handle);
++	return ret;
++}
++
++static ssize_t txbuf_write_iter(struct tipc_msg_buf *txbuf,
++				struct iov_iter *iter)
++{
++	size_t len;
++	/* message length */
++	len = iov_iter_count(iter);
++
++	/* check available space */
++	if (len > mb_avail_space(txbuf))
++		return -EMSGSIZE;
++
++	/* copy in message data */
++	if (copy_from_iter(mb_put_data(txbuf, len), len, iter) != len)
++		return -EFAULT;
++
++	return len;
++}
++
++static ssize_t txbuf_write_handles(struct tipc_msg_buf *txbuf,
++				   struct tipc_shared_handle **shm_handles,
++				   size_t shm_cnt)
++{
++	size_t idx;
++
++	/* message length */
++	size_t len = shm_cnt * sizeof(struct tipc_shm);
++
++	/* check available space */
++	if (len > mb_avail_space(txbuf))
++		return -EMSGSIZE;
++
++	/* copy over handles */
++	for (idx = 0; idx < shm_cnt; idx++) {
++		memcpy(mb_put_data(txbuf, sizeof(struct tipc_shm)),
++		       &shm_handles[idx]->tipc,
++		       sizeof(struct tipc_shm));
++	}
++
++	txbuf->shm_cnt += shm_cnt;
++
++	return len;
++}
++
++static long filp_send_ioctl(struct file *filp,
++			    const struct tipc_send_msg_req __user *arg)
++{
++	struct tipc_send_msg_req req;
++	struct iovec fast_iovs[UIO_FASTIOV];
++	struct iovec *iov = fast_iovs;
++	struct iov_iter iter;
++	struct trusty_shm *shm = NULL;
++	struct tipc_shared_handle **shm_handles = NULL;
++	int shm_idx = 0;
++	int release_idx;
++	struct tipc_dn_chan *dn = filp->private_data;
++	struct tipc_virtio_dev *vds = dn->chan->vds;
++	struct device *dev = &vds->vdev->dev;
++	long timeout = TXBUF_TIMEOUT;
++	struct tipc_msg_buf *txbuf = NULL;
++	long ret = 0;
++	ssize_t data_len = 0;
++	ssize_t shm_len = 0;
++
++	if (copy_from_user(&req, arg, sizeof(req)))
++		return -EFAULT;
++
++	if (req.shm_cnt > U16_MAX)
++		return -E2BIG;
++
++	shm = kmalloc_array(req.shm_cnt, sizeof(*shm), GFP_KERNEL);
++	if (!shm)
++		return -ENOMEM;
++
++	shm_handles = kmalloc_array(req.shm_cnt, sizeof(*shm_handles),
++				    GFP_KERNEL);
++	if (!shm_handles) {
++		ret = -ENOMEM;
++		goto shm_handles_alloc_failed;
++	}
++
++	if (copy_from_user(shm, u64_to_user_ptr(req.shm),
++			   req.shm_cnt * sizeof(struct trusty_shm))) {
++		ret = -EFAULT;
++		goto load_shm_args_failed;
++	}
++
++	ret = import_iovec(READ, u64_to_user_ptr(req.iov), req.iov_cnt,
++			   ARRAY_SIZE(fast_iovs), &iov, &iter);
++	if (ret < 0) {
++		dev_dbg(dev, "Failed to import iovec\n");
++		goto iov_import_failed;
++	}
++
++	for (shm_idx = 0; shm_idx < req.shm_cnt; shm_idx++) {
++		switch (shm[shm_idx].transfer) {
++		case TRUSTY_SHARE:
++		case TRUSTY_LEND:
++		case TRUSTY_SEND_SECURE:
++			break;
++		default:
++			dev_err(dev, "Unknown transfer type: 0x%x\n",
++				shm[shm_idx].transfer);
++			goto shm_share_failed;
++		}
++		ret = dn_share_fd(dn, shm[shm_idx].fd, shm[shm_idx].transfer,
++				  &shm_handles[shm_idx]);
++		if (ret) {
++			dev_dbg(dev, "Forwarding memory failed\n"
++				);
++			goto shm_share_failed;
++		}
++	}
++
++	if (filp->f_flags & O_NONBLOCK)
++		timeout = 0;
++
++	txbuf = tipc_chan_get_txbuf_timeout(dn->chan, timeout);
++	if (IS_ERR(txbuf)) {
++		dev_dbg(dev, "Failed to get txbuffer\n");
++		ret = PTR_ERR(txbuf);
++		goto get_txbuf_failed;
++	}
++
++	data_len = txbuf_write_iter(txbuf, &iter);
++	if (data_len < 0) {
++		ret = data_len;
++		goto txbuf_write_failed;
++	}
++
++	shm_len = txbuf_write_handles(txbuf, shm_handles, req.shm_cnt);
++	if (shm_len < 0) {
++		ret = shm_len;
++		goto txbuf_write_failed;
++	}
++
++	/*
++	 * These need to be aded to the index before queueing the message.
++	 * As soon as the message is sent, we may receive a message back from
++	 * Trusty saying it's no longer in use, and the shared_handle needs
++	 * to be there when that happens.
++	 */
++	for (shm_idx = 0; shm_idx < req.shm_cnt; shm_idx++)
++		tipc_shared_handle_register(shm_handles[shm_idx]);
++
++	ret = tipc_chan_queue_msg(dn->chan, txbuf);
++
++	if (ret)
++		goto queue_failed;
++
++	ret = data_len;
++
++common_cleanup:
++	kfree(iov);
++iov_import_failed:
++load_shm_args_failed:
++	kfree(shm_handles);
++shm_handles_alloc_failed:
++	kfree(shm);
++	return ret;
++
++queue_failed:
++	for (release_idx = 0; release_idx < req.shm_cnt; release_idx++)
++		tipc_shared_handle_take(vds,
++					shm_handles[release_idx]->tipc.obj_id);
++txbuf_write_failed:
++	tipc_chan_put_txbuf(dn->chan, txbuf);
++get_txbuf_failed:
++shm_share_failed:
++	for (shm_idx--; shm_idx >= 0; shm_idx--)
++		tipc_shared_handle_drop(shm_handles[shm_idx]);
++	goto common_cleanup;
++}
++
++static long tipc_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
++{
++	struct tipc_dn_chan *dn = filp->private_data;
++
++	switch (cmd) {
++	case TIPC_IOC_CONNECT:
++		return dn_connect_ioctl(dn, (char __user *)arg);
++	case TIPC_IOC_SEND_MSG:
++		return filp_send_ioctl(filp,
++				       (const struct tipc_send_msg_req __user *)
++				       arg);
++	default:
++		dev_dbg(&dn->chan->vds->vdev->dev,
++			"Unhandled ioctl cmd: 0x%x\n", cmd);
++		return -ENOTTY;
++	}
++}
++
++#ifdef CONFIG_COMPAT
++static long tipc_compat_ioctl(struct file *filp,
++			      unsigned int cmd, unsigned long arg)
++{
++	struct tipc_dn_chan *dn = filp->private_data;
++
++	switch (cmd) {
++	case TIPC_IOC32_CONNECT:
++		cmd = TIPC_IOC_CONNECT;
++		break;
++	default:
++		dev_dbg(&dn->chan->vds->vdev->dev,
++			"Unhandled compat ioctl command: 0x%x\n", cmd);
++		return -ENOTTY;
++	}
++	return tipc_ioctl(filp, cmd, (unsigned long)compat_ptr(arg));
++}
++#endif
++
++static inline bool _got_rx(struct tipc_dn_chan *dn)
++{
++	if (dn->state != TIPC_CONNECTED)
++		return true;
++
++	if (!list_empty(&dn->rx_msg_queue))
++		return true;
++
++	return false;
++}
++
++static ssize_t tipc_read_iter(struct kiocb *iocb, struct iov_iter *iter)
++{
++	ssize_t ret;
++	size_t len;
++	struct tipc_msg_buf *mb;
++	struct file *filp = iocb->ki_filp;
++	struct tipc_dn_chan *dn = filp->private_data;
++
++	mutex_lock(&dn->lock);
++
++	while (list_empty(&dn->rx_msg_queue)) {
++		if (dn->state != TIPC_CONNECTED) {
++			if (dn->state == TIPC_CONNECTING)
++				ret = -ENOTCONN;
++			else if (dn->state == TIPC_DISCONNECTED)
++				ret = -ENOTCONN;
++			else if (dn->state == TIPC_STALE)
++				ret = -ESHUTDOWN;
++			else
++				ret = -EBADFD;
++			goto out;
++		}
++
++		mutex_unlock(&dn->lock);
++
++		if (filp->f_flags & O_NONBLOCK)
++			return -EAGAIN;
++
++		if (wait_event_interruptible(dn->readq, _got_rx(dn)))
++			return -ERESTARTSYS;
++
++		mutex_lock(&dn->lock);
++	}
++
++	mb = list_first_entry(&dn->rx_msg_queue, struct tipc_msg_buf, node);
++
++	len = mb_avail_data(mb);
++	if (len > iov_iter_count(iter)) {
++		ret = -EMSGSIZE;
++		goto out;
++	}
++
++	if (copy_to_iter(mb_get_data(mb, len), len, iter) != len) {
++		ret = -EFAULT;
++		goto out;
++	}
++
++	ret = len;
++	list_del(&mb->node);
++	tipc_chan_put_rxbuf(dn->chan, mb);
++
++out:
++	mutex_unlock(&dn->lock);
++	return ret;
++}
++
++static ssize_t tipc_write_iter(struct kiocb *iocb, struct iov_iter *iter)
++{
++	struct file *filp = iocb->ki_filp;
++	struct tipc_dn_chan *dn = filp->private_data;
++	long timeout = TXBUF_TIMEOUT;
++	struct tipc_msg_buf *txbuf = NULL;
++	ssize_t ret = 0;
++	ssize_t len = 0;
++
++	if (filp->f_flags & O_NONBLOCK)
++		timeout = 0;
++
++	txbuf = tipc_chan_get_txbuf_timeout(dn->chan, timeout);
++
++	if (IS_ERR(txbuf))
++		return PTR_ERR(txbuf);
++
++	len = txbuf_write_iter(txbuf, iter);
++	if (len < 0)
++		goto err_out;
++
++	/* queue message */
++	ret = tipc_chan_queue_msg(dn->chan, txbuf);
++	if (ret)
++		goto err_out;
++
++	return len;
++
++err_out:
++	tipc_chan_put_txbuf(dn->chan, txbuf);
++	return ret;
++}
++
++static __poll_t tipc_poll(struct file *filp, poll_table *wait)
++{
++	__poll_t mask = 0;
++	struct tipc_dn_chan *dn = filp->private_data;
++
++	mutex_lock(&dn->lock);
++
++	poll_wait(filp, &dn->readq, wait);
++
++	/* Writes always succeed for now */
++	mask |= EPOLLOUT | EPOLLWRNORM;
++
++	if (!list_empty(&dn->rx_msg_queue))
++		mask |= EPOLLIN | EPOLLRDNORM;
++
++	if (dn->state != TIPC_CONNECTED)
++		mask |= EPOLLERR;
++
++	mutex_unlock(&dn->lock);
++	return mask;
++}
++
++
++static int tipc_release(struct inode *inode, struct file *filp)
++{
++	struct tipc_dn_chan *dn = filp->private_data;
++
++	dn_shutdown(dn);
++
++	/* free all pending buffers */
++	vds_free_msg_buf_list(dn->chan->vds, &dn->rx_msg_queue);
++
++	/* shutdown channel  */
++	tipc_chan_shutdown(dn->chan);
++
++	/* and destroy it */
++	tipc_chan_destroy(dn->chan);
++
++	return 0;
++}
++
++static const struct file_operations tipc_fops = {
++	.open		= tipc_open,
++	.release	= tipc_release,
++	.unlocked_ioctl	= tipc_ioctl,
++#ifdef CONFIG_COMPAT
++	.compat_ioctl	= tipc_compat_ioctl,
++#endif
++	.read_iter	= tipc_read_iter,
++	.write_iter	= tipc_write_iter,
++	.poll		= tipc_poll,
++	.owner		= THIS_MODULE,
++};
++
++/*****************************************************************************/
++
++static void chan_trigger_event(struct tipc_chan *chan, int event)
++{
++	if (!event)
++		return;
++
++	chan->ops->handle_event(chan->ops_arg, event);
++}
++
++static void _cleanup_vq(struct tipc_virtio_dev *vds, struct virtqueue *vq)
++{
++	struct tipc_msg_buf *mb;
++
++	while ((mb = virtqueue_detach_unused_buf(vq)) != NULL)
++		vds_free_msg_buf(vds, mb);
++}
++
++static int _create_cdev_node(struct device *parent,
++			     struct tipc_cdev_node *cdn,
++			     const char *name)
++{
++	int ret;
++	dev_t devt;
++
++	if (!name) {
++		dev_dbg(parent, "%s: cdev name has to be provided\n",
++			__func__);
++		return -EINVAL;
++	}
++
++	/* allocate minor */
++	ret = idr_alloc(&tipc_devices, cdn, 0, MAX_DEVICES, GFP_KERNEL);
++	if (ret < 0) {
++		dev_dbg(parent, "%s: failed (%d) to get id\n",
++			__func__, ret);
++		return ret;
++	}
++
++	cdn->minor = ret;
++	cdev_init(&cdn->cdev, &tipc_fops);
++	cdn->cdev.owner = THIS_MODULE;
++
++	/* Add character device */
++	devt = MKDEV(tipc_major, cdn->minor);
++	ret = cdev_add(&cdn->cdev, devt, 1);
++	if (ret) {
++		dev_dbg(parent, "%s: cdev_add failed (%d)\n",
++			__func__, ret);
++		goto err_add_cdev;
++	}
++
++	/* Create a device node */
++	cdn->dev = device_create(tipc_class, parent,
++				 devt, NULL, "trusty-ipc-%s", name);
++	if (IS_ERR(cdn->dev)) {
++		ret = PTR_ERR(cdn->dev);
++		dev_dbg(parent, "%s: device_create failed: %d\n",
++			__func__, ret);
++		goto err_device_create;
++	}
++
++	return 0;
++
++err_device_create:
++	cdn->dev = NULL;
++	cdev_del(&cdn->cdev);
++err_add_cdev:
++	idr_remove(&tipc_devices, cdn->minor);
++	return ret;
++}
++
++static void create_cdev_node(struct tipc_virtio_dev *vds,
++			     struct tipc_cdev_node *cdn)
++{
++	int err;
++
++	mutex_lock(&tipc_devices_lock);
++
++	if (!default_vdev) {
++		kref_get(&vds->refcount);
++		default_vdev = vds->vdev;
++	}
++
++	if (vds->cdev_name[0] && !cdn->dev) {
++		kref_get(&vds->refcount);
++		err = _create_cdev_node(&vds->vdev->dev, cdn, vds->cdev_name);
++		if (err) {
++			dev_err(&vds->vdev->dev,
++				"failed (%d) to create cdev node\n", err);
++			kref_put(&vds->refcount, _free_vds);
++		}
++	}
++	mutex_unlock(&tipc_devices_lock);
++}
++
++static void destroy_cdev_node(struct tipc_virtio_dev *vds,
++			      struct tipc_cdev_node *cdn)
++{
++	mutex_lock(&tipc_devices_lock);
++	if (cdn->dev) {
++		device_destroy(tipc_class, MKDEV(tipc_major, cdn->minor));
++		cdev_del(&cdn->cdev);
++		idr_remove(&tipc_devices, cdn->minor);
++		cdn->dev = NULL;
++		kref_put(&vds->refcount, _free_vds);
++	}
++
++	if (default_vdev == vds->vdev) {
++		default_vdev = NULL;
++		kref_put(&vds->refcount, _free_vds);
++	}
++
++	mutex_unlock(&tipc_devices_lock);
++}
++
++static void _go_online(struct tipc_virtio_dev *vds)
++{
++	mutex_lock(&vds->lock);
++	if (vds->state == VDS_OFFLINE)
++		vds->state = VDS_ONLINE;
++	mutex_unlock(&vds->lock);
++
++	create_cdev_node(vds, &vds->cdev_node);
++
++	dev_info(&vds->vdev->dev, "is online\n");
++}
++
++static void _go_offline(struct tipc_virtio_dev *vds)
++{
++	struct tipc_chan *chan;
++
++	/* change state to OFFLINE */
++	mutex_lock(&vds->lock);
++	if (vds->state != VDS_ONLINE) {
++		mutex_unlock(&vds->lock);
++		return;
++	}
++	vds->state = VDS_OFFLINE;
++	mutex_unlock(&vds->lock);
++
++	/* wakeup all waiters */
++	wake_up_interruptible_all(&vds->sendq);
++
++	/* shutdown all channels */
++	while ((chan = vds_lookup_channel(vds, TIPC_ANY_ADDR))) {
++		mutex_lock(&chan->lock);
++		chan->state = TIPC_STALE;
++		chan->remote = 0;
++		chan_trigger_event(chan, TIPC_CHANNEL_SHUTDOWN);
++		mutex_unlock(&chan->lock);
++		kref_put(&chan->refcount, _free_chan);
++	}
++
++	/* shutdown device node */
++	destroy_cdev_node(vds, &vds->cdev_node);
++
++	dev_info(&vds->vdev->dev, "is offline\n");
++}
++
++static void _handle_conn_rsp(struct tipc_virtio_dev *vds,
++			     struct tipc_conn_rsp_body *rsp, size_t len)
++{
++	struct tipc_chan *chan;
++
++	if (sizeof(*rsp) != len) {
++		dev_err(&vds->vdev->dev, "%s: Invalid response length %zd\n",
++			__func__, len);
++		return;
++	}
++
++	dev_dbg(&vds->vdev->dev,
++		"%s: connection response: for addr 0x%x: status %d remote addr 0x%x\n",
++		__func__, rsp->target, rsp->status, rsp->remote);
++
++	/* Lookup channel */
++	chan = vds_lookup_channel(vds, rsp->target);
++	if (chan) {
++		mutex_lock(&chan->lock);
++		if (chan->state == TIPC_CONNECTING) {
++			if (!rsp->status) {
++				chan->state = TIPC_CONNECTED;
++				chan->remote = rsp->remote;
++				chan->max_msg_cnt = rsp->max_msg_cnt;
++				chan->max_msg_size = rsp->max_msg_size;
++				chan_trigger_event(chan,
++						   TIPC_CHANNEL_CONNECTED);
++			} else {
++				chan->state = TIPC_DISCONNECTED;
++				chan->remote = 0;
++				chan_trigger_event(chan,
++						   TIPC_CHANNEL_DISCONNECTED);
++			}
++		}
++		mutex_unlock(&chan->lock);
++		kref_put(&chan->refcount, _free_chan);
++	}
++}
++
++static void _handle_disc_req(struct tipc_virtio_dev *vds,
++			     struct tipc_disc_req_body *req, size_t len)
++{
++	struct tipc_chan *chan;
++
++	if (sizeof(*req) != len) {
++		dev_err(&vds->vdev->dev, "%s: Invalid request length %zd\n",
++			__func__, len);
++		return;
++	}
++
++	dev_dbg(&vds->vdev->dev, "%s: disconnect request: for addr 0x%x\n",
++		__func__, req->target);
++
++	chan = vds_lookup_channel(vds, req->target);
++	if (chan) {
++		mutex_lock(&chan->lock);
++		if (chan->state == TIPC_CONNECTED ||
++			chan->state == TIPC_CONNECTING) {
++			chan->state = TIPC_DISCONNECTED;
++			chan->remote = 0;
++			chan_trigger_event(chan, TIPC_CHANNEL_DISCONNECTED);
++		}
++		mutex_unlock(&chan->lock);
++		kref_put(&chan->refcount, _free_chan);
++	}
++}
++
++static void _handle_release(struct tipc_virtio_dev *vds,
++			    struct tipc_release_body *req, size_t len)
++{
++	struct tipc_shared_handle *handle = NULL;
++	struct device *dev = &vds->vdev->dev;
++	int ret = 0;
++
++	if (len < sizeof(*req)) {
++		dev_err(dev, "Received undersized release control message\n");
++		return;
++	}
++
++	handle = tipc_shared_handle_take(vds, req->id);
++	if (!handle) {
++		dev_err(dev,
++			"Received release control message for untracked handle: 0x%llx\n",
++			req->id);
++		return;
++	}
++
++	ret = tipc_shared_handle_drop(handle);
++
++	if (ret) {
++		dev_err(dev,
++			"Failed to release handle 0x%llx upon request: (%d)\n",
++			req->id, ret);
++		/*
++		 * Put the handle back in case we got a spurious release now and
++		 * get a real one later. This path should not happen, we're
++		 * just trying to be robust.
++		 */
++		tipc_shared_handle_register(handle);
++	}
++}
++
++static void _handle_ctrl_msg(struct tipc_virtio_dev *vds,
++			     void *data, int len, u32 src)
++{
++	struct tipc_ctrl_msg *msg = data;
++
++	if ((len < sizeof(*msg)) || (sizeof(*msg) + msg->body_len != len)) {
++		dev_err(&vds->vdev->dev,
++			"%s: Invalid message length ( %d vs. %d)\n",
++			__func__, (int)(sizeof(*msg) + msg->body_len), len);
++		return;
++	}
++
++	dev_dbg(&vds->vdev->dev,
++		"%s: Incoming ctrl message: src 0x%x type %d len %d\n",
++		__func__, src, msg->type, msg->body_len);
++
++	switch (msg->type) {
++	case TIPC_CTRL_MSGTYPE_GO_ONLINE:
++		_go_online(vds);
++		break;
++
++	case TIPC_CTRL_MSGTYPE_GO_OFFLINE:
++		_go_offline(vds);
++		break;
++
++	case TIPC_CTRL_MSGTYPE_CONN_RSP:
++		_handle_conn_rsp(vds, (struct tipc_conn_rsp_body *)msg->body,
++				 msg->body_len);
++		break;
++
++	case TIPC_CTRL_MSGTYPE_DISC_REQ:
++		_handle_disc_req(vds, (struct tipc_disc_req_body *)msg->body,
++				 msg->body_len);
++		break;
++
++	case TIPC_CTRL_MSGTYPE_RELEASE:
++		_handle_release(vds, (struct tipc_release_body *)msg->body,
++				msg->body_len);
++	break;
++
++	default:
++		dev_warn(&vds->vdev->dev,
++			 "%s: Unexpected message type: %d\n",
++			 __func__, msg->type);
++	}
++}
++
++static void handle_dropped_chan_msg(struct tipc_virtio_dev *vds,
++				    struct tipc_msg_buf *mb,
++				    struct tipc_msg_hdr *msg)
++{
++	int shm_idx;
++	struct tipc_shm *shm;
++	struct tipc_shared_handle *shared_handle;
++	struct device *dev = &vds->vdev->dev;
++	size_t len;
++
++	if (msg->len < msg->shm_cnt * sizeof(*shm)) {
++		dev_err(dev, "shm_cnt does not fit in dropped message");
++		/* The message is corrupt, so we can't recover resources */
++		return;
++	}
++
++	len = msg->len - msg->shm_cnt * sizeof(*shm);
++	/* skip normal data */
++	(void)mb_get_data(mb, len);
++
++	for (shm_idx = 0; shm_idx < msg->shm_cnt; shm_idx++) {
++		shm = mb_get_data(mb, sizeof(*shm));
++		shared_handle = tipc_shared_handle_take(vds, shm->obj_id);
++		if (shared_handle) {
++			if (tipc_shared_handle_drop(shared_handle))
++				dev_err(dev,
++					"Failed to drop handle found in dropped buffer");
++		} else {
++			dev_err(dev,
++				"Found handle in dropped buffer which was not registered to tipc device...");
++		}
++	}
++}
++
++static void handle_dropped_mb(struct tipc_virtio_dev *vds,
++			      struct tipc_msg_buf *mb)
++{
++	struct tipc_msg_hdr *msg;
++
++	mb_reset_read(mb);
++	msg = mb_get_data(mb, sizeof(*msg));
++	if (msg->dst != TIPC_CTRL_ADDR) {
++		handle_dropped_chan_msg(vds, mb, msg);
++	}
++}
++
++static int _handle_rxbuf(struct tipc_virtio_dev *vds,
++			 struct tipc_msg_buf *rxbuf, size_t rxlen)
++{
++	int err;
++	struct scatterlist sg;
++	struct tipc_msg_hdr *msg;
++	struct device *dev = &vds->vdev->dev;
++
++	/* message sanity check */
++	if (rxlen > rxbuf->buf_sz) {
++		dev_warn(dev, "inbound msg is too big: %zd\n", rxlen);
++		goto drop_it;
++	}
++
++	if (rxlen < sizeof(*msg)) {
++		dev_warn(dev, "inbound msg is too short: %zd\n", rxlen);
++		goto drop_it;
++	}
++
++	/* reset buffer and put data  */
++	mb_reset(rxbuf);
++	mb_put_data(rxbuf, rxlen);
++
++	/* get message header */
++	msg = mb_get_data(rxbuf, sizeof(*msg));
++	if (mb_avail_data(rxbuf) != msg->len) {
++		dev_warn(dev, "inbound msg length mismatch: (%zu vs. %d)\n",
++			 mb_avail_data(rxbuf), msg->len);
++		goto drop_it;
++	}
++
++	dev_dbg(dev, "From: %d, To: %d, Len: %d, Flags: 0x%x, Reserved: %d, shm_cnt: %d\n",
++		msg->src, msg->dst, msg->len, msg->flags, msg->reserved,
++		msg->shm_cnt);
++
++	/* message directed to control endpoint is a special case */
++	if (msg->dst == TIPC_CTRL_ADDR) {
++		_handle_ctrl_msg(vds, msg->data, msg->len, msg->src);
++	} else {
++		struct tipc_chan *chan = NULL;
++		/* Lookup channel */
++		chan = vds_lookup_channel(vds, msg->dst);
++		if (chan) {
++			/* handle it */
++			rxbuf = chan->ops->handle_msg(chan->ops_arg, rxbuf);
++			kref_put(&chan->refcount, _free_chan);
++			if (WARN_ON(!rxbuf))
++				return -EINVAL;
++		}
++	}
++
++drop_it:
++	/* add the buffer back to the virtqueue */
++	sg_init_one(&sg, rxbuf, rxbuf->buf_sz);
++	err = virtqueue_add_inbuf(vds->rxvq, &sg, 1, rxbuf, GFP_KERNEL);
++	if (err < 0) {
++		dev_err(dev, "failed to add a virtqueue buffer: %d\n", err);
++		return err;
++	}
++
++	return 0;
++}
++
++static void _rxvq_cb(struct virtqueue *rxvq)
++{
++	unsigned int len;
++	struct tipc_msg_buf *mb;
++	unsigned int msg_cnt = 0;
++	struct tipc_virtio_dev *vds = rxvq->vdev->priv;
++
++	while ((mb = virtqueue_get_buf(rxvq, &len)) != NULL) {
++		if (_handle_rxbuf(vds, mb, len))
++			break;
++		msg_cnt++;
++	}
++
++	/* tell the other size that we added rx buffers */
++	if (msg_cnt)
++		virtqueue_kick(rxvq);
++}
++
++static void _txvq_cb(struct virtqueue *txvq)
++{
++	unsigned int len;
++	struct tipc_msg_buf *mb;
++	bool need_wakeup = false;
++	struct tipc_virtio_dev *vds = txvq->vdev->priv;
++
++	/* detach all buffers */
++	mutex_lock(&vds->lock);
++	while ((mb = virtqueue_get_buf(txvq, &len)) != NULL) {
++		if ((int)len < 0)
++			handle_dropped_mb(vds, mb);
++		need_wakeup |= _put_txbuf_locked(vds, mb);
++	}
++	mutex_unlock(&vds->lock);
++
++	if (need_wakeup) {
++		/* wake up potential senders waiting for a tx buffer */
++		wake_up_interruptible_all(&vds->sendq);
++	}
++}
++
++static int tipc_virtio_probe(struct virtio_device *vdev)
++{
++	int err, i;
++	struct tipc_virtio_dev *vds;
++	struct tipc_dev_config config;
++	struct virtqueue *vqs[2];
++	vq_callback_t *vq_cbs[] = {_rxvq_cb, _txvq_cb};
++	static const char * const vq_names[] = { "rx", "tx" };
++
++	vds = kzalloc(sizeof(*vds), GFP_KERNEL);
++	if (!vds)
++		return -ENOMEM;
++
++	vds->vdev = vdev;
++
++	mutex_init(&vds->lock);
++	mutex_init(&vds->shared_handles_lock);
++	kref_init(&vds->refcount);
++	init_waitqueue_head(&vds->sendq);
++	INIT_LIST_HEAD(&vds->free_buf_list);
++	idr_init(&vds->addr_idr);
++	vds->shared_handles = RB_ROOT;
++	dma_coerce_mask_and_coherent(&vds->vdev->dev,
++				     *vds->vdev->dev.parent->parent->dma_mask);
++
++	/* set default max message size and alignment */
++	memset(&config, 0, sizeof(config));
++	config.msg_buf_max_size  = DEFAULT_MSG_BUF_SIZE;
++	config.msg_buf_alignment = DEFAULT_MSG_BUF_ALIGN;
++
++	/* get configuration if present */
++	vdev->config->get(vdev, 0, &config, sizeof(config));
++
++	/* copy dev name */
++	strncpy(vds->cdev_name, config.dev_name, sizeof(vds->cdev_name));
++	vds->cdev_name[sizeof(vds->cdev_name)-1] = '\0';
++
++	/* find tx virtqueues (rx and tx and in this order) */
++	err = vdev->config->find_vqs(vdev, 2, vqs, vq_cbs, vq_names, NULL,
++				     NULL);
++	if (err)
++		goto err_find_vqs;
++
++	vds->rxvq = vqs[0];
++	vds->txvq = vqs[1];
++
++	/* save max buffer size and count */
++	vds->msg_buf_max_sz = config.msg_buf_max_size;
++	vds->msg_buf_max_cnt = virtqueue_get_vring_size(vds->txvq);
++
++	/* set up the receive buffers */
++	for (i = 0; i < virtqueue_get_vring_size(vds->rxvq); i++) {
++		struct scatterlist sg;
++		struct tipc_msg_buf *rxbuf;
++
++		rxbuf = vds_alloc_msg_buf(vds, true);
++		if (!rxbuf) {
++			dev_err(&vdev->dev, "failed to allocate rx buffer\n");
++			err = -ENOMEM;
++			goto err_free_rx_buffers;
++		}
++
++		sg_init_one(&sg, rxbuf, rxbuf->buf_sz);
++		err = virtqueue_add_inbuf(vds->rxvq, &sg, 1, rxbuf, GFP_KERNEL);
++		WARN_ON(err); /* sanity check; this can't really happen */
++	}
++
++	vdev->priv = vds;
++	vds->state = VDS_OFFLINE;
++
++	dev_dbg(&vdev->dev, "%s: done\n", __func__);
++	return 0;
++
++err_free_rx_buffers:
++	_cleanup_vq(vds, vds->rxvq);
++err_find_vqs:
++	kref_put(&vds->refcount, _free_vds);
++	return err;
++}
++
++static void tipc_virtio_remove(struct virtio_device *vdev)
++{
++	struct tipc_virtio_dev *vds = vdev->priv;
++
++	_go_offline(vds);
++
++	mutex_lock(&vds->lock);
++	vds->state = VDS_DEAD;
++	vds->vdev = NULL;
++	mutex_unlock(&vds->lock);
++
++	vdev->config->reset(vdev);
++
++	idr_destroy(&vds->addr_idr);
++
++	_cleanup_vq(vds, vds->rxvq);
++	_cleanup_vq(vds, vds->txvq);
++	vds_free_msg_buf_list(vds, &vds->free_buf_list);
++
++	vdev->config->del_vqs(vds->vdev);
++
++	kref_put(&vds->refcount, _free_vds);
++}
++
++static const struct virtio_device_id tipc_virtio_id_table[] = {
++	{ VIRTIO_ID_TRUSTY_IPC, VIRTIO_DEV_ANY_ID },
++	{ 0 },
++};
++
++static const unsigned int features[] = {
++	0,
++};
++
++static struct virtio_driver virtio_tipc_driver = {
++	.feature_table	= features,
++	.feature_table_size = ARRAY_SIZE(features),
++	.driver.name	= KBUILD_MODNAME,
++	.driver.owner	= THIS_MODULE,
++	.id_table	= tipc_virtio_id_table,
++	.probe		= tipc_virtio_probe,
++	.remove		= tipc_virtio_remove,
++};
++
++static int __init tipc_init(void)
++{
++	int ret;
++	dev_t dev;
++
++	ret = alloc_chrdev_region(&dev, 0, MAX_DEVICES, KBUILD_MODNAME);
++	if (ret) {
++		pr_err("%s: alloc_chrdev_region failed: %d\n", __func__, ret);
++		return ret;
++	}
++
++	tipc_major = MAJOR(dev);
++	tipc_class = class_create(THIS_MODULE, KBUILD_MODNAME);
++	if (IS_ERR(tipc_class)) {
++		ret = PTR_ERR(tipc_class);
++		pr_err("%s: class_create failed: %d\n", __func__, ret);
++		goto err_class_create;
++	}
++
++	ret = register_virtio_driver(&virtio_tipc_driver);
++	if (ret) {
++		pr_err("failed to register virtio driver: %d\n", ret);
++		goto err_register_virtio_drv;
++	}
++
++	return 0;
++
++err_register_virtio_drv:
++	class_destroy(tipc_class);
++
++err_class_create:
++	unregister_chrdev_region(dev, MAX_DEVICES);
++	return ret;
++}
++
++static void __exit tipc_exit(void)
++{
++	unregister_virtio_driver(&virtio_tipc_driver);
++	class_destroy(tipc_class);
++	unregister_chrdev_region(MKDEV(tipc_major, 0), MAX_DEVICES);
++}
++
++/* We need to init this early */
++subsys_initcall(tipc_init);
++module_exit(tipc_exit);
++
++MODULE_DEVICE_TABLE(tipc, tipc_virtio_id_table);
++MODULE_DESCRIPTION("Trusty IPC driver");
++MODULE_LICENSE("GPL v2");
+diff --git a/drivers/trusty/trusty-irq.c b/drivers/trusty/trusty-irq.c
+new file mode 100644
+index 000000000000..5c6076108d0e
+--- /dev/null
++++ b/drivers/trusty/trusty-irq.c
+@@ -0,0 +1,645 @@
++// SPDX-License-Identifier: GPL-2.0-only
++/*
++ * Copyright (C) 2013 Google, Inc.
++ */
++
++#include <linux/cpu.h>
++#include <linux/interrupt.h>
++#include <linux/irq.h>
++#include <linux/irqdomain.h>
++#include <linux/module.h>
++#include <linux/of.h>
++#include <linux/of_irq.h>
++#include <linux/platform_device.h>
++#include <linux/slab.h>
++#include <linux/string.h>
++#include <linux/trusty/smcall.h>
++#include <linux/trusty/sm_err.h>
++#include <linux/trusty/trusty.h>
++
++struct trusty_irq {
++	struct trusty_irq_state *is;
++	struct hlist_node node;
++	unsigned int irq;
++	bool percpu;
++	bool enable;
++	bool doorbell;
++	struct trusty_irq __percpu *percpu_ptr;
++};
++
++struct trusty_irq_irqset {
++	struct hlist_head pending;
++	struct hlist_head inactive;
++};
++
++struct trusty_irq_state {
++	struct device *dev;
++	struct device *trusty_dev;
++	struct trusty_irq_irqset normal_irqs;
++	spinlock_t normal_irqs_lock;
++	struct trusty_irq_irqset __percpu *percpu_irqs;
++	struct notifier_block trusty_call_notifier;
++	struct hlist_node cpuhp_node;
++};
++
++static int trusty_irq_cpuhp_slot = -1;
++
++static void trusty_irq_enable_pending_irqs(struct trusty_irq_state *is,
++					   struct trusty_irq_irqset *irqset,
++					   bool percpu)
++{
++	struct hlist_node *n;
++	struct trusty_irq *trusty_irq;
++
++	hlist_for_each_entry_safe(trusty_irq, n, &irqset->pending, node) {
++		dev_dbg(is->dev,
++			"%s: enable pending irq %d, percpu %d, cpu %d\n",
++			__func__, trusty_irq->irq, percpu, smp_processor_id());
++		if (percpu)
++			enable_percpu_irq(trusty_irq->irq, 0);
++		else
++			enable_irq(trusty_irq->irq);
++		hlist_del(&trusty_irq->node);
++		hlist_add_head(&trusty_irq->node, &irqset->inactive);
++	}
++}
++
++static void trusty_irq_enable_irqset(struct trusty_irq_state *is,
++				      struct trusty_irq_irqset *irqset)
++{
++	struct trusty_irq *trusty_irq;
++
++	hlist_for_each_entry(trusty_irq, &irqset->inactive, node) {
++		if (trusty_irq->enable) {
++			dev_warn(is->dev,
++				 "%s: percpu irq %d already enabled, cpu %d\n",
++				 __func__, trusty_irq->irq, smp_processor_id());
++			continue;
++		}
++		dev_dbg(is->dev, "%s: enable percpu irq %d, cpu %d\n",
++			__func__, trusty_irq->irq, smp_processor_id());
++		enable_percpu_irq(trusty_irq->irq, 0);
++		trusty_irq->enable = true;
++	}
++}
++
++static void trusty_irq_disable_irqset(struct trusty_irq_state *is,
++				      struct trusty_irq_irqset *irqset)
++{
++	struct hlist_node *n;
++	struct trusty_irq *trusty_irq;
++
++	hlist_for_each_entry(trusty_irq, &irqset->inactive, node) {
++		if (!trusty_irq->enable) {
++			dev_warn(is->dev,
++				 "irq %d already disabled, percpu %d, cpu %d\n",
++				 trusty_irq->irq, trusty_irq->percpu,
++				 smp_processor_id());
++			continue;
++		}
++		dev_dbg(is->dev, "%s: disable irq %d, percpu %d, cpu %d\n",
++			__func__, trusty_irq->irq, trusty_irq->percpu,
++			smp_processor_id());
++		trusty_irq->enable = false;
++		if (trusty_irq->percpu)
++			disable_percpu_irq(trusty_irq->irq);
++		else
++			disable_irq_nosync(trusty_irq->irq);
++	}
++	hlist_for_each_entry_safe(trusty_irq, n, &irqset->pending, node) {
++		if (!trusty_irq->enable) {
++			dev_warn(is->dev,
++				 "pending irq %d already disabled, percpu %d, cpu %d\n",
++				 trusty_irq->irq, trusty_irq->percpu,
++				 smp_processor_id());
++		}
++		dev_dbg(is->dev,
++			"%s: disable pending irq %d, percpu %d, cpu %d\n",
++			__func__, trusty_irq->irq, trusty_irq->percpu,
++			smp_processor_id());
++		trusty_irq->enable = false;
++		hlist_del(&trusty_irq->node);
++		hlist_add_head(&trusty_irq->node, &irqset->inactive);
++	}
++}
++
++static int trusty_irq_call_notify(struct notifier_block *nb,
++				  unsigned long action, void *data)
++{
++	struct trusty_irq_state *is;
++
++	if (WARN_ON(!irqs_disabled()))
++		return NOTIFY_DONE;
++
++	if (action != TRUSTY_CALL_PREPARE)
++		return NOTIFY_DONE;
++
++	is = container_of(nb, struct trusty_irq_state, trusty_call_notifier);
++
++	spin_lock(&is->normal_irqs_lock);
++	trusty_irq_enable_pending_irqs(is, &is->normal_irqs, false);
++	spin_unlock(&is->normal_irqs_lock);
++	trusty_irq_enable_pending_irqs(is, this_cpu_ptr(is->percpu_irqs), true);
++
++	return NOTIFY_OK;
++}
++
++static irqreturn_t trusty_irq_handler(int irq, void *data)
++{
++	struct trusty_irq *trusty_irq = data;
++	struct trusty_irq_state *is = trusty_irq->is;
++	struct trusty_irq_irqset *irqset;
++
++	dev_dbg(is->dev, "%s: irq %d, percpu %d, cpu %d, enable %d\n",
++		__func__, irq, trusty_irq->irq, smp_processor_id(),
++		trusty_irq->enable);
++
++	if (!trusty_irq->doorbell) {
++		if (trusty_irq->percpu) {
++			disable_percpu_irq(irq);
++			irqset = this_cpu_ptr(is->percpu_irqs);
++		} else {
++			disable_irq_nosync(irq);
++			irqset = &is->normal_irqs;
++		}
++
++		spin_lock(&is->normal_irqs_lock);
++		if (trusty_irq->enable) {
++			hlist_del(&trusty_irq->node);
++			hlist_add_head(&trusty_irq->node, &irqset->pending);
++		}
++		spin_unlock(&is->normal_irqs_lock);
++	}
++
++	trusty_enqueue_nop(is->trusty_dev, NULL);
++
++	dev_dbg(is->dev, "%s: irq %d done\n", __func__, irq);
++
++	return IRQ_HANDLED;
++}
++
++static int trusty_irq_cpu_up(unsigned int cpu, struct hlist_node *node)
++{
++	unsigned long irq_flags;
++	struct trusty_irq_state *is;
++
++	is = container_of(node, struct trusty_irq_state, cpuhp_node);
++
++	dev_dbg(is->dev, "%s: cpu %d\n", __func__, cpu);
++
++	local_irq_save(irq_flags);
++	trusty_irq_enable_irqset(is, this_cpu_ptr(is->percpu_irqs));
++	local_irq_restore(irq_flags);
++
++	/*
++	 * Temporary workaround blindly enqueuing work to force trusty scheduler
++	 * to run after a cpu suspend.
++	 * Root causing the workqueue being inappropriately empty
++	 * (e.g. loss of an IPI) may make this workaround unnecessary
++	 * in the future.
++	 */
++	trusty_enqueue_nop(is->trusty_dev, NULL);
++
++	return 0;
++}
++
++static int trusty_irq_cpu_down(unsigned int cpu, struct hlist_node *node)
++{
++	unsigned long irq_flags;
++	struct trusty_irq_state *is;
++
++	is = container_of(node, struct trusty_irq_state, cpuhp_node);
++
++	dev_dbg(is->dev, "%s: cpu %d\n", __func__, cpu);
++
++	local_irq_save(irq_flags);
++	trusty_irq_disable_irqset(is, this_cpu_ptr(is->percpu_irqs));
++	local_irq_restore(irq_flags);
++
++	return 0;
++}
++
++static int trusty_irq_map_ipi(struct trusty_irq_state *is, int irq)
++{
++	int ret;
++	u32 ipi_range[3];
++	struct device_node *gic;
++	struct of_phandle_args oirq = {};
++	u32 beg, end, ipi_base;
++
++	ret = of_property_read_u32_array(is->dev->of_node, "ipi-range",
++					 ipi_range, ARRAY_SIZE(ipi_range));
++	if (ret != 0)
++		return -ENODATA;
++	beg = ipi_range[0];
++	end = ipi_range[1];
++	ipi_base = ipi_range[2];
++
++	if (irq < beg || irq > end)
++		return -ENODATA;
++
++	gic = of_irq_find_parent(is->dev->of_node);
++	if (!gic)
++		return -ENXIO;
++
++	oirq.np = gic;
++	oirq.args_count = 1;
++	oirq.args[0] = ipi_base + (irq - beg);
++
++	ret = irq_create_of_mapping(&oirq);
++
++	of_node_put(gic);
++	return (!ret) ? -EINVAL : ret;
++}
++
++static int trusty_irq_create_irq_mapping(struct trusty_irq_state *is, int irq)
++{
++	int ret;
++	int index;
++	u32 irq_pos;
++	u32 templ_idx;
++	u32 range_base;
++	u32 range_end;
++	struct of_phandle_args oirq;
++
++	/* check if this is an IPI (inter-processor interrupt) */
++	ret = trusty_irq_map_ipi(is, irq);
++	if (ret != -ENODATA)
++		return ret;
++
++	/* check if "interrupt-ranges" property is present */
++	if (!of_find_property(is->dev->of_node, "interrupt-ranges", NULL)) {
++		/* fallback to old behavior to be backward compatible with
++		 * systems that do not need IRQ domains.
++		 */
++		return irq;
++	}
++
++	/* find irq range */
++	for (index = 0;; index += 3) {
++		ret = of_property_read_u32_index(is->dev->of_node,
++						 "interrupt-ranges",
++						 index, &range_base);
++		if (ret)
++			return ret;
++
++		ret = of_property_read_u32_index(is->dev->of_node,
++						 "interrupt-ranges",
++						 index + 1, &range_end);
++		if (ret)
++			return ret;
++
++		if (irq >= range_base && irq <= range_end)
++			break;
++	}
++
++	/*  read the rest of range entry: template index and irq_pos */
++	ret = of_property_read_u32_index(is->dev->of_node,
++					 "interrupt-ranges",
++					 index + 2, &templ_idx);
++	if (ret)
++		return ret;
++
++	/* read irq template */
++	ret = of_parse_phandle_with_args(is->dev->of_node,
++					 "interrupt-templates",
++					 "#interrupt-cells",
++					 templ_idx, &oirq);
++	if (ret)
++		return ret;
++
++	WARN_ON(!oirq.np);
++	WARN_ON(!oirq.args_count);
++
++	/*
++	 * An IRQ template is a non empty array of u32 values describing group
++	 * of interrupts having common properties. The u32 entry with index
++	 * zero contains the position of irq_id in interrupt specifier array
++	 * followed by data representing interrupt specifier array with irq id
++	 * field omitted, so to convert irq template to interrupt specifier
++	 * array we have to move down one slot the first irq_pos entries and
++	 * replace the resulting gap with real irq id.
++	 */
++	irq_pos = oirq.args[0];
++
++	if (irq_pos >= oirq.args_count) {
++		dev_err(is->dev, "irq pos is out of range: %d\n", irq_pos);
++		return -EINVAL;
++	}
++
++	for (index = 1; index <= irq_pos; index++)
++		oirq.args[index - 1] = oirq.args[index];
++
++	oirq.args[irq_pos] = irq - range_base;
++
++	ret = irq_create_of_mapping(&oirq);
++
++	return (!ret) ? -EINVAL : ret;
++}
++
++static int trusty_irq_init_normal_irq(struct trusty_irq_state *is, int tirq)
++{
++	int ret;
++	int irq;
++	unsigned long irq_flags;
++	struct trusty_irq *trusty_irq;
++
++	dev_dbg(is->dev, "%s: irq %d\n", __func__, tirq);
++
++	irq = trusty_irq_create_irq_mapping(is, tirq);
++	if (irq < 0) {
++		dev_err(is->dev,
++			"trusty_irq_create_irq_mapping failed (%d)\n", irq);
++		return irq;
++	}
++
++	trusty_irq = kzalloc(sizeof(*trusty_irq), GFP_KERNEL);
++	if (!trusty_irq)
++		return -ENOMEM;
++
++	trusty_irq->is = is;
++	trusty_irq->irq = irq;
++	trusty_irq->enable = true;
++
++	spin_lock_irqsave(&is->normal_irqs_lock, irq_flags);
++	hlist_add_head(&trusty_irq->node, &is->normal_irqs.inactive);
++	spin_unlock_irqrestore(&is->normal_irqs_lock, irq_flags);
++
++	ret = request_irq(irq, trusty_irq_handler, IRQF_NO_THREAD,
++			  "trusty", trusty_irq);
++	if (ret) {
++		dev_err(is->dev, "request_irq failed %d\n", ret);
++		goto err_request_irq;
++	}
++	return 0;
++
++err_request_irq:
++	spin_lock_irqsave(&is->normal_irqs_lock, irq_flags);
++	hlist_del(&trusty_irq->node);
++	spin_unlock_irqrestore(&is->normal_irqs_lock, irq_flags);
++	kfree(trusty_irq);
++	return ret;
++}
++
++static int trusty_irq_init_per_cpu_irq(struct trusty_irq_state *is, int tirq,
++				       unsigned int type)
++{
++	int ret;
++	int irq;
++	unsigned int cpu;
++	struct trusty_irq __percpu *trusty_irq_handler_data;
++
++	dev_dbg(is->dev, "%s: irq %d\n", __func__, tirq);
++
++	irq = trusty_irq_create_irq_mapping(is, tirq);
++	if (irq <= 0) {
++		dev_err(is->dev,
++			"trusty_irq_create_irq_mapping failed (%d)\n", irq);
++		return irq;
++	}
++
++	trusty_irq_handler_data = alloc_percpu(struct trusty_irq);
++	if (!trusty_irq_handler_data)
++		return -ENOMEM;
++
++	for_each_possible_cpu(cpu) {
++		struct trusty_irq *trusty_irq;
++		struct trusty_irq_irqset *irqset;
++
++		trusty_irq = per_cpu_ptr(trusty_irq_handler_data, cpu);
++		irqset = per_cpu_ptr(is->percpu_irqs, cpu);
++
++		trusty_irq->is = is;
++		hlist_add_head(&trusty_irq->node, &irqset->inactive);
++		trusty_irq->irq = irq;
++		trusty_irq->percpu = true;
++		trusty_irq->doorbell = type == TRUSTY_IRQ_TYPE_DOORBELL;
++		trusty_irq->percpu_ptr = trusty_irq_handler_data;
++	}
++
++	ret = request_percpu_irq(irq, trusty_irq_handler, "trusty",
++				 trusty_irq_handler_data);
++	if (ret) {
++		dev_err(is->dev, "request_percpu_irq failed %d\n", ret);
++		goto err_request_percpu_irq;
++	}
++
++	return 0;
++
++err_request_percpu_irq:
++	for_each_possible_cpu(cpu) {
++		struct trusty_irq *trusty_irq;
++
++		trusty_irq = per_cpu_ptr(trusty_irq_handler_data, cpu);
++		hlist_del(&trusty_irq->node);
++	}
++
++	free_percpu(trusty_irq_handler_data);
++	return ret;
++}
++
++static int trusty_smc_get_next_irq(struct trusty_irq_state *is,
++				   unsigned long min_irq, unsigned int type)
++{
++	return trusty_fast_call32(is->trusty_dev, SMC_FC_GET_NEXT_IRQ,
++				  min_irq, type, 0);
++}
++
++static int trusty_irq_init_one(struct trusty_irq_state *is,
++			       int irq, unsigned int type)
++{
++	int ret;
++
++	irq = trusty_smc_get_next_irq(is, irq, type);
++	if (irq < 0)
++		return irq;
++
++	if (type != TRUSTY_IRQ_TYPE_NORMAL)
++		ret = trusty_irq_init_per_cpu_irq(is, irq, type);
++	else
++		ret = trusty_irq_init_normal_irq(is, irq);
++
++	if (ret) {
++		dev_warn(is->dev,
++			 "failed to initialize irq %d, irq will be ignored\n",
++			 irq);
++	}
++
++	return irq + 1;
++}
++
++static void trusty_irq_free_irqs(struct trusty_irq_state *is)
++{
++	struct trusty_irq *irq;
++	struct hlist_node *n;
++	unsigned int cpu;
++
++	hlist_for_each_entry_safe(irq, n, &is->normal_irqs.inactive, node) {
++		dev_dbg(is->dev, "%s: irq %d\n", __func__, irq->irq);
++		free_irq(irq->irq, irq);
++		hlist_del(&irq->node);
++		kfree(irq);
++	}
++	hlist_for_each_entry_safe(irq, n,
++				  &this_cpu_ptr(is->percpu_irqs)->inactive,
++				  node) {
++		struct trusty_irq __percpu *trusty_irq_handler_data;
++
++		dev_dbg(is->dev, "%s: percpu irq %d\n", __func__, irq->irq);
++		trusty_irq_handler_data = irq->percpu_ptr;
++		free_percpu_irq(irq->irq, trusty_irq_handler_data);
++		for_each_possible_cpu(cpu) {
++			struct trusty_irq *irq_tmp;
++
++			irq_tmp = per_cpu_ptr(trusty_irq_handler_data, cpu);
++			hlist_del(&irq_tmp->node);
++		}
++		free_percpu(trusty_irq_handler_data);
++	}
++}
++
++static int trusty_irq_probe(struct platform_device *pdev)
++{
++	int ret;
++	int irq;
++	unsigned long irq_flags;
++	struct trusty_irq_state *is;
++
++	is = kzalloc(sizeof(*is), GFP_KERNEL);
++	if (!is) {
++		ret = -ENOMEM;
++		goto err_alloc_is;
++	}
++
++	is->dev = &pdev->dev;
++	is->trusty_dev = is->dev->parent;
++	spin_lock_init(&is->normal_irqs_lock);
++	is->percpu_irqs = alloc_percpu(struct trusty_irq_irqset);
++	if (!is->percpu_irqs) {
++		ret = -ENOMEM;
++		goto err_alloc_pending_percpu_irqs;
++	}
++
++	platform_set_drvdata(pdev, is);
++
++	is->trusty_call_notifier.notifier_call = trusty_irq_call_notify;
++	ret = trusty_call_notifier_register(is->trusty_dev,
++					    &is->trusty_call_notifier);
++	if (ret) {
++		dev_err(&pdev->dev,
++			"failed to register trusty call notifier\n");
++		goto err_trusty_call_notifier_register;
++	}
++
++	for (irq = 0; irq >= 0;)
++		irq = trusty_irq_init_one(is, irq, TRUSTY_IRQ_TYPE_PER_CPU);
++	for (irq = 0; irq >= 0;)
++		irq = trusty_irq_init_one(is, irq, TRUSTY_IRQ_TYPE_NORMAL);
++	for (irq = 0; irq >= 0;)
++		irq = trusty_irq_init_one(is, irq, TRUSTY_IRQ_TYPE_DOORBELL);
++
++	ret = cpuhp_state_add_instance(trusty_irq_cpuhp_slot, &is->cpuhp_node);
++	if (ret < 0) {
++		dev_err(&pdev->dev, "cpuhp_state_add_instance failed %d\n",
++			ret);
++		goto err_add_cpuhp_instance;
++	}
++
++	return 0;
++
++err_add_cpuhp_instance:
++	spin_lock_irqsave(&is->normal_irqs_lock, irq_flags);
++	trusty_irq_disable_irqset(is, &is->normal_irqs);
++	spin_unlock_irqrestore(&is->normal_irqs_lock, irq_flags);
++	trusty_irq_free_irqs(is);
++	trusty_call_notifier_unregister(is->trusty_dev,
++					&is->trusty_call_notifier);
++err_trusty_call_notifier_register:
++	free_percpu(is->percpu_irqs);
++err_alloc_pending_percpu_irqs:
++	kfree(is);
++err_alloc_is:
++	return ret;
++}
++
++static int trusty_irq_remove(struct platform_device *pdev)
++{
++	int ret;
++	unsigned long irq_flags;
++	struct trusty_irq_state *is = platform_get_drvdata(pdev);
++
++	ret = cpuhp_state_remove_instance(trusty_irq_cpuhp_slot,
++					  &is->cpuhp_node);
++	if (WARN_ON(ret))
++		return ret;
++
++	spin_lock_irqsave(&is->normal_irqs_lock, irq_flags);
++	trusty_irq_disable_irqset(is, &is->normal_irqs);
++	spin_unlock_irqrestore(&is->normal_irqs_lock, irq_flags);
++
++	trusty_irq_free_irqs(is);
++
++	trusty_call_notifier_unregister(is->trusty_dev,
++					&is->trusty_call_notifier);
++	free_percpu(is->percpu_irqs);
++	kfree(is);
++
++	return 0;
++}
++
++static const struct of_device_id trusty_test_of_match[] = {
++	{ .compatible = "android,trusty-irq-v1", },
++	{},
++};
++
++MODULE_DEVICE_TABLE(trusty, trusty_test_of_match);
++
++static struct platform_driver trusty_irq_driver = {
++	.probe = trusty_irq_probe,
++	.remove = trusty_irq_remove,
++	.driver	= {
++		.name = "trusty-irq",
++		.of_match_table = trusty_test_of_match,
++	},
++};
++
++static int __init trusty_irq_driver_init(void)
++{
++	int ret;
++
++	/* allocate dynamic cpuhp state slot */
++	ret = cpuhp_setup_state_multi(CPUHP_AP_ONLINE_DYN,
++				      "trusty-irq:cpu:online",
++				      trusty_irq_cpu_up,
++				      trusty_irq_cpu_down);
++	if (ret < 0)
++		return ret;
++	trusty_irq_cpuhp_slot = ret;
++
++	/* Register platform driver */
++	ret = platform_driver_register(&trusty_irq_driver);
++	if (ret < 0)
++		goto err_driver_register;
++
++	return ret;
++
++err_driver_register:
++	/* undo cpuhp slot allocation */
++	cpuhp_remove_multi_state(trusty_irq_cpuhp_slot);
++	trusty_irq_cpuhp_slot = -1;
++
++	return ret;
++}
++
++static void __exit trusty_irq_driver_exit(void)
++{
++	platform_driver_unregister(&trusty_irq_driver);
++	cpuhp_remove_multi_state(trusty_irq_cpuhp_slot);
++	trusty_irq_cpuhp_slot = -1;
++}
++
++module_init(trusty_irq_driver_init);
++module_exit(trusty_irq_driver_exit);
++
++MODULE_LICENSE("GPL v2");
++MODULE_DESCRIPTION("Trusty IRQ driver");
+diff --git a/drivers/trusty/trusty-log.c b/drivers/trusty/trusty-log.c
+new file mode 100644
+index 000000000000..7b279fe63766
+--- /dev/null
++++ b/drivers/trusty/trusty-log.c
+@@ -0,0 +1,830 @@
++// SPDX-License-Identifier: GPL-2.0-only
++/*
++ * Copyright (C) 2015 Google, Inc.
++ */
++#include <linux/platform_device.h>
++#include <linux/trusty/smcall.h>
++#include <linux/trusty/trusty.h>
++#include <linux/notifier.h>
++#include <linux/scatterlist.h>
++#include <linux/slab.h>
++#include <linux/mm.h>
++#include <linux/mod_devicetable.h>
++#include <linux/module.h>
++#include <linux/moduleparam.h>
++#include <linux/log2.h>
++#include <linux/miscdevice.h>
++#include <linux/poll.h>
++#include <linux/seq_file.h>
++#include <asm/page.h>
++#include "trusty-log.h"
++
++/*
++ * Rationale for the chosen default log buffer size:
++ *  - the log buffer shall contain unthrottled Trusty crash dump.
++ *  - the register list portion of a crash dump is about 1KB
++ *  - the memory-around-registers portion of a crash dump can be up to 12 KB
++ *  - an average size backtrace is about 1 KB
++ *  - average length of non-crash trusty logs during boot is about 85 characters
++ *  - a crash dump with 50 lines of context therefore requires up to 18 KB
++ *  - buffer size needs to be power-of-two number of bytes
++ *  - rounding up to power of two from 18 KB gives 32 KB
++ *  The log size can be adjusted by setting the "trusty_log.log_size" parameter
++ *  on the kernel command line. The specified value will be adjusted as needed.
++ */
++
++#define TRUSTY_LOG_DEFAULT_SIZE (32768)
++#define TRUSTY_LOG_MIN_SIZE (PAGE_SIZE / 2)
++#define TRUSTY_LOG_MAX_SIZE (1 * 1024 * 1024 * 1024)
++#define TRUSTY_LINE_BUFFER_SIZE (256)
++
++static size_t log_size_param = TRUSTY_LOG_DEFAULT_SIZE;
++
++static int trusty_log_size_set(const char *val, const struct kernel_param *kp)
++{
++	unsigned long long requested = memparse(val, NULL);
++
++	if (requested < TRUSTY_LOG_MIN_SIZE)
++		requested = TRUSTY_LOG_MIN_SIZE;
++	if (requested > TRUSTY_LOG_MAX_SIZE)
++		requested = TRUSTY_LOG_MAX_SIZE;
++	requested = rounddown_pow_of_two(requested);
++	log_size_param = requested;
++	return 0;
++}
++
++static int trusty_log_size_get(char *buffer, const struct kernel_param *kp)
++{
++	sprintf(buffer, "%zu", log_size_param);
++	return strlen(buffer);
++}
++
++module_param_call(log_size, trusty_log_size_set, trusty_log_size_get, NULL,
++		  0644);
++/*
++ * If we log too much and a UART or other slow source is connected, we can stall
++ * out another thread which is doing printk.
++ *
++ * Trusty crash logs are currently ~16 lines, so 100 should include context and
++ * the crash most of the time.
++ */
++static struct ratelimit_state trusty_log_rate_limit =
++	RATELIMIT_STATE_INIT("trusty_log", 1 * HZ, 100);
++
++/**
++ * struct trusty_log_sfile - trusty log misc device state
++ *
++ * @misc:          misc device created for the trusty log virtual file
++ * @device_name:   misc device name following the convention
++ *                 "trusty-<name><id>"
++ */
++struct trusty_log_sfile {
++	struct miscdevice misc;
++	char device_name[64];
++};
++
++/**
++ * struct trusty_log_sink_state - trusty log sink state
++ *
++ * @get:              current read unwrapped index
++ * @trusty_panicked:  trusty panic status at the start of the sink interation
++ *                    (only used for kernel log sink)
++ * @sfile:            seq_file used for sinking to a virtual file (misc device);
++ *                    set to NULL for the kernel log sink.
++ * @ignore_overflow:  ignore_overflow used to coalesce overflow messages and
++ *                    avoid reporting an overflow when sinking the oldest
++ *                    line to the virtual file (only used for virtual file sink)
++ *
++ * A sink state structure is used for both the kernel log sink
++ * and the virtual device sink.
++ * An instance of the sink state structure is dynamically created
++ * for each read iteration of the trusty log virtual file (misc device).
++ *
++ */
++struct trusty_log_sink_state {
++	u32 get;
++	bool trusty_panicked;
++
++	/* virtual file sink specific attributes */
++	struct seq_file *sfile;
++	bool ignore_overflow;
++};
++
++struct trusty_log_state {
++	struct device *dev;
++	struct device *trusty_dev;
++	struct trusty_log_sfile log_sfile;
++
++	struct log_rb *log;
++	struct trusty_log_sink_state klog_sink;
++
++	u32 log_num_pages;
++	struct scatterlist *sg;
++	trusty_shared_mem_id_t log_pages_shared_mem_id;
++
++	struct notifier_block call_notifier;
++	struct notifier_block panic_notifier;
++	char line_buffer[TRUSTY_LINE_BUFFER_SIZE];
++	wait_queue_head_t poll_waiters;
++	/* this lock protects access to wake_put */
++	spinlock_t wake_up_lock;
++	u32 last_wake_put;
++};
++
++static inline u32 u32_add_overflow(u32 a, u32 b)
++{
++	u32 d;
++
++	if (check_add_overflow(a, b, &d)) {
++		/*
++		 * silence the overflow,
++		 * what matters in the log buffer context
++		 * is the casted addition
++		 */
++	}
++	return d;
++}
++
++static inline u32 u32_sub_overflow(u32 a, u32 b)
++{
++	u32 d;
++
++	if (check_sub_overflow(a, b, &d)) {
++		/*
++		 * silence the overflow,
++		 * what matters in the log buffer context
++		 * is the casted substraction
++		 */
++	}
++	return d;
++}
++
++static int log_read_line(struct trusty_log_state *s, u32 put, u32 get)
++{
++	struct log_rb *log = s->log;
++	int i;
++	char c = '\0';
++	size_t max_to_read =
++		min_t(size_t,
++		      u32_sub_overflow(put, get),
++		      sizeof(s->line_buffer) - 1);
++	size_t mask = log->sz - 1;
++
++	for (i = 0; i < max_to_read && c != '\n';) {
++		c = log->data[get & mask];
++		s->line_buffer[i++] = c;
++		get = u32_add_overflow(get, 1);
++	}
++	s->line_buffer[i] = '\0';
++
++	return i;
++}
++
++/**
++ * trusty_log_has_data() - returns true when more data is available to sink
++ * @s:         Current log state.
++ * @sink:      trusty_log_sink_state holding the get index on a given sink
++ *
++ * Return: true if data is available.
++ */
++static bool trusty_log_has_data(struct trusty_log_state *s,
++				struct trusty_log_sink_state *sink)
++{
++	struct log_rb *log = s->log;
++
++	return (log->put != sink->get);
++}
++
++/**
++ * trusty_log_start() - initialize the sink iteration either to kernel log
++ * or to secondary log_sfile
++ * @s:         Current log state.
++ * @sink:      trusty_log_sink_state holding the get index on a given sink
++ * @index:     Unwrapped ring buffer index from where iteration shall start
++ *
++ * Return: 0 if successful, negative error code otherwise
++ */
++static int trusty_log_start(struct trusty_log_state *s,
++			    struct trusty_log_sink_state *sink,
++			    u32 index)
++{
++	struct log_rb *log;
++
++	if (WARN_ON(!s))
++		return -EINVAL;
++
++	log = s->log;
++	if (WARN_ON(!is_power_of_2(log->sz)))
++		return -EINVAL;
++
++	sink->get = index;
++	return 0;
++}
++
++/**
++ * trusty_log_show() - sink log entry at current iteration
++ * @s:         Current log state.
++ * @sink:      trusty_log_sink_state holding the get index on a given sink
++ */
++static void trusty_log_show(struct trusty_log_state *s,
++			    struct trusty_log_sink_state *sink)
++{
++	struct log_rb *log = s->log;
++	u32 alloc, put, get;
++	int read_chars;
++
++	/*
++	 * For this ring buffer, at any given point, alloc >= put >= get.
++	 * The producer side of the buffer is not locked, so the put and alloc
++	 * pointers must be read in a defined order (put before alloc) so
++	 * that the above condition is maintained. A read barrier is needed
++	 * to make sure the hardware and compiler keep the reads ordered.
++	 */
++	get = sink->get;
++	put = log->put;
++
++	/* Make sure that the read of put occurs before the read of log data */
++	rmb();
++
++	/* Read a line from the log */
++	read_chars = log_read_line(s, put, get);
++
++	/* Force the loads from log_read_line to complete. */
++	rmb();
++	alloc = log->alloc;
++
++	/*
++	 * Discard the line that was just read if the data could
++	 * have been corrupted by the producer.
++	 */
++	if (u32_sub_overflow(alloc, get) > log->sz) {
++		/*
++		 * this condition is acceptable in the case of the sfile sink
++		 * when attempting to read the oldest entry (at alloc-log->sz)
++		 * which may be overrun by a new one when ring buffer write
++		 * index wraps around.
++		 * So the overrun is not reported in case the oldest line
++		 * was being read.
++		 */
++		if (sink->sfile) {
++			if (!sink->ignore_overflow)
++				seq_puts(sink->sfile, "log overflow.\n");
++			/* coalesce subsequent contiguous overflows. */
++			sink->ignore_overflow = true;
++		} else {
++			dev_err(s->dev, "log overflow.\n");
++		}
++		sink->get = u32_sub_overflow(alloc, log->sz);
++		return;
++	}
++	/* compute next line index */
++	sink->get = u32_add_overflow(get, read_chars);
++	/* once a line is valid, ignore_overflow must be disabled */
++	sink->ignore_overflow = false;
++	if (sink->sfile) {
++		seq_printf(sink->sfile, "%s", s->line_buffer);
++	} else {
++		if (sink->trusty_panicked ||
++		    __ratelimit(&trusty_log_rate_limit)) {
++			dev_info(s->dev, "%s", s->line_buffer);
++		}
++	}
++}
++
++static void *trusty_log_seq_start(struct seq_file *sfile, loff_t *pos)
++{
++	struct trusty_log_sfile *lb;
++	struct trusty_log_state *s;
++	struct log_rb *log;
++	struct trusty_log_sink_state *log_sfile_sink;
++	u32 index;
++	int rc;
++
++	if (WARN_ON(!pos))
++		return ERR_PTR(-EINVAL);
++
++	lb = sfile->private;
++	if (WARN_ON(!lb))
++		return ERR_PTR(-EINVAL);
++
++	log_sfile_sink = kzalloc(sizeof(*log_sfile_sink), GFP_KERNEL);
++	if (!log_sfile_sink)
++		return ERR_PTR(-ENOMEM);
++
++	s = container_of(lb, struct trusty_log_state, log_sfile);
++	log_sfile_sink->sfile = sfile;
++	log = s->log;
++	if (*pos == 0) {
++		/* start at the oldest line */
++		index = 0;
++		if (log->alloc > log->sz)
++			index = u32_sub_overflow(log->alloc, log->sz);
++	} else {
++		/*
++		 * '*pos>0': pos hold the 32bits unwrapped index from where
++		 * to start iterating
++		 */
++		index = (u32)*pos;
++	}
++	pr_debug("%s start=%u\n", __func__, index);
++
++	log_sfile_sink->ignore_overflow = true;
++	rc = trusty_log_start(s, log_sfile_sink, index);
++	if (rc < 0)
++		goto free_sink;
++
++	if (!trusty_log_has_data(s, log_sfile_sink))
++		goto free_sink;
++
++	return log_sfile_sink;
++
++free_sink:
++	pr_debug("%s kfree\n", __func__);
++	kfree(log_sfile_sink);
++	return rc < 0 ? ERR_PTR(rc) : NULL;
++}
++
++static void *trusty_log_seq_next(struct seq_file *sfile, void *v, loff_t *pos)
++{
++	struct trusty_log_sfile *lb;
++	struct trusty_log_state *s;
++	struct trusty_log_sink_state *log_sfile_sink = v;
++	int rc = 0;
++
++	if (WARN_ON(!log_sfile_sink))
++		return ERR_PTR(-EINVAL);
++
++	lb = sfile->private;
++	if (WARN_ON(!lb)) {
++		rc = -EINVAL;
++		goto end_of_iter;
++	}
++	s = container_of(lb, struct trusty_log_state, log_sfile);
++
++	if (WARN_ON(!pos)) {
++		rc = -EINVAL;
++		goto end_of_iter;
++	}
++	/*
++	 * When starting a virtual file sink, the start function is invoked
++	 * with a pos argument which value is set to zero.
++	 * Subsequent starts are invoked with pos being set to
++	 * the unwrapped read index (get).
++	 * Upon u32 wraparound, the get index could be reset to zero.
++	 * Thus a msb is used to distinguish the `get` zero value
++	 * from the `start of file` zero value.
++	 */
++	*pos = (1UL << 32) + log_sfile_sink->get;
++	if (!trusty_log_has_data(s, log_sfile_sink))
++		goto end_of_iter;
++
++	return log_sfile_sink;
++
++end_of_iter:
++	pr_debug("%s kfree\n", __func__);
++	kfree(log_sfile_sink);
++	return rc < 0 ? ERR_PTR(rc) : NULL;
++}
++
++static void trusty_log_seq_stop(struct seq_file *sfile, void *v)
++{
++	/*
++	 * When iteration completes or on error, the next callback frees
++	 * the sink structure and returns NULL/error-code.
++	 * In that case stop (being invoked with void* v set to the last next
++	 * return value) would be invoked with v == NULL or error code.
++	 * When user space stops the iteration earlier than the end
++	 * (in case of user-space memory allocation limit for example)
++	 * then the stop function receives a non NULL get pointer
++	 * and is in charge or freeing the sink structure.
++	 */
++	struct trusty_log_sink_state *log_sfile_sink = v;
++
++	/* nothing to do - sink structure already freed */
++	if (IS_ERR_OR_NULL(log_sfile_sink))
++		return;
++
++	kfree(log_sfile_sink);
++
++	pr_debug("%s kfree\n", __func__);
++}
++
++static int trusty_log_seq_show(struct seq_file *sfile, void *v)
++{
++	struct trusty_log_sfile *lb;
++	struct trusty_log_state *s;
++	struct trusty_log_sink_state *log_sfile_sink = v;
++
++	if (WARN_ON(!log_sfile_sink))
++		return -EINVAL;
++
++	lb = sfile->private;
++	if (WARN_ON(!lb))
++		return -EINVAL;
++
++	s = container_of(lb, struct trusty_log_state, log_sfile);
++
++	trusty_log_show(s, log_sfile_sink);
++	return 0;
++}
++
++static void trusty_dump_logs(struct trusty_log_state *s)
++{
++	int rc;
++	/*
++	 * note: klog_sink.get initialized to zero by kzalloc
++	 */
++	s->klog_sink.trusty_panicked = trusty_get_panic_status(s->trusty_dev);
++
++	rc = trusty_log_start(s, &s->klog_sink, s->klog_sink.get);
++	if (rc < 0)
++		return;
++
++	while (trusty_log_has_data(s, &s->klog_sink))
++		trusty_log_show(s, &s->klog_sink);
++}
++
++static int trusty_log_call_notify(struct notifier_block *nb,
++				  unsigned long action, void *data)
++{
++	struct trusty_log_state *s;
++	unsigned long flags;
++	u32 cur_put;
++
++	if (action != TRUSTY_CALL_RETURNED)
++		return NOTIFY_DONE;
++
++	s = container_of(nb, struct trusty_log_state, call_notifier);
++	spin_lock_irqsave(&s->wake_up_lock, flags);
++	cur_put = s->log->put;
++	if (cur_put != s->last_wake_put) {
++		s->last_wake_put = cur_put;
++		wake_up_all(&s->poll_waiters);
++	}
++	spin_unlock_irqrestore(&s->wake_up_lock, flags);
++	return NOTIFY_OK;
++}
++
++static int trusty_log_panic_notify(struct notifier_block *nb,
++				   unsigned long action, void *data)
++{
++	struct trusty_log_state *s;
++
++	/*
++	 * Don't grab the spin lock to hold up the panic notifier, even
++	 * though this is racy.
++	 */
++	s = container_of(nb, struct trusty_log_state, panic_notifier);
++	dev_info(s->dev, "panic notifier - trusty version %s",
++		 trusty_version_str_get(s->trusty_dev));
++	trusty_dump_logs(s);
++	return NOTIFY_OK;
++}
++
++const struct seq_operations trusty_log_seq_ops = {
++	.start = trusty_log_seq_start,
++	.stop = trusty_log_seq_stop,
++	.next = trusty_log_seq_next,
++	.show = trusty_log_seq_show,
++};
++
++static int trusty_log_sfile_dev_open(struct inode *inode, struct file *file)
++{
++	struct trusty_log_sfile *ls;
++	struct seq_file *sfile;
++	int rc;
++
++	/*
++	 * file->private_data contains a pointer to the misc_device struct
++	 * passed to misc_register()
++	 */
++	if (WARN_ON(!file->private_data))
++		return -EINVAL;
++
++	ls = container_of(file->private_data, struct trusty_log_sfile, misc);
++
++	/*
++	 * seq_open uses file->private_data to store the seq_file associated
++	 * with the struct file, but it must be NULL when seq_open is called
++	 */
++	file->private_data = NULL;
++	rc = seq_open(file, &trusty_log_seq_ops);
++	if (rc < 0)
++		return rc;
++
++	sfile = file->private_data;
++	if (WARN_ON(!sfile))
++		return -EINVAL;
++
++	sfile->private = ls;
++	return 0;
++}
++
++static unsigned int trusty_log_sfile_dev_poll(struct file *filp,
++					      struct poll_table_struct *wait)
++{
++	struct seq_file *sfile;
++	struct trusty_log_sfile *lb;
++	struct trusty_log_state *s;
++	struct log_rb *log;
++
++	/*
++	 * trusty_log_sfile_dev_open() pointed filp->private_data to a
++	 * seq_file, and that seq_file->private to the trusty_log_sfile
++	 * field of a trusty_log_state
++	 */
++	sfile = filp->private_data;
++	lb = sfile->private;
++	s = container_of(lb, struct trusty_log_state, log_sfile);
++	poll_wait(filp, &s->poll_waiters, wait);
++	log = s->log;
++
++	/*
++	 * Userspace has read up to filp->f_pos so far. Update klog_sink
++	 * to indicate that, so that we don't end up dumping the entire
++	 * Trusty log in case of panic.
++	 */
++	s->klog_sink.get = (u32)filp->f_pos;
++
++	if (log->put != (u32)filp->f_pos) {
++		/* data ready to read */
++		return EPOLLIN | EPOLLRDNORM;
++	}
++	/* no data available, go to sleep */
++	return 0;
++}
++
++static const struct file_operations log_sfile_dev_operations = {
++	.owner = THIS_MODULE,
++	.open = trusty_log_sfile_dev_open,
++	.poll = trusty_log_sfile_dev_poll,
++	.read = seq_read,
++	.release = seq_release,
++};
++
++static int trusty_log_sfile_register(struct trusty_log_state *s)
++{
++	int ret;
++	struct trusty_log_sfile *ls = &s->log_sfile;
++
++	if (WARN_ON(!ls))
++		return -EINVAL;
++
++	snprintf(ls->device_name, sizeof(ls->device_name),
++		 "trusty-log%d", s->dev->id);
++	ls->misc.minor = MISC_DYNAMIC_MINOR;
++	ls->misc.name = ls->device_name;
++	ls->misc.fops = &log_sfile_dev_operations;
++
++	ret = misc_register(&ls->misc);
++	if (ret) {
++		dev_err(s->dev,
++			"log_sfile error while doing misc_register ret=%d\n",
++			ret);
++		return ret;
++	}
++	dev_info(s->dev, "/dev/%s registered\n",
++		 ls->device_name);
++	return 0;
++}
++
++static void trusty_log_sfile_unregister(struct trusty_log_state *s)
++{
++	struct trusty_log_sfile *ls = &s->log_sfile;
++
++	misc_deregister(&ls->misc);
++	if (s->dev) {
++		dev_info(s->dev, "/dev/%s unregistered\n",
++			 ls->misc.name);
++	}
++}
++
++static bool trusty_supports_logging(struct device *device)
++{
++	int result;
++
++	result = trusty_std_call32(device, SMC_SC_SHARED_LOG_VERSION,
++				   TRUSTY_LOG_API_VERSION, 0, 0);
++	if (result == SM_ERR_UNDEFINED_SMC) {
++		dev_info(device, "trusty-log not supported on secure side.\n");
++		return false;
++	} else if (result < 0) {
++		dev_err(device,
++			"trusty std call (SMC_SC_SHARED_LOG_VERSION) failed: %d\n",
++			result);
++		return false;
++	}
++
++	if (result != TRUSTY_LOG_API_VERSION) {
++		dev_info(device, "unsupported api version: %d, supported: %d\n",
++			 result, TRUSTY_LOG_API_VERSION);
++		return false;
++	}
++	return true;
++}
++
++static int trusty_log_init(struct platform_device *pdev)
++{
++	struct trusty_log_state *s;
++	struct scatterlist *sg;
++	unsigned char *mem;
++	int i;
++	int result;
++	trusty_shared_mem_id_t mem_id;
++	int log_size;
++
++	s = kzalloc(sizeof(*s), GFP_KERNEL);
++	if (!s) {
++		result = -ENOMEM;
++		goto error_alloc_state;
++	}
++
++	s->dev = &pdev->dev;
++	s->trusty_dev = s->dev->parent;
++
++	s->log_num_pages = DIV_ROUND_UP(log_size_param + sizeof(struct log_rb),
++					PAGE_SIZE);
++	s->sg = kcalloc(s->log_num_pages, sizeof(*s->sg), GFP_KERNEL);
++	if (!s->sg) {
++		result = -ENOMEM;
++		goto error_alloc_sg;
++	}
++
++	log_size = s->log_num_pages * PAGE_SIZE;
++	mem = vzalloc(log_size);
++	if (!mem) {
++		result = -ENOMEM;
++		goto error_alloc_log;
++	}
++
++	s->log = (struct log_rb *)mem;
++
++	sg_init_table(s->sg, s->log_num_pages);
++	for_each_sg(s->sg, sg, s->log_num_pages, i) {
++		struct page *pg = vmalloc_to_page(mem + (i * PAGE_SIZE));
++
++		if (!pg) {
++			result = -ENOMEM;
++			goto err_share_memory;
++		}
++		sg_set_page(sg, pg, PAGE_SIZE, 0);
++	}
++	/*
++	 * This will fail for Trusty api version < TRUSTY_API_VERSION_MEM_OBJ
++	 * if s->log_num_pages > 1
++	 * Use trusty_share_memory_compat instead of trusty_share_memory in case
++	 * s->log_num_pages == 1 and api version < TRUSTY_API_VERSION_MEM_OBJ,
++	 * In that case SMC_SC_SHARED_LOG_ADD expects a different value than
++	 * what trusty_share_memory returns
++	 */
++	result = trusty_share_memory_compat(s->trusty_dev, &mem_id, s->sg,
++					    s->log_num_pages, PAGE_KERNEL);
++	if (result) {
++		dev_err(s->dev, "trusty_share_memory failed: %d\n", result);
++		goto err_share_memory;
++	}
++	s->log_pages_shared_mem_id = mem_id;
++
++	result = trusty_std_call32(s->trusty_dev,
++				   SMC_SC_SHARED_LOG_ADD,
++				   (u32)(mem_id), (u32)(mem_id >> 32),
++				   log_size);
++	if (result < 0) {
++		dev_err(s->dev,
++			"trusty std call (SMC_SC_SHARED_LOG_ADD) failed: %d 0x%llx\n",
++			result, mem_id);
++		goto error_std_call;
++	}
++
++	init_waitqueue_head(&s->poll_waiters);
++	spin_lock_init(&s->wake_up_lock);
++
++	s->call_notifier.notifier_call = trusty_log_call_notify;
++	result = trusty_call_notifier_register(s->trusty_dev,
++					       &s->call_notifier);
++	if (result < 0) {
++		dev_err(&pdev->dev,
++			"failed to register trusty call notifier\n");
++		goto error_call_notifier;
++	}
++
++	s->panic_notifier.notifier_call = trusty_log_panic_notify;
++	result = atomic_notifier_chain_register(&panic_notifier_list,
++						&s->panic_notifier);
++	if (result < 0) {
++		dev_err(&pdev->dev,
++			"failed to register panic notifier\n");
++		goto error_panic_notifier;
++	}
++
++	result = trusty_log_sfile_register(s);
++	if (result < 0) {
++		dev_err(&pdev->dev, "failed to register log_sfile\n");
++		goto error_log_sfile;
++	}
++
++	platform_set_drvdata(pdev, s);
++
++	return 0;
++
++error_log_sfile:
++	atomic_notifier_chain_unregister(&panic_notifier_list,
++					 &s->panic_notifier);
++error_panic_notifier:
++	trusty_call_notifier_unregister(s->trusty_dev, &s->call_notifier);
++error_call_notifier:
++	trusty_std_call32(s->trusty_dev, SMC_SC_SHARED_LOG_RM,
++			  (u32)mem_id, (u32)(mem_id >> 32), 0);
++error_std_call:
++	if (WARN_ON(trusty_reclaim_memory(s->trusty_dev, mem_id, s->sg,
++					  s->log_num_pages))) {
++		dev_err(&pdev->dev, "trusty_revoke_memory failed: %d 0x%llx\n",
++			result, mem_id);
++		/*
++		 * It is not safe to free this memory if trusty_revoke_memory
++		 * fails. Leak it in that case.
++		 */
++	} else {
++err_share_memory:
++		vfree(s->log);
++	}
++error_alloc_log:
++	kfree(s->sg);
++error_alloc_sg:
++	kfree(s);
++error_alloc_state:
++	return result;
++}
++
++static int trusty_log_probe(struct platform_device *pdev)
++{
++	int rc;
++
++	if (!trusty_supports_logging(pdev->dev.parent))
++		return -ENXIO;
++
++	rc = trusty_log_init(pdev);
++	if (rc && log_size_param > TRUSTY_LOG_MIN_SIZE) {
++		dev_warn(&pdev->dev, "init failed, retrying with 1-page log\n");
++		log_size_param = TRUSTY_LOG_MIN_SIZE;
++		rc = trusty_log_init(pdev);
++	}
++	return rc;
++}
++
++static int trusty_log_remove(struct platform_device *pdev)
++{
++	int result;
++	struct trusty_log_state *s = platform_get_drvdata(pdev);
++	trusty_shared_mem_id_t mem_id = s->log_pages_shared_mem_id;
++
++	trusty_log_sfile_unregister(s);
++	atomic_notifier_chain_unregister(&panic_notifier_list,
++					 &s->panic_notifier);
++	trusty_call_notifier_unregister(s->trusty_dev, &s->call_notifier);
++
++	result = trusty_std_call32(s->trusty_dev, SMC_SC_SHARED_LOG_RM,
++				   (u32)mem_id, (u32)(mem_id >> 32), 0);
++	if (result) {
++		dev_err(&pdev->dev,
++			"trusty std call (SMC_SC_SHARED_LOG_RM) failed: %d\n",
++			result);
++	}
++	result = trusty_reclaim_memory(s->trusty_dev, mem_id, s->sg,
++				       s->log_num_pages);
++	if (WARN_ON(result)) {
++		dev_err(&pdev->dev,
++			"trusty failed to remove shared memory: %d\n", result);
++	} else {
++		/*
++		 * It is not safe to free this memory if trusty_revoke_memory
++		 * fails. Leak it in that case.
++		 */
++		vfree(s->log);
++	}
++	kfree(s->sg);
++	kfree(s);
++
++	return 0;
++}
++
++static const struct of_device_id trusty_test_of_match[] = {
++	{ .compatible = "android,trusty-log-v1", },
++	{},
++};
++
++MODULE_DEVICE_TABLE(trusty, trusty_test_of_match);
++
++static struct platform_driver trusty_log_driver = {
++	.probe = trusty_log_probe,
++	.remove = trusty_log_remove,
++	.driver = {
++		.name = "trusty-log",
++		.of_match_table = trusty_test_of_match,
++	},
++};
++
++module_platform_driver(trusty_log_driver);
++
++MODULE_LICENSE("GPL v2");
++MODULE_DESCRIPTION("Trusty logging driver");
+diff --git a/drivers/trusty/trusty-log.h b/drivers/trusty/trusty-log.h
+new file mode 100644
+index 000000000000..7b5e6096b51e
+--- /dev/null
++++ b/drivers/trusty/trusty-log.h
+@@ -0,0 +1,28 @@
++/* SPDX-License-Identifier: MIT */
++/*
++ * Copyright (c) 2015 Google, Inc.
++ *
++ * Trusty also has a copy of this header.  Please keep the copies in sync.
++ */
++#ifndef _TRUSTY_LOG_H_
++#define _TRUSTY_LOG_H_
++
++/*
++ * Ring buffer that supports one secure producer thread and one
++ * linux side consumer thread.
++ */
++struct log_rb {
++	volatile uint32_t alloc;
++	volatile uint32_t put;
++	uint32_t sz;
++	volatile char data[];
++} __packed;
++
++#define SMC_SC_SHARED_LOG_VERSION	SMC_STDCALL_NR(SMC_ENTITY_LOGGING, 0)
++#define SMC_SC_SHARED_LOG_ADD		SMC_STDCALL_NR(SMC_ENTITY_LOGGING, 1)
++#define SMC_SC_SHARED_LOG_RM		SMC_STDCALL_NR(SMC_ENTITY_LOGGING, 2)
++
++#define TRUSTY_LOG_API_VERSION	1
++
++#endif
++
+diff --git a/drivers/trusty/trusty-mem.c b/drivers/trusty/trusty-mem.c
+new file mode 100644
+index 000000000000..8a360298e501
+--- /dev/null
++++ b/drivers/trusty/trusty-mem.c
+@@ -0,0 +1,139 @@
++// SPDX-License-Identifier: GPL-2.0-only
++/*
++ * Copyright (C) 2015 Google, Inc.
++ */
++
++#include <linux/types.h>
++#include <linux/printk.h>
++#include <linux/trusty/arm_ffa.h>
++#include <linux/trusty/trusty.h>
++#include <linux/trusty/smcall.h>
++
++#define MEM_ATTR_STRONGLY_ORDERED (0x00U)
++#define MEM_ATTR_DEVICE (0x04U)
++#define MEM_ATTR_NORMAL_NON_CACHEABLE (0x44U)
++#define MEM_ATTR_NORMAL_WRITE_THROUGH (0xAAU)
++#define MEM_ATTR_NORMAL_WRITE_BACK_READ_ALLOCATE (0xEEU)
++#define MEM_ATTR_NORMAL_WRITE_BACK_WRITE_ALLOCATE (0xFFU)
++
++#define ATTR_RDONLY (1U << 7)
++#define ATTR_INNER_SHAREABLE (3U << 8)
++
++static int get_mem_attr(struct page *page, pgprot_t pgprot)
++{
++#if defined(CONFIG_ARM64)
++	u64 mair;
++	unsigned int attr_index = (pgprot_val(pgprot) & PTE_ATTRINDX_MASK) >> 2;
++
++	asm ("mrs %0, mair_el1\n" : "=&r" (mair));
++	return (mair >> (attr_index * 8)) & 0xff;
++
++#elif defined(CONFIG_ARM_LPAE)
++	u32 mair;
++	unsigned int attr_index = ((pgprot_val(pgprot) & L_PTE_MT_MASK) >> 2);
++
++	if (attr_index >= 4) {
++		attr_index -= 4;
++		asm volatile("mrc p15, 0, %0, c10, c2, 1\n" : "=&r" (mair));
++	} else {
++		asm volatile("mrc p15, 0, %0, c10, c2, 0\n" : "=&r" (mair));
++	}
++	return (mair >> (attr_index * 8)) & 0xff;
++
++#elif defined(CONFIG_ARM)
++	/* check memory type */
++	switch (pgprot_val(pgprot) & L_PTE_MT_MASK) {
++	case L_PTE_MT_WRITEALLOC:
++		return MEM_ATTR_NORMAL_WRITE_BACK_WRITE_ALLOCATE;
++
++	case L_PTE_MT_BUFFERABLE:
++		return MEM_ATTR_NORMAL_NON_CACHEABLE;
++
++	case L_PTE_MT_WRITEBACK:
++		return MEM_ATTR_NORMAL_WRITE_BACK_READ_ALLOCATE;
++
++	case L_PTE_MT_WRITETHROUGH:
++		return MEM_ATTR_NORMAL_WRITE_THROUGH;
++
++	case L_PTE_MT_UNCACHED:
++		return MEM_ATTR_STRONGLY_ORDERED;
++
++	case L_PTE_MT_DEV_SHARED:
++	case L_PTE_MT_DEV_NONSHARED:
++		return MEM_ATTR_DEVICE;
++
++	default:
++		return -EINVAL;
++	}
++#else
++	return 0;
++#endif
++}
++
++int trusty_encode_page_info(struct ns_mem_page_info *inf,
++			    struct page *page, pgprot_t pgprot)
++{
++	int mem_attr;
++	u64 pte;
++	u8 ffa_mem_attr;
++	u8 ffa_mem_perm = 0;
++
++	if (!inf || !page)
++		return -EINVAL;
++
++	/* get physical address */
++	pte = (u64)page_to_phys(page);
++
++	/* get memory attributes */
++	mem_attr = get_mem_attr(page, pgprot);
++	if (mem_attr < 0)
++		return mem_attr;
++
++	switch (mem_attr) {
++	case MEM_ATTR_STRONGLY_ORDERED:
++		ffa_mem_attr = FFA_MEM_ATTR_DEVICE_NGNRNE;
++		break;
++
++	case MEM_ATTR_DEVICE:
++		ffa_mem_attr = FFA_MEM_ATTR_DEVICE_NGNRE;
++		break;
++
++	case MEM_ATTR_NORMAL_NON_CACHEABLE:
++		ffa_mem_attr = FFA_MEM_ATTR_NORMAL_MEMORY_UNCACHED;
++		break;
++
++	case MEM_ATTR_NORMAL_WRITE_BACK_READ_ALLOCATE:
++	case MEM_ATTR_NORMAL_WRITE_BACK_WRITE_ALLOCATE:
++		ffa_mem_attr = FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB;
++		break;
++
++	default:
++		return -EINVAL;
++	}
++
++	inf->paddr = pte;
++
++	/* add other attributes */
++#if defined(CONFIG_ARM64) || defined(CONFIG_ARM_LPAE)
++	pte |= pgprot_val(pgprot);
++#elif defined(CONFIG_ARM)
++	if (pgprot_val(pgprot) & L_PTE_RDONLY)
++		pte |= ATTR_RDONLY;
++	if (pgprot_val(pgprot) & L_PTE_SHARED)
++		pte |= ATTR_INNER_SHAREABLE; /* inner sharable */
++#endif
++
++	if (!(pte & ATTR_RDONLY))
++		ffa_mem_perm |= FFA_MEM_PERM_RW;
++	else
++		ffa_mem_perm |= FFA_MEM_PERM_RO;
++
++	if ((pte & ATTR_INNER_SHAREABLE) == ATTR_INNER_SHAREABLE)
++		ffa_mem_attr |= FFA_MEM_ATTR_INNER_SHAREABLE;
++
++	inf->ffa_mem_attr = ffa_mem_attr;
++	inf->ffa_mem_perm = ffa_mem_perm;
++	inf->compat_attr = (pte & 0x0000FFFFFFFFFFFFull) |
++			   ((u64)mem_attr << 48);
++	return 0;
++}
+diff --git a/drivers/trusty/trusty-smc-arm.S b/drivers/trusty/trusty-smc-arm.S
+new file mode 100644
+index 000000000000..8ff83547d33f
+--- /dev/null
++++ b/drivers/trusty/trusty-smc-arm.S
+@@ -0,0 +1,41 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Copyright (C) 2020 Google, Inc.
++ */
++
++#include <linux/linkage.h>
++
++.arch_extension sec
++
++ENTRY(trusty_smc8)
++    /* Save stack location where r3-r7 smc arguments are stored */
++    mov     r12, sp
++
++    /* Save original r4-r7 values as caller expects these to be preserved */
++    push    {r4-r7}
++
++    /* Save return value pointer and return address */
++    push    {r0, lr}
++
++    /* arm abi shifts arguments when returning a struct, shift them back */
++    mov     r0, r1
++    mov     r1, r2
++    mov     r2, r3
++
++    /* Load stack based arguments */
++    ldmia   r12, {r3-r7}
++
++    smc     #0
++
++    /* Restore return address and get return value pointer */
++    pop     {r12, lr}
++
++    /* Copy 8-register smc return value to struct smc_ret8 return value */
++    stmia   r12, {r0-r7}
++
++    /* Restore original r4-r7 values */
++    pop     {r4-r7}
++
++    /* Return */
++    bx      lr
++ENDPROC(trusty_smc8)
+diff --git a/drivers/trusty/trusty-smc-arm64.S b/drivers/trusty/trusty-smc-arm64.S
+new file mode 100644
+index 000000000000..14c8fed28a5e
+--- /dev/null
++++ b/drivers/trusty/trusty-smc-arm64.S
+@@ -0,0 +1,35 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Copyright (C) 2020 Google, Inc.
++ */
++
++#include <linux/linkage.h>
++
++.macro push ra, rb
++stp \ra, \rb, [sp,#-16]!
++.endm
++
++.macro pop ra, rb
++ldp \ra, \rb, [sp], #16
++.endm
++
++lr .req x30
++
++SYM_FUNC_START(trusty_smc8)
++    /*
++     * Save x8 (return value ptr) and lr. The SMC calling convention says el3
++     * does not need to preserve x8. The normal ABI does not require either x8
++     * or lr to be preserved.
++     */
++    push    x8, lr
++    smc     #0
++    pop     x8, lr
++
++    /* Copy 8-register smc return value to struct smc_ret8 return value */
++    stp     x0, x1, [x8], #16
++    stp     x2, x3, [x8], #16
++    stp     x4, x5, [x8], #16
++    stp     x6, x7, [x8], #16
++
++    ret
++SYM_FUNC_END(trusty_smc8)
+diff --git a/drivers/trusty/trusty-smc.h b/drivers/trusty/trusty-smc.h
+new file mode 100644
+index 000000000000..b53e5abb4d05
+--- /dev/null
++++ b/drivers/trusty/trusty-smc.h
+@@ -0,0 +1,26 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Copyright (C) 2020 Google, Inc.
++ */
++#ifndef _TRUSTY_SMC_H
++#define _TRUSTY_SMC_H
++
++#include <linux/types.h>
++
++struct smc_ret8 {
++	unsigned long r0;
++	unsigned long r1;
++	unsigned long r2;
++	unsigned long r3;
++	unsigned long r4;
++	unsigned long r5;
++	unsigned long r6;
++	unsigned long r7;
++};
++
++struct smc_ret8 trusty_smc8(unsigned long r0, unsigned long r1,
++			    unsigned long r2, unsigned long r3,
++			    unsigned long r4, unsigned long r5,
++			    unsigned long r6, unsigned long r7);
++
++#endif /* _TRUSTY_SMC_H */
+diff --git a/drivers/trusty/trusty-test.c b/drivers/trusty/trusty-test.c
+new file mode 100644
+index 000000000000..844868981fa5
+--- /dev/null
++++ b/drivers/trusty/trusty-test.c
+@@ -0,0 +1,440 @@
++// SPDX-License-Identifier: GPL-2.0-only
++/*
++ * Copyright (C) 2020 Google, Inc.
++ */
++
++#include <linux/ctype.h>
++#include <linux/list.h>
++#include <linux/platform_device.h>
++#include <linux/trusty/smcall.h>
++#include <linux/trusty/trusty.h>
++#include <linux/scatterlist.h>
++#include <linux/slab.h>
++#include <linux/mm.h>
++#include <linux/mod_devicetable.h>
++#include <linux/module.h>
++
++#include "trusty-test.h"
++
++struct trusty_test_state {
++	struct device *dev;
++	struct device *trusty_dev;
++};
++
++struct trusty_test_shmem_obj {
++	struct list_head node;
++	size_t page_count;
++	struct page **pages;
++	void *buf;
++	struct sg_table sgt;
++	trusty_shared_mem_id_t mem_id;
++};
++
++/*
++ * Allocate a test object with @page_count number of pages, map it and add it to
++ * @list.
++ * For multi-page allocations, order the pages so they are not contiguous.
++ */
++static int trusty_test_alloc_obj(struct trusty_test_state *s,
++				 size_t page_count,
++				 struct list_head *list)
++{
++	size_t i;
++	int ret = -ENOMEM;
++	struct trusty_test_shmem_obj *obj;
++
++	obj = kzalloc(sizeof(*obj), GFP_KERNEL);
++	if (!obj)
++		goto err_alloc_obj;
++	obj->page_count = page_count;
++
++	obj->pages = kmalloc_array(page_count, sizeof(*obj->pages), GFP_KERNEL);
++	if (!obj->pages) {
++		ret = -ENOMEM;
++		dev_err(s->dev, "failed to allocate page array, count %zd\n",
++			page_count);
++		goto err_alloc_pages;
++	}
++
++	for (i = 0; i < page_count; i++) {
++		obj->pages[i] = alloc_page(GFP_KERNEL);
++		if (!obj->pages[i]) {
++			ret = -ENOMEM;
++			dev_err(s->dev, "failed to allocate page %zd/%zd\n",
++				i, page_count);
++			goto err_alloc_page;
++		}
++		if (i > 0 && obj->pages[i - 1] + 1 == obj->pages[i]) {
++			/* swap adacent pages to increase fragmentation */
++			swap(obj->pages[i - 1], obj->pages[i]);
++		}
++	}
++
++	obj->buf = vmap(obj->pages, page_count, VM_MAP, PAGE_KERNEL);
++	if (!obj->buf) {
++		ret = -ENOMEM;
++		dev_err(s->dev, "failed to map test buffer page count %zd\n",
++			page_count);
++		goto err_map_pages;
++	}
++
++	ret = sg_alloc_table_from_pages(&obj->sgt, obj->pages, page_count,
++					0, page_count * PAGE_SIZE, GFP_KERNEL);
++	if (ret) {
++		dev_err(s->dev, "sg_alloc_table_from_pages failed: %d\n", ret);
++		goto err_alloc_sgt;
++	}
++	list_add_tail(&obj->node, list);
++	dev_dbg(s->dev, "buffer has %d page runs\n", obj->sgt.nents);
++	return 0;
++
++err_alloc_sgt:
++	vunmap(obj->buf);
++err_map_pages:
++	for (i = page_count; i > 0; i--) {
++		__free_page(obj->pages[i - 1]);
++err_alloc_page:
++		;
++	}
++	kfree(obj->pages);
++err_alloc_pages:
++	kfree(obj);
++err_alloc_obj:
++	return ret;
++}
++
++/* Unlink, unmap and free a test object and its pages */
++static void trusty_test_free_obj(struct trusty_test_state *s,
++				 struct trusty_test_shmem_obj *obj)
++{
++	size_t i;
++
++	list_del(&obj->node);
++	sg_free_table(&obj->sgt);
++	vunmap(obj->buf);
++	for (i = obj->page_count; i > 0; i--)
++		__free_page(obj->pages[i - 1]);
++	kfree(obj->pages);
++	kfree(obj);
++}
++
++/*
++ * Share all the pages of all the test object in &obj_list.
++ * If sharing a test object fails, free it so that every test object that
++ * remains in @obj_list has been shared when this function returns.
++ * Return a error if any test object failed to be shared.
++ */
++static int trusty_test_share_objs(struct trusty_test_state *s,
++				  struct list_head *obj_list, size_t size)
++{
++	int ret = 0;
++	int tmpret;
++	struct trusty_test_shmem_obj *obj;
++	struct trusty_test_shmem_obj *next_obj;
++	ktime_t t1;
++	ktime_t t2;
++
++	list_for_each_entry_safe(obj, next_obj, obj_list, node) {
++		t1 = ktime_get();
++		tmpret = trusty_share_memory(s->trusty_dev, &obj->mem_id,
++					     obj->sgt.sgl, obj->sgt.nents,
++					     PAGE_KERNEL);
++		t2 = ktime_get();
++		if (tmpret) {
++			ret = tmpret;
++			dev_err(s->dev,
++				"trusty_share_memory failed: %d, size=%zd\n",
++				ret, size);
++
++			/*
++			 * Free obj and continue, so we can revoke the
++			 * whole list in trusty_test_reclaim_objs.
++			 */
++			trusty_test_free_obj(s, obj);
++		}
++		dev_dbg(s->dev, "share id=0x%llx, size=%zu took %lld ns\n",
++			obj->mem_id, size,
++			ktime_to_ns(ktime_sub(t2, t1)));
++	}
++
++	return ret;
++}
++
++/* Reclaim memory shared with trusty for all test objects in @obj_list. */
++static int trusty_test_reclaim_objs(struct trusty_test_state *s,
++				    struct list_head *obj_list, size_t size)
++{
++	int ret = 0;
++	int tmpret;
++	struct trusty_test_shmem_obj *obj;
++	struct trusty_test_shmem_obj *next_obj;
++	ktime_t t1;
++	ktime_t t2;
++
++	list_for_each_entry_safe(obj, next_obj, obj_list, node) {
++		t1 = ktime_get();
++		tmpret = trusty_reclaim_memory(s->trusty_dev, obj->mem_id,
++					       obj->sgt.sgl, obj->sgt.nents);
++		t2 = ktime_get();
++		if (tmpret) {
++			ret = tmpret;
++			dev_err(s->dev,
++				"trusty_reclaim_memory failed: %d, id=0x%llx\n",
++				ret, obj->mem_id);
++
++			/*
++			 * It is not safe to free this memory if
++			 * trusty_reclaim_memory fails. Leak it in that
++			 * case.
++			 */
++			list_del(&obj->node);
++		}
++		dev_dbg(s->dev, "revoke id=0x%llx, size=%zu took %lld ns\n",
++			obj->mem_id, size,
++			ktime_to_ns(ktime_sub(t2, t1)));
++	}
++
++	return ret;
++}
++
++/*
++ * Test a test object. First, initialize the memory, then make a std call into
++ * trusty which will read it and return an error if the initialized value does
++ * not match what it expects. If trusty reads the correct values, it will modify
++ * the memory and return 0. This function then checks that it can read the
++ * correct modified value.
++ */
++static int trusty_test_rw(struct trusty_test_state *s,
++			  struct trusty_test_shmem_obj *obj)
++{
++	size_t size = obj->page_count * PAGE_SIZE;
++	int ret;
++	size_t i;
++	u64 *buf = obj->buf;
++	ktime_t t1;
++	ktime_t t2;
++
++	for (i = 0; i < size / sizeof(*buf); i++)
++		buf[i] = i;
++
++	t1 = ktime_get();
++	ret = trusty_std_call32(s->trusty_dev, SMC_SC_TEST_SHARED_MEM_RW,
++				(u32)(obj->mem_id), (u32)(obj->mem_id >> 32),
++				size);
++	t2 = ktime_get();
++	if (ret < 0) {
++		dev_err(s->dev,
++			"trusty std call (SMC_SC_TEST_SHARED_MEM_RW) failed: %d 0x%llx\n",
++			ret, obj->mem_id);
++		return ret;
++	}
++
++	for (i = 0; i < size / sizeof(*buf); i++) {
++		if (buf[i] != size - i) {
++			dev_err(s->dev,
++				"input mismatch at %zd, got 0x%llx instead of 0x%zx\n",
++				i, buf[i], size - i);
++			return -EIO;
++		}
++	}
++
++	dev_dbg(s->dev, "rw id=0x%llx, size=%zu took %lld ns\n", obj->mem_id,
++		size, ktime_to_ns(ktime_sub(t2, t1)));
++
++	return 0;
++}
++
++/*
++ * Run test on every test object in @obj_list. Repeat @repeat_access times.
++ */
++static int trusty_test_rw_objs(struct trusty_test_state *s,
++			       struct list_head *obj_list,
++			       size_t repeat_access)
++{
++	int ret;
++	size_t i;
++	struct trusty_test_shmem_obj *obj;
++
++	for (i = 0; i < repeat_access; i++) {
++		/*
++		 * Repeat test in case the memory attributes don't match
++		 * and either side see old data.
++		 */
++		list_for_each_entry(obj, obj_list, node) {
++			ret = trusty_test_rw(s, obj);
++			if (ret)
++				return ret;
++		}
++	}
++
++	return 0;
++}
++
++/*
++ * Allocate @obj_count test object that each have @page_count pages. Share each
++ * object @repeat_share times, each time running tests on every object
++ * @repeat_access times.
++ */
++static int trusty_test_run(struct trusty_test_state *s, size_t page_count,
++			   size_t obj_count, size_t repeat_share,
++			   size_t repeat_access)
++{
++	int ret = 0;
++	int tmpret;
++	size_t i;
++	size_t size = page_count * PAGE_SIZE;
++	LIST_HEAD(obj_list);
++	struct trusty_test_shmem_obj *obj;
++	struct trusty_test_shmem_obj *next_obj;
++
++	for (i = 0; i < obj_count && !ret; i++)
++		ret = trusty_test_alloc_obj(s, page_count, &obj_list);
++
++	for (i = 0; i < repeat_share && !ret; i++) {
++		ret = trusty_test_share_objs(s, &obj_list, size);
++		if (ret) {
++			dev_err(s->dev,
++				"trusty_share_memory failed: %d, i=%zd/%zd, size=%zd\n",
++				ret, i, repeat_share, size);
++		} else {
++			ret = trusty_test_rw_objs(s, &obj_list, repeat_access);
++			if (ret)
++				dev_err(s->dev,
++					"test failed: %d, i=%zd/%zd, size=%zd\n",
++					ret, i, repeat_share, size);
++		}
++		tmpret = trusty_test_reclaim_objs(s, &obj_list, size);
++		if (tmpret) {
++			ret = tmpret;
++			dev_err(s->dev,
++				"trusty_reclaim_memory failed: %d, i=%zd/%zd\n",
++				ret, i, repeat_share);
++		}
++	}
++
++	list_for_each_entry_safe(obj, next_obj, &obj_list, node)
++		trusty_test_free_obj(s, obj);
++
++	dev_info(s->dev, "[ %s ] size %zd, obj_count %zd, repeat_share %zd, repeat_access %zd\n",
++		 ret ? "FAILED" : "PASSED", size, obj_count, repeat_share,
++		 repeat_access);
++
++	return ret;
++}
++
++/*
++ * Get an optional numeric argument from @buf, update @buf and return the value.
++ * If @buf does not start with ",", return @default_val instead.
++ */
++static size_t trusty_test_get_arg(const char **buf, size_t default_val)
++{
++	char *buf_next;
++	size_t ret;
++
++	if (**buf != ',')
++		return default_val;
++
++	(*buf)++;
++	ret = simple_strtoul(*buf, &buf_next, 0);
++	if (buf_next == *buf)
++		return default_val;
++
++	*buf = buf_next;
++
++	return ret;
++}
++
++/*
++ * Run tests described by a string in this format:
++ * <obj_size>,<obj_count=1>,<repeat_share=1>,<repeat_access=3>
++ */
++static ssize_t trusty_test_run_store(struct device *dev,
++				     struct device_attribute *attr,
++				     const char *buf, size_t count)
++{
++	struct platform_device *pdev = to_platform_device(dev);
++	struct trusty_test_state *s = platform_get_drvdata(pdev);
++	size_t size;
++	size_t obj_count;
++	size_t repeat_share;
++	size_t repeat_access;
++	int ret;
++	char *buf_next;
++
++	while (true) {
++		while (isspace(*buf))
++			buf++;
++		size = simple_strtoul(buf, &buf_next, 0);
++		if (buf_next == buf)
++			return count;
++		buf = buf_next;
++		obj_count = trusty_test_get_arg(&buf, 1);
++		repeat_share = trusty_test_get_arg(&buf, 1);
++		repeat_access = trusty_test_get_arg(&buf, 3);
++
++		ret = trusty_test_run(s, DIV_ROUND_UP(size, PAGE_SIZE),
++				      obj_count, repeat_share, repeat_access);
++		if (ret)
++			return ret;
++	}
++}
++
++static DEVICE_ATTR_WO(trusty_test_run);
++
++static struct attribute *trusty_test_attrs[] = {
++	&dev_attr_trusty_test_run.attr,
++	NULL,
++};
++ATTRIBUTE_GROUPS(trusty_test);
++
++static int trusty_test_probe(struct platform_device *pdev)
++{
++	struct trusty_test_state *s;
++	int ret;
++
++	ret = trusty_std_call32(pdev->dev.parent, SMC_SC_TEST_VERSION,
++				TRUSTY_STDCALLTEST_API_VERSION, 0, 0);
++	if (ret != TRUSTY_STDCALLTEST_API_VERSION)
++		return -ENOENT;
++
++	s = kzalloc(sizeof(*s), GFP_KERNEL);
++	if (!s)
++		return -ENOMEM;
++
++	s->dev = &pdev->dev;
++	s->trusty_dev = s->dev->parent;
++
++	platform_set_drvdata(pdev, s);
++
++	return 0;
++}
++
++static int trusty_test_remove(struct platform_device *pdev)
++{
++	struct trusty_log_state *s = platform_get_drvdata(pdev);
++
++	kfree(s);
++	return 0;
++}
++
++static const struct of_device_id trusty_test_of_match[] = {
++	{ .compatible = "android,trusty-test-v1", },
++	{},
++};
++
++MODULE_DEVICE_TABLE(trusty, trusty_test_of_match);
++
++static struct platform_driver trusty_test_driver = {
++	.probe = trusty_test_probe,
++	.remove = trusty_test_remove,
++	.driver = {
++		.name = "trusty-test",
++		.of_match_table = trusty_test_of_match,
++		.dev_groups = trusty_test_groups,
++	},
++};
++
++module_platform_driver(trusty_test_driver);
++
++MODULE_LICENSE("GPL v2");
++MODULE_DESCRIPTION("Trusty test driver");
+diff --git a/drivers/trusty/trusty-test.h b/drivers/trusty/trusty-test.h
+new file mode 100644
+index 000000000000..eea7beb96876
+--- /dev/null
++++ b/drivers/trusty/trusty-test.h
+@@ -0,0 +1,13 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Copyright (c) 2020 Google, Inc.
++ */
++#ifndef _TRUSTY_TEST_H
++#define _TRUSTY_TEST_H
++
++#define SMC_SC_TEST_VERSION SMC_STDCALL_NR(SMC_ENTITY_TEST, 0)
++#define SMC_SC_TEST_SHARED_MEM_RW SMC_STDCALL_NR(SMC_ENTITY_TEST, 1)
++
++#define TRUSTY_STDCALLTEST_API_VERSION 1
++
++#endif /* _TRUSTY_TEST_H */
+diff --git a/drivers/trusty/trusty-virtio.c b/drivers/trusty/trusty-virtio.c
+new file mode 100644
+index 000000000000..fea59cd2e218
+--- /dev/null
++++ b/drivers/trusty/trusty-virtio.c
+@@ -0,0 +1,840 @@
++// SPDX-License-Identifier: GPL-2.0-only
++/*
++ * Trusty Virtio driver
++ *
++ * Copyright (C) 2015 Google, Inc.
++ */
++#include <linux/device.h>
++#include <linux/err.h>
++#include <linux/kernel.h>
++
++#include <linux/dma-map-ops.h>
++#include <linux/module.h>
++#include <linux/mutex.h>
++#include <linux/notifier.h>
++#include <linux/workqueue.h>
++#include <linux/remoteproc.h>
++#include <linux/slab.h>
++
++#include <linux/platform_device.h>
++#include <linux/trusty/smcall.h>
++#include <linux/trusty/trusty.h>
++#include <linux/trusty/trusty_ipc.h>
++
++#include <linux/virtio.h>
++#include <linux/virtio_config.h>
++#include <linux/virtio_ids.h>
++#include <linux/virtio_ring.h>
++
++#include <linux/atomic.h>
++
++#define  RSC_DESCR_VER  1
++
++struct trusty_vdev;
++
++struct trusty_ctx {
++	struct device		*dev;
++	void			*shared_va;
++	struct scatterlist	shared_sg;
++	trusty_shared_mem_id_t	shared_id;
++	size_t			shared_sz;
++	struct work_struct	check_vqs;
++	struct work_struct	kick_vqs;
++	struct notifier_block	call_notifier;
++	struct list_head	vdev_list;
++	struct mutex		mlock; /* protects vdev_list */
++	struct workqueue_struct	*kick_wq;
++	struct workqueue_struct	*check_wq;
++};
++
++struct trusty_vring {
++	void			*vaddr;
++	struct scatterlist	sg;
++	trusty_shared_mem_id_t	shared_mem_id;
++	size_t			size;
++	unsigned int		align;
++	unsigned int		elem_num;
++	u32			notifyid;
++	atomic_t		needs_kick;
++	struct fw_rsc_vdev_vring *vr_descr;
++	struct virtqueue	*vq;
++	struct trusty_vdev	*tvdev;
++	struct trusty_nop	kick_nop;
++};
++
++struct trusty_vdev {
++	struct list_head	node;
++	struct virtio_device	vdev;
++	struct trusty_ctx	*tctx;
++	u32			notifyid;
++	unsigned int		config_len;
++	void			*config;
++	struct fw_rsc_vdev	*vdev_descr;
++	unsigned int		vring_num;
++	struct trusty_vring	vrings[];
++};
++
++#define vdev_to_tvdev(vd)  container_of((vd), struct trusty_vdev, vdev)
++
++static void check_all_vqs(struct work_struct *work)
++{
++	unsigned int i;
++	struct trusty_ctx *tctx = container_of(work, struct trusty_ctx,
++					       check_vqs);
++	struct trusty_vdev *tvdev;
++
++	list_for_each_entry(tvdev, &tctx->vdev_list, node) {
++		for (i = 0; i < tvdev->vring_num; i++)
++			if (tvdev->vrings[i].vq)
++				vring_interrupt(0, tvdev->vrings[i].vq);
++	}
++}
++
++static int trusty_call_notify(struct notifier_block *nb,
++			      unsigned long action, void *data)
++{
++	struct trusty_ctx *tctx;
++
++	if (action != TRUSTY_CALL_RETURNED)
++		return NOTIFY_DONE;
++
++	tctx = container_of(nb, struct trusty_ctx, call_notifier);
++	queue_work(tctx->check_wq, &tctx->check_vqs);
++
++	return NOTIFY_OK;
++}
++
++static void kick_vq(struct trusty_ctx *tctx,
++		    struct trusty_vdev *tvdev,
++		    struct trusty_vring *tvr)
++{
++	int ret;
++
++	dev_dbg(tctx->dev, "%s: vdev_id=%d: vq_id=%d\n",
++		__func__, tvdev->notifyid, tvr->notifyid);
++
++	ret = trusty_std_call32(tctx->dev->parent, SMC_SC_VDEV_KICK_VQ,
++				tvdev->notifyid, tvr->notifyid, 0);
++	if (ret) {
++		dev_err(tctx->dev, "vq notify (%d, %d) returned %d\n",
++			tvdev->notifyid, tvr->notifyid, ret);
++	}
++}
++
++static void kick_vqs(struct work_struct *work)
++{
++	unsigned int i;
++	struct trusty_vdev *tvdev;
++	struct trusty_ctx *tctx = container_of(work, struct trusty_ctx,
++					       kick_vqs);
++	mutex_lock(&tctx->mlock);
++	list_for_each_entry(tvdev, &tctx->vdev_list, node) {
++		for (i = 0; i < tvdev->vring_num; i++) {
++			struct trusty_vring *tvr = &tvdev->vrings[i];
++
++			if (atomic_xchg(&tvr->needs_kick, 0))
++				kick_vq(tctx, tvdev, tvr);
++		}
++	}
++	mutex_unlock(&tctx->mlock);
++}
++
++static bool trusty_virtio_notify(struct virtqueue *vq)
++{
++	struct trusty_vring *tvr = vq->priv;
++	struct trusty_vdev *tvdev = tvr->tvdev;
++	struct trusty_ctx *tctx = tvdev->tctx;
++	u32 api_ver = trusty_get_api_version(tctx->dev->parent);
++
++	if (api_ver < TRUSTY_API_VERSION_SMP_NOP) {
++		atomic_set(&tvr->needs_kick, 1);
++		queue_work(tctx->kick_wq, &tctx->kick_vqs);
++	} else {
++		trusty_enqueue_nop(tctx->dev->parent, &tvr->kick_nop);
++	}
++
++	return true;
++}
++
++static int trusty_load_device_descr(struct trusty_ctx *tctx,
++				    trusty_shared_mem_id_t id, size_t sz)
++{
++	int ret;
++
++	dev_dbg(tctx->dev, "%s: %zu bytes @ id %llu\n", __func__, sz, id);
++
++	ret = trusty_std_call32(tctx->dev->parent, SMC_SC_VIRTIO_GET_DESCR,
++				(u32)id, id >> 32, sz);
++	if (ret < 0) {
++		dev_err(tctx->dev, "%s: virtio get descr returned (%d)\n",
++			__func__, ret);
++		return -ENODEV;
++	}
++	return ret;
++}
++
++static void trusty_virtio_stop(struct trusty_ctx *tctx,
++			       trusty_shared_mem_id_t id, size_t sz)
++{
++	int ret;
++
++	dev_dbg(tctx->dev, "%s: %zu bytes @ id %llu\n", __func__, sz, id);
++
++	ret = trusty_std_call32(tctx->dev->parent, SMC_SC_VIRTIO_STOP,
++				(u32)id, id >> 32, sz);
++	if (ret) {
++		dev_err(tctx->dev, "%s: virtio done returned (%d)\n",
++			__func__, ret);
++		return;
++	}
++}
++
++static int trusty_virtio_start(struct trusty_ctx *tctx,
++			       trusty_shared_mem_id_t id, size_t sz)
++{
++	int ret;
++
++	dev_dbg(tctx->dev, "%s: %zu bytes @ id %llu\n", __func__, sz, id);
++
++	ret = trusty_std_call32(tctx->dev->parent, SMC_SC_VIRTIO_START,
++				(u32)id, id >> 32, sz);
++	if (ret) {
++		dev_err(tctx->dev, "%s: virtio start returned (%d)\n",
++			__func__, ret);
++		return -ENODEV;
++	}
++	return 0;
++}
++
++static void trusty_virtio_reset(struct virtio_device *vdev)
++{
++	struct trusty_vdev *tvdev = vdev_to_tvdev(vdev);
++	struct trusty_ctx *tctx = tvdev->tctx;
++
++	dev_dbg(&vdev->dev, "reset vdev_id=%d\n", tvdev->notifyid);
++	trusty_std_call32(tctx->dev->parent, SMC_SC_VDEV_RESET,
++			  tvdev->notifyid, 0, 0);
++}
++
++static u64 trusty_virtio_get_features(struct virtio_device *vdev)
++{
++	struct trusty_vdev *tvdev = vdev_to_tvdev(vdev);
++
++	return tvdev->vdev_descr->dfeatures |
++		(1ULL << VIRTIO_F_ACCESS_PLATFORM);
++}
++
++static int trusty_virtio_finalize_features(struct virtio_device *vdev)
++{
++	struct trusty_vdev *tvdev = vdev_to_tvdev(vdev);
++	u64 features = vdev->features;
++
++	/*
++	 * We set VIRTIO_F_ACCESS_PLATFORM to enable the dma mapping hooks.
++	 * The other side does not need to know.
++	 */
++	features &= ~(1ULL << VIRTIO_F_ACCESS_PLATFORM);
++
++	/* Make sure we don't have any features > 32 bits! */
++	if (WARN_ON((u32)vdev->features != features))
++		return -EINVAL;
++
++	tvdev->vdev_descr->gfeatures = vdev->features;
++	return 0;
++}
++
++static void trusty_virtio_get_config(struct virtio_device *vdev,
++				     unsigned int offset, void *buf,
++				     unsigned int len)
++{
++	struct trusty_vdev *tvdev = vdev_to_tvdev(vdev);
++
++	dev_dbg(&vdev->dev, "%s: %d bytes @ offset %d\n",
++		__func__, len, offset);
++
++	if (tvdev->config) {
++		if (offset + len <= tvdev->config_len)
++			memcpy(buf, tvdev->config + offset, len);
++	}
++}
++
++static void trusty_virtio_set_config(struct virtio_device *vdev,
++				     unsigned int offset, const void *buf,
++				     unsigned int len)
++{
++}
++
++static u8 trusty_virtio_get_status(struct virtio_device *vdev)
++{
++	struct trusty_vdev *tvdev = vdev_to_tvdev(vdev);
++
++	return tvdev->vdev_descr->status;
++}
++
++static void trusty_virtio_set_status(struct virtio_device *vdev, u8 status)
++{
++	struct trusty_vdev *tvdev = vdev_to_tvdev(vdev);
++
++	tvdev->vdev_descr->status = status;
++}
++
++static void _del_vqs(struct virtio_device *vdev)
++{
++	unsigned int i;
++	int ret;
++	struct trusty_vdev *tvdev = vdev_to_tvdev(vdev);
++	struct trusty_vring *tvr = &tvdev->vrings[0];
++
++	for (i = 0; i < tvdev->vring_num; i++, tvr++) {
++		/* dequeue kick_nop */
++		trusty_dequeue_nop(tvdev->tctx->dev->parent, &tvr->kick_nop);
++
++		/* delete vq */
++		if (tvr->vq) {
++			vring_del_virtqueue(tvr->vq);
++			tvr->vq = NULL;
++		}
++		/* delete vring */
++		if (tvr->vaddr) {
++			ret = trusty_reclaim_memory(tvdev->tctx->dev->parent,
++						    tvr->shared_mem_id,
++						    &tvr->sg, 1);
++			if (WARN_ON(ret)) {
++				dev_err(&vdev->dev,
++					"trusty_revoke_memory failed: %d 0x%llx\n",
++					ret, tvr->shared_mem_id);
++				/*
++				 * It is not safe to free this memory if
++				 * trusty_revoke_memory fails. Leak it in that
++				 * case.
++				 */
++			} else {
++				free_pages_exact(tvr->vaddr, tvr->size);
++			}
++			tvr->vaddr = NULL;
++		}
++	}
++}
++
++static void trusty_virtio_del_vqs(struct virtio_device *vdev)
++{
++	_del_vqs(vdev);
++}
++
++
++static struct virtqueue *_find_vq(struct virtio_device *vdev,
++				  unsigned int id,
++				  void (*callback)(struct virtqueue *vq),
++				  const char *name,
++				  bool ctx)
++{
++	struct trusty_vring *tvr;
++	struct trusty_vdev *tvdev = vdev_to_tvdev(vdev);
++	phys_addr_t pa;
++	int ret;
++
++	if (!name)
++		return ERR_PTR(-EINVAL);
++
++	if (id >= tvdev->vring_num)
++		return ERR_PTR(-EINVAL);
++
++	tvr = &tvdev->vrings[id];
++
++	/* actual size of vring (in bytes) */
++	tvr->size = PAGE_ALIGN(vring_size(tvr->elem_num, tvr->align));
++
++	/* allocate memory for the vring. */
++	tvr->vaddr = alloc_pages_exact(tvr->size, GFP_KERNEL | __GFP_ZERO);
++	if (!tvr->vaddr) {
++		dev_err(&vdev->dev, "vring alloc failed\n");
++		return ERR_PTR(-ENOMEM);
++	}
++
++	sg_init_one(&tvr->sg, tvr->vaddr, tvr->size);
++	ret = trusty_share_memory_compat(tvdev->tctx->dev->parent,
++					 &tvr->shared_mem_id, &tvr->sg, 1,
++					 PAGE_KERNEL);
++	if (ret) {
++		pa = virt_to_phys(tvr->vaddr);
++		dev_err(&vdev->dev, "trusty_share_memory failed: %d %pa\n",
++			ret, &pa);
++		goto err_share_memory;
++	}
++
++	/* save vring address to shared structure */
++	tvr->vr_descr->da = (u32)tvr->shared_mem_id;
++
++	/* da field is only 32 bit wide. Use previously unused 'reserved' field
++	 * to store top 32 bits of 64-bit shared_mem_id
++	 */
++	tvr->vr_descr->pa = (u32)(tvr->shared_mem_id >> 32);
++
++	dev_info(&vdev->dev, "vring%d: va(id)  %p(%llx) qsz %d notifyid %d\n",
++		 id, tvr->vaddr, (u64)tvr->shared_mem_id, tvr->elem_num,
++		 tvr->notifyid);
++
++	tvr->vq = vring_new_virtqueue(id, tvr->elem_num, tvr->align,
++				      vdev, true, ctx, tvr->vaddr,
++				      trusty_virtio_notify, callback, name);
++	if (!tvr->vq) {
++		dev_err(&vdev->dev, "vring_new_virtqueue %s failed\n",
++			name);
++		goto err_new_virtqueue;
++	}
++
++	tvr->vq->priv = tvr;
++
++	return tvr->vq;
++
++err_new_virtqueue:
++	ret = trusty_reclaim_memory(tvdev->tctx->dev->parent,
++				    tvr->shared_mem_id, &tvr->sg, 1);
++	if (WARN_ON(ret)) {
++		dev_err(&vdev->dev, "trusty_revoke_memory failed: %d 0x%llx\n",
++			ret, tvr->shared_mem_id);
++		/*
++		 * It is not safe to free this memory if trusty_revoke_memory
++		 * fails. Leak it in that case.
++		 */
++	} else {
++err_share_memory:
++		free_pages_exact(tvr->vaddr, tvr->size);
++	}
++	tvr->vaddr = NULL;
++	return ERR_PTR(-ENOMEM);
++}
++
++static int trusty_virtio_find_vqs(struct virtio_device *vdev, unsigned int nvqs,
++				  struct virtqueue *vqs[],
++				  vq_callback_t *callbacks[],
++				  const char * const names[],
++				  const bool *ctxs,
++				  struct irq_affinity *desc)
++{
++	unsigned int i;
++	int ret;
++	bool ctx = false;
++
++	for (i = 0; i < nvqs; i++) {
++		ctx = false;
++		if (ctxs)
++			ctx = ctxs[i];
++		vqs[i] = _find_vq(vdev, i, callbacks[i], names[i], ctx);
++		if (IS_ERR(vqs[i])) {
++			ret = PTR_ERR(vqs[i]);
++			_del_vqs(vdev);
++			return ret;
++		}
++	}
++	return 0;
++}
++
++static const char *trusty_virtio_bus_name(struct virtio_device *vdev)
++{
++	return "trusty-virtio";
++}
++
++/* The ops structure which hooks everything together. */
++static const struct virtio_config_ops trusty_virtio_config_ops = {
++	.get_features = trusty_virtio_get_features,
++	.finalize_features = trusty_virtio_finalize_features,
++	.get = trusty_virtio_get_config,
++	.set = trusty_virtio_set_config,
++	.get_status = trusty_virtio_get_status,
++	.set_status = trusty_virtio_set_status,
++	.reset    = trusty_virtio_reset,
++	.find_vqs = trusty_virtio_find_vqs,
++	.del_vqs  = trusty_virtio_del_vqs,
++	.bus_name = trusty_virtio_bus_name,
++};
++
++static int trusty_virtio_add_device(struct trusty_ctx *tctx,
++				    struct fw_rsc_vdev *vdev_descr,
++				    struct fw_rsc_vdev_vring *vr_descr,
++				    void *config)
++{
++	int i, ret;
++	struct trusty_vdev *tvdev;
++
++	tvdev = kzalloc(struct_size(tvdev, vrings, vdev_descr->num_of_vrings),
++			GFP_KERNEL);
++	if (!tvdev)
++		return -ENOMEM;
++
++	/* setup vdev */
++	tvdev->tctx = tctx;
++	tvdev->vdev.dev.parent = tctx->dev;
++	tvdev->vdev.id.device  = vdev_descr->id;
++	tvdev->vdev.config = &trusty_virtio_config_ops;
++	tvdev->vdev_descr = vdev_descr;
++	tvdev->notifyid = vdev_descr->notifyid;
++
++	/* setup config */
++	tvdev->config = config;
++	tvdev->config_len = vdev_descr->config_len;
++
++	/* setup vrings and vdev resource */
++	tvdev->vring_num = vdev_descr->num_of_vrings;
++
++	for (i = 0; i < tvdev->vring_num; i++, vr_descr++) {
++		struct trusty_vring *tvr = &tvdev->vrings[i];
++
++		tvr->tvdev    = tvdev;
++		tvr->vr_descr = vr_descr;
++		tvr->align    = vr_descr->align;
++		tvr->elem_num = vr_descr->num;
++		tvr->notifyid = vr_descr->notifyid;
++		trusty_nop_init(&tvr->kick_nop, SMC_NC_VDEV_KICK_VQ,
++				tvdev->notifyid, tvr->notifyid);
++	}
++
++	/* register device */
++	ret = register_virtio_device(&tvdev->vdev);
++	if (ret) {
++		dev_err(tctx->dev,
++			"Failed (%d) to register device dev type %u\n",
++			ret, vdev_descr->id);
++		goto err_register;
++	}
++
++	/* add it to tracking list */
++	list_add_tail(&tvdev->node, &tctx->vdev_list);
++
++	return 0;
++
++err_register:
++	kfree(tvdev);
++	return ret;
++}
++
++static int trusty_parse_device_descr(struct trusty_ctx *tctx,
++				     void *descr_va, size_t descr_sz)
++{
++	u32 i;
++	struct resource_table *descr = descr_va;
++
++	if (descr_sz < sizeof(*descr)) {
++		dev_err(tctx->dev, "descr table is too small (0x%x)\n",
++			(int)descr_sz);
++		return -ENODEV;
++	}
++
++	if (descr->ver != RSC_DESCR_VER) {
++		dev_err(tctx->dev, "unexpected descr ver (0x%x)\n",
++			(int)descr->ver);
++		return -ENODEV;
++	}
++
++	if (descr_sz < (sizeof(*descr) + descr->num * sizeof(u32))) {
++		dev_err(tctx->dev, "descr table is too small (0x%x)\n",
++			(int)descr->ver);
++		return -ENODEV;
++	}
++
++	for (i = 0; i < descr->num; i++) {
++		struct fw_rsc_hdr *hdr;
++		struct fw_rsc_vdev *vd;
++		struct fw_rsc_vdev_vring *vr;
++		void *cfg;
++		size_t vd_sz;
++
++		u32 offset = descr->offset[i];
++
++		if (offset >= descr_sz) {
++			dev_err(tctx->dev, "offset is out of bounds (%u)\n",
++				offset);
++			return -ENODEV;
++		}
++
++		/* check space for rsc header */
++		if ((descr_sz - offset) < sizeof(struct fw_rsc_hdr)) {
++			dev_err(tctx->dev, "no space for rsc header (%u)\n",
++				offset);
++			return -ENODEV;
++		}
++		hdr = (struct fw_rsc_hdr *)((u8 *)descr + offset);
++		offset += sizeof(struct fw_rsc_hdr);
++
++		/* check type */
++		if (hdr->type != RSC_VDEV) {
++			dev_err(tctx->dev, "unsupported rsc type (%u)\n",
++				hdr->type);
++			continue;
++		}
++
++		/* got vdev: check space for vdev */
++		if ((descr_sz - offset) < sizeof(struct fw_rsc_vdev)) {
++			dev_err(tctx->dev, "no space for vdev descr (%u)\n",
++				offset);
++			return -ENODEV;
++		}
++		vd = (struct fw_rsc_vdev *)((u8 *)descr + offset);
++
++		/* check space for vrings and config area */
++		vd_sz = sizeof(struct fw_rsc_vdev) +
++			vd->num_of_vrings * sizeof(struct fw_rsc_vdev_vring) +
++			vd->config_len;
++
++		if ((descr_sz - offset) < vd_sz) {
++			dev_err(tctx->dev, "no space for vdev (%u)\n", offset);
++			return -ENODEV;
++		}
++		vr = (struct fw_rsc_vdev_vring *)vd->vring;
++		cfg = (void *)(vr + vd->num_of_vrings);
++
++		trusty_virtio_add_device(tctx, vd, vr, cfg);
++	}
++
++	return 0;
++}
++
++static void _remove_devices_locked(struct trusty_ctx *tctx)
++{
++	struct trusty_vdev *tvdev, *next;
++
++	list_for_each_entry_safe(tvdev, next, &tctx->vdev_list, node) {
++		list_del(&tvdev->node);
++		unregister_virtio_device(&tvdev->vdev);
++		kfree(tvdev);
++	}
++}
++
++static void trusty_virtio_remove_devices(struct trusty_ctx *tctx)
++{
++	mutex_lock(&tctx->mlock);
++	_remove_devices_locked(tctx);
++	mutex_unlock(&tctx->mlock);
++}
++
++static int trusty_virtio_add_devices(struct trusty_ctx *tctx)
++{
++	int ret;
++	int ret_tmp;
++	void *descr_va;
++	trusty_shared_mem_id_t descr_id;
++	size_t descr_sz;
++	size_t descr_buf_sz;
++
++	/* allocate buffer to load device descriptor into */
++	descr_buf_sz = PAGE_SIZE;
++	descr_va = alloc_pages_exact(descr_buf_sz, GFP_KERNEL | __GFP_ZERO);
++	if (!descr_va) {
++		dev_err(tctx->dev, "Failed to allocate shared area\n");
++		return -ENOMEM;
++	}
++
++	sg_init_one(&tctx->shared_sg, descr_va, descr_buf_sz);
++	ret = trusty_share_memory(tctx->dev->parent, &descr_id,
++				  &tctx->shared_sg, 1, PAGE_KERNEL);
++	if (ret) {
++		dev_err(tctx->dev, "trusty_share_memory failed: %d\n", ret);
++		goto err_share_memory;
++	}
++
++	/* load device descriptors */
++	ret = trusty_load_device_descr(tctx, descr_id, descr_buf_sz);
++	if (ret < 0) {
++		dev_err(tctx->dev, "failed (%d) to load device descr\n", ret);
++		goto err_load_descr;
++	}
++
++	descr_sz = (size_t)ret;
++
++	mutex_lock(&tctx->mlock);
++
++	/* parse device descriptor and add virtio devices */
++	ret = trusty_parse_device_descr(tctx, descr_va, descr_sz);
++	if (ret) {
++		dev_err(tctx->dev, "failed (%d) to parse device descr\n", ret);
++		goto err_parse_descr;
++	}
++
++	/* register call notifier */
++	ret = trusty_call_notifier_register(tctx->dev->parent,
++					    &tctx->call_notifier);
++	if (ret) {
++		dev_err(tctx->dev, "%s: failed (%d) to register notifier\n",
++			__func__, ret);
++		goto err_register_notifier;
++	}
++
++	/* start virtio */
++	ret = trusty_virtio_start(tctx, descr_id, descr_sz);
++	if (ret) {
++		dev_err(tctx->dev, "failed (%d) to start virtio\n", ret);
++		goto err_start_virtio;
++	}
++
++	/* attach shared area */
++	tctx->shared_va = descr_va;
++	tctx->shared_id = descr_id;
++	tctx->shared_sz = descr_buf_sz;
++
++	mutex_unlock(&tctx->mlock);
++
++	return 0;
++
++err_start_virtio:
++	trusty_call_notifier_unregister(tctx->dev->parent,
++					&tctx->call_notifier);
++	cancel_work_sync(&tctx->check_vqs);
++err_register_notifier:
++err_parse_descr:
++	_remove_devices_locked(tctx);
++	mutex_unlock(&tctx->mlock);
++	cancel_work_sync(&tctx->kick_vqs);
++	trusty_virtio_stop(tctx, descr_id, descr_sz);
++err_load_descr:
++	ret_tmp = trusty_reclaim_memory(tctx->dev->parent, descr_id,
++					&tctx->shared_sg, 1);
++	if (WARN_ON(ret_tmp)) {
++		dev_err(tctx->dev, "trusty_revoke_memory failed: %d 0x%llx\n",
++			ret_tmp, tctx->shared_id);
++		/*
++		 * It is not safe to free this memory if trusty_revoke_memory
++		 * fails. Leak it in that case.
++		 */
++	} else {
++err_share_memory:
++		free_pages_exact(descr_va, descr_buf_sz);
++	}
++	return ret;
++}
++
++static dma_addr_t trusty_virtio_dma_map_page(struct device *dev,
++					     struct page *page,
++					     unsigned long offset, size_t size,
++					     enum dma_data_direction dir,
++					     unsigned long attrs)
++{
++	struct tipc_msg_buf *buf = page_to_virt(page) + offset;
++
++	return buf->buf_id;
++}
++
++static const struct dma_map_ops trusty_virtio_dma_map_ops = {
++	.map_page = trusty_virtio_dma_map_page,
++};
++
++static int trusty_virtio_probe(struct platform_device *pdev)
++{
++	int ret;
++	struct trusty_ctx *tctx;
++
++	tctx = kzalloc(sizeof(*tctx), GFP_KERNEL);
++	if (!tctx)
++		return -ENOMEM;
++
++	tctx->dev = &pdev->dev;
++	tctx->call_notifier.notifier_call = trusty_call_notify;
++	mutex_init(&tctx->mlock);
++	INIT_LIST_HEAD(&tctx->vdev_list);
++	INIT_WORK(&tctx->check_vqs, check_all_vqs);
++	INIT_WORK(&tctx->kick_vqs, kick_vqs);
++	platform_set_drvdata(pdev, tctx);
++
++	set_dma_ops(&pdev->dev, &trusty_virtio_dma_map_ops);
++
++	tctx->check_wq = alloc_workqueue("trusty-check-wq", WQ_UNBOUND, 0);
++	if (!tctx->check_wq) {
++		ret = -ENODEV;
++		dev_err(&pdev->dev, "Failed create trusty-check-wq\n");
++		goto err_create_check_wq;
++	}
++
++	tctx->kick_wq = alloc_workqueue("trusty-kick-wq",
++					WQ_UNBOUND | WQ_CPU_INTENSIVE, 0);
++	if (!tctx->kick_wq) {
++		ret = -ENODEV;
++		dev_err(&pdev->dev, "Failed create trusty-kick-wq\n");
++		goto err_create_kick_wq;
++	}
++
++	ret = trusty_virtio_add_devices(tctx);
++	if (ret) {
++		dev_err(&pdev->dev, "Failed to add virtio devices\n");
++		goto err_add_devices;
++	}
++
++	dev_info(&pdev->dev, "initializing done\n");
++	return 0;
++
++err_add_devices:
++	destroy_workqueue(tctx->kick_wq);
++err_create_kick_wq:
++	destroy_workqueue(tctx->check_wq);
++err_create_check_wq:
++	kfree(tctx);
++	return ret;
++}
++
++static int trusty_virtio_remove(struct platform_device *pdev)
++{
++	struct trusty_ctx *tctx = platform_get_drvdata(pdev);
++	int ret;
++
++	/* unregister call notifier and wait until workqueue is done */
++	trusty_call_notifier_unregister(tctx->dev->parent,
++					&tctx->call_notifier);
++	cancel_work_sync(&tctx->check_vqs);
++
++	/* remove virtio devices */
++	trusty_virtio_remove_devices(tctx);
++	cancel_work_sync(&tctx->kick_vqs);
++
++	/* destroy workqueues */
++	destroy_workqueue(tctx->kick_wq);
++	destroy_workqueue(tctx->check_wq);
++
++	/* notify remote that shared area goes away */
++	trusty_virtio_stop(tctx, tctx->shared_id, tctx->shared_sz);
++
++	/* free shared area */
++	ret = trusty_reclaim_memory(tctx->dev->parent, tctx->shared_id,
++				    &tctx->shared_sg, 1);
++	if (WARN_ON(ret)) {
++		dev_err(tctx->dev, "trusty_revoke_memory failed: %d 0x%llx\n",
++			ret, tctx->shared_id);
++		/*
++		 * It is not safe to free this memory if trusty_revoke_memory
++		 * fails. Leak it in that case.
++		 */
++	} else {
++		free_pages_exact(tctx->shared_va, tctx->shared_sz);
++	}
++
++	/* free context */
++	kfree(tctx);
++	return 0;
++}
++
++static const struct of_device_id trusty_of_match[] = {
++	{
++		.compatible = "android,trusty-virtio-v1",
++	},
++	{},
++};
++
++MODULE_DEVICE_TABLE(of, trusty_of_match);
++
++static struct platform_driver trusty_virtio_driver = {
++	.probe = trusty_virtio_probe,
++	.remove = trusty_virtio_remove,
++	.driver = {
++		.name = "trusty-virtio",
++		.of_match_table = trusty_of_match,
++	},
++};
++
++module_platform_driver(trusty_virtio_driver);
++
++MODULE_LICENSE("GPL v2");
++MODULE_DESCRIPTION("Trusty virtio driver");
++/*
++ * TODO(b/168322325): trusty-virtio and trusty-ipc should be independent.
++ * However, trusty-virtio is not completely generic and is aware of trusty-ipc.
++ * See header includes. Particularly, trusty-virtio.ko can't be loaded before
++ * trusty-ipc.ko.
++ */
++MODULE_SOFTDEP("pre: trusty-ipc");
+diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
+new file mode 100644
+index 000000000000..265eab52aea0
+--- /dev/null
++++ b/drivers/trusty/trusty.c
+@@ -0,0 +1,981 @@
++// SPDX-License-Identifier: GPL-2.0-only
++/*
++ * Copyright (C) 2013 Google, Inc.
++ */
++
++#include <linux/delay.h>
++#include <linux/module.h>
++#include <linux/of.h>
++#include <linux/of_platform.h>
++#include <linux/platform_device.h>
++#include <linux/slab.h>
++#include <linux/stat.h>
++#include <linux/string.h>
++#include <linux/trusty/arm_ffa.h>
++#include <linux/trusty/smcall.h>
++#include <linux/trusty/sm_err.h>
++#include <linux/trusty/trusty.h>
++
++#include <linux/scatterlist.h>
++#include <linux/dma-mapping.h>
++
++#include "trusty-smc.h"
++
++struct trusty_state;
++static struct platform_driver trusty_driver;
++
++struct trusty_work {
++	struct trusty_state *ts;
++	struct work_struct work;
++};
++
++struct trusty_state {
++	struct mutex smc_lock;
++	struct atomic_notifier_head notifier;
++	struct completion cpu_idle_completion;
++	char *version_str;
++	u32 api_version;
++	bool trusty_panicked;
++	struct device *dev;
++	struct workqueue_struct *nop_wq;
++	struct trusty_work __percpu *nop_works;
++	struct list_head nop_queue;
++	spinlock_t nop_lock; /* protects nop_queue */
++	struct device_dma_parameters dma_parms;
++	void *ffa_tx;
++	void *ffa_rx;
++	u16 ffa_local_id;
++	u16 ffa_remote_id;
++	struct mutex share_memory_msg_lock; /* protects share_memory_msg */
++};
++
++static inline unsigned long smc(unsigned long r0, unsigned long r1,
++				unsigned long r2, unsigned long r3)
++{
++	return trusty_smc8(r0, r1, r2, r3, 0, 0, 0, 0).r0;
++}
++
++s32 trusty_fast_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	if (WARN_ON(!s))
++		return SM_ERR_INVALID_PARAMETERS;
++	if (WARN_ON(!SMC_IS_FASTCALL(smcnr)))
++		return SM_ERR_INVALID_PARAMETERS;
++	if (WARN_ON(SMC_IS_SMC64(smcnr)))
++		return SM_ERR_INVALID_PARAMETERS;
++
++	return smc(smcnr, a0, a1, a2);
++}
++EXPORT_SYMBOL(trusty_fast_call32);
++
++#ifdef CONFIG_64BIT
++s64 trusty_fast_call64(struct device *dev, u64 smcnr, u64 a0, u64 a1, u64 a2)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	if (WARN_ON(!s))
++		return SM_ERR_INVALID_PARAMETERS;
++	if (WARN_ON(!SMC_IS_FASTCALL(smcnr)))
++		return SM_ERR_INVALID_PARAMETERS;
++	if (WARN_ON(!SMC_IS_SMC64(smcnr)))
++		return SM_ERR_INVALID_PARAMETERS;
++
++	return smc(smcnr, a0, a1, a2);
++}
++EXPORT_SYMBOL(trusty_fast_call64);
++#endif
++
++static unsigned long trusty_std_call_inner(struct device *dev,
++					   unsigned long smcnr,
++					   unsigned long a0, unsigned long a1,
++					   unsigned long a2)
++{
++	unsigned long ret;
++	int retry = 5;
++
++	dev_dbg(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx)\n",
++		__func__, smcnr, a0, a1, a2);
++	while (true) {
++		ret = smc(smcnr, a0, a1, a2);
++		while ((s32)ret == SM_ERR_FIQ_INTERRUPTED)
++			ret = smc(SMC_SC_RESTART_FIQ, 0, 0, 0);
++		if ((int)ret != SM_ERR_BUSY || !retry)
++			break;
++
++		dev_dbg(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx) returned busy, retry\n",
++			__func__, smcnr, a0, a1, a2);
++		retry--;
++	}
++
++	return ret;
++}
++
++static unsigned long trusty_std_call_helper(struct device *dev,
++					    unsigned long smcnr,
++					    unsigned long a0, unsigned long a1,
++					    unsigned long a2)
++{
++	unsigned long ret;
++	int sleep_time = 1;
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	while (true) {
++		local_irq_disable();
++		atomic_notifier_call_chain(&s->notifier, TRUSTY_CALL_PREPARE,
++					   NULL);
++		ret = trusty_std_call_inner(dev, smcnr, a0, a1, a2);
++		if (ret == SM_ERR_PANIC) {
++			s->trusty_panicked = true;
++			if (IS_ENABLED(CONFIG_TRUSTY_CRASH_IS_PANIC))
++				panic("trusty crashed");
++			else
++				WARN_ONCE(1, "trusty crashed");
++		}
++
++		atomic_notifier_call_chain(&s->notifier, TRUSTY_CALL_RETURNED,
++					   NULL);
++		if (ret == SM_ERR_INTERRUPTED) {
++			/*
++			 * Make sure this cpu will eventually re-enter trusty
++			 * even if the std_call resumes on another cpu.
++			 */
++			trusty_enqueue_nop(dev, NULL);
++		}
++		local_irq_enable();
++
++		if ((int)ret != SM_ERR_BUSY)
++			break;
++
++		if (sleep_time == 256)
++			dev_warn(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx) returned busy\n",
++				 __func__, smcnr, a0, a1, a2);
++		dev_dbg(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx) returned busy, wait %d ms\n",
++			__func__, smcnr, a0, a1, a2, sleep_time);
++
++		msleep(sleep_time);
++		if (sleep_time < 1000)
++			sleep_time <<= 1;
++
++		dev_dbg(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx) retry\n",
++			__func__, smcnr, a0, a1, a2);
++	}
++
++	if (sleep_time > 256)
++		dev_warn(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx) busy cleared\n",
++			 __func__, smcnr, a0, a1, a2);
++
++	return ret;
++}
++
++static void trusty_std_call_cpu_idle(struct trusty_state *s)
++{
++	int ret;
++
++	ret = wait_for_completion_timeout(&s->cpu_idle_completion, HZ * 10);
++	if (!ret) {
++		dev_warn(s->dev,
++			 "%s: timed out waiting for cpu idle to clear, retry anyway\n",
++			 __func__);
++	}
++}
++
++s32 trusty_std_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2)
++{
++	int ret;
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	if (WARN_ON(SMC_IS_FASTCALL(smcnr)))
++		return SM_ERR_INVALID_PARAMETERS;
++
++	if (WARN_ON(SMC_IS_SMC64(smcnr)))
++		return SM_ERR_INVALID_PARAMETERS;
++
++	if (s->trusty_panicked) {
++		/*
++		 * Avoid calling the notifiers if trusty has panicked as they
++		 * can trigger more calls.
++		 */
++		return SM_ERR_PANIC;
++	}
++
++	if (smcnr != SMC_SC_NOP) {
++		mutex_lock(&s->smc_lock);
++		reinit_completion(&s->cpu_idle_completion);
++	}
++
++	dev_dbg(dev, "%s(0x%x 0x%x 0x%x 0x%x) started\n",
++		__func__, smcnr, a0, a1, a2);
++
++	ret = trusty_std_call_helper(dev, smcnr, a0, a1, a2);
++	while (ret == SM_ERR_INTERRUPTED || ret == SM_ERR_CPU_IDLE) {
++		dev_dbg(dev, "%s(0x%x 0x%x 0x%x 0x%x) interrupted\n",
++			__func__, smcnr, a0, a1, a2);
++		if (ret == SM_ERR_CPU_IDLE)
++			trusty_std_call_cpu_idle(s);
++		ret = trusty_std_call_helper(dev, SMC_SC_RESTART_LAST, 0, 0, 0);
++	}
++	dev_dbg(dev, "%s(0x%x 0x%x 0x%x 0x%x) returned 0x%x\n",
++		__func__, smcnr, a0, a1, a2, ret);
++
++	if (smcnr == SMC_SC_NOP)
++		complete(&s->cpu_idle_completion);
++	else
++		mutex_unlock(&s->smc_lock);
++
++	return ret;
++}
++EXPORT_SYMBOL(trusty_std_call32);
++
++int trusty_share_memory(struct device *dev, u64 *id,
++			struct scatterlist *sglist, unsigned int nents,
++			pgprot_t pgprot)
++{
++	return trusty_transfer_memory(dev, id, sglist, nents, pgprot, 0,
++				      false);
++}
++EXPORT_SYMBOL(trusty_share_memory);
++
++int trusty_transfer_memory(struct device *dev, u64 *id,
++			   struct scatterlist *sglist, unsigned int nents,
++			   pgprot_t pgprot, u64 tag, bool lend)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++	int ret;
++	struct ns_mem_page_info pg_inf;
++	struct scatterlist *sg;
++	size_t count;
++	size_t i;
++	size_t len;
++	u64 ffa_handle = 0;
++	size_t total_len;
++	size_t endpoint_count = 1;
++	struct ffa_mtd *mtd = s->ffa_tx;
++	size_t comp_mrd_offset = offsetof(struct ffa_mtd, emad[endpoint_count]);
++	struct ffa_comp_mrd *comp_mrd = s->ffa_tx + comp_mrd_offset;
++	struct ffa_cons_mrd *cons_mrd = comp_mrd->address_range_array;
++	size_t cons_mrd_offset = (void *)cons_mrd - s->ffa_tx;
++	struct smc_ret8 smc_ret;
++	u32 cookie_low;
++	u32 cookie_high;
++
++	if (WARN_ON(dev->driver != &trusty_driver.driver))
++		return -EINVAL;
++
++	if (WARN_ON(nents < 1))
++		return -EINVAL;
++
++	if (nents != 1 && s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
++		dev_err(s->dev, "%s: old trusty version does not support non-contiguous memory objects\n",
++			__func__);
++		return -EOPNOTSUPP;
++	}
++
++	count = dma_map_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
++	if (count != nents) {
++		dev_err(s->dev, "failed to dma map sg_table\n");
++		return -EINVAL;
++	}
++
++	sg = sglist;
++	ret = trusty_encode_page_info(&pg_inf, phys_to_page(sg_dma_address(sg)),
++				      pgprot);
++	if (ret) {
++		dev_err(s->dev, "%s: trusty_encode_page_info failed\n",
++			__func__);
++		goto err_encode_page_info;
++	}
++
++	if (s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
++		*id = pg_inf.compat_attr;
++		return 0;
++	}
++
++	len = 0;
++	for_each_sg(sglist, sg, nents, i)
++		len += sg_dma_len(sg);
++
++	mutex_lock(&s->share_memory_msg_lock);
++
++	mtd->sender_id = s->ffa_local_id;
++	mtd->memory_region_attributes = pg_inf.ffa_mem_attr;
++	mtd->reserved_3 = 0;
++	mtd->flags = 0;
++	mtd->handle = 0;
++	mtd->tag = tag;
++	mtd->reserved_24_27 = 0;
++	mtd->emad_count = endpoint_count;
++	for (i = 0; i < endpoint_count; i++) {
++		struct ffa_emad *emad = &mtd->emad[i];
++		/* TODO: support stream ids */
++		emad->mapd.endpoint_id = s->ffa_remote_id;
++		emad->mapd.memory_access_permissions = pg_inf.ffa_mem_perm;
++		emad->mapd.flags = 0;
++		emad->comp_mrd_offset = comp_mrd_offset;
++		emad->reserved_8_15 = 0;
++	}
++	comp_mrd->total_page_count = len / PAGE_SIZE;
++	comp_mrd->address_range_count = nents;
++	comp_mrd->reserved_8_15 = 0;
++
++	total_len = cons_mrd_offset + nents * sizeof(*cons_mrd);
++	sg = sglist;
++	while (count) {
++		size_t lcount =
++			min_t(size_t, count, (PAGE_SIZE - cons_mrd_offset) /
++			      sizeof(*cons_mrd));
++		size_t fragment_len = lcount * sizeof(*cons_mrd) +
++				      cons_mrd_offset;
++
++		for (i = 0; i < lcount; i++) {
++			cons_mrd[i].address = sg_dma_address(sg);
++			cons_mrd[i].page_count = sg_dma_len(sg) / PAGE_SIZE;
++			cons_mrd[i].reserved_12_15 = 0;
++			sg = sg_next(sg);
++		}
++		count -= lcount;
++		if (cons_mrd_offset) {
++			u32 smc = lend ? SMC_FC_FFA_MEM_LEND :
++					 SMC_FC_FFA_MEM_SHARE;
++			/* First fragment */
++			smc_ret = trusty_smc8(smc, total_len,
++					      fragment_len, 0, 0, 0, 0, 0);
++		} else {
++			smc_ret = trusty_smc8(SMC_FC_FFA_MEM_FRAG_TX,
++					      cookie_low, cookie_high,
++					      fragment_len, 0, 0, 0, 0);
++		}
++		if (smc_ret.r0 == SMC_FC_FFA_MEM_FRAG_RX) {
++			cookie_low = smc_ret.r1;
++			cookie_high = smc_ret.r2;
++			dev_dbg(s->dev, "cookie %x %x", cookie_low,
++				cookie_high);
++			if (!count) {
++				/*
++				 * We have sent all our descriptors. Expected
++				 * SMC_FC_FFA_SUCCESS, not a request to send
++				 * another fragment.
++				 */
++				dev_err(s->dev, "%s: fragment_len %zd/%zd, unexpected SMC_FC_FFA_MEM_FRAG_RX\n",
++					__func__, fragment_len, total_len);
++				ret = -EIO;
++				break;
++			}
++		} else if (smc_ret.r0 == SMC_FC_FFA_SUCCESS) {
++			ffa_handle = smc_ret.r2 | (u64)smc_ret.r3 << 32;
++			dev_dbg(s->dev, "%s: fragment_len %zu/%zu, got handle 0x%llx\n",
++				__func__, fragment_len, total_len,
++				ffa_handle);
++			if (count) {
++				/*
++				 * We have not sent all our descriptors.
++				 * Expected SMC_FC_FFA_MEM_FRAG_RX not
++				 * SMC_FC_FFA_SUCCESS.
++				 */
++				dev_err(s->dev, "%s: fragment_len %zu/%zu, unexpected SMC_FC_FFA_SUCCESS, count %zu != 0\n",
++					__func__, fragment_len, total_len,
++					count);
++				ret = -EIO;
++				break;
++			}
++		} else {
++			dev_err(s->dev, "%s: fragment_len %zu/%zu, SMC_FC_FFA_MEM_SHARE failed 0x%lx 0x%lx 0x%lx",
++				__func__, fragment_len, total_len,
++				smc_ret.r0, smc_ret.r1, smc_ret.r2);
++			ret = -EIO;
++			break;
++		}
++
++		cons_mrd = s->ffa_tx;
++		cons_mrd_offset = 0;
++	}
++
++	mutex_unlock(&s->share_memory_msg_lock);
++
++	if (!ret) {
++		*id = ffa_handle;
++		dev_dbg(s->dev, "%s: done\n", __func__);
++		return 0;
++	}
++
++	dev_err(s->dev, "%s: failed %d", __func__, ret);
++
++err_encode_page_info:
++	dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
++	return ret;
++}
++EXPORT_SYMBOL(trusty_transfer_memory);
++
++/*
++ * trusty_share_memory_compat - trusty_share_memory wrapper for old apis
++ *
++ * Call trusty_share_memory and filter out memory attributes if trusty version
++ * is old. Used by clients that used to pass just a physical address to trusty
++ * instead of a physical address plus memory attributes value.
++ */
++int trusty_share_memory_compat(struct device *dev, u64 *id,
++			       struct scatterlist *sglist, unsigned int nents,
++			       pgprot_t pgprot)
++{
++	int ret;
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	ret = trusty_share_memory(dev, id, sglist, nents, pgprot);
++	if (!ret && s->api_version < TRUSTY_API_VERSION_PHYS_MEM_OBJ)
++		*id &= 0x0000FFFFFFFFF000ull;
++
++	return ret;
++}
++EXPORT_SYMBOL(trusty_share_memory_compat);
++
++int trusty_reclaim_memory(struct device *dev, u64 id,
++			  struct scatterlist *sglist, unsigned int nents)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++	int ret = 0;
++	struct smc_ret8 smc_ret;
++
++	if (WARN_ON(dev->driver != &trusty_driver.driver))
++		return -EINVAL;
++
++	if (WARN_ON(nents < 1))
++		return -EINVAL;
++
++	if (s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
++		if (nents != 1) {
++			dev_err(s->dev, "%s: not supported\n", __func__);
++			return -EOPNOTSUPP;
++		}
++
++		dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
++
++		dev_dbg(s->dev, "%s: done\n", __func__);
++		return 0;
++	}
++
++	mutex_lock(&s->share_memory_msg_lock);
++
++	smc_ret = trusty_smc8(SMC_FC_FFA_MEM_RECLAIM, (u32)id, id >> 32, 0, 0,
++			      0, 0, 0);
++	if (smc_ret.r0 != SMC_FC_FFA_SUCCESS) {
++		dev_err(s->dev, "%s: SMC_FC_FFA_MEM_RECLAIM failed 0x%lx 0x%lx 0x%lx",
++			__func__, smc_ret.r0, smc_ret.r1, smc_ret.r2);
++		if (smc_ret.r0 == SMC_FC_FFA_ERROR &&
++		    smc_ret.r2 == FFA_ERROR_DENIED)
++			ret = -EBUSY;
++		else
++			ret = -EIO;
++	}
++
++	mutex_unlock(&s->share_memory_msg_lock);
++
++	if (ret != 0)
++		return ret;
++
++	dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
++
++	dev_dbg(s->dev, "%s: done\n", __func__);
++	return 0;
++}
++EXPORT_SYMBOL(trusty_reclaim_memory);
++
++int trusty_call_notifier_register(struct device *dev, struct notifier_block *n)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	return atomic_notifier_chain_register(&s->notifier, n);
++}
++EXPORT_SYMBOL(trusty_call_notifier_register);
++
++int trusty_call_notifier_unregister(struct device *dev,
++				    struct notifier_block *n)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	return atomic_notifier_chain_unregister(&s->notifier, n);
++}
++EXPORT_SYMBOL(trusty_call_notifier_unregister);
++
++static int trusty_remove_child(struct device *dev, void *data)
++{
++	platform_device_unregister(to_platform_device(dev));
++	return 0;
++}
++
++static ssize_t trusty_version_show(struct device *dev,
++				   struct device_attribute *attr, char *buf)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	return scnprintf(buf, PAGE_SIZE, "%s\n", s->version_str ?: "unknown");
++}
++
++static DEVICE_ATTR(trusty_version, 0400, trusty_version_show, NULL);
++
++static struct attribute *trusty_attrs[] = {
++	&dev_attr_trusty_version.attr,
++	NULL,
++};
++ATTRIBUTE_GROUPS(trusty);
++
++const char *trusty_version_str_get(struct device *dev)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	return s->version_str;
++}
++EXPORT_SYMBOL(trusty_version_str_get);
++
++static int trusty_init_msg_buf(struct trusty_state *s, struct device *dev)
++{
++	phys_addr_t tx_paddr;
++	phys_addr_t rx_paddr;
++	int ret;
++	struct smc_ret8 smc_ret;
++
++	if (s->api_version < TRUSTY_API_VERSION_MEM_OBJ)
++		return 0;
++
++	/* Get supported FF-A version and check if it is compatible */
++	smc_ret = trusty_smc8(SMC_FC_FFA_VERSION, FFA_CURRENT_VERSION, 0, 0,
++			      0, 0, 0, 0);
++	if (FFA_VERSION_TO_MAJOR(smc_ret.r0) != FFA_CURRENT_VERSION_MAJOR) {
++		dev_err(s->dev,
++			"%s: Unsupported FF-A version 0x%lx, expected 0x%x\n",
++			__func__, smc_ret.r0, FFA_CURRENT_VERSION);
++		ret = -EIO;
++		goto err_version;
++	}
++
++	/* Check that SMC_FC_FFA_MEM_SHARE is implemented */
++	smc_ret = trusty_smc8(SMC_FC_FFA_FEATURES, SMC_FC_FFA_MEM_SHARE, 0, 0,
++			      0, 0, 0, 0);
++	if (smc_ret.r0 != SMC_FC_FFA_SUCCESS) {
++		dev_err(s->dev,
++			"%s: SMC_FC_FFA_FEATURES(SMC_FC_FFA_MEM_SHARE) failed 0x%lx 0x%lx 0x%lx\n",
++			__func__, smc_ret.r0, smc_ret.r1, smc_ret.r2);
++		ret = -EIO;
++		goto err_features;
++	}
++
++	/*
++	 * Set FF-A endpoint IDs.
++	 *
++	 * Hardcode 0x8000 for the secure os.
++	 * TODO: Use FF-A call or device tree to configure this dynamically
++	 */
++	smc_ret = trusty_smc8(SMC_FC_FFA_ID_GET, 0, 0, 0, 0, 0, 0, 0);
++	if (smc_ret.r0 != SMC_FC_FFA_SUCCESS) {
++		dev_err(s->dev,
++			"%s: SMC_FC_FFA_ID_GET failed 0x%lx 0x%lx 0x%lx\n",
++			__func__, smc_ret.r0, smc_ret.r1, smc_ret.r2);
++		ret = -EIO;
++		goto err_id_get;
++	}
++
++	s->ffa_local_id = smc_ret.r2;
++	s->ffa_remote_id = 0x8000;
++
++	s->ffa_tx = kmalloc(PAGE_SIZE, GFP_KERNEL);
++	if (!s->ffa_tx) {
++		ret = -ENOMEM;
++		goto err_alloc_tx;
++	}
++	tx_paddr = virt_to_phys(s->ffa_tx);
++	if (WARN_ON(tx_paddr & (PAGE_SIZE - 1))) {
++		ret = -EINVAL;
++		goto err_unaligned_tx_buf;
++	}
++
++	s->ffa_rx = kmalloc(PAGE_SIZE, GFP_KERNEL);
++	if (!s->ffa_rx) {
++		ret = -ENOMEM;
++		goto err_alloc_rx;
++	}
++	rx_paddr = virt_to_phys(s->ffa_rx);
++	if (WARN_ON(rx_paddr & (PAGE_SIZE - 1))) {
++		ret = -EINVAL;
++		goto err_unaligned_rx_buf;
++	}
++
++	smc_ret = trusty_smc8(SMC_FCZ_FFA_RXTX_MAP, tx_paddr, rx_paddr, 1, 0,
++			      0, 0, 0);
++	if (smc_ret.r0 != SMC_FC_FFA_SUCCESS) {
++		dev_err(s->dev, "%s: SMC_FCZ_FFA_RXTX_MAP failed 0x%lx 0x%lx 0x%lx\n",
++			__func__, smc_ret.r0, smc_ret.r1, smc_ret.r2);
++		ret = -EIO;
++		goto err_rxtx_map;
++	}
++
++	return 0;
++
++err_rxtx_map:
++err_unaligned_rx_buf:
++	kfree(s->ffa_rx);
++	s->ffa_rx = NULL;
++err_alloc_rx:
++err_unaligned_tx_buf:
++	kfree(s->ffa_tx);
++	s->ffa_tx = NULL;
++err_alloc_tx:
++err_id_get:
++err_features:
++err_version:
++	return ret;
++}
++
++static void trusty_free_msg_buf(struct trusty_state *s, struct device *dev)
++{
++	struct smc_ret8 smc_ret;
++
++	smc_ret = trusty_smc8(SMC_FC_FFA_RXTX_UNMAP, 0, 0, 0, 0, 0, 0, 0);
++	if (smc_ret.r0 != SMC_FC_FFA_SUCCESS) {
++		dev_err(s->dev, "%s: SMC_FC_FFA_RXTX_UNMAP failed 0x%lx 0x%lx 0x%lx\n",
++			__func__, smc_ret.r0, smc_ret.r1, smc_ret.r2);
++	} else {
++		kfree(s->ffa_rx);
++		kfree(s->ffa_tx);
++	}
++}
++
++static void trusty_init_version(struct trusty_state *s, struct device *dev)
++{
++	int ret;
++	int i;
++	int version_str_len;
++
++	ret = trusty_fast_call32(dev, SMC_FC_GET_VERSION_STR, -1, 0, 0);
++	if (ret <= 0)
++		goto err_get_size;
++
++	version_str_len = ret;
++
++	s->version_str = kmalloc(version_str_len + 1, GFP_KERNEL);
++	for (i = 0; i < version_str_len; i++) {
++		ret = trusty_fast_call32(dev, SMC_FC_GET_VERSION_STR, i, 0, 0);
++		if (ret < 0)
++			goto err_get_char;
++		s->version_str[i] = ret;
++	}
++	s->version_str[i] = '\0';
++
++	dev_info(dev, "trusty version: %s\n", s->version_str);
++	return;
++
++err_get_char:
++	kfree(s->version_str);
++	s->version_str = NULL;
++err_get_size:
++	dev_err(dev, "failed to get version: %d\n", ret);
++}
++
++u32 trusty_get_api_version(struct device *dev)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	return s->api_version;
++}
++EXPORT_SYMBOL(trusty_get_api_version);
++
++bool trusty_get_panic_status(struct device *dev)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++	if (WARN_ON(dev->driver != &trusty_driver.driver))
++		return false;
++	return s->trusty_panicked;
++}
++EXPORT_SYMBOL(trusty_get_panic_status);
++
++static int trusty_init_api_version(struct trusty_state *s, struct device *dev)
++{
++	u32 api_version;
++
++	api_version = trusty_fast_call32(dev, SMC_FC_API_VERSION,
++					 TRUSTY_API_VERSION_CURRENT, 0, 0);
++	if (api_version == SM_ERR_UNDEFINED_SMC)
++		api_version = 0;
++
++	if (api_version > TRUSTY_API_VERSION_CURRENT) {
++		dev_err(dev, "unsupported api version %u > %u\n",
++			api_version, TRUSTY_API_VERSION_CURRENT);
++		return -EINVAL;
++	}
++
++	dev_info(dev, "selected api version: %u (requested %u)\n",
++		 api_version, TRUSTY_API_VERSION_CURRENT);
++	s->api_version = api_version;
++
++	return 0;
++}
++
++static bool dequeue_nop(struct trusty_state *s, u32 *args)
++{
++	unsigned long flags;
++	struct trusty_nop *nop = NULL;
++
++	spin_lock_irqsave(&s->nop_lock, flags);
++	if (!list_empty(&s->nop_queue)) {
++		nop = list_first_entry(&s->nop_queue,
++				       struct trusty_nop, node);
++		list_del_init(&nop->node);
++		args[0] = nop->args[0];
++		args[1] = nop->args[1];
++		args[2] = nop->args[2];
++	} else {
++		args[0] = 0;
++		args[1] = 0;
++		args[2] = 0;
++	}
++	spin_unlock_irqrestore(&s->nop_lock, flags);
++	return nop;
++}
++
++static void locked_nop_work_func(struct work_struct *work)
++{
++	int ret;
++	struct trusty_work *tw = container_of(work, struct trusty_work, work);
++	struct trusty_state *s = tw->ts;
++
++	ret = trusty_std_call32(s->dev, SMC_SC_LOCKED_NOP, 0, 0, 0);
++	if (ret != 0)
++		dev_err(s->dev, "%s: SMC_SC_LOCKED_NOP failed %d",
++			__func__, ret);
++
++	dev_dbg(s->dev, "%s: done\n", __func__);
++}
++
++static void nop_work_func(struct work_struct *work)
++{
++	int ret;
++	bool next;
++	u32 args[3];
++	u32 last_arg0;
++	struct trusty_work *tw = container_of(work, struct trusty_work, work);
++	struct trusty_state *s = tw->ts;
++
++	dequeue_nop(s, args);
++	do {
++		dev_dbg(s->dev, "%s: %x %x %x\n",
++			__func__, args[0], args[1], args[2]);
++
++		last_arg0 = args[0];
++		ret = trusty_std_call32(s->dev, SMC_SC_NOP,
++					args[0], args[1], args[2]);
++
++		next = dequeue_nop(s, args);
++
++		if (ret == SM_ERR_NOP_INTERRUPTED) {
++			next = true;
++		} else if (ret != SM_ERR_NOP_DONE) {
++			dev_err(s->dev, "%s: SMC_SC_NOP %x failed %d",
++				__func__, last_arg0, ret);
++			if (last_arg0) {
++				/*
++				 * Don't break out of the loop if a non-default
++				 * nop-handler returns an error.
++				 */
++				next = true;
++			}
++		}
++	} while (next);
++
++	dev_dbg(s->dev, "%s: done\n", __func__);
++}
++
++void trusty_enqueue_nop(struct device *dev, struct trusty_nop *nop)
++{
++	unsigned long flags;
++	struct trusty_work *tw;
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	preempt_disable();
++	tw = this_cpu_ptr(s->nop_works);
++	if (nop) {
++		WARN_ON(s->api_version < TRUSTY_API_VERSION_SMP_NOP);
++
++		spin_lock_irqsave(&s->nop_lock, flags);
++		if (list_empty(&nop->node))
++			list_add_tail(&nop->node, &s->nop_queue);
++		spin_unlock_irqrestore(&s->nop_lock, flags);
++	}
++	queue_work(s->nop_wq, &tw->work);
++	preempt_enable();
++}
++EXPORT_SYMBOL(trusty_enqueue_nop);
++
++void trusty_dequeue_nop(struct device *dev, struct trusty_nop *nop)
++{
++	unsigned long flags;
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	if (WARN_ON(!nop))
++		return;
++
++	spin_lock_irqsave(&s->nop_lock, flags);
++	if (!list_empty(&nop->node))
++		list_del_init(&nop->node);
++	spin_unlock_irqrestore(&s->nop_lock, flags);
++}
++EXPORT_SYMBOL(trusty_dequeue_nop);
++
++static int trusty_probe(struct platform_device *pdev)
++{
++	int ret;
++	unsigned int cpu;
++	work_func_t work_func;
++	struct trusty_state *s;
++	struct device_node *node = pdev->dev.of_node;
++
++	if (!node) {
++		dev_err(&pdev->dev, "of_node required\n");
++		return -EINVAL;
++	}
++
++	s = kzalloc(sizeof(*s), GFP_KERNEL);
++	if (!s) {
++		ret = -ENOMEM;
++		goto err_allocate_state;
++	}
++
++	s->dev = &pdev->dev;
++	spin_lock_init(&s->nop_lock);
++	INIT_LIST_HEAD(&s->nop_queue);
++	mutex_init(&s->smc_lock);
++	mutex_init(&s->share_memory_msg_lock);
++	ATOMIC_INIT_NOTIFIER_HEAD(&s->notifier);
++	init_completion(&s->cpu_idle_completion);
++
++	s->dev->dma_parms = &s->dma_parms;
++	dma_set_max_seg_size(s->dev, 0xfffff000); /* dma_parms limit */
++	/*
++	 * Set dma mask to 48 bits. This is the current limit of
++	 * trusty_encode_page_info.
++	 */
++	dma_coerce_mask_and_coherent(s->dev, DMA_BIT_MASK(48));
++
++	platform_set_drvdata(pdev, s);
++
++	trusty_init_version(s, &pdev->dev);
++
++	ret = trusty_init_api_version(s, &pdev->dev);
++	if (ret < 0)
++		goto err_api_version;
++
++	ret = trusty_init_msg_buf(s, &pdev->dev);
++	if (ret < 0)
++		goto err_init_msg_buf;
++
++	s->nop_wq = alloc_workqueue("trusty-nop-wq", WQ_CPU_INTENSIVE, 0);
++	if (!s->nop_wq) {
++		ret = -ENODEV;
++		dev_err(&pdev->dev, "Failed create trusty-nop-wq\n");
++		goto err_create_nop_wq;
++	}
++
++	s->nop_works = alloc_percpu(struct trusty_work);
++	if (!s->nop_works) {
++		ret = -ENOMEM;
++		dev_err(&pdev->dev, "Failed to allocate works\n");
++		goto err_alloc_works;
++	}
++
++	if (s->api_version < TRUSTY_API_VERSION_SMP)
++		work_func = locked_nop_work_func;
++	else
++		work_func = nop_work_func;
++
++	for_each_possible_cpu(cpu) {
++		struct trusty_work *tw = per_cpu_ptr(s->nop_works, cpu);
++
++		tw->ts = s;
++		INIT_WORK(&tw->work, work_func);
++	}
++
++	ret = of_platform_populate(pdev->dev.of_node, NULL, NULL, &pdev->dev);
++	if (ret < 0) {
++		dev_err(&pdev->dev, "Failed to add children: %d\n", ret);
++		goto err_add_children;
++	}
++
++	return 0;
++
++err_add_children:
++	for_each_possible_cpu(cpu) {
++		struct trusty_work *tw = per_cpu_ptr(s->nop_works, cpu);
++
++		flush_work(&tw->work);
++	}
++	free_percpu(s->nop_works);
++err_alloc_works:
++	destroy_workqueue(s->nop_wq);
++err_create_nop_wq:
++	trusty_free_msg_buf(s, &pdev->dev);
++err_init_msg_buf:
++err_api_version:
++	s->dev->dma_parms = NULL;
++	kfree(s->version_str);
++	device_for_each_child(&pdev->dev, NULL, trusty_remove_child);
++	mutex_destroy(&s->share_memory_msg_lock);
++	mutex_destroy(&s->smc_lock);
++	kfree(s);
++err_allocate_state:
++	return ret;
++}
++
++static int trusty_remove(struct platform_device *pdev)
++{
++	unsigned int cpu;
++	struct trusty_state *s = platform_get_drvdata(pdev);
++
++	device_for_each_child(&pdev->dev, NULL, trusty_remove_child);
++
++	for_each_possible_cpu(cpu) {
++		struct trusty_work *tw = per_cpu_ptr(s->nop_works, cpu);
++
++		flush_work(&tw->work);
++	}
++	free_percpu(s->nop_works);
++	destroy_workqueue(s->nop_wq);
++
++	mutex_destroy(&s->share_memory_msg_lock);
++	mutex_destroy(&s->smc_lock);
++	trusty_free_msg_buf(s, &pdev->dev);
++	s->dev->dma_parms = NULL;
++	kfree(s->version_str);
++	kfree(s);
++	return 0;
++}
++
++static const struct of_device_id trusty_of_match[] = {
++	{ .compatible = "android,trusty-smc-v1", },
++	{},
++};
++
++MODULE_DEVICE_TABLE(trusty, trusty_of_match);
++
++static struct platform_driver trusty_driver = {
++	.probe = trusty_probe,
++	.remove = trusty_remove,
++	.driver	= {
++		.name = "trusty",
++		.of_match_table = trusty_of_match,
++		.dev_groups = trusty_groups,
++	},
++};
++
++static int __init trusty_driver_init(void)
++{
++	return platform_driver_register(&trusty_driver);
++}
++
++static void __exit trusty_driver_exit(void)
++{
++	platform_driver_unregister(&trusty_driver);
++}
++
++subsys_initcall(trusty_driver_init);
++module_exit(trusty_driver_exit);
++
++MODULE_LICENSE("GPL v2");
++MODULE_DESCRIPTION("Trusty core driver");
+diff --git a/include/linux/trusty/arm_ffa.h b/include/linux/trusty/arm_ffa.h
+new file mode 100644
+index 000000000000..ab7b2afb794c
+--- /dev/null
++++ b/include/linux/trusty/arm_ffa.h
+@@ -0,0 +1,590 @@
++/* SPDX-License-Identifier: MIT */
++/*
++ * Copyright (C) 2020 Google, Inc.
++ *
++ * Trusty and TF-A also have a copy of this header.
++ * Please keep the copies in sync.
++ */
++#ifndef __LINUX_TRUSTY_ARM_FFA_H
++#define __LINUX_TRUSTY_ARM_FFA_H
++
++/*
++ * Subset of Arm PSA Firmware Framework for Arm v8-A 1.0 EAC 1_0
++ * (https://developer.arm.com/docs/den0077/a) needed for shared memory.
++ */
++
++#include "smcall.h"
++
++#ifndef STATIC_ASSERT
++#define STATIC_ASSERT(e) _Static_assert(e, #e)
++#endif
++
++#define FFA_CURRENT_VERSION_MAJOR (1U)
++#define FFA_CURRENT_VERSION_MINOR (0U)
++
++#define FFA_VERSION_TO_MAJOR(version) ((version) >> 16)
++#define FFA_VERSION_TO_MINOR(version) ((version) & (0xffff))
++#define FFA_VERSION(major, minor) (((major) << 16) | (minor))
++#define FFA_CURRENT_VERSION \
++	FFA_VERSION(FFA_CURRENT_VERSION_MAJOR, FFA_CURRENT_VERSION_MINOR)
++
++#define SMC_ENTITY_SHARED_MEMORY 4
++
++#define SMC_FASTCALL_NR_SHARED_MEMORY(nr) \
++	SMC_FASTCALL_NR(SMC_ENTITY_SHARED_MEMORY, nr)
++#define SMC_FASTCALL64_NR_SHARED_MEMORY(nr) \
++	SMC_FASTCALL64_NR(SMC_ENTITY_SHARED_MEMORY, nr)
++
++/**
++ * typedef ffa_endpoint_id16_t - Endpoint ID
++ *
++ * Current implementation only supports VMIDs. FFA spec also support stream
++ * endpoint ids.
++ */
++typedef uint16_t ffa_endpoint_id16_t;
++
++/**
++ * struct ffa_cons_mrd - Constituent memory region descriptor
++ * @address:
++ *         Start address of contiguous memory region. Must be 4K page aligned.
++ * @page_count:
++ *         Number of 4K pages in region.
++ * @reserved_12_15:
++ *         Reserve bytes 12-15 to pad struct size to 16 bytes.
++ */
++struct ffa_cons_mrd {
++	uint64_t address;
++	uint32_t page_count;
++	uint32_t reserved_12_15;
++};
++STATIC_ASSERT(sizeof(struct ffa_cons_mrd) == 16);
++
++/**
++ * struct ffa_comp_mrd - Composite memory region descriptor
++ * @total_page_count:
++ *         Number of 4k pages in memory region. Must match sum of
++ *         @address_range_array[].page_count.
++ * @address_range_count:
++ *         Number of entries in @address_range_array.
++ * @reserved_8_15:
++ *         Reserve bytes 8-15 to pad struct size to 16 byte alignment and
++ *         make @address_range_array 16 byte aligned.
++ * @address_range_array:
++ *         Array of &struct ffa_cons_mrd entries.
++ */
++struct ffa_comp_mrd {
++	uint32_t total_page_count;
++	uint32_t address_range_count;
++	uint64_t reserved_8_15;
++	struct ffa_cons_mrd address_range_array[];
++};
++STATIC_ASSERT(sizeof(struct ffa_comp_mrd) == 16);
++
++/**
++ * typedef ffa_mem_attr8_t - Memory region attributes
++ *
++ * * @FFA_MEM_ATTR_DEVICE_NGNRNE:
++ *     Device-nGnRnE.
++ * * @FFA_MEM_ATTR_DEVICE_NGNRE:
++ *     Device-nGnRE.
++ * * @FFA_MEM_ATTR_DEVICE_NGRE:
++ *     Device-nGRE.
++ * * @FFA_MEM_ATTR_DEVICE_GRE:
++ *     Device-GRE.
++ * * @FFA_MEM_ATTR_NORMAL_MEMORY_UNCACHED
++ *     Normal memory. Non-cacheable.
++ * * @FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB
++ *     Normal memory. Write-back cached.
++ * * @FFA_MEM_ATTR_NON_SHAREABLE
++ *     Non-shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
++ * * @FFA_MEM_ATTR_OUTER_SHAREABLE
++ *     Outer Shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
++ * * @FFA_MEM_ATTR_INNER_SHAREABLE
++ *     Inner Shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
++ */
++typedef uint8_t ffa_mem_attr8_t;
++#define FFA_MEM_ATTR_DEVICE_NGNRNE ((1U << 4) | (0x0U << 2))
++#define FFA_MEM_ATTR_DEVICE_NGNRE ((1U << 4) | (0x1U << 2))
++#define FFA_MEM_ATTR_DEVICE_NGRE ((1U << 4) | (0x2U << 2))
++#define FFA_MEM_ATTR_DEVICE_GRE ((1U << 4) | (0x3U << 2))
++#define FFA_MEM_ATTR_NORMAL_MEMORY_UNCACHED ((2U << 4) | (0x1U << 2))
++#define FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB ((2U << 4) | (0x3U << 2))
++#define FFA_MEM_ATTR_NON_SHAREABLE (0x0U << 0)
++#define FFA_MEM_ATTR_OUTER_SHAREABLE (0x2U << 0)
++#define FFA_MEM_ATTR_INNER_SHAREABLE (0x3U << 0)
++
++/**
++ * typedef ffa_mem_perm8_t - Memory access permissions
++ *
++ * * @FFA_MEM_ATTR_RO
++ *     Request or specify read-only mapping.
++ * * @FFA_MEM_ATTR_RW
++ *     Request or allow read-write mapping.
++ * * @FFA_MEM_PERM_NX
++ *     Deny executable mapping.
++ * * @FFA_MEM_PERM_X
++ *     Request executable mapping.
++ */
++typedef uint8_t ffa_mem_perm8_t;
++#define FFA_MEM_PERM_RO (1U << 0)
++#define FFA_MEM_PERM_RW (1U << 1)
++#define FFA_MEM_PERM_NX (1U << 2)
++#define FFA_MEM_PERM_X (1U << 3)
++
++/**
++ * typedef ffa_mem_flag8_t - Endpoint memory flags
++ *
++ * * @FFA_MEM_FLAG_OTHER
++ *     Other borrower. Memory region must not be or was not retrieved on behalf
++ *     of this endpoint.
++ */
++typedef uint8_t ffa_mem_flag8_t;
++#define FFA_MEM_FLAG_OTHER (1U << 0)
++
++/**
++ * typedef ffa_mtd_flag32_t - Memory transaction descriptor flags
++ *
++ * * @FFA_MTD_FLAG_ZERO_MEMORY
++ *     Zero memory after unmapping from sender (must be 0 for share).
++ * * @FFA_MTD_FLAG_TIME_SLICING
++ *     Not supported by this implementation.
++ * * @FFA_MTD_FLAG_ZERO_MEMORY_AFTER_RELINQUISH
++ *     Zero memory after unmapping from borrowers (must be 0 for share).
++ * * @FFA_MTD_FLAG_TYPE_MASK
++ *     Bit-mask to extract memory management transaction type from flags.
++ * * @FFA_MTD_FLAG_TYPE_SHARE_MEMORY
++ *     Share memory transaction flag.
++ *     Used by @SMC_FC_FFA_MEM_RETRIEVE_RESP to indicate that memory came from
++ *     @SMC_FC_FFA_MEM_SHARE and by @SMC_FC_FFA_MEM_RETRIEVE_REQ to specify that
++ *     it must have.
++ * * @FFA_MTD_FLAG_ADDRESS_RANGE_ALIGNMENT_HINT_MASK
++ *     Not supported by this implementation.
++ */
++typedef uint32_t ffa_mtd_flag32_t;
++#define FFA_MTD_FLAG_ZERO_MEMORY (1U << 0)
++#define FFA_MTD_FLAG_TIME_SLICING (1U << 1)
++#define FFA_MTD_FLAG_ZERO_MEMORY_AFTER_RELINQUISH (1U << 2)
++#define FFA_MTD_FLAG_TYPE_MASK (3U << 3)
++#define FFA_MTD_FLAG_TYPE_SHARE_MEMORY (1U << 3)
++#define FFA_MTD_FLAG_ADDRESS_RANGE_ALIGNMENT_HINT_MASK (0x1FU << 5)
++
++/**
++ * struct ffa_mapd - Memory access permissions descriptor
++ * @endpoint_id:
++ *         Endpoint id that @memory_access_permissions and @flags apply to.
++ *         (&typedef ffa_endpoint_id16_t).
++ * @memory_access_permissions:
++ *         FFA_MEM_PERM_* values or'ed together (&typedef ffa_mem_perm8_t).
++ * @flags:
++ *         FFA_MEM_FLAG_* values or'ed together (&typedef ffa_mem_flag8_t).
++ */
++struct ffa_mapd {
++	ffa_endpoint_id16_t endpoint_id;
++	ffa_mem_perm8_t memory_access_permissions;
++	ffa_mem_flag8_t flags;
++};
++STATIC_ASSERT(sizeof(struct ffa_mapd) == 4);
++
++/**
++ * struct ffa_emad - Endpoint memory access descriptor.
++ * @mapd:  &struct ffa_mapd.
++ * @comp_mrd_offset:
++ *         Offset of &struct ffa_comp_mrd form start of &struct ffa_mtd.
++ * @reserved_8_15:
++ *         Reserved bytes 8-15. Must be 0.
++ */
++struct ffa_emad {
++	struct ffa_mapd mapd;
++	uint32_t comp_mrd_offset;
++	uint64_t reserved_8_15;
++};
++STATIC_ASSERT(sizeof(struct ffa_emad) == 16);
++
++/**
++ * struct ffa_mtd - Memory transaction descriptor.
++ * @sender_id:
++ *         Sender endpoint id.
++ * @memory_region_attributes:
++ *         FFA_MEM_ATTR_* values or'ed together (&typedef ffa_mem_attr8_t).
++ * @reserved_3:
++ *         Reserved bytes 3. Must be 0.
++ * @flags:
++ *         FFA_MTD_FLAG_* values or'ed together (&typedef ffa_mtd_flag32_t).
++ * @handle:
++ *         Id of shared memory object. Most be 0 for MEM_SHARE.
++ * @tag:   Client allocated tag. Must match original value.
++ * @reserved_24_27:
++ *         Reserved bytes 24-27. Must be 0.
++ * @emad_count:
++ *         Number of entries in @emad. Must be 1 in current implementation.
++ *         FFA spec allows more entries.
++ * @emad:
++ *         Endpoint memory access descriptor array (see @struct ffa_emad).
++ */
++struct ffa_mtd {
++	ffa_endpoint_id16_t sender_id;
++	ffa_mem_attr8_t memory_region_attributes;
++	uint8_t reserved_3;
++	ffa_mtd_flag32_t flags;
++	uint64_t handle;
++	uint64_t tag;
++	uint32_t reserved_24_27;
++	uint32_t emad_count;
++	struct ffa_emad emad[];
++};
++STATIC_ASSERT(sizeof(struct ffa_mtd) == 32);
++
++/**
++ * struct ffa_mem_relinquish_descriptor - Relinquish request descriptor.
++ * @handle:
++ *         Id of shared memory object to relinquish.
++ * @flags:
++ *         If bit 0 is set clear memory after unmapping from borrower. Must be 0
++ *         for share. Bit[1]: Time slicing. Not supported, must be 0. All other
++ *         bits are reserved 0.
++ * @endpoint_count:
++ *         Number of entries in @endpoint_array.
++ * @endpoint_array:
++ *         Array of endpoint ids.
++ */
++struct ffa_mem_relinquish_descriptor {
++	uint64_t handle;
++	uint32_t flags;
++	uint32_t endpoint_count;
++	ffa_endpoint_id16_t endpoint_array[];
++};
++STATIC_ASSERT(sizeof(struct ffa_mem_relinquish_descriptor) == 16);
++
++/**
++ * enum ffa_error - FF-A error code
++ * @FFA_ERROR_NOT_SUPPORTED:
++ *         Operation contained possibly valid parameters not supported by the
++ *         current implementation. Does not match FF-A 1.0 EAC 1_0 definition.
++ * @FFA_ERROR_INVALID_PARAMETERS:
++ *         Invalid parameters. Conditions function specific.
++ * @FFA_ERROR_NO_MEMORY:
++ *         Not enough memory.
++ * @FFA_ERROR_DENIED:
++ *         Operation not allowed. Conditions function specific.
++ *
++ * FF-A 1.0 EAC 1_0 defines other error codes as well but the current
++ * implementation does not use them.
++ */
++enum ffa_error {
++	FFA_ERROR_NOT_SUPPORTED = -1,
++	FFA_ERROR_INVALID_PARAMETERS = -2,
++	FFA_ERROR_NO_MEMORY = -3,
++	FFA_ERROR_DENIED = -6,
++};
++
++/**
++ * SMC_FC32_FFA_MIN - First 32 bit SMC opcode reserved for FFA
++ */
++#define SMC_FC32_FFA_MIN SMC_FASTCALL_NR_SHARED_MEMORY(0x60)
++
++/**
++ * SMC_FC32_FFA_MAX - Last 32 bit SMC opcode reserved for FFA
++ */
++#define SMC_FC32_FFA_MAX SMC_FASTCALL_NR_SHARED_MEMORY(0x7F)
++
++/**
++ * SMC_FC64_FFA_MIN - First 64 bit SMC opcode reserved for FFA
++ */
++#define SMC_FC64_FFA_MIN SMC_FASTCALL64_NR_SHARED_MEMORY(0x60)
++
++/**
++ * SMC_FC64_FFA_MAX - Last 64 bit SMC opcode reserved for FFA
++ */
++#define SMC_FC64_FFA_MAX SMC_FASTCALL64_NR_SHARED_MEMORY(0x7F)
++
++/**
++ * SMC_FC_FFA_ERROR - SMC error return opcode
++ *
++ * Register arguments:
++ *
++ * * w1:     VMID in [31:16], vCPU in [15:0]
++ * * w2:     Error code (&enum ffa_error)
++ */
++#define SMC_FC_FFA_ERROR SMC_FASTCALL_NR_SHARED_MEMORY(0x60)
++
++/**
++ * SMC_FC_FFA_SUCCESS - 32 bit SMC success return opcode
++ *
++ * Register arguments:
++ *
++ * * w1:     VMID in [31:16], vCPU in [15:0]
++ * * w2-w7:  Function specific
++ */
++#define SMC_FC_FFA_SUCCESS SMC_FASTCALL_NR_SHARED_MEMORY(0x61)
++
++/**
++ * SMC_FC64_FFA_SUCCESS - 64 bit SMC success return opcode
++ *
++ * Register arguments:
++ *
++ * * w1:             VMID in [31:16], vCPU in [15:0]
++ * * w2/x2-w7/x7:    Function specific
++ */
++#define SMC_FC64_FFA_SUCCESS SMC_FASTCALL64_NR_SHARED_MEMORY(0x61)
++
++/**
++ * SMC_FC_FFA_VERSION - SMC opcode to return supported FF-A version
++ *
++ * Register arguments:
++ *
++ * * w1:     Major version bit[30:16] and minor version in bit[15:0] supported
++ *           by caller. Bit[31] must be 0.
++ *
++ * Return:
++ * * w0:     &SMC_FC_FFA_SUCCESS
++ * * w2:     Major version bit[30:16], minor version in bit[15:0], bit[31] must
++ *           be 0.
++ *
++ * or
++ *
++ * * w0:     SMC_FC_FFA_ERROR
++ * * w2:     FFA_ERROR_NOT_SUPPORTED if major version passed in is less than the
++ *           minimum major version supported.
++ */
++#define SMC_FC_FFA_VERSION SMC_FASTCALL_NR_SHARED_MEMORY(0x63)
++
++/**
++ * SMC_FC_FFA_FEATURES - SMC opcode to check optional feature support
++ *
++ * Register arguments:
++ *
++ * * w1:     FF-A function ID
++ *
++ * Return:
++ * * w0:     &SMC_FC_FFA_SUCCESS
++ * * w2:     Bit[0]: Supports custom buffers for memory transactions.
++ *           Bit[1:0]: For RXTX_MAP min buffer size and alignment boundary.
++ *           Other bits must be 0.
++ * * w3:     For FFA_MEM_RETRIEVE_REQ, bit[7-0]: Number of times receiver can
++ *           retrieve each memory region before relinquishing it specified as
++ *           ((1U << (value + 1)) - 1 (or value = bits in reference count - 1).
++ *           For all other bits and commands: must be 0.
++ * or
++ *
++ * * w0:     SMC_FC_FFA_ERROR
++ * * w2:     FFA_ERROR_NOT_SUPPORTED if function is not implemented, or
++ *           FFA_ERROR_INVALID_PARAMETERS if function id is not valid.
++ */
++#define SMC_FC_FFA_FEATURES SMC_FASTCALL_NR_SHARED_MEMORY(0x64)
++
++/**
++ * SMC_FC_FFA_RXTX_MAP - 32 bit SMC opcode to map message buffers
++ *
++ * Register arguments:
++ *
++ * * w1:     TX address
++ * * w2:     RX address
++ * * w3:     RX/TX page count in bit[5:0]
++ *
++ * Return:
++ * * w0:     &SMC_FC_FFA_SUCCESS
++ */
++#define SMC_FC_FFA_RXTX_MAP SMC_FASTCALL_NR_SHARED_MEMORY(0x66)
++
++/**
++ * SMC_FC64_FFA_RXTX_MAP - 64 bit SMC opcode to map message buffers
++ *
++ * Register arguments:
++ *
++ * * x1:     TX address
++ * * x2:     RX address
++ * * x3:     RX/TX page count in bit[5:0]
++ *
++ * Return:
++ * * w0:     &SMC_FC_FFA_SUCCESS
++ */
++#define SMC_FC64_FFA_RXTX_MAP SMC_FASTCALL64_NR_SHARED_MEMORY(0x66)
++#ifdef CONFIG_64BIT
++#define SMC_FCZ_FFA_RXTX_MAP SMC_FC64_FFA_RXTX_MAP
++#else
++#define SMC_FCZ_FFA_RXTX_MAP SMC_FC_FFA_RXTX_MAP
++#endif
++
++/**
++ * SMC_FC_FFA_RXTX_UNMAP - SMC opcode to unmap message buffers
++ *
++ * Register arguments:
++ *
++ * * w1:     ID in [31:16]
++ *
++ * Return:
++ * * w0:     &SMC_FC_FFA_SUCCESS
++ */
++#define SMC_FC_FFA_RXTX_UNMAP SMC_FASTCALL_NR_SHARED_MEMORY(0x67)
++
++/**
++ * SMC_FC_FFA_ID_GET - SMC opcode to get endpoint id of caller
++ *
++ * Return:
++ * * w0:     &SMC_FC_FFA_SUCCESS
++ * * w2:     ID in bit[15:0], bit[31:16] must be 0.
++ */
++#define SMC_FC_FFA_ID_GET SMC_FASTCALL_NR_SHARED_MEMORY(0x69)
++
++/**
++ * SMC_FC_FFA_MEM_DONATE - 32 bit SMC opcode to donate memory
++ *
++ * Not supported.
++ */
++#define SMC_FC_FFA_MEM_DONATE SMC_FASTCALL_NR_SHARED_MEMORY(0x71)
++
++/**
++ * SMC_FC_FFA_MEM_LEND - 32 bit SMC opcode to lend memory
++ *
++ * Not currently supported.
++ */
++#define SMC_FC_FFA_MEM_LEND SMC_FASTCALL_NR_SHARED_MEMORY(0x72)
++
++/**
++ * SMC_FC_FFA_MEM_SHARE - 32 bit SMC opcode to share memory
++ *
++ * Register arguments:
++ *
++ * * w1:     Total length
++ * * w2:     Fragment length
++ * * w3:     Address
++ * * w4:     Page count
++ *
++ * Return:
++ * * w0:     &SMC_FC_FFA_SUCCESS
++ * * w2/w3:  Handle
++ *
++ * or
++ *
++ * * w0:     &SMC_FC_FFA_MEM_FRAG_RX
++ * * w1-:    See &SMC_FC_FFA_MEM_FRAG_RX
++ *
++ * or
++ *
++ * * w0:     SMC_FC_FFA_ERROR
++ * * w2:     Error code (&enum ffa_error)
++ */
++#define SMC_FC_FFA_MEM_SHARE SMC_FASTCALL_NR_SHARED_MEMORY(0x73)
++
++/**
++ * SMC_FC64_FFA_MEM_SHARE - 64 bit SMC opcode to share memory
++ *
++ * Register arguments:
++ *
++ * * w1:     Total length
++ * * w2:     Fragment length
++ * * x3:     Address
++ * * w4:     Page count
++ *
++ * Return:
++ * * w0:     &SMC_FC_FFA_SUCCESS
++ * * w2/w3:  Handle
++ *
++ * or
++ *
++ * * w0:     &SMC_FC_FFA_MEM_FRAG_RX
++ * * w1-:    See &SMC_FC_FFA_MEM_FRAG_RX
++ *
++ * or
++ *
++ * * w0:     SMC_FC_FFA_ERROR
++ * * w2:     Error code (&enum ffa_error)
++ */
++#define SMC_FC64_FFA_MEM_SHARE SMC_FASTCALL64_NR_SHARED_MEMORY(0x73)
++
++/**
++ * SMC_FC_FFA_MEM_RETRIEVE_REQ - 32 bit SMC opcode to retrieve shared memory
++ *
++ * Register arguments:
++ *
++ * * w1:     Total length
++ * * w2:     Fragment length
++ * * w3:     Address
++ * * w4:     Page count
++ *
++ * Return:
++ * * w0:             &SMC_FC_FFA_MEM_RETRIEVE_RESP
++ * * w1/x1-w5/x5:    See &SMC_FC_FFA_MEM_RETRIEVE_RESP
++ */
++#define SMC_FC_FFA_MEM_RETRIEVE_REQ SMC_FASTCALL_NR_SHARED_MEMORY(0x74)
++
++/**
++ * SMC_FC64_FFA_MEM_RETRIEVE_REQ - 64 bit SMC opcode to retrieve shared memory
++ *
++ * Register arguments:
++ *
++ * * w1:     Total length
++ * * w2:     Fragment length
++ * * x3:     Address
++ * * w4:     Page count
++ *
++ * Return:
++ * * w0:             &SMC_FC_FFA_MEM_RETRIEVE_RESP
++ * * w1/x1-w5/x5:    See &SMC_FC_FFA_MEM_RETRIEVE_RESP
++ */
++#define SMC_FC64_FFA_MEM_RETRIEVE_REQ SMC_FASTCALL64_NR_SHARED_MEMORY(0x74)
++
++/**
++ * SMC_FC_FFA_MEM_RETRIEVE_RESP - Retrieve 32 bit SMC return opcode
++ *
++ * Register arguments:
++ *
++ * * w1:     Total length
++ * * w2:     Fragment length
++ */
++#define SMC_FC_FFA_MEM_RETRIEVE_RESP SMC_FASTCALL_NR_SHARED_MEMORY(0x75)
++
++/**
++ * SMC_FC_FFA_MEM_RELINQUISH - SMC opcode to relinquish shared memory
++ *
++ * Input in &struct ffa_mem_relinquish_descriptor format in message buffer.
++ *
++ * Return:
++ * * w0:     &SMC_FC_FFA_SUCCESS
++ */
++#define SMC_FC_FFA_MEM_RELINQUISH SMC_FASTCALL_NR_SHARED_MEMORY(0x76)
++
++/**
++ * SMC_FC_FFA_MEM_RECLAIM - SMC opcode to reclaim shared memory
++ *
++ * Register arguments:
++ *
++ * * w1/w2:  Handle
++ * * w3:     Flags
++ *
++ * Return:
++ * * w0:     &SMC_FC_FFA_SUCCESS
++ */
++#define SMC_FC_FFA_MEM_RECLAIM SMC_FASTCALL_NR_SHARED_MEMORY(0x77)
++
++/**
++ * SMC_FC_FFA_MEM_FRAG_RX - SMC opcode to request next fragment.
++ *
++ * Register arguments:
++ *
++ * * w1/w2:  Cookie
++ * * w3:     Fragment offset.
++ * * w4:     Endpoint id ID in [31:16], if client is hypervisor.
++ *
++ * Return:
++ * * w0:             &SMC_FC_FFA_MEM_FRAG_TX
++ * * w1/x1-w5/x5:    See &SMC_FC_FFA_MEM_FRAG_TX
++ */
++#define SMC_FC_FFA_MEM_FRAG_RX SMC_FASTCALL_NR_SHARED_MEMORY(0x7A)
++
++/**
++ * SMC_FC_FFA_MEM_FRAG_TX - SMC opcode to transmit next fragment
++ *
++ * Register arguments:
++ *
++ * * w1/w2:  Cookie
++ * * w3:     Fragment length.
++ * * w4:     Sender endpoint id ID in [31:16], if client is hypervisor.
++ *
++ * Return:
++ * * w0:             &SMC_FC_FFA_MEM_FRAG_RX or &SMC_FC_FFA_SUCCESS.
++ * * w1/x1-w5/x5:    See opcode in w0.
++ */
++#define SMC_FC_FFA_MEM_FRAG_TX SMC_FASTCALL_NR_SHARED_MEMORY(0x7B)
++
++#endif /* __LINUX_TRUSTY_ARM_FFA_H */
+diff --git a/include/linux/trusty/sm_err.h b/include/linux/trusty/sm_err.h
+new file mode 100644
+index 000000000000..f6504448c6c3
+--- /dev/null
++++ b/include/linux/trusty/sm_err.h
+@@ -0,0 +1,28 @@
++/* SPDX-License-Identifier: MIT */
++/*
++ * Copyright (c) 2013 Google Inc. All rights reserved
++ *
++ * Trusty and TF-A also have a copy of this header.
++ * Please keep the copies in sync.
++ */
++#ifndef __LINUX_TRUSTY_SM_ERR_H
++#define __LINUX_TRUSTY_SM_ERR_H
++
++/* Errors from the secure monitor */
++#define SM_ERR_UNDEFINED_SMC		0xFFFFFFFF /* Unknown SMC (defined by ARM DEN 0028A(0.9.0) */
++#define SM_ERR_INVALID_PARAMETERS	-2
++#define SM_ERR_INTERRUPTED		-3	/* Got interrupted. Call back with restart SMC */
++#define SM_ERR_UNEXPECTED_RESTART	-4	/* Got an restart SMC when we didn't expect it */
++#define SM_ERR_BUSY			-5	/* Temporarily busy. Call back with original args */
++#define SM_ERR_INTERLEAVED_SMC		-6	/* Got a trusted_service SMC when a restart SMC is required */
++#define SM_ERR_INTERNAL_FAILURE		-7	/* Unknown error */
++#define SM_ERR_NOT_SUPPORTED		-8
++#define SM_ERR_NOT_ALLOWED		-9	/* SMC call not allowed */
++#define SM_ERR_END_OF_INPUT		-10
++#define SM_ERR_PANIC			-11	/* Secure OS crashed */
++#define SM_ERR_FIQ_INTERRUPTED		-12	/* Got interrupted by FIQ. Call back with SMC_SC_RESTART_FIQ on same CPU */
++#define SM_ERR_CPU_IDLE			-13	/* SMC call waiting for another CPU */
++#define SM_ERR_NOP_INTERRUPTED		-14	/* Got interrupted. Call back with new SMC_SC_NOP */
++#define SM_ERR_NOP_DONE			-15	/* Cpu idle after SMC_SC_NOP (not an error) */
++
++#endif
+diff --git a/include/linux/trusty/smcall.h b/include/linux/trusty/smcall.h
+new file mode 100644
+index 000000000000..aea3f6068593
+--- /dev/null
++++ b/include/linux/trusty/smcall.h
+@@ -0,0 +1,124 @@
++/* SPDX-License-Identifier: MIT */
++/*
++ * Copyright (c) 2013-2014 Google Inc. All rights reserved
++ *
++ * Trusty and TF-A also have a copy of this header.
++ * Please keep the copies in sync.
++ */
++#ifndef __LINUX_TRUSTY_SMCALL_H
++#define __LINUX_TRUSTY_SMCALL_H
++
++#define SMC_NUM_ENTITIES	64
++#define SMC_NUM_ARGS		4
++#define SMC_NUM_PARAMS		(SMC_NUM_ARGS - 1)
++
++#define SMC_IS_FASTCALL(smc_nr)	((smc_nr) & 0x80000000)
++#define SMC_IS_SMC64(smc_nr)	((smc_nr) & 0x40000000)
++#define SMC_ENTITY(smc_nr)	(((smc_nr) & 0x3F000000) >> 24)
++#define SMC_FUNCTION(smc_nr)	((smc_nr) & 0x0000FFFF)
++
++#define SMC_NR(entity, fn, fastcall, smc64) ((((fastcall) & 0x1U) << 31) | \
++					     (((smc64) & 0x1U) << 30) | \
++					     (((entity) & 0x3FU) << 24) | \
++					     ((fn) & 0xFFFFU) \
++					    )
++
++#define SMC_FASTCALL_NR(entity, fn)	SMC_NR((entity), (fn), 1, 0)
++#define SMC_STDCALL_NR(entity, fn)	SMC_NR((entity), (fn), 0, 0)
++#define SMC_FASTCALL64_NR(entity, fn)	SMC_NR((entity), (fn), 1, 1)
++#define SMC_STDCALL64_NR(entity, fn)	SMC_NR((entity), (fn), 0, 1)
++
++#define	SMC_ENTITY_ARCH			0	/* ARM Architecture calls */
++#define	SMC_ENTITY_CPU			1	/* CPU Service calls */
++#define	SMC_ENTITY_SIP			2	/* SIP Service calls */
++#define	SMC_ENTITY_OEM			3	/* OEM Service calls */
++#define	SMC_ENTITY_STD			4	/* Standard Service calls */
++#define	SMC_ENTITY_RESERVED		5	/* Reserved for future use */
++#define	SMC_ENTITY_TRUSTED_APP		48	/* Trusted Application calls */
++#define	SMC_ENTITY_TRUSTED_OS		50	/* Trusted OS calls */
++#define	SMC_ENTITY_LOGGING		51	/* Used for secure -> nonsecure logging */
++#define	SMC_ENTITY_TEST			52	/* Used for secure -> nonsecure tests */
++#define	SMC_ENTITY_SECURE_MONITOR	60	/* Trusted OS calls internal to secure monitor */
++
++/* FC = Fast call, SC = Standard call */
++#define SMC_SC_RESTART_LAST	SMC_STDCALL_NR(SMC_ENTITY_SECURE_MONITOR, 0)
++#define SMC_SC_LOCKED_NOP	SMC_STDCALL_NR(SMC_ENTITY_SECURE_MONITOR, 1)
++
++/**
++ * SMC_SC_RESTART_FIQ - Re-enter trusty after it was interrupted by an fiq
++ *
++ * No arguments, no return value.
++ *
++ * Re-enter trusty after returning to ns to process an fiq. Must be called iff
++ * trusty returns SM_ERR_FIQ_INTERRUPTED.
++ *
++ * Enable by selecting api version TRUSTY_API_VERSION_RESTART_FIQ (1) or later.
++ */
++#define SMC_SC_RESTART_FIQ	SMC_STDCALL_NR(SMC_ENTITY_SECURE_MONITOR, 2)
++
++/**
++ * SMC_SC_NOP - Enter trusty to run pending work.
++ *
++ * No arguments.
++ *
++ * Returns SM_ERR_NOP_INTERRUPTED or SM_ERR_NOP_DONE.
++ * If SM_ERR_NOP_INTERRUPTED is returned, the call must be repeated.
++ *
++ * Enable by selecting api version TRUSTY_API_VERSION_SMP (2) or later.
++ */
++#define SMC_SC_NOP		SMC_STDCALL_NR(SMC_ENTITY_SECURE_MONITOR, 3)
++
++/*
++ * Return from secure os to non-secure os with return value in r1
++ */
++#define SMC_SC_NS_RETURN	SMC_STDCALL_NR(SMC_ENTITY_SECURE_MONITOR, 0)
++
++#define SMC_FC_RESERVED		SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 0)
++#define SMC_FC_FIQ_EXIT		SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 1)
++#define SMC_FC_REQUEST_FIQ	SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 2)
++
++#define TRUSTY_IRQ_TYPE_NORMAL		(0)
++#define TRUSTY_IRQ_TYPE_PER_CPU		(1)
++#define TRUSTY_IRQ_TYPE_DOORBELL	(2)
++#define SMC_FC_GET_NEXT_IRQ	SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 3)
++
++#define SMC_FC_CPU_SUSPEND	SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 7)
++#define SMC_FC_CPU_RESUME	SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 8)
++
++#define SMC_FC_AARCH_SWITCH	SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 9)
++#define SMC_FC_GET_VERSION_STR	SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 10)
++
++/**
++ * SMC_FC_API_VERSION - Find and select supported API version.
++ *
++ * @r1: Version supported by client.
++ *
++ * Returns version supported by trusty.
++ *
++ * If multiple versions are supported, the client should start by calling
++ * SMC_FC_API_VERSION with the largest version it supports. Trusty will then
++ * return a version it supports. If the client does not support the version
++ * returned by trusty and the version returned is less than the version
++ * requested, repeat the call with the largest supported version less than the
++ * last returned version.
++ *
++ * This call must be made before any calls that are affected by the api version.
++ */
++#define TRUSTY_API_VERSION_RESTART_FIQ	(1)
++#define TRUSTY_API_VERSION_SMP		(2)
++#define TRUSTY_API_VERSION_SMP_NOP	(3)
++#define TRUSTY_API_VERSION_PHYS_MEM_OBJ	(4)
++#define TRUSTY_API_VERSION_MEM_OBJ	(5)
++#define TRUSTY_API_VERSION_CURRENT	(5)
++#define SMC_FC_API_VERSION	SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 11)
++
++/* TRUSTED_OS entity calls */
++#define SMC_SC_VIRTIO_GET_DESCR	SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 20)
++#define SMC_SC_VIRTIO_START	SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 21)
++#define SMC_SC_VIRTIO_STOP	SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 22)
++
++#define SMC_SC_VDEV_RESET	SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 23)
++#define SMC_SC_VDEV_KICK_VQ	SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 24)
++#define SMC_NC_VDEV_KICK_VQ	SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 25)
++
++#endif /* __LINUX_TRUSTY_SMCALL_H */
+diff --git a/include/linux/trusty/trusty.h b/include/linux/trusty/trusty.h
+new file mode 100644
+index 000000000000..efbb36999a8b
+--- /dev/null
++++ b/include/linux/trusty/trusty.h
+@@ -0,0 +1,131 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Copyright (C) 2013 Google, Inc.
++ */
++#ifndef __LINUX_TRUSTY_TRUSTY_H
++#define __LINUX_TRUSTY_TRUSTY_H
++
++#include <linux/kernel.h>
++#include <linux/trusty/sm_err.h>
++#include <linux/types.h>
++#include <linux/device.h>
++#include <linux/pagemap.h>
++
++
++#if IS_ENABLED(CONFIG_TRUSTY)
++s32 trusty_std_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2);
++s32 trusty_fast_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2);
++#ifdef CONFIG_64BIT
++s64 trusty_fast_call64(struct device *dev, u64 smcnr, u64 a0, u64 a1, u64 a2);
++#endif
++#else
++static inline s32 trusty_std_call32(struct device *dev, u32 smcnr,
++				    u32 a0, u32 a1, u32 a2)
++{
++	return SM_ERR_UNDEFINED_SMC;
++}
++static inline s32 trusty_fast_call32(struct device *dev, u32 smcnr,
++				     u32 a0, u32 a1, u32 a2)
++{
++	return SM_ERR_UNDEFINED_SMC;
++}
++#ifdef CONFIG_64BIT
++static inline s64 trusty_fast_call64(struct device *dev,
++				     u64 smcnr, u64 a0, u64 a1, u64 a2)
++{
++	return SM_ERR_UNDEFINED_SMC;
++}
++#endif
++#endif
++
++struct notifier_block;
++enum {
++	TRUSTY_CALL_PREPARE,
++	TRUSTY_CALL_RETURNED,
++};
++int trusty_call_notifier_register(struct device *dev,
++				  struct notifier_block *n);
++int trusty_call_notifier_unregister(struct device *dev,
++				    struct notifier_block *n);
++const char *trusty_version_str_get(struct device *dev);
++u32 trusty_get_api_version(struct device *dev);
++bool trusty_get_panic_status(struct device *dev);
++
++struct ns_mem_page_info {
++	u64 paddr;
++	u8 ffa_mem_attr;
++	u8 ffa_mem_perm;
++	u64 compat_attr;
++};
++
++int trusty_encode_page_info(struct ns_mem_page_info *inf,
++			    struct page *page, pgprot_t pgprot);
++
++struct scatterlist;
++typedef u64 trusty_shared_mem_id_t;
++int trusty_share_memory(struct device *dev, trusty_shared_mem_id_t *id,
++			struct scatterlist *sglist, unsigned int nents,
++			pgprot_t pgprot);
++int trusty_share_memory_compat(struct device *dev, trusty_shared_mem_id_t *id,
++			       struct scatterlist *sglist, unsigned int nents,
++			       pgprot_t pgprot);
++int trusty_transfer_memory(struct device *dev, u64 *id,
++			   struct scatterlist *sglist, unsigned int nents,
++			   pgprot_t pgprot, u64 tag, bool lend);
++int trusty_reclaim_memory(struct device *dev, trusty_shared_mem_id_t id,
++			  struct scatterlist *sglist, unsigned int nents);
++
++struct dma_buf;
++#ifdef CONFIG_TRUSTY_DMA_BUF_FFA_TAG
++u64 trusty_dma_buf_get_ffa_tag(struct dma_buf *dma_buf);
++#else
++static inline u64 trusty_dma_buf_get_ffa_tag(struct dma_buf *dma_buf)
++{
++	return 0;
++}
++#endif
++
++/* Invalid handle value is defined by FF-A spec */
++#ifdef CONFIG_TRUSTY_DMA_BUF_SHARED_MEM_ID
++/**
++ * trusty_dma_buf_get_shared_mem_id() - Get memory ID corresponding to a dma_buf
++ * @dma_buf: DMA buffer
++ * @id:      Pointer to output trusty_shared_mem_id_t
++ *
++ * Sets @id to trusty_shared_mem_id_t corresponding to the given @dma_buf.
++ * @dma_buf "owns" the ID, i.e. is responsible for allocating/releasing it.
++ * @dma_buf with an allocated @id must be in secure memory and should only be
++ * sent to Trusty using TRUSTY_SEND_SECURE.
++ *
++ * Return:
++ * * 0        - success
++ * * -ENODATA - @dma_buf does not own a trusty_shared_mem_id_t
++ * * ...      - @dma_buf should not be lent or shared
++ */
++int trusty_dma_buf_get_shared_mem_id(struct dma_buf *dma_buf,
++				     trusty_shared_mem_id_t *id);
++#else
++static inline int trusty_dma_buf_get_shared_mem_id(struct dma_buf *dma_buf,
++						   trusty_shared_mem_id_t *id)
++{
++	return -ENODATA;
++}
++#endif
++
++struct trusty_nop {
++	struct list_head node;
++	u32 args[3];
++};
++
++static inline void trusty_nop_init(struct trusty_nop *nop,
++				   u32 arg0, u32 arg1, u32 arg2) {
++	INIT_LIST_HEAD(&nop->node);
++	nop->args[0] = arg0;
++	nop->args[1] = arg1;
++	nop->args[2] = arg2;
++}
++
++void trusty_enqueue_nop(struct device *dev, struct trusty_nop *nop);
++void trusty_dequeue_nop(struct device *dev, struct trusty_nop *nop);
++
++#endif
+diff --git a/include/linux/trusty/trusty_ipc.h b/include/linux/trusty/trusty_ipc.h
+new file mode 100644
+index 000000000000..9386392f3a64
+--- /dev/null
++++ b/include/linux/trusty/trusty_ipc.h
+@@ -0,0 +1,89 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Copyright (C) 2015 Google, Inc.
++ */
++#ifndef __LINUX_TRUSTY_TRUSTY_IPC_H
++#define __LINUX_TRUSTY_TRUSTY_IPC_H
++
++#include <linux/list.h>
++#include <linux/scatterlist.h>
++#include <linux/trusty/trusty.h>
++#include <linux/types.h>
++
++struct tipc_chan;
++
++struct tipc_msg_buf {
++	void *buf_va;
++	struct scatterlist sg;
++	trusty_shared_mem_id_t buf_id;
++	size_t buf_sz;
++	size_t wpos;
++	size_t rpos;
++	size_t shm_cnt;
++	struct list_head node;
++};
++
++enum tipc_chan_event {
++	TIPC_CHANNEL_CONNECTED = 1,
++	TIPC_CHANNEL_DISCONNECTED,
++	TIPC_CHANNEL_SHUTDOWN,
++};
++
++struct tipc_chan_ops {
++	void (*handle_event)(void *cb_arg, int event);
++	struct tipc_msg_buf *(*handle_msg)(void *cb_arg,
++					   struct tipc_msg_buf *mb);
++	void (*handle_release)(void *cb_arg);
++};
++
++struct tipc_chan *tipc_create_channel(struct device *dev,
++				      const struct tipc_chan_ops *ops,
++				      void *cb_arg);
++
++int tipc_chan_connect(struct tipc_chan *chan, const char *port);
++
++int tipc_chan_queue_msg(struct tipc_chan *chan, struct tipc_msg_buf *mb);
++
++int tipc_chan_shutdown(struct tipc_chan *chan);
++
++void tipc_chan_destroy(struct tipc_chan *chan);
++
++struct tipc_msg_buf *tipc_chan_get_rxbuf(struct tipc_chan *chan);
++
++void tipc_chan_put_rxbuf(struct tipc_chan *chan, struct tipc_msg_buf *mb);
++
++struct tipc_msg_buf *
++tipc_chan_get_txbuf_timeout(struct tipc_chan *chan, long timeout);
++
++void tipc_chan_put_txbuf(struct tipc_chan *chan, struct tipc_msg_buf *mb);
++
++static inline size_t mb_avail_space(struct tipc_msg_buf *mb)
++{
++	return mb->buf_sz - mb->wpos;
++}
++
++static inline size_t mb_avail_data(struct tipc_msg_buf *mb)
++{
++	return mb->wpos - mb->rpos;
++}
++
++static inline void *mb_put_data(struct tipc_msg_buf *mb, size_t len)
++{
++	void *pos = (u8 *)mb->buf_va + mb->wpos;
++
++	BUG_ON(mb->wpos + len > mb->buf_sz);
++	mb->wpos += len;
++	return pos;
++}
++
++static inline void *mb_get_data(struct tipc_msg_buf *mb, size_t len)
++{
++	void *pos = (u8 *)mb->buf_va + mb->rpos;
++
++	BUG_ON(mb->rpos + len > mb->wpos);
++	mb->rpos += len;
++	return pos;
++}
++
++#endif /* __LINUX_TRUSTY_TRUSTY_IPC_H */
++
+diff --git a/include/uapi/linux/trusty/ipc.h b/include/uapi/linux/trusty/ipc.h
+new file mode 100644
+index 000000000000..af91035484f1
+--- /dev/null
++++ b/include/uapi/linux/trusty/ipc.h
+@@ -0,0 +1,65 @@
++/* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */
++
++#ifndef _UAPI_LINUX_TRUSTY_IPC_H_
++#define _UAPI_LINUX_TRUSTY_IPC_H_
++
++#include <linux/ioctl.h>
++#include <linux/types.h>
++#include <linux/uio.h>
++
++/**
++ * enum transfer_kind - How to send an fd to Trusty
++ * @TRUSTY_SHARE:       Memory will be accessible by Linux and Trusty. On ARM it
++ *                      will be mapped as nonsecure. Suitable for shared memory.
++ *                      The paired fd must be a "dma_buf".
++ * @TRUSTY_LEND:        Memory will be accessible only to Trusty. On ARM it will
++ *                      be transitioned to "Secure" memory if Trusty is in
++ *                      TrustZone. This transfer kind is suitable for donating
++ *                      video buffers or other similar resources. The paired fd
++ *                      may need to come from a platform-specific allocator for
++ *                      memory that may be transitioned to "Secure".
++ * @TRUSTY_SEND_SECURE: Send memory that is already "Secure". Memory will be
++ *                      accessible only to Trusty. The paired fd may need to
++ *                      come from a platform-specific allocator that returns
++ *                      "Secure" buffers.
++ *
++ * Describes how the user would like the resource in question to be sent to
++ * Trusty. Options may be valid only for certain kinds of fds.
++ */
++enum transfer_kind {
++	TRUSTY_SHARE = 0,
++	TRUSTY_LEND = 1,
++	TRUSTY_SEND_SECURE = 2,
++};
++
++/**
++ * struct trusty_shm - Describes a transfer of memory to Trusty
++ * @fd:       The fd to transfer
++ * @transfer: How to transfer it - see &enum transfer_kind
++ */
++struct trusty_shm {
++	__s32 fd;
++	__u32 transfer;
++};
++
++/**
++ * struct tipc_send_msg_req - Request struct for @TIPC_IOC_SEND_MSG
++ * @iov:     Pointer to an array of &struct iovec describing data to be sent
++ * @shm:     Pointer to an array of &struct trusty_shm describing any file
++ *           descriptors to be transferred.
++ * @iov_cnt: Number of elements in the @iov array
++ * @shm_cnt: Number of elements in the @shm array
++ */
++struct tipc_send_msg_req {
++	__u64 iov;
++	__u64 shm;
++	__u64 iov_cnt;
++	__u64 shm_cnt;
++};
++
++#define TIPC_IOC_MAGIC			'r'
++#define TIPC_IOC_CONNECT		_IOW(TIPC_IOC_MAGIC, 0x80, char *)
++#define TIPC_IOC_SEND_MSG		_IOW(TIPC_IOC_MAGIC, 0x81, \
++					     struct tipc_send_msg_req)
++
++#endif
+diff --git a/include/uapi/linux/virtio_ids.h b/include/uapi/linux/virtio_ids.h
+index b052355ac7a3..cf6b95d9a1ec 100644
+--- a/include/uapi/linux/virtio_ids.h
++++ b/include/uapi/linux/virtio_ids.h
+@@ -39,6 +39,7 @@
+ #define VIRTIO_ID_9P		9 /* 9p virtio console */
+ #define VIRTIO_ID_RPROC_SERIAL 11 /* virtio remoteproc serial link */
+ #define VIRTIO_ID_CAIF	       12 /* Virtio caif */
++#define VIRTIO_ID_TRUSTY_IPC   13 /* virtio trusty ipc */
+ #define VIRTIO_ID_GPU          16 /* virtio GPU */
+ #define VIRTIO_ID_INPUT        18 /* virtio input */
+ #define VIRTIO_ID_VSOCK        19 /* virtio vsock transport */
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0036-ANDROID-trusty-Remove-FFA-specific-initilization.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0036-ANDROID-trusty-Remove-FFA-specific-initilization.patch
new file mode 100644
index 0000000..6dd8af2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0036-ANDROID-trusty-Remove-FFA-specific-initilization.patch
@@ -0,0 +1,1076 @@
+From 8318af58a0f5d29352d3c84be6b20fe6d1ca352f Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 14 Jan 2022 13:41:26 +0000
+Subject: [PATCH 23/32] ANDROID: trusty: Remove FFA specific initilization
+
+Remove FFA specific initialization and its arm_ffa.h file from Trusty
+driver. These changes are done so that Trusty can use ARM FFA driver
+and its related header files.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Iaad473659de94930cdf78cd7201f016d59cee8d7
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/trusty/trusty-mem.c    |  37 ---
+ drivers/trusty/trusty.c        | 286 +---------------
+ include/linux/trusty/arm_ffa.h | 590 ---------------------------------
+ include/linux/trusty/trusty.h  |   3 -
+ 4 files changed, 3 insertions(+), 913 deletions(-)
+ delete mode 100644 include/linux/trusty/arm_ffa.h
+
+diff --git a/drivers/trusty/trusty-mem.c b/drivers/trusty/trusty-mem.c
+index 8a360298e501..7775ff76c38c 100644
+--- a/drivers/trusty/trusty-mem.c
++++ b/drivers/trusty/trusty-mem.c
+@@ -5,7 +5,6 @@
+ 
+ #include <linux/types.h>
+ #include <linux/printk.h>
+-#include <linux/trusty/arm_ffa.h>
+ #include <linux/trusty/trusty.h>
+ #include <linux/trusty/smcall.h>
+ 
+@@ -75,8 +74,6 @@ int trusty_encode_page_info(struct ns_mem_page_info *inf,
+ {
+ 	int mem_attr;
+ 	u64 pte;
+-	u8 ffa_mem_attr;
+-	u8 ffa_mem_perm = 0;
+ 
+ 	if (!inf || !page)
+ 		return -EINVAL;
+@@ -89,30 +86,6 @@ int trusty_encode_page_info(struct ns_mem_page_info *inf,
+ 	if (mem_attr < 0)
+ 		return mem_attr;
+ 
+-	switch (mem_attr) {
+-	case MEM_ATTR_STRONGLY_ORDERED:
+-		ffa_mem_attr = FFA_MEM_ATTR_DEVICE_NGNRNE;
+-		break;
+-
+-	case MEM_ATTR_DEVICE:
+-		ffa_mem_attr = FFA_MEM_ATTR_DEVICE_NGNRE;
+-		break;
+-
+-	case MEM_ATTR_NORMAL_NON_CACHEABLE:
+-		ffa_mem_attr = FFA_MEM_ATTR_NORMAL_MEMORY_UNCACHED;
+-		break;
+-
+-	case MEM_ATTR_NORMAL_WRITE_BACK_READ_ALLOCATE:
+-	case MEM_ATTR_NORMAL_WRITE_BACK_WRITE_ALLOCATE:
+-		ffa_mem_attr = FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB;
+-		break;
+-
+-	default:
+-		return -EINVAL;
+-	}
+-
+-	inf->paddr = pte;
+-
+ 	/* add other attributes */
+ #if defined(CONFIG_ARM64) || defined(CONFIG_ARM_LPAE)
+ 	pte |= pgprot_val(pgprot);
+@@ -123,16 +96,6 @@ int trusty_encode_page_info(struct ns_mem_page_info *inf,
+ 		pte |= ATTR_INNER_SHAREABLE; /* inner sharable */
+ #endif
+ 
+-	if (!(pte & ATTR_RDONLY))
+-		ffa_mem_perm |= FFA_MEM_PERM_RW;
+-	else
+-		ffa_mem_perm |= FFA_MEM_PERM_RO;
+-
+-	if ((pte & ATTR_INNER_SHAREABLE) == ATTR_INNER_SHAREABLE)
+-		ffa_mem_attr |= FFA_MEM_ATTR_INNER_SHAREABLE;
+-
+-	inf->ffa_mem_attr = ffa_mem_attr;
+-	inf->ffa_mem_perm = ffa_mem_perm;
+ 	inf->compat_attr = (pte & 0x0000FFFFFFFFFFFFull) |
+ 			   ((u64)mem_attr << 48);
+ 	return 0;
+diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
+index 265eab52aea0..2dec75398f69 100644
+--- a/drivers/trusty/trusty.c
++++ b/drivers/trusty/trusty.c
+@@ -11,7 +11,6 @@
+ #include <linux/slab.h>
+ #include <linux/stat.h>
+ #include <linux/string.h>
+-#include <linux/trusty/arm_ffa.h>
+ #include <linux/trusty/smcall.h>
+ #include <linux/trusty/sm_err.h>
+ #include <linux/trusty/trusty.h>
+@@ -42,11 +41,6 @@ struct trusty_state {
+ 	struct list_head nop_queue;
+ 	spinlock_t nop_lock; /* protects nop_queue */
+ 	struct device_dma_parameters dma_parms;
+-	void *ffa_tx;
+-	void *ffa_rx;
+-	u16 ffa_local_id;
+-	u16 ffa_remote_id;
+-	struct mutex share_memory_msg_lock; /* protects share_memory_msg */
+ };
+ 
+ static inline unsigned long smc(unsigned long r0, unsigned long r1,
+@@ -246,19 +240,6 @@ int trusty_transfer_memory(struct device *dev, u64 *id,
+ 	struct ns_mem_page_info pg_inf;
+ 	struct scatterlist *sg;
+ 	size_t count;
+-	size_t i;
+-	size_t len;
+-	u64 ffa_handle = 0;
+-	size_t total_len;
+-	size_t endpoint_count = 1;
+-	struct ffa_mtd *mtd = s->ffa_tx;
+-	size_t comp_mrd_offset = offsetof(struct ffa_mtd, emad[endpoint_count]);
+-	struct ffa_comp_mrd *comp_mrd = s->ffa_tx + comp_mrd_offset;
+-	struct ffa_cons_mrd *cons_mrd = comp_mrd->address_range_array;
+-	size_t cons_mrd_offset = (void *)cons_mrd - s->ffa_tx;
+-	struct smc_ret8 smc_ret;
+-	u32 cookie_low;
+-	u32 cookie_high;
+ 
+ 	if (WARN_ON(dev->driver != &trusty_driver.driver))
+ 		return -EINVAL;
+@@ -284,126 +265,11 @@ int trusty_transfer_memory(struct device *dev, u64 *id,
+ 	if (ret) {
+ 		dev_err(s->dev, "%s: trusty_encode_page_info failed\n",
+ 			__func__);
+-		goto err_encode_page_info;
+-	}
+-
+-	if (s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
+-		*id = pg_inf.compat_attr;
+-		return 0;
+-	}
+-
+-	len = 0;
+-	for_each_sg(sglist, sg, nents, i)
+-		len += sg_dma_len(sg);
+-
+-	mutex_lock(&s->share_memory_msg_lock);
+-
+-	mtd->sender_id = s->ffa_local_id;
+-	mtd->memory_region_attributes = pg_inf.ffa_mem_attr;
+-	mtd->reserved_3 = 0;
+-	mtd->flags = 0;
+-	mtd->handle = 0;
+-	mtd->tag = tag;
+-	mtd->reserved_24_27 = 0;
+-	mtd->emad_count = endpoint_count;
+-	for (i = 0; i < endpoint_count; i++) {
+-		struct ffa_emad *emad = &mtd->emad[i];
+-		/* TODO: support stream ids */
+-		emad->mapd.endpoint_id = s->ffa_remote_id;
+-		emad->mapd.memory_access_permissions = pg_inf.ffa_mem_perm;
+-		emad->mapd.flags = 0;
+-		emad->comp_mrd_offset = comp_mrd_offset;
+-		emad->reserved_8_15 = 0;
+-	}
+-	comp_mrd->total_page_count = len / PAGE_SIZE;
+-	comp_mrd->address_range_count = nents;
+-	comp_mrd->reserved_8_15 = 0;
+-
+-	total_len = cons_mrd_offset + nents * sizeof(*cons_mrd);
+-	sg = sglist;
+-	while (count) {
+-		size_t lcount =
+-			min_t(size_t, count, (PAGE_SIZE - cons_mrd_offset) /
+-			      sizeof(*cons_mrd));
+-		size_t fragment_len = lcount * sizeof(*cons_mrd) +
+-				      cons_mrd_offset;
+-
+-		for (i = 0; i < lcount; i++) {
+-			cons_mrd[i].address = sg_dma_address(sg);
+-			cons_mrd[i].page_count = sg_dma_len(sg) / PAGE_SIZE;
+-			cons_mrd[i].reserved_12_15 = 0;
+-			sg = sg_next(sg);
+-		}
+-		count -= lcount;
+-		if (cons_mrd_offset) {
+-			u32 smc = lend ? SMC_FC_FFA_MEM_LEND :
+-					 SMC_FC_FFA_MEM_SHARE;
+-			/* First fragment */
+-			smc_ret = trusty_smc8(smc, total_len,
+-					      fragment_len, 0, 0, 0, 0, 0);
+-		} else {
+-			smc_ret = trusty_smc8(SMC_FC_FFA_MEM_FRAG_TX,
+-					      cookie_low, cookie_high,
+-					      fragment_len, 0, 0, 0, 0);
+-		}
+-		if (smc_ret.r0 == SMC_FC_FFA_MEM_FRAG_RX) {
+-			cookie_low = smc_ret.r1;
+-			cookie_high = smc_ret.r2;
+-			dev_dbg(s->dev, "cookie %x %x", cookie_low,
+-				cookie_high);
+-			if (!count) {
+-				/*
+-				 * We have sent all our descriptors. Expected
+-				 * SMC_FC_FFA_SUCCESS, not a request to send
+-				 * another fragment.
+-				 */
+-				dev_err(s->dev, "%s: fragment_len %zd/%zd, unexpected SMC_FC_FFA_MEM_FRAG_RX\n",
+-					__func__, fragment_len, total_len);
+-				ret = -EIO;
+-				break;
+-			}
+-		} else if (smc_ret.r0 == SMC_FC_FFA_SUCCESS) {
+-			ffa_handle = smc_ret.r2 | (u64)smc_ret.r3 << 32;
+-			dev_dbg(s->dev, "%s: fragment_len %zu/%zu, got handle 0x%llx\n",
+-				__func__, fragment_len, total_len,
+-				ffa_handle);
+-			if (count) {
+-				/*
+-				 * We have not sent all our descriptors.
+-				 * Expected SMC_FC_FFA_MEM_FRAG_RX not
+-				 * SMC_FC_FFA_SUCCESS.
+-				 */
+-				dev_err(s->dev, "%s: fragment_len %zu/%zu, unexpected SMC_FC_FFA_SUCCESS, count %zu != 0\n",
+-					__func__, fragment_len, total_len,
+-					count);
+-				ret = -EIO;
+-				break;
+-			}
+-		} else {
+-			dev_err(s->dev, "%s: fragment_len %zu/%zu, SMC_FC_FFA_MEM_SHARE failed 0x%lx 0x%lx 0x%lx",
+-				__func__, fragment_len, total_len,
+-				smc_ret.r0, smc_ret.r1, smc_ret.r2);
+-			ret = -EIO;
+-			break;
+-		}
+-
+-		cons_mrd = s->ffa_tx;
+-		cons_mrd_offset = 0;
+-	}
+-
+-	mutex_unlock(&s->share_memory_msg_lock);
+-
+-	if (!ret) {
+-		*id = ffa_handle;
+-		dev_dbg(s->dev, "%s: done\n", __func__);
+-		return 0;
++		return ret;
+ 	}
+ 
+-	dev_err(s->dev, "%s: failed %d", __func__, ret);
+-
+-err_encode_page_info:
+-	dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
+-	return ret;
++	*id = pg_inf.compat_attr;
++	return 0;
+ }
+ EXPORT_SYMBOL(trusty_transfer_memory);
+ 
+@@ -433,8 +299,6 @@ int trusty_reclaim_memory(struct device *dev, u64 id,
+ 			  struct scatterlist *sglist, unsigned int nents)
+ {
+ 	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+-	int ret = 0;
+-	struct smc_ret8 smc_ret;
+ 
+ 	if (WARN_ON(dev->driver != &trusty_driver.driver))
+ 		return -EINVAL;
+@@ -454,28 +318,6 @@ int trusty_reclaim_memory(struct device *dev, u64 id,
+ 		return 0;
+ 	}
+ 
+-	mutex_lock(&s->share_memory_msg_lock);
+-
+-	smc_ret = trusty_smc8(SMC_FC_FFA_MEM_RECLAIM, (u32)id, id >> 32, 0, 0,
+-			      0, 0, 0);
+-	if (smc_ret.r0 != SMC_FC_FFA_SUCCESS) {
+-		dev_err(s->dev, "%s: SMC_FC_FFA_MEM_RECLAIM failed 0x%lx 0x%lx 0x%lx",
+-			__func__, smc_ret.r0, smc_ret.r1, smc_ret.r2);
+-		if (smc_ret.r0 == SMC_FC_FFA_ERROR &&
+-		    smc_ret.r2 == FFA_ERROR_DENIED)
+-			ret = -EBUSY;
+-		else
+-			ret = -EIO;
+-	}
+-
+-	mutex_unlock(&s->share_memory_msg_lock);
+-
+-	if (ret != 0)
+-		return ret;
+-
+-	dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
+-
+-	dev_dbg(s->dev, "%s: done\n", __func__);
+ 	return 0;
+ }
+ EXPORT_SYMBOL(trusty_reclaim_memory);
+@@ -527,118 +369,6 @@ const char *trusty_version_str_get(struct device *dev)
+ }
+ EXPORT_SYMBOL(trusty_version_str_get);
+ 
+-static int trusty_init_msg_buf(struct trusty_state *s, struct device *dev)
+-{
+-	phys_addr_t tx_paddr;
+-	phys_addr_t rx_paddr;
+-	int ret;
+-	struct smc_ret8 smc_ret;
+-
+-	if (s->api_version < TRUSTY_API_VERSION_MEM_OBJ)
+-		return 0;
+-
+-	/* Get supported FF-A version and check if it is compatible */
+-	smc_ret = trusty_smc8(SMC_FC_FFA_VERSION, FFA_CURRENT_VERSION, 0, 0,
+-			      0, 0, 0, 0);
+-	if (FFA_VERSION_TO_MAJOR(smc_ret.r0) != FFA_CURRENT_VERSION_MAJOR) {
+-		dev_err(s->dev,
+-			"%s: Unsupported FF-A version 0x%lx, expected 0x%x\n",
+-			__func__, smc_ret.r0, FFA_CURRENT_VERSION);
+-		ret = -EIO;
+-		goto err_version;
+-	}
+-
+-	/* Check that SMC_FC_FFA_MEM_SHARE is implemented */
+-	smc_ret = trusty_smc8(SMC_FC_FFA_FEATURES, SMC_FC_FFA_MEM_SHARE, 0, 0,
+-			      0, 0, 0, 0);
+-	if (smc_ret.r0 != SMC_FC_FFA_SUCCESS) {
+-		dev_err(s->dev,
+-			"%s: SMC_FC_FFA_FEATURES(SMC_FC_FFA_MEM_SHARE) failed 0x%lx 0x%lx 0x%lx\n",
+-			__func__, smc_ret.r0, smc_ret.r1, smc_ret.r2);
+-		ret = -EIO;
+-		goto err_features;
+-	}
+-
+-	/*
+-	 * Set FF-A endpoint IDs.
+-	 *
+-	 * Hardcode 0x8000 for the secure os.
+-	 * TODO: Use FF-A call or device tree to configure this dynamically
+-	 */
+-	smc_ret = trusty_smc8(SMC_FC_FFA_ID_GET, 0, 0, 0, 0, 0, 0, 0);
+-	if (smc_ret.r0 != SMC_FC_FFA_SUCCESS) {
+-		dev_err(s->dev,
+-			"%s: SMC_FC_FFA_ID_GET failed 0x%lx 0x%lx 0x%lx\n",
+-			__func__, smc_ret.r0, smc_ret.r1, smc_ret.r2);
+-		ret = -EIO;
+-		goto err_id_get;
+-	}
+-
+-	s->ffa_local_id = smc_ret.r2;
+-	s->ffa_remote_id = 0x8000;
+-
+-	s->ffa_tx = kmalloc(PAGE_SIZE, GFP_KERNEL);
+-	if (!s->ffa_tx) {
+-		ret = -ENOMEM;
+-		goto err_alloc_tx;
+-	}
+-	tx_paddr = virt_to_phys(s->ffa_tx);
+-	if (WARN_ON(tx_paddr & (PAGE_SIZE - 1))) {
+-		ret = -EINVAL;
+-		goto err_unaligned_tx_buf;
+-	}
+-
+-	s->ffa_rx = kmalloc(PAGE_SIZE, GFP_KERNEL);
+-	if (!s->ffa_rx) {
+-		ret = -ENOMEM;
+-		goto err_alloc_rx;
+-	}
+-	rx_paddr = virt_to_phys(s->ffa_rx);
+-	if (WARN_ON(rx_paddr & (PAGE_SIZE - 1))) {
+-		ret = -EINVAL;
+-		goto err_unaligned_rx_buf;
+-	}
+-
+-	smc_ret = trusty_smc8(SMC_FCZ_FFA_RXTX_MAP, tx_paddr, rx_paddr, 1, 0,
+-			      0, 0, 0);
+-	if (smc_ret.r0 != SMC_FC_FFA_SUCCESS) {
+-		dev_err(s->dev, "%s: SMC_FCZ_FFA_RXTX_MAP failed 0x%lx 0x%lx 0x%lx\n",
+-			__func__, smc_ret.r0, smc_ret.r1, smc_ret.r2);
+-		ret = -EIO;
+-		goto err_rxtx_map;
+-	}
+-
+-	return 0;
+-
+-err_rxtx_map:
+-err_unaligned_rx_buf:
+-	kfree(s->ffa_rx);
+-	s->ffa_rx = NULL;
+-err_alloc_rx:
+-err_unaligned_tx_buf:
+-	kfree(s->ffa_tx);
+-	s->ffa_tx = NULL;
+-err_alloc_tx:
+-err_id_get:
+-err_features:
+-err_version:
+-	return ret;
+-}
+-
+-static void trusty_free_msg_buf(struct trusty_state *s, struct device *dev)
+-{
+-	struct smc_ret8 smc_ret;
+-
+-	smc_ret = trusty_smc8(SMC_FC_FFA_RXTX_UNMAP, 0, 0, 0, 0, 0, 0, 0);
+-	if (smc_ret.r0 != SMC_FC_FFA_SUCCESS) {
+-		dev_err(s->dev, "%s: SMC_FC_FFA_RXTX_UNMAP failed 0x%lx 0x%lx 0x%lx\n",
+-			__func__, smc_ret.r0, smc_ret.r1, smc_ret.r2);
+-	} else {
+-		kfree(s->ffa_rx);
+-		kfree(s->ffa_tx);
+-	}
+-}
+-
+ static void trusty_init_version(struct trusty_state *s, struct device *dev)
+ {
+ 	int ret;
+@@ -842,7 +572,6 @@ static int trusty_probe(struct platform_device *pdev)
+ 	spin_lock_init(&s->nop_lock);
+ 	INIT_LIST_HEAD(&s->nop_queue);
+ 	mutex_init(&s->smc_lock);
+-	mutex_init(&s->share_memory_msg_lock);
+ 	ATOMIC_INIT_NOTIFIER_HEAD(&s->notifier);
+ 	init_completion(&s->cpu_idle_completion);
+ 
+@@ -862,10 +591,6 @@ static int trusty_probe(struct platform_device *pdev)
+ 	if (ret < 0)
+ 		goto err_api_version;
+ 
+-	ret = trusty_init_msg_buf(s, &pdev->dev);
+-	if (ret < 0)
+-		goto err_init_msg_buf;
+-
+ 	s->nop_wq = alloc_workqueue("trusty-nop-wq", WQ_CPU_INTENSIVE, 0);
+ 	if (!s->nop_wq) {
+ 		ret = -ENODEV;
+@@ -910,13 +635,10 @@ static int trusty_probe(struct platform_device *pdev)
+ err_alloc_works:
+ 	destroy_workqueue(s->nop_wq);
+ err_create_nop_wq:
+-	trusty_free_msg_buf(s, &pdev->dev);
+-err_init_msg_buf:
+ err_api_version:
+ 	s->dev->dma_parms = NULL;
+ 	kfree(s->version_str);
+ 	device_for_each_child(&pdev->dev, NULL, trusty_remove_child);
+-	mutex_destroy(&s->share_memory_msg_lock);
+ 	mutex_destroy(&s->smc_lock);
+ 	kfree(s);
+ err_allocate_state:
+@@ -938,9 +660,7 @@ static int trusty_remove(struct platform_device *pdev)
+ 	free_percpu(s->nop_works);
+ 	destroy_workqueue(s->nop_wq);
+ 
+-	mutex_destroy(&s->share_memory_msg_lock);
+ 	mutex_destroy(&s->smc_lock);
+-	trusty_free_msg_buf(s, &pdev->dev);
+ 	s->dev->dma_parms = NULL;
+ 	kfree(s->version_str);
+ 	kfree(s);
+diff --git a/include/linux/trusty/arm_ffa.h b/include/linux/trusty/arm_ffa.h
+deleted file mode 100644
+index ab7b2afb794c..000000000000
+--- a/include/linux/trusty/arm_ffa.h
++++ /dev/null
+@@ -1,590 +0,0 @@
+-/* SPDX-License-Identifier: MIT */
+-/*
+- * Copyright (C) 2020 Google, Inc.
+- *
+- * Trusty and TF-A also have a copy of this header.
+- * Please keep the copies in sync.
+- */
+-#ifndef __LINUX_TRUSTY_ARM_FFA_H
+-#define __LINUX_TRUSTY_ARM_FFA_H
+-
+-/*
+- * Subset of Arm PSA Firmware Framework for Arm v8-A 1.0 EAC 1_0
+- * (https://developer.arm.com/docs/den0077/a) needed for shared memory.
+- */
+-
+-#include "smcall.h"
+-
+-#ifndef STATIC_ASSERT
+-#define STATIC_ASSERT(e) _Static_assert(e, #e)
+-#endif
+-
+-#define FFA_CURRENT_VERSION_MAJOR (1U)
+-#define FFA_CURRENT_VERSION_MINOR (0U)
+-
+-#define FFA_VERSION_TO_MAJOR(version) ((version) >> 16)
+-#define FFA_VERSION_TO_MINOR(version) ((version) & (0xffff))
+-#define FFA_VERSION(major, minor) (((major) << 16) | (minor))
+-#define FFA_CURRENT_VERSION \
+-	FFA_VERSION(FFA_CURRENT_VERSION_MAJOR, FFA_CURRENT_VERSION_MINOR)
+-
+-#define SMC_ENTITY_SHARED_MEMORY 4
+-
+-#define SMC_FASTCALL_NR_SHARED_MEMORY(nr) \
+-	SMC_FASTCALL_NR(SMC_ENTITY_SHARED_MEMORY, nr)
+-#define SMC_FASTCALL64_NR_SHARED_MEMORY(nr) \
+-	SMC_FASTCALL64_NR(SMC_ENTITY_SHARED_MEMORY, nr)
+-
+-/**
+- * typedef ffa_endpoint_id16_t - Endpoint ID
+- *
+- * Current implementation only supports VMIDs. FFA spec also support stream
+- * endpoint ids.
+- */
+-typedef uint16_t ffa_endpoint_id16_t;
+-
+-/**
+- * struct ffa_cons_mrd - Constituent memory region descriptor
+- * @address:
+- *         Start address of contiguous memory region. Must be 4K page aligned.
+- * @page_count:
+- *         Number of 4K pages in region.
+- * @reserved_12_15:
+- *         Reserve bytes 12-15 to pad struct size to 16 bytes.
+- */
+-struct ffa_cons_mrd {
+-	uint64_t address;
+-	uint32_t page_count;
+-	uint32_t reserved_12_15;
+-};
+-STATIC_ASSERT(sizeof(struct ffa_cons_mrd) == 16);
+-
+-/**
+- * struct ffa_comp_mrd - Composite memory region descriptor
+- * @total_page_count:
+- *         Number of 4k pages in memory region. Must match sum of
+- *         @address_range_array[].page_count.
+- * @address_range_count:
+- *         Number of entries in @address_range_array.
+- * @reserved_8_15:
+- *         Reserve bytes 8-15 to pad struct size to 16 byte alignment and
+- *         make @address_range_array 16 byte aligned.
+- * @address_range_array:
+- *         Array of &struct ffa_cons_mrd entries.
+- */
+-struct ffa_comp_mrd {
+-	uint32_t total_page_count;
+-	uint32_t address_range_count;
+-	uint64_t reserved_8_15;
+-	struct ffa_cons_mrd address_range_array[];
+-};
+-STATIC_ASSERT(sizeof(struct ffa_comp_mrd) == 16);
+-
+-/**
+- * typedef ffa_mem_attr8_t - Memory region attributes
+- *
+- * * @FFA_MEM_ATTR_DEVICE_NGNRNE:
+- *     Device-nGnRnE.
+- * * @FFA_MEM_ATTR_DEVICE_NGNRE:
+- *     Device-nGnRE.
+- * * @FFA_MEM_ATTR_DEVICE_NGRE:
+- *     Device-nGRE.
+- * * @FFA_MEM_ATTR_DEVICE_GRE:
+- *     Device-GRE.
+- * * @FFA_MEM_ATTR_NORMAL_MEMORY_UNCACHED
+- *     Normal memory. Non-cacheable.
+- * * @FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB
+- *     Normal memory. Write-back cached.
+- * * @FFA_MEM_ATTR_NON_SHAREABLE
+- *     Non-shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
+- * * @FFA_MEM_ATTR_OUTER_SHAREABLE
+- *     Outer Shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
+- * * @FFA_MEM_ATTR_INNER_SHAREABLE
+- *     Inner Shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
+- */
+-typedef uint8_t ffa_mem_attr8_t;
+-#define FFA_MEM_ATTR_DEVICE_NGNRNE ((1U << 4) | (0x0U << 2))
+-#define FFA_MEM_ATTR_DEVICE_NGNRE ((1U << 4) | (0x1U << 2))
+-#define FFA_MEM_ATTR_DEVICE_NGRE ((1U << 4) | (0x2U << 2))
+-#define FFA_MEM_ATTR_DEVICE_GRE ((1U << 4) | (0x3U << 2))
+-#define FFA_MEM_ATTR_NORMAL_MEMORY_UNCACHED ((2U << 4) | (0x1U << 2))
+-#define FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB ((2U << 4) | (0x3U << 2))
+-#define FFA_MEM_ATTR_NON_SHAREABLE (0x0U << 0)
+-#define FFA_MEM_ATTR_OUTER_SHAREABLE (0x2U << 0)
+-#define FFA_MEM_ATTR_INNER_SHAREABLE (0x3U << 0)
+-
+-/**
+- * typedef ffa_mem_perm8_t - Memory access permissions
+- *
+- * * @FFA_MEM_ATTR_RO
+- *     Request or specify read-only mapping.
+- * * @FFA_MEM_ATTR_RW
+- *     Request or allow read-write mapping.
+- * * @FFA_MEM_PERM_NX
+- *     Deny executable mapping.
+- * * @FFA_MEM_PERM_X
+- *     Request executable mapping.
+- */
+-typedef uint8_t ffa_mem_perm8_t;
+-#define FFA_MEM_PERM_RO (1U << 0)
+-#define FFA_MEM_PERM_RW (1U << 1)
+-#define FFA_MEM_PERM_NX (1U << 2)
+-#define FFA_MEM_PERM_X (1U << 3)
+-
+-/**
+- * typedef ffa_mem_flag8_t - Endpoint memory flags
+- *
+- * * @FFA_MEM_FLAG_OTHER
+- *     Other borrower. Memory region must not be or was not retrieved on behalf
+- *     of this endpoint.
+- */
+-typedef uint8_t ffa_mem_flag8_t;
+-#define FFA_MEM_FLAG_OTHER (1U << 0)
+-
+-/**
+- * typedef ffa_mtd_flag32_t - Memory transaction descriptor flags
+- *
+- * * @FFA_MTD_FLAG_ZERO_MEMORY
+- *     Zero memory after unmapping from sender (must be 0 for share).
+- * * @FFA_MTD_FLAG_TIME_SLICING
+- *     Not supported by this implementation.
+- * * @FFA_MTD_FLAG_ZERO_MEMORY_AFTER_RELINQUISH
+- *     Zero memory after unmapping from borrowers (must be 0 for share).
+- * * @FFA_MTD_FLAG_TYPE_MASK
+- *     Bit-mask to extract memory management transaction type from flags.
+- * * @FFA_MTD_FLAG_TYPE_SHARE_MEMORY
+- *     Share memory transaction flag.
+- *     Used by @SMC_FC_FFA_MEM_RETRIEVE_RESP to indicate that memory came from
+- *     @SMC_FC_FFA_MEM_SHARE and by @SMC_FC_FFA_MEM_RETRIEVE_REQ to specify that
+- *     it must have.
+- * * @FFA_MTD_FLAG_ADDRESS_RANGE_ALIGNMENT_HINT_MASK
+- *     Not supported by this implementation.
+- */
+-typedef uint32_t ffa_mtd_flag32_t;
+-#define FFA_MTD_FLAG_ZERO_MEMORY (1U << 0)
+-#define FFA_MTD_FLAG_TIME_SLICING (1U << 1)
+-#define FFA_MTD_FLAG_ZERO_MEMORY_AFTER_RELINQUISH (1U << 2)
+-#define FFA_MTD_FLAG_TYPE_MASK (3U << 3)
+-#define FFA_MTD_FLAG_TYPE_SHARE_MEMORY (1U << 3)
+-#define FFA_MTD_FLAG_ADDRESS_RANGE_ALIGNMENT_HINT_MASK (0x1FU << 5)
+-
+-/**
+- * struct ffa_mapd - Memory access permissions descriptor
+- * @endpoint_id:
+- *         Endpoint id that @memory_access_permissions and @flags apply to.
+- *         (&typedef ffa_endpoint_id16_t).
+- * @memory_access_permissions:
+- *         FFA_MEM_PERM_* values or'ed together (&typedef ffa_mem_perm8_t).
+- * @flags:
+- *         FFA_MEM_FLAG_* values or'ed together (&typedef ffa_mem_flag8_t).
+- */
+-struct ffa_mapd {
+-	ffa_endpoint_id16_t endpoint_id;
+-	ffa_mem_perm8_t memory_access_permissions;
+-	ffa_mem_flag8_t flags;
+-};
+-STATIC_ASSERT(sizeof(struct ffa_mapd) == 4);
+-
+-/**
+- * struct ffa_emad - Endpoint memory access descriptor.
+- * @mapd:  &struct ffa_mapd.
+- * @comp_mrd_offset:
+- *         Offset of &struct ffa_comp_mrd form start of &struct ffa_mtd.
+- * @reserved_8_15:
+- *         Reserved bytes 8-15. Must be 0.
+- */
+-struct ffa_emad {
+-	struct ffa_mapd mapd;
+-	uint32_t comp_mrd_offset;
+-	uint64_t reserved_8_15;
+-};
+-STATIC_ASSERT(sizeof(struct ffa_emad) == 16);
+-
+-/**
+- * struct ffa_mtd - Memory transaction descriptor.
+- * @sender_id:
+- *         Sender endpoint id.
+- * @memory_region_attributes:
+- *         FFA_MEM_ATTR_* values or'ed together (&typedef ffa_mem_attr8_t).
+- * @reserved_3:
+- *         Reserved bytes 3. Must be 0.
+- * @flags:
+- *         FFA_MTD_FLAG_* values or'ed together (&typedef ffa_mtd_flag32_t).
+- * @handle:
+- *         Id of shared memory object. Most be 0 for MEM_SHARE.
+- * @tag:   Client allocated tag. Must match original value.
+- * @reserved_24_27:
+- *         Reserved bytes 24-27. Must be 0.
+- * @emad_count:
+- *         Number of entries in @emad. Must be 1 in current implementation.
+- *         FFA spec allows more entries.
+- * @emad:
+- *         Endpoint memory access descriptor array (see @struct ffa_emad).
+- */
+-struct ffa_mtd {
+-	ffa_endpoint_id16_t sender_id;
+-	ffa_mem_attr8_t memory_region_attributes;
+-	uint8_t reserved_3;
+-	ffa_mtd_flag32_t flags;
+-	uint64_t handle;
+-	uint64_t tag;
+-	uint32_t reserved_24_27;
+-	uint32_t emad_count;
+-	struct ffa_emad emad[];
+-};
+-STATIC_ASSERT(sizeof(struct ffa_mtd) == 32);
+-
+-/**
+- * struct ffa_mem_relinquish_descriptor - Relinquish request descriptor.
+- * @handle:
+- *         Id of shared memory object to relinquish.
+- * @flags:
+- *         If bit 0 is set clear memory after unmapping from borrower. Must be 0
+- *         for share. Bit[1]: Time slicing. Not supported, must be 0. All other
+- *         bits are reserved 0.
+- * @endpoint_count:
+- *         Number of entries in @endpoint_array.
+- * @endpoint_array:
+- *         Array of endpoint ids.
+- */
+-struct ffa_mem_relinquish_descriptor {
+-	uint64_t handle;
+-	uint32_t flags;
+-	uint32_t endpoint_count;
+-	ffa_endpoint_id16_t endpoint_array[];
+-};
+-STATIC_ASSERT(sizeof(struct ffa_mem_relinquish_descriptor) == 16);
+-
+-/**
+- * enum ffa_error - FF-A error code
+- * @FFA_ERROR_NOT_SUPPORTED:
+- *         Operation contained possibly valid parameters not supported by the
+- *         current implementation. Does not match FF-A 1.0 EAC 1_0 definition.
+- * @FFA_ERROR_INVALID_PARAMETERS:
+- *         Invalid parameters. Conditions function specific.
+- * @FFA_ERROR_NO_MEMORY:
+- *         Not enough memory.
+- * @FFA_ERROR_DENIED:
+- *         Operation not allowed. Conditions function specific.
+- *
+- * FF-A 1.0 EAC 1_0 defines other error codes as well but the current
+- * implementation does not use them.
+- */
+-enum ffa_error {
+-	FFA_ERROR_NOT_SUPPORTED = -1,
+-	FFA_ERROR_INVALID_PARAMETERS = -2,
+-	FFA_ERROR_NO_MEMORY = -3,
+-	FFA_ERROR_DENIED = -6,
+-};
+-
+-/**
+- * SMC_FC32_FFA_MIN - First 32 bit SMC opcode reserved for FFA
+- */
+-#define SMC_FC32_FFA_MIN SMC_FASTCALL_NR_SHARED_MEMORY(0x60)
+-
+-/**
+- * SMC_FC32_FFA_MAX - Last 32 bit SMC opcode reserved for FFA
+- */
+-#define SMC_FC32_FFA_MAX SMC_FASTCALL_NR_SHARED_MEMORY(0x7F)
+-
+-/**
+- * SMC_FC64_FFA_MIN - First 64 bit SMC opcode reserved for FFA
+- */
+-#define SMC_FC64_FFA_MIN SMC_FASTCALL64_NR_SHARED_MEMORY(0x60)
+-
+-/**
+- * SMC_FC64_FFA_MAX - Last 64 bit SMC opcode reserved for FFA
+- */
+-#define SMC_FC64_FFA_MAX SMC_FASTCALL64_NR_SHARED_MEMORY(0x7F)
+-
+-/**
+- * SMC_FC_FFA_ERROR - SMC error return opcode
+- *
+- * Register arguments:
+- *
+- * * w1:     VMID in [31:16], vCPU in [15:0]
+- * * w2:     Error code (&enum ffa_error)
+- */
+-#define SMC_FC_FFA_ERROR SMC_FASTCALL_NR_SHARED_MEMORY(0x60)
+-
+-/**
+- * SMC_FC_FFA_SUCCESS - 32 bit SMC success return opcode
+- *
+- * Register arguments:
+- *
+- * * w1:     VMID in [31:16], vCPU in [15:0]
+- * * w2-w7:  Function specific
+- */
+-#define SMC_FC_FFA_SUCCESS SMC_FASTCALL_NR_SHARED_MEMORY(0x61)
+-
+-/**
+- * SMC_FC64_FFA_SUCCESS - 64 bit SMC success return opcode
+- *
+- * Register arguments:
+- *
+- * * w1:             VMID in [31:16], vCPU in [15:0]
+- * * w2/x2-w7/x7:    Function specific
+- */
+-#define SMC_FC64_FFA_SUCCESS SMC_FASTCALL64_NR_SHARED_MEMORY(0x61)
+-
+-/**
+- * SMC_FC_FFA_VERSION - SMC opcode to return supported FF-A version
+- *
+- * Register arguments:
+- *
+- * * w1:     Major version bit[30:16] and minor version in bit[15:0] supported
+- *           by caller. Bit[31] must be 0.
+- *
+- * Return:
+- * * w0:     &SMC_FC_FFA_SUCCESS
+- * * w2:     Major version bit[30:16], minor version in bit[15:0], bit[31] must
+- *           be 0.
+- *
+- * or
+- *
+- * * w0:     SMC_FC_FFA_ERROR
+- * * w2:     FFA_ERROR_NOT_SUPPORTED if major version passed in is less than the
+- *           minimum major version supported.
+- */
+-#define SMC_FC_FFA_VERSION SMC_FASTCALL_NR_SHARED_MEMORY(0x63)
+-
+-/**
+- * SMC_FC_FFA_FEATURES - SMC opcode to check optional feature support
+- *
+- * Register arguments:
+- *
+- * * w1:     FF-A function ID
+- *
+- * Return:
+- * * w0:     &SMC_FC_FFA_SUCCESS
+- * * w2:     Bit[0]: Supports custom buffers for memory transactions.
+- *           Bit[1:0]: For RXTX_MAP min buffer size and alignment boundary.
+- *           Other bits must be 0.
+- * * w3:     For FFA_MEM_RETRIEVE_REQ, bit[7-0]: Number of times receiver can
+- *           retrieve each memory region before relinquishing it specified as
+- *           ((1U << (value + 1)) - 1 (or value = bits in reference count - 1).
+- *           For all other bits and commands: must be 0.
+- * or
+- *
+- * * w0:     SMC_FC_FFA_ERROR
+- * * w2:     FFA_ERROR_NOT_SUPPORTED if function is not implemented, or
+- *           FFA_ERROR_INVALID_PARAMETERS if function id is not valid.
+- */
+-#define SMC_FC_FFA_FEATURES SMC_FASTCALL_NR_SHARED_MEMORY(0x64)
+-
+-/**
+- * SMC_FC_FFA_RXTX_MAP - 32 bit SMC opcode to map message buffers
+- *
+- * Register arguments:
+- *
+- * * w1:     TX address
+- * * w2:     RX address
+- * * w3:     RX/TX page count in bit[5:0]
+- *
+- * Return:
+- * * w0:     &SMC_FC_FFA_SUCCESS
+- */
+-#define SMC_FC_FFA_RXTX_MAP SMC_FASTCALL_NR_SHARED_MEMORY(0x66)
+-
+-/**
+- * SMC_FC64_FFA_RXTX_MAP - 64 bit SMC opcode to map message buffers
+- *
+- * Register arguments:
+- *
+- * * x1:     TX address
+- * * x2:     RX address
+- * * x3:     RX/TX page count in bit[5:0]
+- *
+- * Return:
+- * * w0:     &SMC_FC_FFA_SUCCESS
+- */
+-#define SMC_FC64_FFA_RXTX_MAP SMC_FASTCALL64_NR_SHARED_MEMORY(0x66)
+-#ifdef CONFIG_64BIT
+-#define SMC_FCZ_FFA_RXTX_MAP SMC_FC64_FFA_RXTX_MAP
+-#else
+-#define SMC_FCZ_FFA_RXTX_MAP SMC_FC_FFA_RXTX_MAP
+-#endif
+-
+-/**
+- * SMC_FC_FFA_RXTX_UNMAP - SMC opcode to unmap message buffers
+- *
+- * Register arguments:
+- *
+- * * w1:     ID in [31:16]
+- *
+- * Return:
+- * * w0:     &SMC_FC_FFA_SUCCESS
+- */
+-#define SMC_FC_FFA_RXTX_UNMAP SMC_FASTCALL_NR_SHARED_MEMORY(0x67)
+-
+-/**
+- * SMC_FC_FFA_ID_GET - SMC opcode to get endpoint id of caller
+- *
+- * Return:
+- * * w0:     &SMC_FC_FFA_SUCCESS
+- * * w2:     ID in bit[15:0], bit[31:16] must be 0.
+- */
+-#define SMC_FC_FFA_ID_GET SMC_FASTCALL_NR_SHARED_MEMORY(0x69)
+-
+-/**
+- * SMC_FC_FFA_MEM_DONATE - 32 bit SMC opcode to donate memory
+- *
+- * Not supported.
+- */
+-#define SMC_FC_FFA_MEM_DONATE SMC_FASTCALL_NR_SHARED_MEMORY(0x71)
+-
+-/**
+- * SMC_FC_FFA_MEM_LEND - 32 bit SMC opcode to lend memory
+- *
+- * Not currently supported.
+- */
+-#define SMC_FC_FFA_MEM_LEND SMC_FASTCALL_NR_SHARED_MEMORY(0x72)
+-
+-/**
+- * SMC_FC_FFA_MEM_SHARE - 32 bit SMC opcode to share memory
+- *
+- * Register arguments:
+- *
+- * * w1:     Total length
+- * * w2:     Fragment length
+- * * w3:     Address
+- * * w4:     Page count
+- *
+- * Return:
+- * * w0:     &SMC_FC_FFA_SUCCESS
+- * * w2/w3:  Handle
+- *
+- * or
+- *
+- * * w0:     &SMC_FC_FFA_MEM_FRAG_RX
+- * * w1-:    See &SMC_FC_FFA_MEM_FRAG_RX
+- *
+- * or
+- *
+- * * w0:     SMC_FC_FFA_ERROR
+- * * w2:     Error code (&enum ffa_error)
+- */
+-#define SMC_FC_FFA_MEM_SHARE SMC_FASTCALL_NR_SHARED_MEMORY(0x73)
+-
+-/**
+- * SMC_FC64_FFA_MEM_SHARE - 64 bit SMC opcode to share memory
+- *
+- * Register arguments:
+- *
+- * * w1:     Total length
+- * * w2:     Fragment length
+- * * x3:     Address
+- * * w4:     Page count
+- *
+- * Return:
+- * * w0:     &SMC_FC_FFA_SUCCESS
+- * * w2/w3:  Handle
+- *
+- * or
+- *
+- * * w0:     &SMC_FC_FFA_MEM_FRAG_RX
+- * * w1-:    See &SMC_FC_FFA_MEM_FRAG_RX
+- *
+- * or
+- *
+- * * w0:     SMC_FC_FFA_ERROR
+- * * w2:     Error code (&enum ffa_error)
+- */
+-#define SMC_FC64_FFA_MEM_SHARE SMC_FASTCALL64_NR_SHARED_MEMORY(0x73)
+-
+-/**
+- * SMC_FC_FFA_MEM_RETRIEVE_REQ - 32 bit SMC opcode to retrieve shared memory
+- *
+- * Register arguments:
+- *
+- * * w1:     Total length
+- * * w2:     Fragment length
+- * * w3:     Address
+- * * w4:     Page count
+- *
+- * Return:
+- * * w0:             &SMC_FC_FFA_MEM_RETRIEVE_RESP
+- * * w1/x1-w5/x5:    See &SMC_FC_FFA_MEM_RETRIEVE_RESP
+- */
+-#define SMC_FC_FFA_MEM_RETRIEVE_REQ SMC_FASTCALL_NR_SHARED_MEMORY(0x74)
+-
+-/**
+- * SMC_FC64_FFA_MEM_RETRIEVE_REQ - 64 bit SMC opcode to retrieve shared memory
+- *
+- * Register arguments:
+- *
+- * * w1:     Total length
+- * * w2:     Fragment length
+- * * x3:     Address
+- * * w4:     Page count
+- *
+- * Return:
+- * * w0:             &SMC_FC_FFA_MEM_RETRIEVE_RESP
+- * * w1/x1-w5/x5:    See &SMC_FC_FFA_MEM_RETRIEVE_RESP
+- */
+-#define SMC_FC64_FFA_MEM_RETRIEVE_REQ SMC_FASTCALL64_NR_SHARED_MEMORY(0x74)
+-
+-/**
+- * SMC_FC_FFA_MEM_RETRIEVE_RESP - Retrieve 32 bit SMC return opcode
+- *
+- * Register arguments:
+- *
+- * * w1:     Total length
+- * * w2:     Fragment length
+- */
+-#define SMC_FC_FFA_MEM_RETRIEVE_RESP SMC_FASTCALL_NR_SHARED_MEMORY(0x75)
+-
+-/**
+- * SMC_FC_FFA_MEM_RELINQUISH - SMC opcode to relinquish shared memory
+- *
+- * Input in &struct ffa_mem_relinquish_descriptor format in message buffer.
+- *
+- * Return:
+- * * w0:     &SMC_FC_FFA_SUCCESS
+- */
+-#define SMC_FC_FFA_MEM_RELINQUISH SMC_FASTCALL_NR_SHARED_MEMORY(0x76)
+-
+-/**
+- * SMC_FC_FFA_MEM_RECLAIM - SMC opcode to reclaim shared memory
+- *
+- * Register arguments:
+- *
+- * * w1/w2:  Handle
+- * * w3:     Flags
+- *
+- * Return:
+- * * w0:     &SMC_FC_FFA_SUCCESS
+- */
+-#define SMC_FC_FFA_MEM_RECLAIM SMC_FASTCALL_NR_SHARED_MEMORY(0x77)
+-
+-/**
+- * SMC_FC_FFA_MEM_FRAG_RX - SMC opcode to request next fragment.
+- *
+- * Register arguments:
+- *
+- * * w1/w2:  Cookie
+- * * w3:     Fragment offset.
+- * * w4:     Endpoint id ID in [31:16], if client is hypervisor.
+- *
+- * Return:
+- * * w0:             &SMC_FC_FFA_MEM_FRAG_TX
+- * * w1/x1-w5/x5:    See &SMC_FC_FFA_MEM_FRAG_TX
+- */
+-#define SMC_FC_FFA_MEM_FRAG_RX SMC_FASTCALL_NR_SHARED_MEMORY(0x7A)
+-
+-/**
+- * SMC_FC_FFA_MEM_FRAG_TX - SMC opcode to transmit next fragment
+- *
+- * Register arguments:
+- *
+- * * w1/w2:  Cookie
+- * * w3:     Fragment length.
+- * * w4:     Sender endpoint id ID in [31:16], if client is hypervisor.
+- *
+- * Return:
+- * * w0:             &SMC_FC_FFA_MEM_FRAG_RX or &SMC_FC_FFA_SUCCESS.
+- * * w1/x1-w5/x5:    See opcode in w0.
+- */
+-#define SMC_FC_FFA_MEM_FRAG_TX SMC_FASTCALL_NR_SHARED_MEMORY(0x7B)
+-
+-#endif /* __LINUX_TRUSTY_ARM_FFA_H */
+diff --git a/include/linux/trusty/trusty.h b/include/linux/trusty/trusty.h
+index efbb36999a8b..272d96c1c696 100644
+--- a/include/linux/trusty/trusty.h
++++ b/include/linux/trusty/trusty.h
+@@ -52,9 +52,6 @@ u32 trusty_get_api_version(struct device *dev);
+ bool trusty_get_panic_status(struct device *dev);
+ 
+ struct ns_mem_page_info {
+-	u64 paddr;
+-	u8 ffa_mem_attr;
+-	u8 ffa_mem_perm;
+ 	u64 compat_attr;
+ };
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0037-ANDROID-trusty-Rename-transfer-memory-function-to-le.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0037-ANDROID-trusty-Rename-transfer-memory-function-to-le.patch
new file mode 100644
index 0000000..a7bc060
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0037-ANDROID-trusty-Rename-transfer-memory-function-to-le.patch
@@ -0,0 +1,191 @@
+From 804ef860d9757cbe31b606fd5ec68cc5454c88f8 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Tue, 18 Jan 2022 18:27:09 +0000
+Subject: [PATCH 24/32] ANDROID: trusty: Rename transfer memory function to
+ lend memory
+
+Renaming trusty_transfer_memory to trusty_lend_memory allows Trusty
+to export memory operation like share, lend, reclaim and use common
+code for memory share and lend operations.
+
+Define TRUSTY_DEFAULT_MEM_OBJ_TAG as 0 and use that in existing calls.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Ie165a609cc4398bb916967595d0b748d54d75faf
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/trusty/trusty-ipc.c    | 14 ++++++++----
+ drivers/trusty/trusty-test.c   |  3 ++-
+ drivers/trusty/trusty-virtio.c |  3 ++-
+ drivers/trusty/trusty.c        | 41 ++++++++++++++++++++++------------
+ include/linux/trusty/trusty.h  | 11 ++++-----
+ 5 files changed, 47 insertions(+), 25 deletions(-)
+
+diff --git a/drivers/trusty/trusty-ipc.c b/drivers/trusty/trusty-ipc.c
+index 82d6ddeb41f4..0a27af2063a7 100644
+--- a/drivers/trusty/trusty-ipc.c
++++ b/drivers/trusty/trusty-ipc.c
+@@ -1233,10 +1233,16 @@ static int dn_share_fd(struct tipc_dn_chan *dn, int fd,
+ 		goto cleanup_handle;
+ 	}
+ 
+-	ret = trusty_transfer_memory(tipc_shared_handle_dev(shared_handle),
+-				     &mem_id, shared_handle->sgt->sgl,
+-				     shared_handle->sgt->orig_nents, prot, tag,
+-				     lend);
++	if (lend)
++		ret = trusty_lend_memory(tipc_shared_handle_dev(shared_handle),
++					 &mem_id, shared_handle->sgt->sgl,
++					 shared_handle->sgt->orig_nents, prot,
++					 tag);
++	else
++		ret = trusty_share_memory(tipc_shared_handle_dev(shared_handle),
++					  &mem_id, shared_handle->sgt->sgl,
++					  shared_handle->sgt->orig_nents, prot,
++					  tag);
+ 
+ 	if (ret < 0) {
+ 		dev_dbg(dev, "Transferring memory failed: %d\n", ret);
+diff --git a/drivers/trusty/trusty-test.c b/drivers/trusty/trusty-test.c
+index 844868981fa5..c25fc0f2fcf0 100644
+--- a/drivers/trusty/trusty-test.c
++++ b/drivers/trusty/trusty-test.c
+@@ -138,7 +138,8 @@ static int trusty_test_share_objs(struct trusty_test_state *s,
+ 		t1 = ktime_get();
+ 		tmpret = trusty_share_memory(s->trusty_dev, &obj->mem_id,
+ 					     obj->sgt.sgl, obj->sgt.nents,
+-					     PAGE_KERNEL);
++					     PAGE_KERNEL,
++					     TRUSTY_DEFAULT_MEM_OBJ_TAG);
+ 		t2 = ktime_get();
+ 		if (tmpret) {
+ 			ret = tmpret;
+diff --git a/drivers/trusty/trusty-virtio.c b/drivers/trusty/trusty-virtio.c
+index fea59cd2e218..365e7c04bcf4 100644
+--- a/drivers/trusty/trusty-virtio.c
++++ b/drivers/trusty/trusty-virtio.c
+@@ -626,7 +626,8 @@ static int trusty_virtio_add_devices(struct trusty_ctx *tctx)
+ 
+ 	sg_init_one(&tctx->shared_sg, descr_va, descr_buf_sz);
+ 	ret = trusty_share_memory(tctx->dev->parent, &descr_id,
+-				  &tctx->shared_sg, 1, PAGE_KERNEL);
++				  &tctx->shared_sg, 1, PAGE_KERNEL,
++				  TRUSTY_DEFAULT_MEM_OBJ_TAG);
+ 	if (ret) {
+ 		dev_err(tctx->dev, "trusty_share_memory failed: %d\n", ret);
+ 		goto err_share_memory;
+diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
+index 2dec75398f69..6bd30bc1bbc9 100644
+--- a/drivers/trusty/trusty.c
++++ b/drivers/trusty/trusty.c
+@@ -222,18 +222,9 @@ s32 trusty_std_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2)
+ }
+ EXPORT_SYMBOL(trusty_std_call32);
+ 
+-int trusty_share_memory(struct device *dev, u64 *id,
+-			struct scatterlist *sglist, unsigned int nents,
+-			pgprot_t pgprot)
+-{
+-	return trusty_transfer_memory(dev, id, sglist, nents, pgprot, 0,
+-				      false);
+-}
+-EXPORT_SYMBOL(trusty_share_memory);
+-
+-int trusty_transfer_memory(struct device *dev, u64 *id,
+-			   struct scatterlist *sglist, unsigned int nents,
+-			   pgprot_t pgprot, u64 tag, bool lend)
++static int __trusty_share_memory(struct device *dev, u64 *id,
++				 struct scatterlist *sglist, unsigned int nents,
++				 pgprot_t pgprot, u64 tag, bool mem_share)
+ {
+ 	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+ 	int ret;
+@@ -253,6 +244,12 @@ int trusty_transfer_memory(struct device *dev, u64 *id,
+ 		return -EOPNOTSUPP;
+ 	}
+ 
++	if (mem_share == false && s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
++		dev_err(s->dev, "%s: old trusty version does not support lending memory objects\n",
++			__func__);
++		return -EOPNOTSUPP;
++	}
++
+ 	count = dma_map_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
+ 	if (count != nents) {
+ 		dev_err(s->dev, "failed to dma map sg_table\n");
+@@ -271,7 +268,22 @@ int trusty_transfer_memory(struct device *dev, u64 *id,
+ 	*id = pg_inf.compat_attr;
+ 	return 0;
+ }
+-EXPORT_SYMBOL(trusty_transfer_memory);
++
++int trusty_share_memory(struct device *dev, u64 *id,
++			struct scatterlist *sglist, unsigned int nents,
++			pgprot_t pgprot, u64 tag)
++{
++	return __trusty_share_memory(dev, id, sglist, nents, pgprot, tag, true);
++}
++EXPORT_SYMBOL(trusty_share_memory);
++
++int trusty_lend_memory(struct device *dev, u64 *id,
++		       struct scatterlist *sglist, unsigned int nents,
++		       pgprot_t pgprot, u64 tag)
++{
++	return __trusty_share_memory(dev, id, sglist, nents, pgprot, tag, false);
++}
++EXPORT_SYMBOL(trusty_lend_memory);
+ 
+ /*
+  * trusty_share_memory_compat - trusty_share_memory wrapper for old apis
+@@ -287,7 +299,8 @@ int trusty_share_memory_compat(struct device *dev, u64 *id,
+ 	int ret;
+ 	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+ 
+-	ret = trusty_share_memory(dev, id, sglist, nents, pgprot);
++	ret = trusty_share_memory(dev, id, sglist, nents, pgprot,
++				  TRUSTY_DEFAULT_MEM_OBJ_TAG);
+ 	if (!ret && s->api_version < TRUSTY_API_VERSION_PHYS_MEM_OBJ)
+ 		*id &= 0x0000FFFFFFFFF000ull;
+ 
+diff --git a/include/linux/trusty/trusty.h b/include/linux/trusty/trusty.h
+index 272d96c1c696..27f635f2d12d 100644
+--- a/include/linux/trusty/trusty.h
++++ b/include/linux/trusty/trusty.h
+@@ -11,6 +11,7 @@
+ #include <linux/device.h>
+ #include <linux/pagemap.h>
+ 
++#define TRUSTY_DEFAULT_MEM_OBJ_TAG	(0)
+ 
+ #if IS_ENABLED(CONFIG_TRUSTY)
+ s32 trusty_std_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2);
+@@ -62,13 +63,13 @@ struct scatterlist;
+ typedef u64 trusty_shared_mem_id_t;
+ int trusty_share_memory(struct device *dev, trusty_shared_mem_id_t *id,
+ 			struct scatterlist *sglist, unsigned int nents,
+-			pgprot_t pgprot);
++			pgprot_t pgprot, u64 tag);
+ int trusty_share_memory_compat(struct device *dev, trusty_shared_mem_id_t *id,
+ 			       struct scatterlist *sglist, unsigned int nents,
+ 			       pgprot_t pgprot);
+-int trusty_transfer_memory(struct device *dev, u64 *id,
+-			   struct scatterlist *sglist, unsigned int nents,
+-			   pgprot_t pgprot, u64 tag, bool lend);
++int trusty_lend_memory(struct device *dev, u64 *id,
++		       struct scatterlist *sglist, unsigned int nents,
++		       pgprot_t pgprot, u64 tag);
+ int trusty_reclaim_memory(struct device *dev, trusty_shared_mem_id_t id,
+ 			  struct scatterlist *sglist, unsigned int nents);
+ 
+@@ -78,7 +79,7 @@ u64 trusty_dma_buf_get_ffa_tag(struct dma_buf *dma_buf);
+ #else
+ static inline u64 trusty_dma_buf_get_ffa_tag(struct dma_buf *dma_buf)
+ {
+-	return 0;
++	return TRUSTY_DEFAULT_MEM_OBJ_TAG;
+ }
+ #endif
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0038-ANDROID-trusty-Separate-out-SMC-based-transport.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0038-ANDROID-trusty-Separate-out-SMC-based-transport.patch
new file mode 100644
index 0000000..c4ff31c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0038-ANDROID-trusty-Separate-out-SMC-based-transport.patch
@@ -0,0 +1,495 @@
+From 844cdefb8b0f6b1f75cf4cbaa2d9260959a26e02 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 14 Jan 2022 14:02:39 +0000
+Subject: [PATCH 25/32] ANDROID: trusty: Separate out SMC based transport
+
+This commit refactors SMC based transport operation like
+smc_fastcalls, smc memory operations in a separate file.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Iebee505b7172f6247186e3bf1e0b50740b2e4dfa
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/trusty/Makefile         |   1 +
+ drivers/trusty/trusty-private.h |  61 ++++++++++++++
+ drivers/trusty/trusty-smc.c     | 136 ++++++++++++++++++++++++++++++
+ drivers/trusty/trusty.c         | 144 +++++++++-----------------------
+ 4 files changed, 237 insertions(+), 105 deletions(-)
+ create mode 100644 drivers/trusty/trusty-private.h
+ create mode 100644 drivers/trusty/trusty-smc.c
+
+diff --git a/drivers/trusty/Makefile b/drivers/trusty/Makefile
+index 2cf1cfccf97b..fbb53ee93003 100644
+--- a/drivers/trusty/Makefile
++++ b/drivers/trusty/Makefile
+@@ -5,6 +5,7 @@
+ 
+ obj-$(CONFIG_TRUSTY)		+= trusty-core.o
+ trusty-core-objs		+= trusty.o trusty-mem.o
++trusty-core-objs		+= trusty-smc.o
+ trusty-core-$(CONFIG_ARM)	+= trusty-smc-arm.o
+ trusty-core-$(CONFIG_ARM64)	+= trusty-smc-arm64.o
+ obj-$(CONFIG_TRUSTY_IRQ)	+= trusty-irq.o
+diff --git a/drivers/trusty/trusty-private.h b/drivers/trusty/trusty-private.h
+new file mode 100644
+index 000000000000..4d73c6ae35d4
+--- /dev/null
++++ b/drivers/trusty/trusty-private.h
+@@ -0,0 +1,61 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Copyright (C) 2022 ARM Ltd.
++ */
++
++#ifndef _TRUSTY_PRIVATE_H
++#define _TRUSTY_PRIVATE_H
++
++#include <linux/types.h>
++
++struct trusty_work {
++	struct trusty_state *ts;
++	struct work_struct work;
++};
++
++struct trusty_msg_ops {
++	u32 (*send_direct_msg)(struct device *dev, unsigned long fid,
++			       unsigned long a0, unsigned long a1,
++			       unsigned long a2);
++};
++
++struct trusty_mem_ops {
++	int (*trusty_share_memory)(struct device *dev, u64 *id,
++				   struct scatterlist *sglist,
++				   unsigned int nents, pgprot_t pgprot, u64 tag);
++	int (*trusty_lend_memory)(struct device *dev, u64 *id,
++				  struct scatterlist *sglist,
++				  unsigned int nents, pgprot_t pgprot, u64 tag);
++	int (*trusty_reclaim_memory)(struct device *dev, u64 id,
++				     struct scatterlist *sglist,
++				     unsigned int nents);
++};
++
++struct trusty_state {
++	struct mutex smc_lock;
++	struct atomic_notifier_head notifier;
++	struct completion cpu_idle_completion;
++	char *version_str;
++	u32 api_version;
++	bool trusty_panicked;
++	struct device *dev;
++	struct workqueue_struct *nop_wq;
++	struct trusty_work __percpu *nop_works;
++	struct list_head nop_queue;
++	spinlock_t nop_lock; /* protects nop_queue */
++	struct device_dma_parameters dma_parms;
++	const struct trusty_msg_ops *msg_ops;
++	const struct trusty_mem_ops *mem_ops;
++};
++
++int trusty_init_api_version(struct trusty_state *s, struct device *dev,
++			    u32 (*send_direct_msg)(struct device *dev,
++						   unsigned long fid,
++						   unsigned long a0,
++						   unsigned long a1,
++						   unsigned long a2));
++
++int trusty_smc_transport_setup(struct device *dev);
++void trusty_smc_transport_cleanup(struct device *dev);
++
++#endif /* _TRUSTY_PRIVATE_H */
+diff --git a/drivers/trusty/trusty-smc.c b/drivers/trusty/trusty-smc.c
+new file mode 100644
+index 000000000000..8fa841e0e253
+--- /dev/null
++++ b/drivers/trusty/trusty-smc.c
+@@ -0,0 +1,136 @@
++// SPDX-License-Identifier: GPL-2.0-only
++/*
++ * Copyright (C) 2013 Google, Inc.
++ */
++
++#include <linux/platform_device.h>
++#include <linux/trusty/smcall.h>
++#include <linux/trusty/trusty.h>
++
++#include <linux/scatterlist.h>
++#include <linux/dma-mapping.h>
++
++#include "trusty-smc.h"
++#include "trusty-private.h"
++
++static u32 trusty_smc_send_direct_msg(struct device *dev, unsigned long fid,
++				      unsigned long a0, unsigned long a1,
++				      unsigned long a2)
++{
++	return trusty_smc8(fid, a0, a1, a2, 0, 0, 0, 0).r0;
++}
++
++static int trusty_smc_share_memory(struct device *dev, u64 *id,
++				   struct scatterlist *sglist,
++				   unsigned int nents, pgprot_t pgprot, u64 tag)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++	int ret;
++	struct ns_mem_page_info pg_inf;
++	struct scatterlist *sg;
++	size_t count;
++
++	if (WARN_ON(nents < 1))
++		return -EINVAL;
++
++	if (nents != 1) {
++		dev_err(s->dev, "%s: old trusty version does not support "
++			"non-contiguous memory objects\n", __func__);
++		return -EOPNOTSUPP;
++	}
++
++	count = dma_map_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
++	if (count != nents) {
++		dev_err(s->dev, "failed to dma map sg_table\n");
++		return -EINVAL;
++	}
++
++	sg = sglist;
++	ret = trusty_encode_page_info(&pg_inf, phys_to_page(sg_dma_address(sg)),
++				      pgprot);
++	if (ret) {
++		dev_err(s->dev, "%s: trusty_encode_page_info failed\n",
++			__func__);
++		dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
++		return ret;
++	}
++
++	*id = pg_inf.compat_attr;
++	return 0;
++}
++
++static int trusty_smc_lend_memory(struct device *dev, u64 *id,
++				  struct scatterlist *sglist,
++				  unsigned int nents, pgprot_t pgprot, u64 tag)
++{
++	return -EOPNOTSUPP;
++}
++
++static int trusty_smc_reclaim_memory(struct device *dev, u64 id,
++				     struct scatterlist *sglist,
++				     unsigned int nents)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	if (WARN_ON(nents < 1))
++		return -EINVAL;
++
++	if (WARN_ON(s->api_version >= TRUSTY_API_VERSION_MEM_OBJ))
++		return -EINVAL;
++
++	if (nents != 1) {
++		dev_err(s->dev, "%s: not supported\n", __func__);
++		return -EOPNOTSUPP;
++	}
++
++	dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
++
++	dev_dbg(s->dev, "%s: done\n", __func__);
++	return 0;
++}
++
++static const struct trusty_msg_ops trusty_smc_msg_ops = {
++	.send_direct_msg = &trusty_smc_send_direct_msg,
++};
++
++static const struct trusty_mem_ops trusty_smc_mem_ops = {
++	.trusty_share_memory = &trusty_smc_share_memory,
++	.trusty_lend_memory = &trusty_smc_lend_memory,
++	.trusty_reclaim_memory = &trusty_smc_reclaim_memory,
++};
++
++int trusty_smc_transport_setup(struct device *dev)
++{
++	int rc;
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	rc = trusty_init_api_version(s, dev, &trusty_smc_send_direct_msg);
++	if (rc != 0) {
++		return rc;
++	}
++
++	/*
++	 * Initialize Trusty msg calls with Trusty SMC ABI
++	 */
++	s->msg_ops = &trusty_smc_msg_ops;
++
++	/*
++	 * Initialize Trusty memory operations with Trusty SMC ABI only when
++	 * Trusty API version is below TRUSTY_API_VERSION_MEM_OBJ.
++	 */
++	if (s->api_version < TRUSTY_API_VERSION_MEM_OBJ)
++		s->mem_ops = &trusty_smc_mem_ops;
++
++	return 0;
++}
++
++void trusty_smc_transport_cleanup(struct device *dev)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	if (s->msg_ops == &trusty_smc_msg_ops)
++		s->msg_ops = NULL;
++
++	if (s->mem_ops == &trusty_smc_mem_ops)
++		s->mem_ops = NULL;
++}
+diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
+index 6bd30bc1bbc9..0486827a45ca 100644
+--- a/drivers/trusty/trusty.c
++++ b/drivers/trusty/trusty.c
+@@ -18,37 +18,10 @@
+ #include <linux/scatterlist.h>
+ #include <linux/dma-mapping.h>
+ 
+-#include "trusty-smc.h"
++#include "trusty-private.h"
+ 
+-struct trusty_state;
+ static struct platform_driver trusty_driver;
+ 
+-struct trusty_work {
+-	struct trusty_state *ts;
+-	struct work_struct work;
+-};
+-
+-struct trusty_state {
+-	struct mutex smc_lock;
+-	struct atomic_notifier_head notifier;
+-	struct completion cpu_idle_completion;
+-	char *version_str;
+-	u32 api_version;
+-	bool trusty_panicked;
+-	struct device *dev;
+-	struct workqueue_struct *nop_wq;
+-	struct trusty_work __percpu *nop_works;
+-	struct list_head nop_queue;
+-	spinlock_t nop_lock; /* protects nop_queue */
+-	struct device_dma_parameters dma_parms;
+-};
+-
+-static inline unsigned long smc(unsigned long r0, unsigned long r1,
+-				unsigned long r2, unsigned long r3)
+-{
+-	return trusty_smc8(r0, r1, r2, r3, 0, 0, 0, 0).r0;
+-}
+-
+ s32 trusty_fast_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2)
+ {
+ 	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+@@ -60,7 +33,7 @@ s32 trusty_fast_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2)
+ 	if (WARN_ON(SMC_IS_SMC64(smcnr)))
+ 		return SM_ERR_INVALID_PARAMETERS;
+ 
+-	return smc(smcnr, a0, a1, a2);
++	return s->msg_ops->send_direct_msg(dev, smcnr, a0, a1, a2);
+ }
+ EXPORT_SYMBOL(trusty_fast_call32);
+ 
+@@ -76,7 +49,7 @@ s64 trusty_fast_call64(struct device *dev, u64 smcnr, u64 a0, u64 a1, u64 a2)
+ 	if (WARN_ON(!SMC_IS_SMC64(smcnr)))
+ 		return SM_ERR_INVALID_PARAMETERS;
+ 
+-	return smc(smcnr, a0, a1, a2);
++	return s->msg_ops->send_direct_msg(dev, smcnr, a0, a1, a2);
+ }
+ EXPORT_SYMBOL(trusty_fast_call64);
+ #endif
+@@ -88,13 +61,16 @@ static unsigned long trusty_std_call_inner(struct device *dev,
+ {
+ 	unsigned long ret;
+ 	int retry = 5;
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+ 
+ 	dev_dbg(dev, "%s(0x%lx 0x%lx 0x%lx 0x%lx)\n",
+ 		__func__, smcnr, a0, a1, a2);
+ 	while (true) {
+-		ret = smc(smcnr, a0, a1, a2);
++		ret = s->msg_ops->send_direct_msg(dev, smcnr, a0, a1, a2);
+ 		while ((s32)ret == SM_ERR_FIQ_INTERRUPTED)
+-			ret = smc(SMC_SC_RESTART_FIQ, 0, 0, 0);
++			ret = s->msg_ops->send_direct_msg(dev,
++							  SMC_SC_RESTART_FIQ,
++							  0, 0, 0);
+ 		if ((int)ret != SM_ERR_BUSY || !retry)
+ 			break;
+ 
+@@ -222,58 +198,17 @@ s32 trusty_std_call32(struct device *dev, u32 smcnr, u32 a0, u32 a1, u32 a2)
+ }
+ EXPORT_SYMBOL(trusty_std_call32);
+ 
+-static int __trusty_share_memory(struct device *dev, u64 *id,
+-				 struct scatterlist *sglist, unsigned int nents,
+-				 pgprot_t pgprot, u64 tag, bool mem_share)
++int trusty_share_memory(struct device *dev, u64 *id,
++			struct scatterlist *sglist, unsigned int nents,
++			pgprot_t pgprot, u64 tag)
+ {
+ 	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+-	int ret;
+-	struct ns_mem_page_info pg_inf;
+-	struct scatterlist *sg;
+-	size_t count;
+ 
+ 	if (WARN_ON(dev->driver != &trusty_driver.driver))
+ 		return -EINVAL;
+ 
+-	if (WARN_ON(nents < 1))
+-		return -EINVAL;
+-
+-	if (nents != 1 && s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
+-		dev_err(s->dev, "%s: old trusty version does not support non-contiguous memory objects\n",
+-			__func__);
+-		return -EOPNOTSUPP;
+-	}
+-
+-	if (mem_share == false && s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
+-		dev_err(s->dev, "%s: old trusty version does not support lending memory objects\n",
+-			__func__);
+-		return -EOPNOTSUPP;
+-	}
+-
+-	count = dma_map_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
+-	if (count != nents) {
+-		dev_err(s->dev, "failed to dma map sg_table\n");
+-		return -EINVAL;
+-	}
+-
+-	sg = sglist;
+-	ret = trusty_encode_page_info(&pg_inf, phys_to_page(sg_dma_address(sg)),
+-				      pgprot);
+-	if (ret) {
+-		dev_err(s->dev, "%s: trusty_encode_page_info failed\n",
+-			__func__);
+-		return ret;
+-	}
+-
+-	*id = pg_inf.compat_attr;
+-	return 0;
+-}
+-
+-int trusty_share_memory(struct device *dev, u64 *id,
+-			struct scatterlist *sglist, unsigned int nents,
+-			pgprot_t pgprot, u64 tag)
+-{
+-	return __trusty_share_memory(dev, id, sglist, nents, pgprot, tag, true);
++	return s->mem_ops->trusty_share_memory(dev, id, sglist, nents, pgprot,
++					       tag);
+ }
+ EXPORT_SYMBOL(trusty_share_memory);
+ 
+@@ -281,7 +216,13 @@ int trusty_lend_memory(struct device *dev, u64 *id,
+ 		       struct scatterlist *sglist, unsigned int nents,
+ 		       pgprot_t pgprot, u64 tag)
+ {
+-	return __trusty_share_memory(dev, id, sglist, nents, pgprot, tag, false);
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	if (WARN_ON(dev->driver != &trusty_driver.driver))
++		return -EINVAL;
++
++	return s->mem_ops->trusty_lend_memory(dev, id, sglist, nents, pgprot,
++					      tag);
+ }
+ EXPORT_SYMBOL(trusty_lend_memory);
+ 
+@@ -316,22 +257,7 @@ int trusty_reclaim_memory(struct device *dev, u64 id,
+ 	if (WARN_ON(dev->driver != &trusty_driver.driver))
+ 		return -EINVAL;
+ 
+-	if (WARN_ON(nents < 1))
+-		return -EINVAL;
+-
+-	if (s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
+-		if (nents != 1) {
+-			dev_err(s->dev, "%s: not supported\n", __func__);
+-			return -EOPNOTSUPP;
+-		}
+-
+-		dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
+-
+-		dev_dbg(s->dev, "%s: done\n", __func__);
+-		return 0;
+-	}
+-
+-	return 0;
++	return s->mem_ops->trusty_reclaim_memory(dev, id, sglist, nents);
+ }
+ EXPORT_SYMBOL(trusty_reclaim_memory);
+ 
+@@ -382,7 +308,7 @@ const char *trusty_version_str_get(struct device *dev)
+ }
+ EXPORT_SYMBOL(trusty_version_str_get);
+ 
+-static void trusty_init_version(struct trusty_state *s, struct device *dev)
++static void trusty_init_version_str(struct trusty_state *s, struct device *dev)
+ {
+ 	int ret;
+ 	int i;
+@@ -430,12 +356,17 @@ bool trusty_get_panic_status(struct device *dev)
+ }
+ EXPORT_SYMBOL(trusty_get_panic_status);
+ 
+-static int trusty_init_api_version(struct trusty_state *s, struct device *dev)
++int trusty_init_api_version(struct trusty_state *s, struct device *dev,
++			    u32 (*send_direct_msg)(struct device *dev,
++						   unsigned long fid,
++						   unsigned long a0,
++						   unsigned long a1,
++						   unsigned long a2))
+ {
+ 	u32 api_version;
+ 
+-	api_version = trusty_fast_call32(dev, SMC_FC_API_VERSION,
+-					 TRUSTY_API_VERSION_CURRENT, 0, 0);
++	api_version = send_direct_msg(dev, SMC_FC_API_VERSION,
++				      TRUSTY_API_VERSION_CURRENT, 0, 0);
+ 	if (api_version == SM_ERR_UNDEFINED_SMC)
+ 		api_version = 0;
+ 
+@@ -598,11 +529,12 @@ static int trusty_probe(struct platform_device *pdev)
+ 
+ 	platform_set_drvdata(pdev, s);
+ 
+-	trusty_init_version(s, &pdev->dev);
++	/* Initialize SMC transport */
++	ret = trusty_smc_transport_setup(s->dev);
++	if (ret != 0 || s->msg_ops == NULL || s->mem_ops == NULL)
++		goto err_transport_setup;
+ 
+-	ret = trusty_init_api_version(s, &pdev->dev);
+-	if (ret < 0)
+-		goto err_api_version;
++	trusty_init_version_str(s, &pdev->dev);
+ 
+ 	s->nop_wq = alloc_workqueue("trusty-nop-wq", WQ_CPU_INTENSIVE, 0);
+ 	if (!s->nop_wq) {
+@@ -648,9 +580,10 @@ static int trusty_probe(struct platform_device *pdev)
+ err_alloc_works:
+ 	destroy_workqueue(s->nop_wq);
+ err_create_nop_wq:
+-err_api_version:
+-	s->dev->dma_parms = NULL;
+ 	kfree(s->version_str);
++        trusty_smc_transport_cleanup(s->dev);
++err_transport_setup:
++	s->dev->dma_parms = NULL;
+ 	device_for_each_child(&pdev->dev, NULL, trusty_remove_child);
+ 	mutex_destroy(&s->smc_lock);
+ 	kfree(s);
+@@ -673,6 +606,7 @@ static int trusty_remove(struct platform_device *pdev)
+ 	free_percpu(s->nop_works);
+ 	destroy_workqueue(s->nop_wq);
+ 
++	trusty_smc_transport_cleanup(s->dev);
+ 	mutex_destroy(&s->smc_lock);
+ 	s->dev->dma_parms = NULL;
+ 	kfree(s->version_str);
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0039-ANDROID-trusty-Modify-device-compatible-string.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0039-ANDROID-trusty-Modify-device-compatible-string.patch
new file mode 100644
index 0000000..8fd1c7c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0039-ANDROID-trusty-Modify-device-compatible-string.patch
@@ -0,0 +1,56 @@
+From 5566c2a41443e26068fe3a8e4a8e4b0c3a4e8ed6 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 14 Jan 2022 14:22:42 +0000
+Subject: [PATCH 26/32] ANDROID: trusty: Modify device compatible string
+
+Drop smc keyword from device tree node as Trusty can use SMC or FFA
+based transport.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Id99b52f32a2122434a22f1991c0b4cd52b0676ed
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ Documentation/devicetree/bindings/trusty/trusty-irq.txt | 2 +-
+ Documentation/devicetree/bindings/trusty/trusty-smc.txt | 2 +-
+ drivers/trusty/trusty.c                                 | 2 +-
+ 3 files changed, 3 insertions(+), 3 deletions(-)
+
+diff --git a/Documentation/devicetree/bindings/trusty/trusty-irq.txt b/Documentation/devicetree/bindings/trusty/trusty-irq.txt
+index cbb545ad452b..ae02030be4e7 100644
+--- a/Documentation/devicetree/bindings/trusty/trusty-irq.txt
++++ b/Documentation/devicetree/bindings/trusty/trusty-irq.txt
+@@ -48,7 +48,7 @@ Example:
+ 	};
+ 	...
+ 	trusty {
+-		compatible = "android,trusty-smc-v1";
++		compatible = "android,trusty-v1";
+ 		ranges;
+ 		#address-cells = <2>;
+ 		#size-cells = <2>;
+diff --git a/Documentation/devicetree/bindings/trusty/trusty-smc.txt b/Documentation/devicetree/bindings/trusty/trusty-smc.txt
+index 1b39ad317c67..8d02a31ba814 100644
+--- a/Documentation/devicetree/bindings/trusty/trusty-smc.txt
++++ b/Documentation/devicetree/bindings/trusty/trusty-smc.txt
+@@ -3,4 +3,4 @@ Trusty smc interface
+ Trusty is running in secure mode on the same (arm) cpu(s) as the current os.
+ 
+ Required properties:
+-- compatible: "android,trusty-smc-v1"
++- compatible: "android,trusty-v1"
+diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
+index 0486827a45ca..757dd7b2c527 100644
+--- a/drivers/trusty/trusty.c
++++ b/drivers/trusty/trusty.c
+@@ -615,7 +615,7 @@ static int trusty_remove(struct platform_device *pdev)
+ }
+ 
+ static const struct of_device_id trusty_of_match[] = {
+-	{ .compatible = "android,trusty-smc-v1", },
++	{ .compatible = "android,trusty-v1", },
+ 	{},
+ };
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0040-ANDROID-trusty-Add-transport-descriptor.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0040-ANDROID-trusty-Add-transport-descriptor.patch
new file mode 100644
index 0000000..53c76be
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0040-ANDROID-trusty-Add-transport-descriptor.patch
@@ -0,0 +1,209 @@
+From 27248b5c8cb5c1a59b08e46eb3ab918867282c1c Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 14 Jan 2022 17:52:33 +0000
+Subject: [PATCH 27/32] ANDROID: trusty: Add transport descriptor
+
+Use transport descriptor to hold transport specific operations. This
+helps to add new transport to Trusty core.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Ibbde50de0302f6d259a7d572f0910067ce712b37
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/trusty/trusty-private.h | 20 +++++++++-
+ drivers/trusty/trusty-smc.c     |  6 +++
+ drivers/trusty/trusty.c         | 71 ++++++++++++++++++++++++++++++---
+ 3 files changed, 90 insertions(+), 7 deletions(-)
+
+diff --git a/drivers/trusty/trusty-private.h b/drivers/trusty/trusty-private.h
+index 4d73c6ae35d4..74b88bb8f83b 100644
+--- a/drivers/trusty/trusty-private.h
++++ b/drivers/trusty/trusty-private.h
+@@ -14,12 +14,14 @@ struct trusty_work {
+ };
+ 
+ struct trusty_msg_ops {
++	const struct trusty_transport_desc *desc;
+ 	u32 (*send_direct_msg)(struct device *dev, unsigned long fid,
+ 			       unsigned long a0, unsigned long a1,
+ 			       unsigned long a2);
+ };
+ 
+ struct trusty_mem_ops {
++	const struct trusty_transport_desc *desc;
+ 	int (*trusty_share_memory)(struct device *dev, u64 *id,
+ 				   struct scatterlist *sglist,
+ 				   unsigned int nents, pgprot_t pgprot, u64 tag);
+@@ -46,6 +48,19 @@ struct trusty_state {
+ 	struct device_dma_parameters dma_parms;
+ 	const struct trusty_msg_ops *msg_ops;
+ 	const struct trusty_mem_ops *mem_ops;
++	struct trusty_ffa_state *ffa;
++};
++
++struct trusty_ffa_state {
++	struct device *dev; /* ffa device */
++	const struct ffa_dev_ops *ops;
++	struct mutex share_memory_msg_lock; /* protects share_memory_msg */
++};
++
++struct trusty_transport_desc {
++	const char *name;
++	int (*setup)(struct device *dev);
++	void (*cleanup)(struct device *dev);
+ };
+ 
+ int trusty_init_api_version(struct trusty_state *s, struct device *dev,
+@@ -55,7 +70,8 @@ int trusty_init_api_version(struct trusty_state *s, struct device *dev,
+ 						   unsigned long a1,
+ 						   unsigned long a2));
+ 
+-int trusty_smc_transport_setup(struct device *dev);
+-void trusty_smc_transport_cleanup(struct device *dev);
++typedef const struct trusty_transport_desc *trusty_transports_t;
++
++extern const struct trusty_transport_desc trusty_smc_transport;
+ 
+ #endif /* _TRUSTY_PRIVATE_H */
+diff --git a/drivers/trusty/trusty-smc.c b/drivers/trusty/trusty-smc.c
+index 8fa841e0e253..62d1d7060744 100644
+--- a/drivers/trusty/trusty-smc.c
++++ b/drivers/trusty/trusty-smc.c
+@@ -134,3 +134,9 @@ void trusty_smc_transport_cleanup(struct device *dev)
+ 	if (s->mem_ops == &trusty_smc_mem_ops)
+ 		s->mem_ops = NULL;
+ }
++
++const struct trusty_transport_desc trusty_smc_transport = {
++	.name = "smc",
++	.setup = trusty_smc_transport_setup,
++	.cleanup = trusty_smc_transport_cleanup,
++};
+diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
+index 757dd7b2c527..ec0fccfaa24c 100644
+--- a/drivers/trusty/trusty.c
++++ b/drivers/trusty/trusty.c
+@@ -493,6 +493,46 @@ void trusty_dequeue_nop(struct device *dev, struct trusty_nop *nop)
+ }
+ EXPORT_SYMBOL(trusty_dequeue_nop);
+ 
++static int
++trusty_transports_setup(const trusty_transports_t *transports,
++			struct device *dev)
++{
++	const struct trusty_transport_desc *transport;
++	int ret;
++	int transports_ret = -ENODEV;
++
++	if (!transports)
++		return -EINVAL;
++
++	for (; (transport = *transports); transports++) {
++		if (!transport->setup)
++			return -EINVAL;
++
++		ret = transport->setup(dev);
++		transports_ret &= ret;
++	}
++
++	/* One transport needs to complete setup without error. */
++	if (transports_ret < 0)
++		return -ENODEV;
++
++	return 0;
++}
++
++static void
++trusty_transports_cleanup(const trusty_transports_t *transports,
++			  struct device *dev)
++{
++	const struct trusty_transport_desc *transport;
++
++	for (; (transport = *transports); transports++) {
++		if (!transport->cleanup)
++			continue;
++
++		transport->cleanup(dev);
++	}
++}
++
+ static int trusty_probe(struct platform_device *pdev)
+ {
+ 	int ret;
+@@ -500,6 +540,7 @@ static int trusty_probe(struct platform_device *pdev)
+ 	work_func_t work_func;
+ 	struct trusty_state *s;
+ 	struct device_node *node = pdev->dev.of_node;
++	const trusty_transports_t *descs;
+ 
+ 	if (!node) {
+ 		dev_err(&pdev->dev, "of_node required\n");
+@@ -529,8 +570,12 @@ static int trusty_probe(struct platform_device *pdev)
+ 
+ 	platform_set_drvdata(pdev, s);
+ 
+-	/* Initialize SMC transport */
+-	ret = trusty_smc_transport_setup(s->dev);
++	/*
++	 * Initialize Trusty transport. Trusty msg and mem ops has to be
++	 * initialized as part of transport setup.
++	 */
++	descs = of_device_get_match_data(&pdev->dev);
++	ret = trusty_transports_setup(descs, s->dev);
+ 	if (ret != 0 || s->msg_ops == NULL || s->mem_ops == NULL)
+ 		goto err_transport_setup;
+ 
+@@ -581,7 +626,7 @@ static int trusty_probe(struct platform_device *pdev)
+ 	destroy_workqueue(s->nop_wq);
+ err_create_nop_wq:
+ 	kfree(s->version_str);
+-        trusty_smc_transport_cleanup(s->dev);
++	trusty_transports_cleanup(descs, s->dev);
+ err_transport_setup:
+ 	s->dev->dma_parms = NULL;
+ 	device_for_each_child(&pdev->dev, NULL, trusty_remove_child);
+@@ -595,6 +640,7 @@ static int trusty_remove(struct platform_device *pdev)
+ {
+ 	unsigned int cpu;
+ 	struct trusty_state *s = platform_get_drvdata(pdev);
++	const trusty_transports_t *descs;
+ 
+ 	device_for_each_child(&pdev->dev, NULL, trusty_remove_child);
+ 
+@@ -606,7 +652,10 @@ static int trusty_remove(struct platform_device *pdev)
+ 	free_percpu(s->nop_works);
+ 	destroy_workqueue(s->nop_wq);
+ 
+-	trusty_smc_transport_cleanup(s->dev);
++	/* call transport cleanup */
++	descs = of_device_get_match_data(&pdev->dev);
++	trusty_transports_cleanup(descs, s->dev);
++
+ 	mutex_destroy(&s->smc_lock);
+ 	s->dev->dma_parms = NULL;
+ 	kfree(s->version_str);
+@@ -614,8 +663,20 @@ static int trusty_remove(struct platform_device *pdev)
+ 	return 0;
+ }
+ 
++/*
++ * Trusty probe will try all compiled in transports and will use the transport
++ * supported by the Trusty kernel.
++ *
++ * For Trusty API version < TRUSTY_API_VERSION_MEM_OBJ:
++ *     trusty_smc_transport used for messaging.
++ */
++static const trusty_transports_t trusty_transports[] = {
++	&trusty_smc_transport,
++	NULL,
++};
++
+ static const struct of_device_id trusty_of_match[] = {
+-	{ .compatible = "android,trusty-v1", },
++	{ .compatible = "android,trusty-v1", .data = trusty_transports },
+ 	{},
+ };
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0041-ANDROID-trusty-Add-trusty-ffa-driver.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0041-ANDROID-trusty-Add-trusty-ffa-driver.patch
new file mode 100644
index 0000000..a61d2c8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0041-ANDROID-trusty-Add-trusty-ffa-driver.patch
@@ -0,0 +1,312 @@
+From 3104eb14f62df1c7c4b9038eb914514b0ff371dc Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Fri, 14 Jan 2022 18:47:08 +0000
+Subject: [PATCH 28/32] ANDROID: trusty: Add trusty-ffa driver
+
+Initial changes related to FFA transport support
+  - Adds FFA transport descriptor
+  - Defines Trusty UUID
+  - Initializes FFA transport does probe, sets ffa_ops
+  - Defers Trusty probe if ARM FF-A driver is not initialized or
+    Trusty SP not found.
+  - Link FF-A device as the supplier for Trusty platform device.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I78f72b85c20e4bad4c24cf0826e96f27dcf2ee1d
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/trusty/Makefile         |   1 +
+ drivers/trusty/trusty-ffa.c     | 196 ++++++++++++++++++++++++++++++++
+ drivers/trusty/trusty-ffa.h     |  28 +++++
+ drivers/trusty/trusty-private.h |   1 +
+ drivers/trusty/trusty.c         |   6 +
+ 5 files changed, 232 insertions(+)
+ create mode 100644 drivers/trusty/trusty-ffa.c
+ create mode 100644 drivers/trusty/trusty-ffa.h
+
+diff --git a/drivers/trusty/Makefile b/drivers/trusty/Makefile
+index fbb53ee93003..797d61bf68ef 100644
+--- a/drivers/trusty/Makefile
++++ b/drivers/trusty/Makefile
+@@ -6,6 +6,7 @@
+ obj-$(CONFIG_TRUSTY)		+= trusty-core.o
+ trusty-core-objs		+= trusty.o trusty-mem.o
+ trusty-core-objs		+= trusty-smc.o
++trusty-core-objs		+= trusty-ffa.o
+ trusty-core-$(CONFIG_ARM)	+= trusty-smc-arm.o
+ trusty-core-$(CONFIG_ARM64)	+= trusty-smc-arm64.o
+ obj-$(CONFIG_TRUSTY_IRQ)	+= trusty-irq.o
+diff --git a/drivers/trusty/trusty-ffa.c b/drivers/trusty/trusty-ffa.c
+new file mode 100644
+index 000000000000..c8c16a1fc700
+--- /dev/null
++++ b/drivers/trusty/trusty-ffa.c
+@@ -0,0 +1,196 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Copyright (C) 2022 ARM Ltd.
++ */
++
++#include <linux/platform_device.h>
++#include <linux/slab.h>
++#include <linux/trusty/smcall.h>
++#include <linux/arm_ffa.h>
++#include <linux/trusty/trusty.h>
++
++#include <linux/scatterlist.h>
++#include <linux/dma-mapping.h>
++
++#include "trusty-ffa.h"
++#include "trusty-private.h"
++
++static const struct trusty_mem_ops trusty_ffa_mem_ops = {
++	.desc = &trusty_ffa_transport,
++};
++
++static const struct ffa_device_id trusty_ffa_device_id[] = {
++	/*
++	 * Trusty UID: RFC-4122 compliant UUID version 4
++	 * 40ee25f0-a2bc-304c-8c4ca173c57d8af1
++	 */
++	{ UUID_INIT(0x40ee25f0, 0xa2bc, 0x304c,
++		    0x8c, 0x4c, 0xa1, 0x73, 0xc5, 0x7d, 0x8a, 0xf1) },
++	{}
++};
++
++static int trusty_ffa_dev_match(struct device *dev, const void *uuid)
++{
++	struct ffa_device *ffa_dev;
++
++	ffa_dev = to_ffa_dev(dev);
++	if (uuid_equal(&ffa_dev->uuid, uuid))
++		return 1;
++
++	return 0;
++}
++
++static struct ffa_device *trusty_ffa_dev_find(void)
++{
++	const void *data;
++	struct device *dev;
++
++	/* currently only one trusty instance is probed */
++	data = &trusty_ffa_device_id[0].uuid;
++
++	dev = bus_find_device(&ffa_bus_type, NULL, data, trusty_ffa_dev_match);
++	if (dev) {
++		/* drop reference count */
++		put_device(dev);
++		return to_ffa_dev(dev);
++	}
++
++	return NULL;
++}
++
++static int trusty_ffa_link_supplier(struct device *c_dev, struct device *s_dev)
++{
++	if (!c_dev || !s_dev)
++		return -EINVAL;
++
++	if (!device_link_add(c_dev, s_dev, DL_FLAG_AUTOREMOVE_CONSUMER)) {
++		return -ENODEV;
++	}
++
++	return 0;
++}
++
++/*
++ * called from trusty probe
++ */
++static int trusty_ffa_transport_setup(struct device *dev)
++{
++	int rc;
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++	struct trusty_ffa_state *ffa_state;
++	struct ffa_device *ffa_dev;
++
++	/* ffa transport not required for lower api versions */
++	if (s->api_version != 0 && s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
++		return -EINVAL;
++	}
++
++	ffa_dev = trusty_ffa_dev_find();
++	if (!ffa_dev) {
++		dev_dbg(dev, "FFA: Trusty device not found defer probe\n");
++		return -EPROBE_DEFER;
++	}
++
++	ffa_state = ffa_dev_get_drvdata(ffa_dev);
++	if (!ffa_state)
++		return -EINVAL;
++
++	rc = trusty_ffa_link_supplier(dev, &ffa_dev->dev);
++	if (rc != 0)
++		return rc;
++
++	/* FFA used only for memory sharing operations */
++	if (s->api_version == TRUSTY_API_VERSION_MEM_OBJ) {
++		s->ffa = ffa_state;
++		s->mem_ops = &trusty_ffa_mem_ops;
++		return 0;
++	}
++
++	return -EINVAL;
++}
++
++static void trusty_ffa_transport_cleanup(struct device *dev)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++
++	/* ffa transport not setup for lower api versions */
++	if (s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
++		return;
++	}
++
++	s->ffa = NULL;
++	s->mem_ops = NULL;
++}
++
++static int trusty_ffa_probe(struct ffa_device *ffa_dev)
++{
++	const struct ffa_dev_ops *ffa_ops;
++	struct trusty_ffa_state *s;
++	u32 ffa_drv_version;
++
++	ffa_ops = ffa_dev_ops_get(ffa_dev);
++	if (!ffa_ops) {
++		dev_dbg(&ffa_dev->dev, "ffa_dev_ops_get: failed\n");
++		return -ENOENT;
++	}
++
++	/* check ffa driver version compatibility */
++	ffa_drv_version = ffa_ops->api_version_get();
++	if (TO_TRUSTY_FFA_MAJOR(ffa_drv_version) != TRUSTY_FFA_VERSION_MAJOR ||
++	    TO_TRUSTY_FFA_MINOR(ffa_drv_version) < TRUSTY_FFA_VERSION_MINOR)
++		return -EINVAL;
++
++	s = kzalloc(sizeof(*s), GFP_KERNEL);
++	if (!s)
++		return -ENOMEM;
++
++	s->dev = &ffa_dev->dev;
++	s->ops = ffa_ops;
++	mutex_init(&s->share_memory_msg_lock);
++	ffa_dev_set_drvdata(ffa_dev, s);
++
++	ffa_ops->mode_32bit_set(ffa_dev);
++
++	return 0;
++}
++
++static void trusty_ffa_remove(struct ffa_device *ffa_dev)
++{
++	struct trusty_ffa_state *s;
++
++	s = ffa_dev_get_drvdata(ffa_dev);
++
++	mutex_destroy(&s->share_memory_msg_lock);
++	memset(s, 0, sizeof(struct trusty_ffa_state));
++	kfree(s);
++}
++
++static struct ffa_driver trusty_ffa_driver = {
++	.name = "trusty-ffa",
++	.probe = trusty_ffa_probe,
++	.remove = trusty_ffa_remove,
++	.id_table = trusty_ffa_device_id,
++};
++
++static int __init trusty_ffa_transport_init(void)
++{
++	if (IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT)) {
++		return ffa_register(&trusty_ffa_driver);
++	} else
++		return -ENODEV;
++}
++
++static void __exit trusty_ffa_transport_exit(void)
++{
++	if (IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
++		ffa_unregister(&trusty_ffa_driver);
++}
++
++const struct trusty_transport_desc trusty_ffa_transport = {
++	.name = "ffa",
++	.setup = trusty_ffa_transport_setup,
++	.cleanup = trusty_ffa_transport_cleanup,
++};
++
++module_init(trusty_ffa_transport_init);
++module_exit(trusty_ffa_transport_exit);
+diff --git a/drivers/trusty/trusty-ffa.h b/drivers/trusty/trusty-ffa.h
+new file mode 100644
+index 000000000000..267ca2c5db29
+--- /dev/null
++++ b/drivers/trusty/trusty-ffa.h
+@@ -0,0 +1,28 @@
++/* SPDX-License-Identifier: GPL-2.0-only */
++/*
++ * Copyright (C) 2022 ARM Ltd.
++ */
++
++#ifndef __LINUX_TRUSTY_FFA_H
++#define __LINUX_TRUSTY_FFA_H
++
++#include <linux/types.h>
++#include <linux/uuid.h>
++#include <linux/arm_ffa.h>
++
++#define TRUSTY_FFA_VERSION_MAJOR	(1U)
++#define TRUSTY_FFA_VERSION_MINOR	(0U)
++#define TRUSTY_FFA_VERSION_MAJOR_SHIFT	(16U)
++#define TRUSTY_FFA_VERSION_MAJOR_MASK	(0x7fffU)
++#define TRUSTY_FFA_VERSION_MINOR_SHIFT	(0U)
++#define TRUSTY_FFA_VERSION_MINOR_MASK	(0U)
++
++#define TO_TRUSTY_FFA_MAJOR(v)					\
++	  ((u16)((v >> TRUSTY_FFA_VERSION_MAJOR_SHIFT) &	\
++		 TRUSTY_FFA_VERSION_MAJOR_MASK))
++
++#define TO_TRUSTY_FFA_MINOR(v)					\
++	  ((u16)((v >> TRUSTY_FFA_VERSION_MINOR_SHIFT) &	\
++		 TRUSTY_FFA_VERSION_MINOR_MASK))
++
++#endif /* __LINUX_TRUSTY_FFA_H */
+diff --git a/drivers/trusty/trusty-private.h b/drivers/trusty/trusty-private.h
+index 74b88bb8f83b..2496f397e5d2 100644
+--- a/drivers/trusty/trusty-private.h
++++ b/drivers/trusty/trusty-private.h
+@@ -73,5 +73,6 @@ int trusty_init_api_version(struct trusty_state *s, struct device *dev,
+ typedef const struct trusty_transport_desc *trusty_transports_t;
+ 
+ extern const struct trusty_transport_desc trusty_smc_transport;
++extern const struct trusty_transport_desc trusty_ffa_transport;
+ 
+ #endif /* _TRUSTY_PRIVATE_H */
+diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
+index ec0fccfaa24c..4686b0d34f61 100644
+--- a/drivers/trusty/trusty.c
++++ b/drivers/trusty/trusty.c
+@@ -509,6 +509,11 @@ trusty_transports_setup(const trusty_transports_t *transports,
+ 			return -EINVAL;
+ 
+ 		ret = transport->setup(dev);
++		if (ret == -EPROBE_DEFER) {
++			dev_notice(dev, "transport %s: defer probe\n",
++				   transport->name);
++			return ret;
++		}
+ 		transports_ret &= ret;
+ 	}
+ 
+@@ -672,6 +677,7 @@ static int trusty_remove(struct platform_device *pdev)
+  */
+ static const trusty_transports_t trusty_transports[] = {
+ 	&trusty_smc_transport,
++	&trusty_ffa_transport,
+ 	NULL,
+ };
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0042-ANDROID-trusty-ffa-Add-support-for-FFA-memory-operat.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0042-ANDROID-trusty-ffa-Add-support-for-FFA-memory-operat.patch
new file mode 100644
index 0000000..2b15009
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0042-ANDROID-trusty-ffa-Add-support-for-FFA-memory-operat.patch
@@ -0,0 +1,151 @@
+From c552f8ed6bbd68e838732598ca74055bb696dcb3 Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Tue, 18 Jan 2022 15:11:46 +0000
+Subject: [PATCH 29/32] ANDROID: trusty-ffa: Add support for FFA memory
+ operations
+
+Initializes Trusty mem_ops with FFA memory operations for share,
+lend, reclaim.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Id3a1eb5ae8e4721cb983c624773d39bafe25a77d
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/trusty/trusty-ffa.c | 102 ++++++++++++++++++++++++++++++++++++
+ drivers/trusty/trusty.c     |   5 ++
+ 2 files changed, 107 insertions(+)
+
+diff --git a/drivers/trusty/trusty-ffa.c b/drivers/trusty/trusty-ffa.c
+index c8c16a1fc700..0655b3887b52 100644
+--- a/drivers/trusty/trusty-ffa.c
++++ b/drivers/trusty/trusty-ffa.c
+@@ -15,8 +15,110 @@
+ #include "trusty-ffa.h"
+ #include "trusty-private.h"
+ 
++static int __trusty_ffa_share_memory(struct device *dev, u64 *id,
++				     struct scatterlist *sglist,
++				     unsigned int nents, pgprot_t pgprot,
++				     u64 tag, bool share)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++	int ret;
++	struct scatterlist *sg;
++	size_t count;
++	struct ffa_device *ffa_dev = to_ffa_dev(s->ffa->dev);
++	const struct ffa_dev_ops *ffa_ops = s->ffa->ops;
++	struct ffa_mem_region_attributes ffa_mem_attr;
++	struct ffa_mem_ops_args ffa_mem_args;
++
++	if (WARN_ON(nents < 1))
++		return -EINVAL;
++
++	count = dma_map_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
++	if (count != nents) {
++		dev_err(s->dev, "failed to dma map sg_table\n");
++		return -EINVAL;
++	}
++
++	sg = sglist;
++
++	mutex_lock(&s->ffa->share_memory_msg_lock);
++
++	ffa_mem_attr.receiver = ffa_dev->vm_id;
++	ffa_mem_attr.attrs = FFA_MEM_RW;
++
++	ffa_mem_args.use_txbuf = 1;
++	ffa_mem_args.tag = tag;
++	ffa_mem_args.attrs = &ffa_mem_attr;
++	ffa_mem_args.nattrs = 1;
++	ffa_mem_args.sg = sg;
++	ffa_mem_args.flags = 0;
++
++	if (share) {
++		ret = ffa_ops->memory_share(ffa_dev, &ffa_mem_args);
++	} else {
++		ret = ffa_ops->memory_lend(ffa_dev, &ffa_mem_args);
++	}
++
++	mutex_unlock(&s->ffa->share_memory_msg_lock);
++
++	if (!ret) {
++		*id = ffa_mem_args.g_handle;
++		dev_dbg(s->dev, "%s: done\n", __func__);
++		return 0;
++	}
++
++	dev_err(s->dev, "%s: failed %d", __func__, ret);
++
++	dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
++	return ret;
++}
++
++static int trusty_ffa_share_memory(struct device *dev, u64 *id,
++				   struct scatterlist *sglist,
++				   unsigned int nents, pgprot_t pgprot, u64 tag)
++{
++	return __trusty_ffa_share_memory(dev, id, sglist, nents, pgprot, tag,
++					 true);
++}
++
++static int trusty_ffa_lend_memory(struct device *dev, u64 *id,
++				  struct scatterlist *sglist,
++				  unsigned int nents, pgprot_t pgprot, u64 tag)
++{
++	return __trusty_ffa_share_memory(dev, id, sglist, nents, pgprot, tag,
++					 false);
++}
++
++static int trusty_ffa_reclaim_memory(struct device *dev, u64 id,
++				     struct scatterlist *sglist,
++				     unsigned int nents)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++	int ret = 0;
++	const struct ffa_dev_ops *ffa_ops = s->ffa->ops;
++
++	if (WARN_ON(nents < 1))
++		return -EINVAL;
++
++	mutex_lock(&s->ffa->share_memory_msg_lock);
++
++	ret = ffa_ops->memory_reclaim(id, 0);
++
++	mutex_unlock(&s->ffa->share_memory_msg_lock);
++
++	if (ret != 0)
++		return ret;
++
++	dma_unmap_sg(dev, sglist, nents, DMA_BIDIRECTIONAL);
++
++	dev_dbg(s->dev, "%s: done\n", __func__);
++	return 0;
++}
++
+ static const struct trusty_mem_ops trusty_ffa_mem_ops = {
+ 	.desc = &trusty_ffa_transport,
++	.trusty_share_memory = &trusty_ffa_share_memory,
++	.trusty_lend_memory = &trusty_ffa_lend_memory,
++	.trusty_reclaim_memory = &trusty_ffa_reclaim_memory,
+ };
+ 
+ static const struct ffa_device_id trusty_ffa_device_id[] = {
+diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
+index 4686b0d34f61..f91c255c9897 100644
+--- a/drivers/trusty/trusty.c
++++ b/drivers/trusty/trusty.c
+@@ -674,6 +674,11 @@ static int trusty_remove(struct platform_device *pdev)
+  *
+  * For Trusty API version < TRUSTY_API_VERSION_MEM_OBJ:
+  *     trusty_smc_transport used for messaging.
++ *
++ * For Trusty API version == TRUSTY_API_VERSION_MEM_OBJ:
++ *     trusty_smc_transport used for messaging.
++ *     trusty_ffa_transport used for memory sharing.
++ *
+  */
+ static const trusty_transports_t trusty_transports[] = {
+ 	&trusty_smc_transport,
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0043-ANDROID-trusty-ffa-Enable-FFA-transport-for-both-mem.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0043-ANDROID-trusty-ffa-Enable-FFA-transport-for-both-mem.patch
new file mode 100644
index 0000000..2c1623a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0043-ANDROID-trusty-ffa-Enable-FFA-transport-for-both-mem.patch
@@ -0,0 +1,142 @@
+From e67cd78142984c7c4120f15ef14e1e026746af5a Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Thu, 3 Feb 2022 11:19:38 +0000
+Subject: [PATCH 30/32] ANDROID: trusty-ffa: Enable FFA transport for both
+ memory and message ops
+
+Adds new API version TRUSTY_API_VERSION_FFA and sets this as current
+API version.
+
+If Trusty on the secure side supports receipt of FFA direct request,
+then trusty core uses FFA calls for messages and memory operations.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: I4a8b060f906a96935a7df10713026fb543e2b9a7
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/trusty/trusty-ffa.c   | 58 +++++++++++++++++++++++++++++++++++
+ drivers/trusty/trusty.c       |  3 ++
+ include/linux/trusty/smcall.h |  3 +-
+ 3 files changed, 63 insertions(+), 1 deletion(-)
+
+diff --git a/drivers/trusty/trusty-ffa.c b/drivers/trusty/trusty-ffa.c
+index 0655b3887b52..543f5a0d31cb 100644
+--- a/drivers/trusty/trusty-ffa.c
++++ b/drivers/trusty/trusty-ffa.c
+@@ -15,6 +15,36 @@
+ #include "trusty-ffa.h"
+ #include "trusty-private.h"
+ 
++/* partition property: Supports receipt of direct requests */
++#define FFA_PARTITION_DIRECT_REQ_RECV	BIT(0)
++
++/* string representation of trusty UUID used for partition info get call */
++static const char *trusty_uuid = "40ee25f0-a2bc-304c-8c4c-a173c57d8af1";
++
++static u32 trusty_ffa_send_direct_msg(struct device *dev, unsigned long fid,
++				      unsigned long a0, unsigned long a1,
++				      unsigned long a2)
++{
++	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
++	struct ffa_send_direct_data ffa_msg;
++	struct ffa_device *ffa_dev;
++	int ret;
++
++	ffa_dev = to_ffa_dev(s->ffa->dev);
++
++	ffa_msg.data0 = fid;
++	ffa_msg.data1 = a0;
++	ffa_msg.data2 = a1;
++	ffa_msg.data3 = a2;
++	ffa_msg.data4 = 0;
++
++	ret = s->ffa->ops->sync_send_receive(ffa_dev, &ffa_msg);
++	if (!ret)
++		return ffa_msg.data0;
++
++	return ret;
++}
++
+ static int __trusty_ffa_share_memory(struct device *dev, u64 *id,
+ 				     struct scatterlist *sglist,
+ 				     unsigned int nents, pgprot_t pgprot,
+@@ -114,6 +144,11 @@ static int trusty_ffa_reclaim_memory(struct device *dev, u64 id,
+ 	return 0;
+ }
+ 
++static const struct trusty_msg_ops trusty_ffa_msg_ops = {
++	.desc = &trusty_ffa_transport,
++	.send_direct_msg = &trusty_ffa_send_direct_msg,
++};
++
+ static const struct trusty_mem_ops trusty_ffa_mem_ops = {
+ 	.desc = &trusty_ffa_transport,
+ 	.trusty_share_memory = &trusty_ffa_share_memory,
+@@ -181,6 +216,7 @@ static int trusty_ffa_transport_setup(struct device *dev)
+ 	struct trusty_state *s = platform_get_drvdata(to_platform_device(dev));
+ 	struct trusty_ffa_state *ffa_state;
+ 	struct ffa_device *ffa_dev;
++	struct ffa_partition_info pinfo = { 0 };
+ 
+ 	/* ffa transport not required for lower api versions */
+ 	if (s->api_version != 0 && s->api_version < TRUSTY_API_VERSION_MEM_OBJ) {
+@@ -208,6 +244,28 @@ static int trusty_ffa_transport_setup(struct device *dev)
+ 		return 0;
+ 	}
+ 
++	/* check if Trusty partition can support receipt of direct requests. */
++	rc = ffa_state->ops->partition_info_get(trusty_uuid, &pinfo);
++	if (rc || !(pinfo.properties & FFA_PARTITION_DIRECT_REQ_RECV)) {
++		dev_err(ffa_state->dev, "trusty_ffa_pinfo: ret: 0x%x, prop: 0x%x\n",
++			rc, pinfo.properties);
++		return -EINVAL;
++	}
++
++	/* query and check Trusty API version */
++	s->ffa = ffa_state;
++	rc = trusty_init_api_version(s, dev, trusty_ffa_send_direct_msg);
++	if (rc) {
++		s->ffa = NULL;
++		return -EINVAL;
++	}
++
++	if (s->api_version == TRUSTY_API_VERSION_FFA) {
++		s->msg_ops = &trusty_ffa_msg_ops;
++		s->mem_ops = &trusty_ffa_mem_ops;
++		return 0;
++	}
++
+ 	return -EINVAL;
+ }
+ 
+diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
+index f91c255c9897..66273873f169 100644
+--- a/drivers/trusty/trusty.c
++++ b/drivers/trusty/trusty.c
+@@ -679,6 +679,9 @@ static int trusty_remove(struct platform_device *pdev)
+  *     trusty_smc_transport used for messaging.
+  *     trusty_ffa_transport used for memory sharing.
+  *
++ * For Trusty API version > TRUSTY_API_VERSION_MEM_OBJ:
++ *     trusty_ffa_transport used for messaging and memory sharing operations.
++ *
+  */
+ static const trusty_transports_t trusty_transports[] = {
+ 	&trusty_smc_transport,
+diff --git a/include/linux/trusty/smcall.h b/include/linux/trusty/smcall.h
+index aea3f6068593..17b3d1c2c4f6 100644
+--- a/include/linux/trusty/smcall.h
++++ b/include/linux/trusty/smcall.h
+@@ -109,7 +109,8 @@
+ #define TRUSTY_API_VERSION_SMP_NOP	(3)
+ #define TRUSTY_API_VERSION_PHYS_MEM_OBJ	(4)
+ #define TRUSTY_API_VERSION_MEM_OBJ	(5)
+-#define TRUSTY_API_VERSION_CURRENT	(5)
++#define TRUSTY_API_VERSION_FFA		(6)
++#define TRUSTY_API_VERSION_CURRENT	(6)
+ #define SMC_FC_API_VERSION	SMC_FASTCALL_NR(SMC_ENTITY_SECURE_MONITOR, 11)
+ 
+ /* TRUSTED_OS entity calls */
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0044-ANDROID-trusty-Make-trusty-transports-configurable.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0044-ANDROID-trusty-Make-trusty-transports-configurable.patch
new file mode 100644
index 0000000..3076eca
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/0044-ANDROID-trusty-Make-trusty-transports-configurable.patch
@@ -0,0 +1,146 @@
+From 088162ab1852aa0f2034199e97a327b6240231db Mon Sep 17 00:00:00 2001
+From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Date: Wed, 16 Mar 2022 11:14:09 +0000
+Subject: [PATCH 31/32] ANDROID: trusty: Make trusty transports configurable
+
+With TRUSTY_SMC_TRANSPORT set to 'y', SMC based message passing and
+memory sharing support will be compiled in to trusty core.
+
+With TRUSTY_FFA_TRANSPORT set to 'y', FFA based message passing and
+memory sharing support will be compiled in to trusty core. This
+depends on ARM FF-A driver (ARM_FFA_TRANSPORT).
+
+Enabling any of the transport sets config TRUSTY_HAVE_TRANSPORT to 'y'.
+Not enabling any of the transport causes the build to break.
+
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+Change-Id: Ib5bbf0d39202e6897700264d14371ae33101c1d1
+Upstream-Status: Pending [Not submitted to upstream yet]
+---
+ drivers/trusty/Kconfig          | 30 ++++++++++++++++++++++++++++++
+ drivers/trusty/Makefile         | 26 +++++++++++++++-----------
+ drivers/trusty/trusty-private.h |  4 ++++
+ drivers/trusty/trusty.c         |  7 +++++++
+ 4 files changed, 56 insertions(+), 11 deletions(-)
+
+diff --git a/drivers/trusty/Kconfig b/drivers/trusty/Kconfig
+index fcde7f097acf..260022e4595b 100644
+--- a/drivers/trusty/Kconfig
++++ b/drivers/trusty/Kconfig
+@@ -21,6 +21,36 @@ config TRUSTY
+ 
+ if TRUSTY
+ 
++config TRUSTY_HAVE_TRANSPORT
++	bool
++	help
++	  If any of the Trusty transport is enabled then it sets this config
++	  option. This variable is used to break the build when none of the
++	  Trusty transports are enabled.
++
++config TRUSTY_SMC_TRANSPORT
++	bool "Trusty transport based on SMC"
++	select TRUSTY_HAVE_TRANSPORT
++	default n
++	help
++	  Enable SMC based transport for Trusty. This transport is required for
++	  Trusty API version <= TRUSTY_API_VERSION_MEM_OBJ.
++
++	  If you want to use legacy SMC based transport for sending Trusty
++	  messages to secure world, answer Y.
++
++config TRUSTY_FFA_TRANSPORT
++	bool "Trusty transport based on FFA"
++	select TRUSTY_HAVE_TRANSPORT
++	depends on ARM_FFA_TRANSPORT
++	default y
++	help
++	  Enable ARM FF-A based transport for Trusty. This transport is required
++	  for Trusty API version >= TRUSTY_API_VERSION_MEM_OBJ.
++
++	  If you want to use ARM FF-A based transport for sending Trusty messages
++	  to secure world, answer Y.
++
+ config TRUSTY_IRQ
+ 	tristate "Trusty IRQ support"
+ 	default y
+diff --git a/drivers/trusty/Makefile b/drivers/trusty/Makefile
+index 797d61bf68ef..104a4d0ed35c 100644
+--- a/drivers/trusty/Makefile
++++ b/drivers/trusty/Makefile
+@@ -3,14 +3,18 @@
+ # Makefile for trusty components
+ #
+ 
+-obj-$(CONFIG_TRUSTY)		+= trusty-core.o
+-trusty-core-objs		+= trusty.o trusty-mem.o
+-trusty-core-objs		+= trusty-smc.o
+-trusty-core-objs		+= trusty-ffa.o
+-trusty-core-$(CONFIG_ARM)	+= trusty-smc-arm.o
+-trusty-core-$(CONFIG_ARM64)	+= trusty-smc-arm64.o
+-obj-$(CONFIG_TRUSTY_IRQ)	+= trusty-irq.o
+-obj-$(CONFIG_TRUSTY_LOG)	+= trusty-log.o
+-obj-$(CONFIG_TRUSTY_TEST)	+= trusty-test.o
+-obj-$(CONFIG_TRUSTY_VIRTIO)	+= trusty-virtio.o
+-obj-$(CONFIG_TRUSTY_VIRTIO_IPC)	+= trusty-ipc.o
++obj-$(CONFIG_TRUSTY)				+= trusty-core.o
++trusty-core-objs				+= trusty.o
++trusty-arm-smc-$(CONFIG_ARM)			+= trusty-smc-arm.o
++trusty-arm-smc64-$(CONFIG_ARM64)		+= trusty-smc-arm64.o
++trusty-transport-$(CONFIG_TRUSTY_SMC_TRANSPORT) += trusty-smc.o
++trusty-transport-$(CONFIG_TRUSTY_SMC_TRANSPORT) += trusty-mem.o
++trusty-transport-$(CONFIG_TRUSTY_SMC_TRANSPORT) += $(trusty-arm-smc-y)
++trusty-transport-$(CONFIG_TRUSTY_SMC_TRANSPORT) += $(trusty-arm-smc64-y)
++trusty-transport-$(CONFIG_TRUSTY_FFA_TRANSPORT) += trusty-ffa.o
++trusty-core-objs				+= $(trusty-transport-y)
++obj-$(CONFIG_TRUSTY_IRQ)			+= trusty-irq.o
++obj-$(CONFIG_TRUSTY_LOG)			+= trusty-log.o
++obj-$(CONFIG_TRUSTY_TEST)			+= trusty-test.o
++obj-$(CONFIG_TRUSTY_VIRTIO)			+= trusty-virtio.o
++obj-$(CONFIG_TRUSTY_VIRTIO_IPC)			+= trusty-ipc.o
+diff --git a/drivers/trusty/trusty-private.h b/drivers/trusty/trusty-private.h
+index 2496f397e5d2..386ca9ae5af3 100644
+--- a/drivers/trusty/trusty-private.h
++++ b/drivers/trusty/trusty-private.h
+@@ -72,7 +72,11 @@ int trusty_init_api_version(struct trusty_state *s, struct device *dev,
+ 
+ typedef const struct trusty_transport_desc *trusty_transports_t;
+ 
++#ifdef CONFIG_TRUSTY_SMC_TRANSPORT
+ extern const struct trusty_transport_desc trusty_smc_transport;
++#endif
++#ifdef CONFIG_TRUSTY_FFA_TRANSPORT
+ extern const struct trusty_transport_desc trusty_ffa_transport;
++#endif
+ 
+ #endif /* _TRUSTY_PRIVATE_H */
+diff --git a/drivers/trusty/trusty.c b/drivers/trusty/trusty.c
+index 66273873f169..06698f3c67f9 100644
+--- a/drivers/trusty/trusty.c
++++ b/drivers/trusty/trusty.c
+@@ -684,8 +684,12 @@ static int trusty_remove(struct platform_device *pdev)
+  *
+  */
+ static const trusty_transports_t trusty_transports[] = {
++#ifdef CONFIG_TRUSTY_SMC_TRANSPORT
+ 	&trusty_smc_transport,
++#endif
++#ifdef CONFIG_TRUSTY_FFA_TRANSPORT
+ 	&trusty_ffa_transport,
++#endif
+ 	NULL,
+ };
+ 
+@@ -708,6 +712,9 @@ static struct platform_driver trusty_driver = {
+ 
+ static int __init trusty_driver_init(void)
+ {
++	BUILD_BUG_ON_MSG(!IS_ENABLED(CONFIG_TRUSTY_HAVE_TRANSPORT),
++			 "Trusty transport not configured");
++
+ 	return platform_driver_register(&trusty_driver);
+ }
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/gki_defconfig b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/gki_defconfig
new file mode 100644
index 0000000..30bd964
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-5.10/tc/gki_defconfig
@@ -0,0 +1,689 @@
+CONFIG_AUDIT=y
+CONFIG_NO_HZ=y
+CONFIG_HIGH_RES_TIMERS=y
+CONFIG_PREEMPT=y
+CONFIG_IRQ_TIME_ACCOUNTING=y
+CONFIG_TASK_XACCT=y
+CONFIG_TASK_IO_ACCOUNTING=y
+CONFIG_PSI=y
+CONFIG_RCU_EXPERT=y
+CONFIG_RCU_FAST_NO_HZ=y
+CONFIG_RCU_NOCB_CPU=y
+CONFIG_IKCONFIG=y
+CONFIG_IKCONFIG_PROC=y
+CONFIG_IKHEADERS=y
+CONFIG_UCLAMP_TASK=y
+CONFIG_UCLAMP_BUCKETS_COUNT=20
+CONFIG_CGROUPS=y
+CONFIG_MEMCG=y
+CONFIG_BLK_CGROUP=y
+CONFIG_CGROUP_SCHED=y
+CONFIG_UCLAMP_TASK_GROUP=y
+CONFIG_CGROUP_FREEZER=y
+CONFIG_CPUSETS=y
+CONFIG_CGROUP_CPUACCT=y
+CONFIG_CGROUP_BPF=y
+CONFIG_NAMESPACES=y
+# CONFIG_PID_NS is not set
+CONFIG_RT_SOFTINT_OPTIMIZATION=y
+# CONFIG_RD_BZIP2 is not set
+# CONFIG_RD_LZMA is not set
+# CONFIG_RD_XZ is not set
+# CONFIG_RD_LZO is not set
+CONFIG_BOOT_CONFIG=y
+# CONFIG_SYSFS_SYSCALL is not set
+# CONFIG_FHANDLE is not set
+CONFIG_KALLSYMS_ALL=y
+CONFIG_BPF_SYSCALL=y
+CONFIG_BPF_JIT_ALWAYS_ON=y
+CONFIG_USERFAULTFD=y
+# CONFIG_RSEQ is not set
+CONFIG_EMBEDDED=y
+# CONFIG_COMPAT_BRK is not set
+# CONFIG_SLAB_MERGE_DEFAULT is not set
+CONFIG_SLAB_FREELIST_RANDOM=y
+CONFIG_SLAB_FREELIST_HARDENED=y
+CONFIG_SHUFFLE_PAGE_ALLOCATOR=y
+CONFIG_PROFILING=y
+# CONFIG_ZONE_DMA is not set
+CONFIG_ARCH_SUNXI=y
+CONFIG_ARCH_HISI=y
+CONFIG_ARCH_QCOM=y
+CONFIG_ARCH_SPRD=y
+CONFIG_SCHED_MC=y
+CONFIG_NR_CPUS=32
+CONFIG_PARAVIRT=y
+CONFIG_ARM64_SW_TTBR0_PAN=y
+CONFIG_COMPAT=y
+CONFIG_ARMV8_DEPRECATED=y
+CONFIG_SWP_EMULATION=y
+CONFIG_CP15_BARRIER_EMULATION=y
+CONFIG_SETEND_EMULATION=y
+CONFIG_RANDOMIZE_BASE=y
+# CONFIG_RANDOMIZE_MODULE_REGION_FULL is not set
+CONFIG_CMDLINE="stack_depot_disable=on kasan.stacktrace=off kvm-arm.mode=protected cgroup_disable=pressure cgroup.memory=nokmem"
+CONFIG_CMDLINE_EXTEND=y
+# CONFIG_DMI is not set
+CONFIG_PM_WAKELOCKS=y
+CONFIG_PM_WAKELOCKS_LIMIT=0
+# CONFIG_PM_WAKELOCKS_GC is not set
+CONFIG_ENERGY_MODEL=y
+CONFIG_CPU_IDLE=y
+CONFIG_CPU_IDLE_GOV_MENU=y
+CONFIG_CPU_IDLE_GOV_TEO=y
+CONFIG_ARM_CPUIDLE=y
+CONFIG_ARM_PSCI_CPUIDLE=y
+CONFIG_CPU_FREQ=y
+CONFIG_CPU_FREQ_STAT=y
+CONFIG_CPU_FREQ_TIMES=y
+CONFIG_CPU_FREQ_GOV_POWERSAVE=y
+CONFIG_CPU_FREQ_GOV_CONSERVATIVE=y
+CONFIG_ARM_SCPI_CPUFREQ=y
+CONFIG_ARM_SCMI_CPUFREQ=y
+CONFIG_ARM_SCMI_PROTOCOL=y
+# CONFIG_ARM_SCMI_POWER_DOMAIN is not set
+CONFIG_ARM_SCPI_PROTOCOL=y
+# CONFIG_ARM_SCPI_POWER_DOMAIN is not set
+# CONFIG_EFI_ARMSTUB_DTB_LOADER is not set
+CONFIG_VIRTUALIZATION=y
+CONFIG_KVM=y
+CONFIG_CRYPTO_SHA2_ARM64_CE=y
+CONFIG_CRYPTO_AES_ARM64_CE_BLK=y
+CONFIG_KPROBES=y
+CONFIG_JUMP_LABEL=y
+CONFIG_SHADOW_CALL_STACK=y
+CONFIG_LTO_CLANG_FULL=y
+CONFIG_CFI_CLANG=y
+CONFIG_MODULES=y
+CONFIG_MODULE_UNLOAD=y
+CONFIG_MODVERSIONS=y
+CONFIG_MODULE_SCMVERSION=y
+CONFIG_BLK_CGROUP_IOCOST=y
+CONFIG_BLK_INLINE_ENCRYPTION=y
+CONFIG_BLK_INLINE_ENCRYPTION_FALLBACK=y
+CONFIG_IOSCHED_BFQ=y
+CONFIG_BFQ_GROUP_IOSCHED=y
+CONFIG_GKI_HACKS_TO_FIX=y
+# CONFIG_CORE_DUMP_DEFAULT_ELF_HEADERS is not set
+CONFIG_BINFMT_MISC=y
+CONFIG_MEMORY_HOTPLUG=y
+CONFIG_MEMORY_HOTREMOVE=y
+CONFIG_DEFAULT_MMAP_MIN_ADDR=32768
+CONFIG_TRANSPARENT_HUGEPAGE=y
+CONFIG_TRANSPARENT_HUGEPAGE_MADVISE=y
+CONFIG_CLEANCACHE=y
+CONFIG_CMA=y
+CONFIG_CMA_DEBUGFS=y
+CONFIG_CMA_SYSFS=y
+CONFIG_CMA_AREAS=16
+CONFIG_READ_ONLY_THP_FOR_FS=y
+CONFIG_NET=y
+CONFIG_PACKET=y
+CONFIG_UNIX=y
+CONFIG_XFRM_USER=y
+CONFIG_XFRM_INTERFACE=y
+CONFIG_XFRM_MIGRATE=y
+CONFIG_XFRM_STATISTICS=y
+CONFIG_NET_KEY=y
+CONFIG_XDP_SOCKETS=y
+CONFIG_INET=y
+CONFIG_IP_MULTICAST=y
+CONFIG_IP_ADVANCED_ROUTER=y
+CONFIG_IP_MULTIPLE_TABLES=y
+CONFIG_NET_IPIP=y
+CONFIG_NET_IPGRE_DEMUX=y
+CONFIG_NET_IPGRE=y
+CONFIG_NET_IPVTI=y
+CONFIG_INET_ESP=y
+CONFIG_INET_UDP_DIAG=y
+CONFIG_INET_DIAG_DESTROY=y
+CONFIG_IPV6_ROUTER_PREF=y
+CONFIG_IPV6_ROUTE_INFO=y
+CONFIG_IPV6_OPTIMISTIC_DAD=y
+CONFIG_INET6_ESP=y
+CONFIG_INET6_IPCOMP=y
+CONFIG_IPV6_MIP6=y
+CONFIG_IPV6_VTI=y
+CONFIG_IPV6_GRE=y
+CONFIG_IPV6_MULTIPLE_TABLES=y
+CONFIG_NETFILTER=y
+CONFIG_NF_CONNTRACK=y
+CONFIG_NF_CONNTRACK_SECMARK=y
+CONFIG_NF_CONNTRACK_EVENTS=y
+CONFIG_NF_CONNTRACK_AMANDA=y
+CONFIG_NF_CONNTRACK_FTP=y
+CONFIG_NF_CONNTRACK_H323=y
+CONFIG_NF_CONNTRACK_IRC=y
+CONFIG_NF_CONNTRACK_NETBIOS_NS=y
+CONFIG_NF_CONNTRACK_PPTP=y
+CONFIG_NF_CONNTRACK_SANE=y
+CONFIG_NF_CONNTRACK_TFTP=y
+CONFIG_NF_CT_NETLINK=y
+CONFIG_NETFILTER_XT_TARGET_CLASSIFY=y
+CONFIG_NETFILTER_XT_TARGET_CONNMARK=y
+CONFIG_NETFILTER_XT_TARGET_CONNSECMARK=y
+CONFIG_NETFILTER_XT_TARGET_DSCP=y
+CONFIG_NETFILTER_XT_TARGET_IDLETIMER=y
+CONFIG_NETFILTER_XT_TARGET_MARK=y
+CONFIG_NETFILTER_XT_TARGET_NFLOG=y
+CONFIG_NETFILTER_XT_TARGET_NFQUEUE=y
+CONFIG_NETFILTER_XT_TARGET_NOTRACK=y
+CONFIG_NETFILTER_XT_TARGET_TEE=y
+CONFIG_NETFILTER_XT_TARGET_TPROXY=y
+CONFIG_NETFILTER_XT_TARGET_TRACE=y
+CONFIG_NETFILTER_XT_TARGET_SECMARK=y
+CONFIG_NETFILTER_XT_TARGET_TCPMSS=y
+CONFIG_NETFILTER_XT_MATCH_BPF=y
+CONFIG_NETFILTER_XT_MATCH_COMMENT=y
+CONFIG_NETFILTER_XT_MATCH_CONNLIMIT=y
+CONFIG_NETFILTER_XT_MATCH_CONNMARK=y
+CONFIG_NETFILTER_XT_MATCH_CONNTRACK=y
+CONFIG_NETFILTER_XT_MATCH_DSCP=y
+CONFIG_NETFILTER_XT_MATCH_ESP=y
+CONFIG_NETFILTER_XT_MATCH_HASHLIMIT=y
+CONFIG_NETFILTER_XT_MATCH_HELPER=y
+CONFIG_NETFILTER_XT_MATCH_IPRANGE=y
+CONFIG_NETFILTER_XT_MATCH_LENGTH=y
+CONFIG_NETFILTER_XT_MATCH_LIMIT=y
+CONFIG_NETFILTER_XT_MATCH_MAC=y
+CONFIG_NETFILTER_XT_MATCH_MARK=y
+CONFIG_NETFILTER_XT_MATCH_MULTIPORT=y
+CONFIG_NETFILTER_XT_MATCH_OWNER=y
+CONFIG_NETFILTER_XT_MATCH_POLICY=y
+CONFIG_NETFILTER_XT_MATCH_PKTTYPE=y
+CONFIG_NETFILTER_XT_MATCH_QUOTA=y
+CONFIG_NETFILTER_XT_MATCH_QUOTA2=y
+CONFIG_NETFILTER_XT_MATCH_QUOTA2_LOG=y
+CONFIG_NETFILTER_XT_MATCH_SOCKET=y
+CONFIG_NETFILTER_XT_MATCH_STATE=y
+CONFIG_NETFILTER_XT_MATCH_STATISTIC=y
+CONFIG_NETFILTER_XT_MATCH_STRING=y
+CONFIG_NETFILTER_XT_MATCH_TIME=y
+CONFIG_NETFILTER_XT_MATCH_U32=y
+CONFIG_IP_NF_IPTABLES=y
+CONFIG_IP_NF_MATCH_ECN=y
+CONFIG_IP_NF_MATCH_TTL=y
+CONFIG_IP_NF_FILTER=y
+CONFIG_IP_NF_TARGET_REJECT=y
+CONFIG_IP_NF_NAT=y
+CONFIG_IP_NF_TARGET_MASQUERADE=y
+CONFIG_IP_NF_TARGET_NETMAP=y
+CONFIG_IP_NF_TARGET_REDIRECT=y
+CONFIG_IP_NF_MANGLE=y
+CONFIG_IP_NF_RAW=y
+CONFIG_IP_NF_SECURITY=y
+CONFIG_IP_NF_ARPTABLES=y
+CONFIG_IP_NF_ARPFILTER=y
+CONFIG_IP_NF_ARP_MANGLE=y
+CONFIG_IP6_NF_IPTABLES=y
+CONFIG_IP6_NF_MATCH_RPFILTER=y
+CONFIG_IP6_NF_FILTER=y
+CONFIG_IP6_NF_TARGET_REJECT=y
+CONFIG_IP6_NF_MANGLE=y
+CONFIG_IP6_NF_RAW=y
+CONFIG_TIPC=y
+CONFIG_L2TP=y
+CONFIG_BRIDGE=y
+CONFIG_6LOWPAN=y
+CONFIG_IEEE802154=y
+CONFIG_IEEE802154_6LOWPAN=y
+CONFIG_MAC802154=y
+CONFIG_NET_SCHED=y
+CONFIG_NET_SCH_HTB=y
+CONFIG_NET_SCH_PRIO=y
+CONFIG_NET_SCH_MULTIQ=y
+CONFIG_NET_SCH_SFQ=y
+CONFIG_NET_SCH_TBF=y
+CONFIG_NET_SCH_NETEM=y
+CONFIG_NET_SCH_CODEL=y
+CONFIG_NET_SCH_FQ_CODEL=y
+CONFIG_NET_SCH_FQ=y
+CONFIG_NET_SCH_INGRESS=y
+CONFIG_NET_CLS_BASIC=y
+CONFIG_NET_CLS_TCINDEX=y
+CONFIG_NET_CLS_FW=y
+CONFIG_NET_CLS_U32=y
+CONFIG_CLS_U32_MARK=y
+CONFIG_NET_CLS_FLOW=y
+CONFIG_NET_CLS_BPF=y
+CONFIG_NET_CLS_MATCHALL=y
+CONFIG_NET_EMATCH=y
+CONFIG_NET_EMATCH_CMP=y
+CONFIG_NET_EMATCH_NBYTE=y
+CONFIG_NET_EMATCH_U32=y
+CONFIG_NET_EMATCH_META=y
+CONFIG_NET_EMATCH_TEXT=y
+CONFIG_NET_CLS_ACT=y
+CONFIG_NET_ACT_POLICE=y
+CONFIG_NET_ACT_GACT=y
+CONFIG_NET_ACT_MIRRED=y
+CONFIG_NET_ACT_SKBEDIT=y
+CONFIG_VSOCKETS=y
+CONFIG_CGROUP_NET_PRIO=y
+CONFIG_BPF_JIT=y
+CONFIG_CAN=y
+CONFIG_BT=y
+CONFIG_BT_RFCOMM=y
+CONFIG_BT_RFCOMM_TTY=y
+CONFIG_BT_HIDP=y
+CONFIG_BT_HCIBTSDIO=y
+CONFIG_BT_HCIUART=y
+CONFIG_BT_HCIUART_LL=y
+CONFIG_BT_HCIUART_BCM=y
+CONFIG_BT_HCIUART_QCA=y
+CONFIG_RFKILL=y
+CONFIG_NFC=y
+CONFIG_PCI=y
+CONFIG_PCIEPORTBUS=y
+CONFIG_PCIEAER=y
+CONFIG_PCI_IOV=y
+CONFIG_PCI_HOST_GENERIC=y
+CONFIG_PCIE_DW_PLAT_EP=y
+CONFIG_PCIE_QCOM=y
+CONFIG_PCIE_KIRIN=y
+CONFIG_PCI_ENDPOINT=y
+CONFIG_FW_LOADER_USER_HELPER=y
+# CONFIG_FW_CACHE is not set
+# CONFIG_SUN50I_DE2_BUS is not set
+# CONFIG_SUNXI_RSB is not set
+CONFIG_GNSS=y
+CONFIG_BLK_DEV_LOOP=y
+CONFIG_BLK_DEV_LOOP_MIN_COUNT=16
+CONFIG_BLK_DEV_RAM=y
+CONFIG_BLK_DEV_RAM_SIZE=8192
+CONFIG_SRAM=y
+CONFIG_UID_SYS_STATS=y
+CONFIG_SCSI=y
+# CONFIG_SCSI_PROC_FS is not set
+CONFIG_BLK_DEV_SD=y
+CONFIG_SCSI_UFSHCD=y
+CONFIG_SCSI_UFSHCD_PCI=y
+CONFIG_SCSI_UFSHCD_PLATFORM=y
+CONFIG_SCSI_UFS_DWC_TC_PLATFORM=y
+CONFIG_SCSI_UFS_HISI=y
+CONFIG_SCSI_UFS_BSG=y
+CONFIG_SCSI_UFS_CRYPTO=y
+CONFIG_SCSI_UFS_HPB=y
+CONFIG_MD=y
+CONFIG_BLK_DEV_DM=y
+CONFIG_DM_CRYPT=y
+CONFIG_DM_DEFAULT_KEY=y
+CONFIG_DM_SNAPSHOT=y
+CONFIG_DM_UEVENT=y
+CONFIG_DM_VERITY=y
+CONFIG_DM_VERITY_FEC=y
+CONFIG_DM_BOW=y
+CONFIG_NETDEVICES=y
+CONFIG_DUMMY=y
+CONFIG_WIREGUARD=y
+CONFIG_IFB=y
+CONFIG_TUN=y
+CONFIG_VETH=y
+CONFIG_PPP=y
+CONFIG_PPP_BSDCOMP=y
+CONFIG_PPP_DEFLATE=y
+CONFIG_PPP_MPPE=y
+CONFIG_PPTP=y
+CONFIG_PPPOL2TP=y
+CONFIG_USB_RTL8150=y
+CONFIG_USB_RTL8152=y
+CONFIG_USB_USBNET=y
+# CONFIG_USB_NET_AX8817X is not set
+# CONFIG_USB_NET_AX88179_178A is not set
+CONFIG_USB_NET_CDC_EEM=y
+# CONFIG_USB_NET_NET1080 is not set
+# CONFIG_USB_NET_CDC_SUBSET is not set
+# CONFIG_USB_NET_ZAURUS is not set
+CONFIG_USB_NET_AQC111=y
+# CONFIG_WLAN_VENDOR_ADMTEK is not set
+# CONFIG_WLAN_VENDOR_ATH is not set
+# CONFIG_WLAN_VENDOR_ATMEL is not set
+# CONFIG_WLAN_VENDOR_BROADCOM is not set
+# CONFIG_WLAN_VENDOR_CISCO is not set
+# CONFIG_WLAN_VENDOR_INTEL is not set
+# CONFIG_WLAN_VENDOR_INTERSIL is not set
+# CONFIG_WLAN_VENDOR_MARVELL is not set
+# CONFIG_WLAN_VENDOR_MEDIATEK is not set
+# CONFIG_WLAN_VENDOR_RALINK is not set
+# CONFIG_WLAN_VENDOR_REALTEK is not set
+# CONFIG_WLAN_VENDOR_RSI is not set
+# CONFIG_WLAN_VENDOR_ST is not set
+# CONFIG_WLAN_VENDOR_TI is not set
+# CONFIG_WLAN_VENDOR_ZYDAS is not set
+# CONFIG_WLAN_VENDOR_QUANTENNA is not set
+CONFIG_INPUT_EVDEV=y
+CONFIG_KEYBOARD_GPIO=y
+# CONFIG_MOUSE_PS2 is not set
+CONFIG_INPUT_JOYSTICK=y
+CONFIG_JOYSTICK_XPAD=y
+CONFIG_JOYSTICK_XPAD_FF=y
+CONFIG_JOYSTICK_XPAD_LEDS=y
+CONFIG_INPUT_TOUCHSCREEN=y
+CONFIG_INPUT_MISC=y
+CONFIG_INPUT_UINPUT=y
+# CONFIG_VT is not set
+# CONFIG_LEGACY_PTYS is not set
+CONFIG_SERIAL_8250=y
+# CONFIG_SERIAL_8250_DEPRECATED_OPTIONS is not set
+CONFIG_SERIAL_8250_CONSOLE=y
+# CONFIG_SERIAL_8250_EXAR is not set
+CONFIG_SERIAL_OF_PLATFORM=y
+CONFIG_SERIAL_AMBA_PL011=y
+CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
+CONFIG_SERIAL_SAMSUNG=y
+CONFIG_SERIAL_SAMSUNG_CONSOLE=y
+CONFIG_SERIAL_MSM_GENI_EARLY_CONSOLE=y
+CONFIG_SERIAL_SPRD=y
+CONFIG_SERIAL_SPRD_CONSOLE=y
+CONFIG_HVC_DCC=y
+CONFIG_HVC_DCC_SERIALIZE_SMP=y
+CONFIG_SERIAL_DEV_BUS=y
+CONFIG_HW_RANDOM=y
+# CONFIG_HW_RANDOM_CAVIUM is not set
+# CONFIG_DEVMEM is not set
+# CONFIG_DEVPORT is not set
+# CONFIG_I2C_COMPAT is not set
+# CONFIG_I2C_HELPER_AUTO is not set
+CONFIG_I3C=y
+CONFIG_SPI=y
+CONFIG_SPMI=y
+# CONFIG_SPMI_MSM_PMIC_ARB is not set
+# CONFIG_PINCTRL_SUN8I_H3_R is not set
+# CONFIG_PINCTRL_SUN50I_A64 is not set
+# CONFIG_PINCTRL_SUN50I_A64_R is not set
+# CONFIG_PINCTRL_SUN50I_H5 is not set
+# CONFIG_PINCTRL_SUN50I_H6 is not set
+# CONFIG_PINCTRL_SUN50I_H6_R is not set
+CONFIG_GPIO_GENERIC_PLATFORM=y
+CONFIG_POWER_RESET_HISI=y
+# CONFIG_HWMON is not set
+CONFIG_THERMAL=y
+CONFIG_THERMAL_NETLINK=y
+CONFIG_THERMAL_STATISTICS=y
+CONFIG_THERMAL_EMERGENCY_POWEROFF_DELAY_MS=100
+CONFIG_THERMAL_WRITABLE_TRIPS=y
+CONFIG_THERMAL_GOV_USER_SPACE=y
+CONFIG_THERMAL_GOV_POWER_ALLOCATOR=y
+CONFIG_CPU_THERMAL=y
+CONFIG_DEVFREQ_THERMAL=y
+CONFIG_THERMAL_EMULATION=y
+CONFIG_WATCHDOG=y
+CONFIG_WATCHDOG_CORE=y
+CONFIG_MFD_ACT8945A=y
+CONFIG_MFD_SYSCON=y
+CONFIG_REGULATOR=y
+CONFIG_REGULATOR_FIXED_VOLTAGE=y
+CONFIG_RC_CORE=y
+# CONFIG_RC_MAP is not set
+CONFIG_LIRC=y
+CONFIG_BPF_LIRC_MODE2=y
+CONFIG_RC_DECODERS=y
+CONFIG_RC_DEVICES=y
+CONFIG_MEDIA_CEC_RC=y
+# CONFIG_MEDIA_ANALOG_TV_SUPPORT is not set
+# CONFIG_MEDIA_DIGITAL_TV_SUPPORT is not set
+# CONFIG_MEDIA_RADIO_SUPPORT is not set
+# CONFIG_MEDIA_SDR_SUPPORT is not set
+# CONFIG_MEDIA_TEST_SUPPORT is not set
+CONFIG_VIDEO_V4L2_SUBDEV_API=y
+CONFIG_MEDIA_USB_SUPPORT=y
+CONFIG_USB_VIDEO_CLASS=y
+CONFIG_USB_GSPCA=y
+CONFIG_V4L_PLATFORM_DRIVERS=y
+CONFIG_V4L_MEM2MEM_DRIVERS=y
+# CONFIG_VGA_ARB is not set
+CONFIG_DRM=y
+# CONFIG_DRM_FBDEV_EMULATION is not set
+CONFIG_BACKLIGHT_CLASS_DEVICE=y
+CONFIG_SOUND=y
+CONFIG_SND=y
+CONFIG_SND_HRTIMER=y
+CONFIG_SND_DYNAMIC_MINORS=y
+# CONFIG_SND_SUPPORT_OLD_API is not set
+# CONFIG_SND_DRIVERS is not set
+CONFIG_SND_USB_AUDIO=y
+CONFIG_SND_SOC=y
+CONFIG_HID_BATTERY_STRENGTH=y
+CONFIG_HIDRAW=y
+CONFIG_UHID=y
+CONFIG_HID_APPLE=y
+CONFIG_HID_BETOP_FF=y
+CONFIG_HID_PRODIKEYS=y
+CONFIG_HID_ELECOM=y
+CONFIG_HID_UCLOGIC=y
+CONFIG_HID_LOGITECH=y
+CONFIG_HID_LOGITECH_DJ=y
+CONFIG_HID_MAGICMOUSE=y
+CONFIG_HID_MICROSOFT=y
+CONFIG_HID_MULTITOUCH=y
+CONFIG_HID_NINTENDO=y
+CONFIG_HID_PICOLCD=y
+CONFIG_HID_PLANTRONICS=y
+CONFIG_HID_PLAYSTATION=y
+CONFIG_PLAYSTATION_FF=y
+CONFIG_HID_ROCCAT=y
+CONFIG_HID_SONY=y
+CONFIG_SONY_FF=y
+CONFIG_HID_STEAM=y
+CONFIG_HID_WACOM=y
+CONFIG_HID_WIIMOTE=y
+CONFIG_USB_HIDDEV=y
+CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
+CONFIG_USB_OTG=y
+CONFIG_USB_XHCI_HCD=y
+CONFIG_USB_EHCI_HCD=y
+CONFIG_USB_EHCI_ROOT_HUB_TT=y
+CONFIG_USB_EHCI_HCD_PLATFORM=y
+CONFIG_USB_ACM=y
+CONFIG_USB_STORAGE=y
+CONFIG_USB_UAS=y
+CONFIG_USB_DWC3=y
+CONFIG_USB_GADGET=y
+CONFIG_USB_DUMMY_HCD=y
+CONFIG_USB_CONFIGFS=y
+CONFIG_USB_CONFIGFS_UEVENT=y
+CONFIG_USB_CONFIGFS_SERIAL=y
+CONFIG_USB_CONFIGFS_ACM=y
+CONFIG_USB_CONFIGFS_NCM=y
+CONFIG_USB_CONFIGFS_ECM=y
+CONFIG_USB_CONFIGFS_RNDIS=y
+CONFIG_USB_CONFIGFS_EEM=y
+CONFIG_USB_CONFIGFS_MASS_STORAGE=y
+CONFIG_USB_CONFIGFS_F_FS=y
+CONFIG_USB_CONFIGFS_F_ACC=y
+CONFIG_USB_CONFIGFS_F_AUDIO_SRC=y
+CONFIG_USB_CONFIGFS_F_UAC2=y
+CONFIG_USB_CONFIGFS_F_MIDI=y
+CONFIG_USB_CONFIGFS_F_HID=y
+CONFIG_TYPEC=y
+CONFIG_TYPEC_TCPM=y
+CONFIG_TYPEC_TCPCI=y
+CONFIG_TYPEC_UCSI=y
+CONFIG_MMC=y
+# CONFIG_PWRSEQ_EMMC is not set
+# CONFIG_PWRSEQ_SIMPLE is not set
+CONFIG_MMC_CRYPTO=y
+CONFIG_MMC_SDHCI=y
+CONFIG_MMC_SDHCI_PLTFM=y
+CONFIG_LEDS_CLASS_FLASH=y
+CONFIG_LEDS_TRIGGER_TIMER=y
+CONFIG_LEDS_TRIGGER_TRANSIENT=y
+CONFIG_EDAC=y
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_DRV_PL030=y
+CONFIG_RTC_DRV_PL031=y
+CONFIG_DMABUF_HEAPS=y
+CONFIG_DMABUF_SYSFS_STATS=y
+CONFIG_DMABUF_HEAPS_DEFERRED_FREE=y
+CONFIG_DMABUF_HEAPS_PAGE_POOL=y
+CONFIG_UIO=y
+CONFIG_VHOST_VSOCK=y
+CONFIG_STAGING=y
+CONFIG_ASHMEM=y
+CONFIG_DEBUG_KINFO=y
+CONFIG_COMMON_CLK_SCPI=y
+# CONFIG_SPRD_COMMON_CLK is not set
+# CONFIG_CLK_SUNXI is not set
+# CONFIG_SUNXI_CCU is not set
+CONFIG_HWSPINLOCK=y
+CONFIG_SUN4I_TIMER=y
+# CONFIG_SUN50I_ERRATUM_UNKNOWN1 is not set
+CONFIG_MTK_TIMER=y
+CONFIG_MAILBOX=y
+CONFIG_IOMMU_LIMIT_IOVA_ALIGNMENT=y
+CONFIG_IOMMU_IO_PGTABLE_ARMV7S=y
+CONFIG_REMOTEPROC=y
+CONFIG_REMOTEPROC_CDEV=y
+CONFIG_RPMSG_CHAR=y
+CONFIG_DEVFREQ_GOV_PERFORMANCE=y
+CONFIG_DEVFREQ_GOV_POWERSAVE=y
+CONFIG_DEVFREQ_GOV_USERSPACE=y
+CONFIG_DEVFREQ_GOV_PASSIVE=y
+CONFIG_PM_DEVFREQ_EVENT=y
+CONFIG_IIO=y
+CONFIG_IIO_BUFFER=y
+CONFIG_IIO_TRIGGER=y
+CONFIG_PWM=y
+CONFIG_GENERIC_PHY=y
+CONFIG_POWERCAP=y
+CONFIG_DTPM=y
+CONFIG_ANDROID=y
+CONFIG_ANDROID_BINDER_IPC=y
+CONFIG_ANDROID_BINDERFS=y
+CONFIG_ANDROID_DEBUG_SYMBOLS=y
+CONFIG_ANDROID_VENDOR_HOOKS=y
+CONFIG_LIBNVDIMM=y
+# CONFIG_ND_BLK is not set
+CONFIG_INTERCONNECT=y
+CONFIG_EXT4_FS=y
+CONFIG_EXT4_FS_POSIX_ACL=y
+CONFIG_EXT4_FS_SECURITY=y
+CONFIG_F2FS_FS=y
+CONFIG_F2FS_FS_SECURITY=y
+CONFIG_F2FS_FS_COMPRESSION=y
+CONFIG_FS_ENCRYPTION=y
+CONFIG_FS_ENCRYPTION_INLINE_CRYPT=y
+CONFIG_FS_VERITY=y
+CONFIG_FS_VERITY_BUILTIN_SIGNATURES=y
+# CONFIG_DNOTIFY is not set
+CONFIG_QUOTA=y
+CONFIG_QFMT_V2=y
+CONFIG_FUSE_FS=y
+CONFIG_VIRTIO_FS=y
+CONFIG_OVERLAY_FS=y
+CONFIG_INCREMENTAL_FS=y
+CONFIG_MSDOS_FS=y
+CONFIG_VFAT_FS=y
+CONFIG_EXFAT_FS=y
+CONFIG_TMPFS=y
+# CONFIG_EFIVAR_FS is not set
+CONFIG_PSTORE=y
+CONFIG_PSTORE_CONSOLE=y
+CONFIG_PSTORE_PMSG=y
+CONFIG_PSTORE_RAM=y
+CONFIG_EROFS_FS=y
+CONFIG_NLS_CODEPAGE_437=y
+CONFIG_NLS_CODEPAGE_737=y
+CONFIG_NLS_CODEPAGE_775=y
+CONFIG_NLS_CODEPAGE_850=y
+CONFIG_NLS_CODEPAGE_852=y
+CONFIG_NLS_CODEPAGE_855=y
+CONFIG_NLS_CODEPAGE_857=y
+CONFIG_NLS_CODEPAGE_860=y
+CONFIG_NLS_CODEPAGE_861=y
+CONFIG_NLS_CODEPAGE_862=y
+CONFIG_NLS_CODEPAGE_863=y
+CONFIG_NLS_CODEPAGE_864=y
+CONFIG_NLS_CODEPAGE_865=y
+CONFIG_NLS_CODEPAGE_866=y
+CONFIG_NLS_CODEPAGE_869=y
+CONFIG_NLS_CODEPAGE_936=y
+CONFIG_NLS_CODEPAGE_950=y
+CONFIG_NLS_CODEPAGE_932=y
+CONFIG_NLS_CODEPAGE_949=y
+CONFIG_NLS_CODEPAGE_874=y
+CONFIG_NLS_ISO8859_8=y
+CONFIG_NLS_CODEPAGE_1250=y
+CONFIG_NLS_CODEPAGE_1251=y
+CONFIG_NLS_ASCII=y
+CONFIG_NLS_ISO8859_1=y
+CONFIG_NLS_ISO8859_2=y
+CONFIG_NLS_ISO8859_3=y
+CONFIG_NLS_ISO8859_4=y
+CONFIG_NLS_ISO8859_5=y
+CONFIG_NLS_ISO8859_6=y
+CONFIG_NLS_ISO8859_7=y
+CONFIG_NLS_ISO8859_9=y
+CONFIG_NLS_ISO8859_13=y
+CONFIG_NLS_ISO8859_14=y
+CONFIG_NLS_ISO8859_15=y
+CONFIG_NLS_KOI8_R=y
+CONFIG_NLS_KOI8_U=y
+CONFIG_NLS_MAC_ROMAN=y
+CONFIG_NLS_MAC_CELTIC=y
+CONFIG_NLS_MAC_CENTEURO=y
+CONFIG_NLS_MAC_CROATIAN=y
+CONFIG_NLS_MAC_CYRILLIC=y
+CONFIG_NLS_MAC_GAELIC=y
+CONFIG_NLS_MAC_GREEK=y
+CONFIG_NLS_MAC_ICELAND=y
+CONFIG_NLS_MAC_INUIT=y
+CONFIG_NLS_MAC_ROMANIAN=y
+CONFIG_NLS_MAC_TURKISH=y
+CONFIG_NLS_UTF8=y
+CONFIG_UNICODE=y
+CONFIG_SECURITY=y
+CONFIG_SECURITYFS=y
+CONFIG_SECURITY_NETWORK=y
+CONFIG_HARDENED_USERCOPY=y
+# CONFIG_HARDENED_USERCOPY_FALLBACK is not set
+CONFIG_FORTIFY_SOURCE=y
+CONFIG_STATIC_USERMODEHELPER=y
+CONFIG_STATIC_USERMODEHELPER_PATH=""
+CONFIG_SECURITY_SELINUX=y
+CONFIG_INIT_STACK_ALL_ZERO=y
+CONFIG_INIT_ON_ALLOC_DEFAULT_ON=y
+CONFIG_CRYPTO_CHACHA20POLY1305=y
+CONFIG_CRYPTO_ADIANTUM=y
+CONFIG_CRYPTO_XCBC=y
+CONFIG_CRYPTO_BLAKE2B=y
+CONFIG_CRYPTO_MD5=y
+CONFIG_CRYPTO_LZ4=y
+CONFIG_CRYPTO_ZSTD=y
+CONFIG_CRYPTO_ANSI_CPRNG=y
+CONFIG_CRC8=y
+CONFIG_XZ_DEC=y
+CONFIG_DMA_CMA=y
+CONFIG_STACK_HASH_ORDER=12
+CONFIG_PRINTK_TIME=y
+CONFIG_PRINTK_CALLER=y
+CONFIG_DYNAMIC_DEBUG_CORE=y
+CONFIG_DEBUG_INFO=y
+CONFIG_DEBUG_INFO_DWARF4=y
+# CONFIG_ENABLE_MUST_CHECK is not set
+CONFIG_HEADERS_INSTALL=y
+# CONFIG_SECTION_MISMATCH_WARN_ONLY is not set
+CONFIG_MAGIC_SYSRQ=y
+CONFIG_UBSAN=y
+CONFIG_UBSAN_TRAP=y
+CONFIG_UBSAN_LOCAL_BOUNDS=y
+# CONFIG_UBSAN_MISC is not set
+CONFIG_PAGE_OWNER=y
+CONFIG_PAGE_PINNER=y
+CONFIG_DEBUG_MEMORY_INIT=y
+CONFIG_KASAN=y
+CONFIG_KASAN_HW_TAGS=y
+CONFIG_KFENCE=y
+CONFIG_KFENCE_SAMPLE_INTERVAL=500
+CONFIG_KFENCE_NUM_OBJECTS=63
+CONFIG_PANIC_ON_OOPS=y
+CONFIG_PANIC_TIMEOUT=-1
+CONFIG_DETECT_HUNG_TASK=y
+CONFIG_WQ_WATCHDOG=y
+CONFIG_SCHEDSTATS=y
+# CONFIG_DEBUG_PREEMPT is not set
+CONFIG_BUG_ON_DATA_CORRUPTION=y
+CONFIG_TRACE_MMIO_ACCESS=y
+CONFIG_TRACEFS_DISABLE_AUTOMOUNT=y
+CONFIG_HIST_TRIGGERS=y
+# CONFIG_RUNTIME_TESTING_MENU is not set
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-clang.inc b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-clang.inc
new file mode 100644
index 0000000..c5b7463
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack-clang.inc
@@ -0,0 +1,8 @@
+# Clang-specific configuration of kernel build
+
+# We need to add this dependency as the kernel configuration depends on the compiler
+do_kernel_configme[depends] += "androidclang-native:do_populate_sysroot"
+
+DEPENDS:append = " androidclang-native"
+
+KERNEL_CC = "${CCACHE}clang ${HOST_CC_KERNEL_ARCH}"
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack_%.bbappend b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack_%.bbappend
new file mode 100644
index 0000000..301041b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack_%.bbappend
@@ -0,0 +1,3 @@
+LINUX_ARM64_ACK_TOOLCHAIN_REQUIRE = "${@oe.utils.ifelse(d.getVar('LINUX_ACK_TOOLCHAIN_CLANG'), 'linux-arm64-ack-clang.inc', '')}"
+
+require ${LINUX_ARM64_ACK_TOOLCHAIN_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack_5.10.bbappend b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack_5.10.bbappend
new file mode 100644
index 0000000..e2f9145
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-arm64-ack_5.10.bbappend
@@ -0,0 +1,5 @@
+# Machine specific configurations
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/${BP}:"
+
+require linux-arm-platforms.inc
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0001-iommu-arm-smmu-v3-workaround-for-ATC_INV_SIZE_ALL-in.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0001-iommu-arm-smmu-v3-workaround-for-ATC_INV_SIZE_ALL-in.patch
new file mode 100644
index 0000000..a75ca24
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0001-iommu-arm-smmu-v3-workaround-for-ATC_INV_SIZE_ALL-in.patch
@@ -0,0 +1,49 @@
+Upstream-Status: Inappropriate [Workaround]
+Signed-off-by: Manoj Kumar <manoj.kumar3@arm.com>
+
+From 949ba3f12ec1f3177a82a9228dc402ab5d8c9d60 Mon Sep 17 00:00:00 2001
+From: Manoj Kumar <manoj.kumar3@arm.com>
+Date: Mon, 1 Feb 2021 21:36:43 +0530
+Subject: [PATCH 1/5] iommu/arm-smmu-v3: workaround for ATC_INV_SIZE_ALL in
+ N1SDP
+
+ATC_INV_SIZE_ALL request should automatically translate to ATS
+address which is not happening in SMMUv3 version gone into
+N1SDP platform. This workaround manually sets the ATS address
+field to proper value for ATC_INV_SIZE_ALL command.
+
+Change-Id: If89465be94720a62be85e1e6612f17e93fa9b8a5
+Signed-off-by: Manoj Kumar <manoj.kumar3@arm.com>
+Signed-off-by: Khasim Syed Mohammed <khasim.mohammed@arm.com>
+---
+ drivers/iommu/arm/arm-smmu-v3/arm-smmu-v3.c | 1 +
+ drivers/iommu/arm/arm-smmu-v3/arm-smmu-v3.h | 1 +
+ 2 files changed, 2 insertions(+)
+
+diff --git a/drivers/iommu/arm/arm-smmu-v3/arm-smmu-v3.c b/drivers/iommu/arm/arm-smmu-v3/arm-smmu-v3.c
+index a388e318f86e..ceca576b0bf6 100644
+--- a/drivers/iommu/arm/arm-smmu-v3/arm-smmu-v3.c
++++ b/drivers/iommu/arm/arm-smmu-v3/arm-smmu-v3.c
+@@ -1724,6 +1724,7 @@ arm_smmu_atc_inv_to_cmd(int ssid, unsigned long iova, size_t size,
+ 	};
+ 
+ 	if (!size) {
++		cmd->atc.addr = ATC_INV_ADDR_ALL;
+ 		cmd->atc.size = ATC_INV_SIZE_ALL;
+ 		return;
+ 	}
+diff --git a/drivers/iommu/arm/arm-smmu-v3/arm-smmu-v3.h b/drivers/iommu/arm/arm-smmu-v3/arm-smmu-v3.h
+index 4cb136f07914..5615ffd24e46 100644
+--- a/drivers/iommu/arm/arm-smmu-v3/arm-smmu-v3.h
++++ b/drivers/iommu/arm/arm-smmu-v3/arm-smmu-v3.h
+@@ -473,6 +473,7 @@ struct arm_smmu_cmdq_ent {
+ 
+ 		#define CMDQ_OP_ATC_INV		0x40
+ 		#define ATC_INV_SIZE_ALL	52
++		#define ATC_INV_ADDR_ALL        0x7FFFFFFFFFFFF000UL
+ 		struct {
+ 			u32			sid;
+ 			u32			ssid;
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0002-n1sdp-pci_quirk-add-acs-override-for-PCI-devices.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0002-n1sdp-pci_quirk-add-acs-override-for-PCI-devices.patch
new file mode 100644
index 0000000..a628409
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0002-n1sdp-pci_quirk-add-acs-override-for-PCI-devices.patch
@@ -0,0 +1,160 @@
+Upstream-Status: Inappropriate [will not be submitted as its a workaround to address hardware issue]
+Signed-off-by: Khasim Syed Mohammed <khasim.mohammed@arm.com>
+
+From e47ab593ee36b2480f8c2196722cded42749629a Mon Sep 17 00:00:00 2001
+From: Manoj Kumar <manoj.kumar3@arm.com>
+Date: Tue, 31 Aug 2021 16:15:38 +0000
+Subject: [PATCH 2/5] n1sdp: pci_quirk: add acs override for PCI devices
+
+Patch taken from:
+https://gitlab.com/Queuecumber/linux-acs-override/raw/master/workspaces/5.4/acso.patch
+
+Change-Id: Ib926bf50524ce9990fbaa2f2f8670fe84bd571f9
+Signed-off-by: Manoj Kumar <manoj.kumar3@arm.com>
+---
+ .../admin-guide/kernel-parameters.txt         |   8 ++
+ drivers/pci/quirks.c                          | 102 ++++++++++++++++++
+ 2 files changed, 110 insertions(+)
+
+diff --git a/Documentation/admin-guide/kernel-parameters.txt b/Documentation/admin-guide/kernel-parameters.txt
+index 43dc35fe5bc0..a60e454854d7 100644
+--- a/Documentation/admin-guide/kernel-parameters.txt
++++ b/Documentation/admin-guide/kernel-parameters.txt
+@@ -3892,6 +3892,14 @@
+ 		nomsi		[MSI] If the PCI_MSI kernel config parameter is
+ 				enabled, this kernel boot option can be used to
+ 				disable the use of MSI interrupts system-wide.
++		pcie_acs_override [PCIE] Override missing PCIe ACS support for
++				downstream
++				All downstream ports - full ACS capabilities
++				multfunction
++				All multifunction devices - multifunction ACS subset
++				id:nnnn:nnnn
++				Specfic device - full ACS capabilities
++				Specified as vid:did (vendor/device ID) in hex
+ 		noioapicquirk	[APIC] Disable all boot interrupt quirks.
+ 				Safety option to keep boot IRQs enabled. This
+ 				should never be necessary.
+diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
+index 4537d1ea14fd..984f30d25a6d 100644
+--- a/drivers/pci/quirks.c
++++ b/drivers/pci/quirks.c
+@@ -3588,6 +3588,107 @@ static void quirk_no_bus_reset(struct pci_dev *dev)
+ 	dev->dev_flags |= PCI_DEV_FLAGS_NO_BUS_RESET;
+ }
+ 
++static bool acs_on_downstream;
++static bool acs_on_multifunction;
++
++#define NUM_ACS_IDS 16
++struct acs_on_id {
++	unsigned short vendor;
++	unsigned short device;
++};
++static struct acs_on_id acs_on_ids[NUM_ACS_IDS];
++static u8 max_acs_id;
++
++static __init int pcie_acs_override_setup(char *p)
++{
++	if (!p)
++		return -EINVAL;
++
++	while (*p) {
++		if (!strncmp(p, "downstream", 10))
++			acs_on_downstream = true;
++		if (!strncmp(p, "multifunction", 13))
++			acs_on_multifunction = true;
++		if (!strncmp(p, "id:", 3)) {
++			char opt[5];
++			int ret;
++			long val;
++
++			if (max_acs_id >= NUM_ACS_IDS - 1) {
++				pr_warn("Out of PCIe ACS override slots (%d)\n",
++						NUM_ACS_IDS);
++				goto next;
++			}
++
++			p += 3;
++			snprintf(opt, 5, "%s", p);
++			ret = kstrtol(opt, 16, &val);
++			if (ret) {
++				pr_warn("PCIe ACS ID parse error %d\n", ret);
++				goto next;
++			}
++			acs_on_ids[max_acs_id].vendor = val;
++
++			p += strcspn(p, ":");
++			if (*p != ':') {
++				pr_warn("PCIe ACS invalid ID\n");
++				goto next;
++			}
++
++			p++;
++			snprintf(opt, 5, "%s", p);
++			ret = kstrtol(opt, 16, &val);
++			if (ret) {
++				pr_warn("PCIe ACS ID parse error %d\n", ret);
++				goto next;
++			}
++			acs_on_ids[max_acs_id].device = val;
++			max_acs_id++;
++		}
++next:
++		p += strcspn(p, ",");
++		if (*p == ',')
++			p++;
++	}
++
++	if (acs_on_downstream || acs_on_multifunction || max_acs_id)
++		pr_warn("Warning: PCIe ACS overrides enabled; This may allow non-IOMMU protected peer-to-peer DMA\n");
++
++	return 0;
++}
++early_param("pcie_acs_override", pcie_acs_override_setup);
++
++static int pcie_acs_overrides(struct pci_dev *dev, u16 acs_flags)
++{
++	int i;
++
++	/* Never override ACS for legacy devices or devices with ACS caps */
++	if (!pci_is_pcie(dev) ||
++		pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ACS))
++			return -ENOTTY;
++
++	for (i = 0; i < max_acs_id; i++)
++		if (acs_on_ids[i].vendor == dev->vendor &&
++			acs_on_ids[i].device == dev->device)
++				return 1;
++
++	switch (pci_pcie_type(dev)) {
++	case PCI_EXP_TYPE_DOWNSTREAM:
++	case PCI_EXP_TYPE_ROOT_PORT:
++		if (acs_on_downstream)
++			return 1;
++		break;
++	case PCI_EXP_TYPE_ENDPOINT:
++	case PCI_EXP_TYPE_UPSTREAM:
++	case PCI_EXP_TYPE_LEG_END:
++	case PCI_EXP_TYPE_RC_END:
++		if (acs_on_multifunction && dev->multifunction)
++			return 1;
++	}
++
++	return -ENOTTY;
++}
++
+ /*
+  * Some NVIDIA GPU devices do not work with bus reset, SBR needs to be
+  * prevented for those affected devices.
+@@ -4949,6 +5050,7 @@ static const struct pci_dev_acs_enabled {
+ 	{ PCI_VENDOR_ID_NXP, 0x8d9b, pci_quirk_nxp_rp_acs },
+ 	/* Zhaoxin Root/Downstream Ports */
+ 	{ PCI_VENDOR_ID_ZHAOXIN, PCI_ANY_ID, pci_quirk_zhaoxin_pcie_ports_acs },
++	{ PCI_ANY_ID, PCI_ANY_ID, pcie_acs_overrides },
+ 	{ 0 }
+ };
+ 
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0003-pcie-Add-quirk-for-the-Arm-Neoverse-N1SDP-platform.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0003-pcie-Add-quirk-for-the-Arm-Neoverse-N1SDP-platform.patch
new file mode 100644
index 0000000..f0184a0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0003-pcie-Add-quirk-for-the-Arm-Neoverse-N1SDP-platform.patch
@@ -0,0 +1,323 @@
+Upstream-Status: Inappropriate [will not be submitted as its a workaround to address hardware issue]
+Signed-off-by: Deepak Pandey <Deepak.Pandey@arm.com>
+
+From 63ee3a71eeb778a632f5683f3b9e404a70760e75 Mon Sep 17 00:00:00 2001
+From: Deepak Pandey <Deepak.Pandey@arm.com>
+Date: Fri, 31 May 2019 16:42:43 +0100
+Subject: [PATCH 3/5] pcie: Add quirk for the Arm Neoverse N1SDP platform
+
+The Arm N1SDP SoC suffers from some PCIe integration issues, most
+prominently config space accesses to not existing BDFs being answered
+with a bus abort, resulting in an SError.
+To mitigate this, the firmware scans the bus before boot (catching the
+SErrors) and creates a table with valid BDFs, which acts as a filter for
+Linux' config space accesses.
+
+Add code consulting the table as an ACPI PCIe quirk, also register the
+corresponding device tree based description of the host controller.
+Also fix the other two minor issues on the way, namely not being fully
+ECAM compliant and config space accesses being restricted to 32-bit
+accesses only.
+
+This allows the Arm Neoverse N1SDP board to boot Linux without crashing
+and to access *any* devices (there are no platform devices except UART).
+
+Signed-off-by: Deepak Pandey <Deepak.Pandey@arm.com>
+[Sudipto: extend to cover the CCIX root port as well]
+Signed-off-by: Sudipto Paul <sudipto.paul@arm.com>
+[Andre: fix coding style issues, rewrite some parts, add DT support]
+Signed-off-by: Andre Przywara <andre.przywara@arm.com>
+---
+ arch/arm64/configs/defconfig        |   1 +
+ drivers/acpi/pci_mcfg.c             |   7 +
+ drivers/pci/controller/Kconfig      |  11 ++
+ drivers/pci/controller/Makefile     |   1 +
+ drivers/pci/controller/pcie-n1sdp.c | 198 ++++++++++++++++++++++++++++
+ include/linux/pci-ecam.h            |   2 +
+ 6 files changed, 220 insertions(+)
+ create mode 100644 drivers/pci/controller/pcie-n1sdp.c
+
+diff --git a/arch/arm64/configs/defconfig b/arch/arm64/configs/defconfig
+index 545197bc0501..57ae850ccdf0 100644
+--- a/arch/arm64/configs/defconfig
++++ b/arch/arm64/configs/defconfig
+@@ -212,6 +212,7 @@ CONFIG_NFC_NCI=m
+ CONFIG_NFC_S3FWRN5_I2C=m
+ CONFIG_PCI=y
+ CONFIG_PCIEPORTBUS=y
++CONFIG_PCI_QUIRKS=y
+ CONFIG_PCI_IOV=y
+ CONFIG_PCI_PASID=y
+ CONFIG_HOTPLUG_PCI=y
+diff --git a/drivers/acpi/pci_mcfg.c b/drivers/acpi/pci_mcfg.c
+index 53cab975f612..f31727da21ac 100644
+--- a/drivers/acpi/pci_mcfg.c
++++ b/drivers/acpi/pci_mcfg.c
+@@ -169,6 +169,13 @@ static struct mcfg_fixup mcfg_quirks[] = {
+ 	ALTRA_ECAM_QUIRK(1, 13),
+ 	ALTRA_ECAM_QUIRK(1, 14),
+ 	ALTRA_ECAM_QUIRK(1, 15),
++
++#define N1SDP_ECAM_MCFG(rev, seg, ops) \
++	{"ARMLTD", "ARMN1SDP", rev, seg, MCFG_BUS_ANY, ops }
++
++	/* N1SDP SoC with v1 PCIe controller */
++	N1SDP_ECAM_MCFG(0x20181101, 0, &pci_n1sdp_pcie_ecam_ops),
++	N1SDP_ECAM_MCFG(0x20181101, 1, &pci_n1sdp_ccix_ecam_ops),
+ };
+ 
+ static char mcfg_oem_id[ACPI_OEM_ID_SIZE];
+diff --git a/drivers/pci/controller/Kconfig b/drivers/pci/controller/Kconfig
+index 326f7d13024f..f9700d037c46 100644
+--- a/drivers/pci/controller/Kconfig
++++ b/drivers/pci/controller/Kconfig
+@@ -46,6 +46,17 @@ config PCI_IXP4XX
+ 	  Say Y here if you want support for the PCI host controller found
+ 	  in the Intel IXP4xx XScale-based network processor SoC.
+ 
++config PCIE_HOST_N1SDP_ECAM
++	bool "ARM N1SDP PCIe Controller"
++	depends on ARM64
++	depends on OF || (ACPI && PCI_QUIRKS)
++	select PCI_HOST_COMMON
++	default y if ARCH_VEXPRESS
++	help
++	  Say Y here if you want PCIe support for the Arm N1SDP platform.
++	  The controller is ECAM compliant, but needs a quirk to workaround
++	  an integration issue.
++
+ config PCI_TEGRA
+ 	bool "NVIDIA Tegra PCIe controller"
+ 	depends on ARCH_TEGRA || COMPILE_TEST
+diff --git a/drivers/pci/controller/Makefile b/drivers/pci/controller/Makefile
+index aaf30b3dcc14..2012ab2b7913 100644
+--- a/drivers/pci/controller/Makefile
++++ b/drivers/pci/controller/Makefile
+@@ -37,6 +37,7 @@ obj-$(CONFIG_VMD) += vmd.o
+ obj-$(CONFIG_PCIE_BRCMSTB) += pcie-brcmstb.o
+ obj-$(CONFIG_PCI_LOONGSON) += pci-loongson.o
+ obj-$(CONFIG_PCIE_HISI_ERR) += pcie-hisi-error.o
++obj-$(CONFIG_PCIE_HOST_N1SDP_ECAM) += pcie-n1sdp.o
+ # pcie-hisi.o quirks are needed even without CONFIG_PCIE_DW
+ obj-y				+= dwc/
+ obj-y				+= mobiveil/
+diff --git a/drivers/pci/controller/pcie-n1sdp.c b/drivers/pci/controller/pcie-n1sdp.c
+new file mode 100644
+index 000000000000..408699b9dcb1
+--- /dev/null
++++ b/drivers/pci/controller/pcie-n1sdp.c
+@@ -0,0 +1,198 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * Copyright (C) 2018/2019 ARM Ltd.
++ *
++ * This quirk is to mask the following issues:
++ * - PCIE SLVERR: config space accesses to invalid PCIe BDFs cause a bus
++ *		  error (signalled as an asynchronous SError)
++ * - MCFG BDF mapping: the root complex is mapped separately from the device
++ *		       config space
++ * - Non 32-bit accesses to config space are not supported.
++ *
++ * At boot time the SCP board firmware creates a discovery table with
++ * the root complex' base address and the valid BDF values, discovered while
++ * scanning the config space and catching the SErrors.
++ * Linux responds only to the EPs listed in this table, returning NULL
++ * for the rest.
++ */
++
++#include <linux/kernel.h>
++#include <linux/init.h>
++#include <linux/ioport.h>
++#include <linux/sizes.h>
++#include <linux/of_pci.h>
++#include <linux/of.h>
++#include <linux/pci-ecam.h>
++#include <linux/platform_device.h>
++#include <linux/module.h>
++
++#include "../pci.h"
++
++/* Platform specific values as hardcoded in the firmware. */
++#define AP_NS_SHARED_MEM_BASE	0x06000000
++#define MAX_SEGMENTS		2		/* Two PCIe root complexes. */
++#define BDF_TABLE_SIZE		SZ_16K
++
++/*
++ * Shared memory layout as written by the SCP upon boot time:
++ *  ----
++ *  Discover data header --> RC base address
++ *                       \-> BDF Count
++ *  Discover data        --> BDF 0...n
++ *  ----
++ */
++struct pcie_discovery_data {
++	u32 rc_base_addr;
++	u32 nr_bdfs;
++	u32 valid_bdfs[0];
++} *pcie_discovery_data[MAX_SEGMENTS];
++
++void __iomem *rc_remapped_addr[MAX_SEGMENTS];
++
++/*
++ * map_bus() is called before we do a config space access for a certain
++ * device. We use this to check whether this device is valid, avoiding
++ * config space accesses which would result in an SError otherwise.
++ */
++static void __iomem *pci_n1sdp_map_bus(struct pci_bus *bus, unsigned int devfn,
++				       int where)
++{
++	struct pci_config_window *cfg = bus->sysdata;
++	unsigned int devfn_shift = cfg->ops->bus_shift - 8;
++	unsigned int busn = bus->number;
++	unsigned int segment = bus->domain_nr;
++	unsigned int bdf_addr;
++	unsigned int table_count, i;
++	struct pci_dev *dev;
++
++	if (segment >= MAX_SEGMENTS ||
++	    busn < cfg->busr.start || busn > cfg->busr.end)
++		return NULL;
++
++	/* The PCIe root complex has a separate config space mapping. */
++	if (busn == 0 && devfn == 0)
++		return rc_remapped_addr[segment] + where;
++
++	dev = pci_get_domain_bus_and_slot(segment, busn, devfn);
++	if (dev && dev->is_virtfn)
++		return pci_ecam_map_bus(bus, devfn, where);
++
++	/* Accesses beyond the vendor ID always go to existing devices. */
++	if (where > 0)
++		return pci_ecam_map_bus(bus, devfn, where);
++
++	busn -= cfg->busr.start;
++	bdf_addr = (busn << cfg->ops->bus_shift) + (devfn << devfn_shift);
++	table_count = pcie_discovery_data[segment]->nr_bdfs;
++	for (i = 0; i < table_count; i++) {
++		if (bdf_addr == pcie_discovery_data[segment]->valid_bdfs[i])
++			return pci_ecam_map_bus(bus, devfn, where);
++	}
++
++	return NULL;
++}
++
++static int pci_n1sdp_init(struct pci_config_window *cfg, unsigned int segment)
++{
++	phys_addr_t table_base;
++	struct device *dev = cfg->parent;
++	struct pcie_discovery_data *shared_data;
++	size_t bdfs_size;
++
++	if (segment >= MAX_SEGMENTS)
++		return -ENODEV;
++
++	table_base = AP_NS_SHARED_MEM_BASE + segment * BDF_TABLE_SIZE;
++
++	if (!request_mem_region(table_base, BDF_TABLE_SIZE,
++				"PCIe valid BDFs")) {
++		dev_err(dev, "PCIe BDF shared region request failed\n");
++		return -ENOMEM;
++	}
++
++	shared_data = devm_ioremap(dev,
++				   table_base, BDF_TABLE_SIZE);
++	if (!shared_data)
++		return -ENOMEM;
++
++	/* Copy the valid BDFs structure to allocated normal memory. */
++	bdfs_size = sizeof(struct pcie_discovery_data) +
++		    sizeof(u32) * shared_data->nr_bdfs;
++	pcie_discovery_data[segment] = devm_kmalloc(dev, bdfs_size, GFP_KERNEL);
++	if (!pcie_discovery_data[segment])
++		return -ENOMEM;
++
++	memcpy_fromio(pcie_discovery_data[segment], shared_data, bdfs_size);
++
++	rc_remapped_addr[segment] = devm_ioremap(dev,
++						 shared_data->rc_base_addr,
++						 PCI_CFG_SPACE_EXP_SIZE);
++	if (!rc_remapped_addr[segment]) {
++		dev_err(dev, "Cannot remap root port base\n");
++		return -ENOMEM;
++	}
++
++	devm_iounmap(dev, shared_data);
++
++	return 0;
++}
++
++/* Called for ACPI segment 0, and for all segments when using DT. */
++static int pci_n1sdp_pcie_init(struct pci_config_window *cfg)
++{
++	struct platform_device *pdev = to_platform_device(cfg->parent);
++	int segment = 0;
++
++	if (pdev->dev.of_node)
++		segment = of_get_pci_domain_nr(pdev->dev.of_node);
++	if (segment < 0 || segment > MAX_SEGMENTS) {
++		dev_err(&pdev->dev, "N1SDP PCI controllers require linux,pci-domain property\n");
++		dev_err(&pdev->dev, "Or invalid segment number, must be smaller than %d\n",
++			MAX_SEGMENTS);
++		return -EINVAL;
++	}
++
++	return pci_n1sdp_init(cfg, segment);
++}
++
++/* Called for ACPI segment 1. */
++static int pci_n1sdp_ccix_init(struct pci_config_window *cfg)
++{
++	return pci_n1sdp_init(cfg, 1);
++}
++
++const struct pci_ecam_ops pci_n1sdp_pcie_ecam_ops = {
++	.bus_shift	= 20,
++	.init		= pci_n1sdp_pcie_init,
++	.pci_ops	= {
++		.map_bus        = pci_n1sdp_map_bus,
++		.read           = pci_generic_config_read32,
++		.write          = pci_generic_config_write32,
++	}
++};
++
++const struct pci_ecam_ops pci_n1sdp_ccix_ecam_ops = {
++	.bus_shift	= 20,
++	.init		= pci_n1sdp_ccix_init,
++	.pci_ops	= {
++		.map_bus        = pci_n1sdp_map_bus,
++		.read           = pci_generic_config_read32,
++		.write          = pci_generic_config_write32,
++	}
++};
++
++static const struct of_device_id n1sdp_pcie_of_match[] = {
++	{ .compatible = "arm,n1sdp-pcie", .data = &pci_n1sdp_pcie_ecam_ops },
++	{ },
++};
++MODULE_DEVICE_TABLE(of, n1sdp_pcie_of_match);
++
++static struct platform_driver n1sdp_pcie_driver = {
++	.driver = {
++		.name = KBUILD_MODNAME,
++		.of_match_table = n1sdp_pcie_of_match,
++		.suppress_bind_attrs = true,
++	},
++	.probe = pci_host_common_probe,
++};
++builtin_platform_driver(n1sdp_pcie_driver);
+diff --git a/include/linux/pci-ecam.h b/include/linux/pci-ecam.h
+index adea5a4771cf..e6bbc037cef8 100644
+--- a/include/linux/pci-ecam.h
++++ b/include/linux/pci-ecam.h
+@@ -87,6 +87,8 @@ extern const struct pci_ecam_ops xgene_v1_pcie_ecam_ops; /* APM X-Gene PCIe v1 *
+ extern const struct pci_ecam_ops xgene_v2_pcie_ecam_ops; /* APM X-Gene PCIe v2.x */
+ extern const struct pci_ecam_ops al_pcie_ops;	/* Amazon Annapurna Labs PCIe */
+ extern const struct pci_ecam_ops tegra194_pcie_ops; /* Tegra194 PCIe */
++extern const struct pci_ecam_ops pci_n1sdp_pcie_ecam_ops; /* Arm N1SDP PCIe */
++extern const struct pci_ecam_ops pci_n1sdp_ccix_ecam_ops; /* Arm N1SDP PCIe */
+ #endif
+ 
+ #if IS_ENABLED(CONFIG_PCI_HOST_COMMON)
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0004-n1sdp-pcie-add-quirk-support-enabling-remote-chip-PC.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0004-n1sdp-pcie-add-quirk-support-enabling-remote-chip-PC.patch
new file mode 100644
index 0000000..c15b464
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0004-n1sdp-pcie-add-quirk-support-enabling-remote-chip-PC.patch
@@ -0,0 +1,137 @@
+Upstream-Status: Inappropriate [will not be submitted as its an hack required to fix the hardware issue]
+Signed-off-by: Sayanta Pattanayak <sayanta.pattanayak@arm.com>
+
+From 4d69e38213bf52a48f2f0239da8c7b76501428b2 Mon Sep 17 00:00:00 2001
+From: Sayanta Pattanayak <sayanta.pattanayak@arm.com>
+Date: Wed, 9 Feb 2022 20:37:43 +0530
+Subject: [PATCH 4/5] n1sdp: pcie: add quirk support enabling remote chip PCIe
+
+Base address mapping for remote chip Root PCIe ECAM space.
+
+When two N1SDP boards are coupled via the CCIX connection, the PCI host
+complex of the remote board appears as PCIe segment 2 on the primary board.
+The resources of the secondary board, including the host complex, are
+mapped at offset 0x40000000000 into the address space of the primary
+board, so take that into account when accessing the remote PCIe segment.
+
+Change-Id: I0e8d1eb119aef6444b9df854a39b24441c12195a
+Signed-off-by: Sayanta Pattanayak <sayanta.pattanayak@arm.com>
+Signed-off-by: Khasim Syed Mohammed <khasim.mohammed@arm.com>
+Signed-off-by: Andre Przywara <andre.przywara@arm.com>
+Signed-off-by: sahil <sahil@arm.com>
+---
+ drivers/acpi/pci_mcfg.c             |  1 +
+ drivers/pci/controller/pcie-n1sdp.c | 32 +++++++++++++++++++++++++----
+ include/linux/pci-ecam.h            |  1 +
+ 3 files changed, 30 insertions(+), 4 deletions(-)
+
+diff --git a/drivers/acpi/pci_mcfg.c b/drivers/acpi/pci_mcfg.c
+index f31727da21ac..58f59b5fffa2 100644
+--- a/drivers/acpi/pci_mcfg.c
++++ b/drivers/acpi/pci_mcfg.c
+@@ -176,6 +176,7 @@ static struct mcfg_fixup mcfg_quirks[] = {
+ 	/* N1SDP SoC with v1 PCIe controller */
+ 	N1SDP_ECAM_MCFG(0x20181101, 0, &pci_n1sdp_pcie_ecam_ops),
+ 	N1SDP_ECAM_MCFG(0x20181101, 1, &pci_n1sdp_ccix_ecam_ops),
++	N1SDP_ECAM_MCFG(0x20181101, 2, &pci_n1sdp_remote_pcie_ecam_ops),
+ };
+ 
+ static char mcfg_oem_id[ACPI_OEM_ID_SIZE];
+diff --git a/drivers/pci/controller/pcie-n1sdp.c b/drivers/pci/controller/pcie-n1sdp.c
+index 408699b9dcb1..a03665dd056a 100644
+--- a/drivers/pci/controller/pcie-n1sdp.c
++++ b/drivers/pci/controller/pcie-n1sdp.c
+@@ -30,8 +30,10 @@
+ 
+ /* Platform specific values as hardcoded in the firmware. */
+ #define AP_NS_SHARED_MEM_BASE	0x06000000
+-#define MAX_SEGMENTS		2		/* Two PCIe root complexes. */
++/* Two PCIe root complexes in One Chip + One PCIe RC in Remote Chip */
++#define MAX_SEGMENTS		3
+ #define BDF_TABLE_SIZE		SZ_16K
++#define REMOTE_CHIP_ADDR_OFFSET	0x40000000000
+ 
+ /*
+  * Shared memory layout as written by the SCP upon boot time:
+@@ -97,12 +99,17 @@ static int pci_n1sdp_init(struct pci_config_window *cfg, unsigned int segment)
+ 	phys_addr_t table_base;
+ 	struct device *dev = cfg->parent;
+ 	struct pcie_discovery_data *shared_data;
+-	size_t bdfs_size;
++	size_t bdfs_size, rc_base_addr = 0;
+ 
+ 	if (segment >= MAX_SEGMENTS)
+ 		return -ENODEV;
+ 
+-	table_base = AP_NS_SHARED_MEM_BASE + segment * BDF_TABLE_SIZE;
++	if (segment > 1) {
++	    rc_base_addr = REMOTE_CHIP_ADDR_OFFSET;
++	    table_base = AP_NS_SHARED_MEM_BASE + REMOTE_CHIP_ADDR_OFFSET;
++        } else {
++            table_base = AP_NS_SHARED_MEM_BASE + segment * BDF_TABLE_SIZE;
++	}
+ 
+ 	if (!request_mem_region(table_base, BDF_TABLE_SIZE,
+ 				"PCIe valid BDFs")) {
+@@ -114,6 +121,7 @@ static int pci_n1sdp_init(struct pci_config_window *cfg, unsigned int segment)
+ 				   table_base, BDF_TABLE_SIZE);
+ 	if (!shared_data)
+ 		return -ENOMEM;
++	rc_base_addr += shared_data->rc_base_addr;
+ 
+ 	/* Copy the valid BDFs structure to allocated normal memory. */
+ 	bdfs_size = sizeof(struct pcie_discovery_data) +
+@@ -125,7 +133,7 @@ static int pci_n1sdp_init(struct pci_config_window *cfg, unsigned int segment)
+ 	memcpy_fromio(pcie_discovery_data[segment], shared_data, bdfs_size);
+ 
+ 	rc_remapped_addr[segment] = devm_ioremap(dev,
+-						 shared_data->rc_base_addr,
++						 rc_base_addr,
+ 						 PCI_CFG_SPACE_EXP_SIZE);
+ 	if (!rc_remapped_addr[segment]) {
+ 		dev_err(dev, "Cannot remap root port base\n");
+@@ -161,6 +169,12 @@ static int pci_n1sdp_ccix_init(struct pci_config_window *cfg)
+ 	return pci_n1sdp_init(cfg, 1);
+ }
+ 
++/* Called for ACPI segment 2. */
++static int pci_n1sdp_remote_pcie_init(struct pci_config_window *cfg)
++{
++        return pci_n1sdp_init(cfg, 2);
++}
++
+ const struct pci_ecam_ops pci_n1sdp_pcie_ecam_ops = {
+ 	.bus_shift	= 20,
+ 	.init		= pci_n1sdp_pcie_init,
+@@ -181,6 +195,16 @@ const struct pci_ecam_ops pci_n1sdp_ccix_ecam_ops = {
+ 	}
+ };
+ 
++const struct pci_ecam_ops pci_n1sdp_remote_pcie_ecam_ops = {
++	.bus_shift	= 20,
++	.init		= pci_n1sdp_remote_pcie_init,
++	.pci_ops	= {
++		.map_bus        = pci_n1sdp_map_bus,
++		.read           = pci_generic_config_read32,
++		.write          = pci_generic_config_write32,
++	}
++};
++
+ static const struct of_device_id n1sdp_pcie_of_match[] = {
+ 	{ .compatible = "arm,n1sdp-pcie", .data = &pci_n1sdp_pcie_ecam_ops },
+ 	{ },
+diff --git a/include/linux/pci-ecam.h b/include/linux/pci-ecam.h
+index e6bbc037cef8..7bd8c1d702ee 100644
+--- a/include/linux/pci-ecam.h
++++ b/include/linux/pci-ecam.h
+@@ -89,6 +89,7 @@ extern const struct pci_ecam_ops al_pcie_ops;	/* Amazon Annapurna Labs PCIe */
+ extern const struct pci_ecam_ops tegra194_pcie_ops; /* Tegra194 PCIe */
+ extern const struct pci_ecam_ops pci_n1sdp_pcie_ecam_ops; /* Arm N1SDP PCIe */
+ extern const struct pci_ecam_ops pci_n1sdp_ccix_ecam_ops; /* Arm N1SDP PCIe */
++extern const struct pci_ecam_ops pci_n1sdp_remote_pcie_ecam_ops; /* Arm N1SDP PCIe */
+ #endif
+ 
+ #if IS_ENABLED(CONFIG_PCI_HOST_COMMON)
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0005-arm64-kpti-Whitelist-early-Arm-Neoverse-N1-revisions.patch b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0005-arm64-kpti-Whitelist-early-Arm-Neoverse-N1-revisions.patch
new file mode 100644
index 0000000..0404086
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/0005-arm64-kpti-Whitelist-early-Arm-Neoverse-N1-revisions.patch
@@ -0,0 +1,34 @@
+Upstream-Status: Inappropriate
+Signed-off-by: Andre Przywara <andre.przywara@arm.com>
+
+From d20f5afffadcdbaca7032f547cce80720d8a414a Mon Sep 17 00:00:00 2001
+From: Andre Przywara <andre.przywara@arm.com>
+Date: Fri, 17 May 2019 17:39:27 +0100
+Subject: [PATCH 5/5] arm64: kpti: Whitelist early Arm Neoverse N1 revisions
+
+Early revisions (r1p0) of the Neoverse N1 core did not feature the
+CSV3 field in ID_AA64PFR0_EL1 to advertise they are not affected by
+the Spectre variant 3 (aka Meltdown) vulnerability.
+
+Add this particular revision to the whitelist to avoid enabling KPTI.
+
+Signed-off-by: Andre Przywara <andre.przywara@arm.com>
+---
+ arch/arm64/kernel/cpufeature.c | 1 +
+ 1 file changed, 1 insertion(+)
+
+diff --git a/arch/arm64/kernel/cpufeature.c b/arch/arm64/kernel/cpufeature.c
+index 6ec7036ef7e1..ceba98773608 100644
+--- a/arch/arm64/kernel/cpufeature.c
++++ b/arch/arm64/kernel/cpufeature.c
+@@ -1509,6 +1509,7 @@ static bool unmap_kernel_at_el0(const struct arm64_cpu_capabilities *entry,
+ 		MIDR_ALL_VERSIONS(MIDR_QCOM_KRYO_2XX_SILVER),
+ 		MIDR_ALL_VERSIONS(MIDR_QCOM_KRYO_3XX_SILVER),
+ 		MIDR_ALL_VERSIONS(MIDR_QCOM_KRYO_4XX_SILVER),
++		MIDR_REV(MIDR_NEOVERSE_N1, 1, 0),	/* missing CSV3 */
+ 		{ /* sentinel */ }
+ 	};
+ 	char const *str = "kpti command line option";
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/enable-realtek-R8169.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/enable-realtek-R8169.cfg
new file mode 100644
index 0000000..7a57474
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/enable-realtek-R8169.cfg
@@ -0,0 +1,3 @@
+# Enable Realtek Gigabit Ethernet adapter
+CONFIG_REALTEK_PHY=y
+CONFIG_R8169=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/enable-usb_conn_gpio.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/enable-usb_conn_gpio.cfg
new file mode 100644
index 0000000..128c902
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/enable-usb_conn_gpio.cfg
@@ -0,0 +1,2 @@
+# PHY_TEGRA_XUSB sets this to y, but its set as m in defconfig
+CONFIG_USB_CONN_GPIO=y
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/usb_xhci_pci_renesas.cfg b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/usb_xhci_pci_renesas.cfg
new file mode 100644
index 0000000..c06507c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-5.15/n1sdp/usb_xhci_pci_renesas.cfg
@@ -0,0 +1,2 @@
+# CONFIG_USB_XHCI_PCI is not set
+# CONFIG_USB_XHCI_PCI_RENESAS is not set
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-rt_%.bbappend b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-rt_%.bbappend
new file mode 100644
index 0000000..8994c24
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto-rt_%.bbappend
@@ -0,0 +1,6 @@
+# Only enable linux-yocto-rt for n1sdp and the Armv8-R AArch64 AEM FVP
+LINUX_YOCTO_RT_REQUIRE ?= ""
+LINUX_YOCTO_RT_REQUIRE:n1sdp = "linux-arm-platforms.inc"
+LINUX_YOCTO_RT_REQUIRE:fvp-baser-aemv8r64 = "linux-arm-platforms.inc"
+
+require ${LINUX_YOCTO_RT_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto_%.bbappend b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto_%.bbappend
new file mode 100644
index 0000000..db850ea
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-kernel/linux/linux-yocto_%.bbappend
@@ -0,0 +1,3 @@
+# Add support for Arm Platforms (boards or simulators)
+
+require linux-arm-platforms.inc
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0001-plat-corstone1000-add-corstone1000-platform.patch b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0001-plat-corstone1000-add-corstone1000-platform.patch
new file mode 100644
index 0000000..7666486
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0001-plat-corstone1000-add-corstone1000-platform.patch
@@ -0,0 +1,201 @@
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Arpita S.K <arpita.s.k@arm.com>
+
+From 439a87df6a9f60f2b29afd988ad58a67e6f0b603 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Tue, 22 Jun 2021 22:09:28 +0100
+Subject: [PATCH] plat-corstone1000: add corstone1000 platform
+
+These changes are to add corstone1000 platform to optee core
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+diff --git a/core/arch/arm/plat-corstone1000/conf.mk b/core/arch/arm/plat-corstone1000/conf.mk
+new file mode 100644
+index 00000000..b14dd442
+--- /dev/null
++++ b/core/arch/arm/plat-corstone1000/conf.mk
+@@ -0,0 +1,37 @@
++PLATFORM_FLAVOR ?= mps3
++
++$(call force,CFG_HWSUPP_MEM_PERM_WXN,y)
++$(call force,CFG_HWSUPP_MEM_PERM_PXN,y)
++$(call force,CFG_ENABLE_SCTLR_RR,n)
++$(call force,CFG_ENABLE_SCTLR_Z,n)
++
++arm64-platform-cpuarch := cortex-a35
++arm64-platform-cflags += -mcpu=$(arm64-platform-cpuarch)
++arm64-platform-aflags += -mcpu=$(arm64-platform-cpuarch)
++platform-flavor-armv8 := 1
++
++$(call force,CFG_GIC,y)
++$(call force,CFG_PL011,y)
++$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
++
++$(call force,CFG_ARM64_core,y)
++
++CFG_WITH_STATS ?= y
++
++CFG_WITH_ARM_TRUSTED_FW ?= y
++CFG_WITH_LPAE ?=y
++
++CFG_TEE_CORE_NB_CORE = 1
++CFG_TZDRAM_START ?= 0x02002000
++CFG_TZDRAM_SIZE  ?= 0x000FE000
++CFG_TEE_RAM_VA_SIZE ?= 0x00AF000
++CFG_SHMEM_START  ?= 0x86000000
++CFG_SHMEM_SIZE   ?= 0x00200000
++
++CFG_DDR_SIZE ?= 0x80000000
++CFG_DT_ADDR ?= 0x82100000
++CFG_DTB_MAX_SIZE ?= 0x100000
++
++$(call force,CFG_PSCI_ARM64,y)
++$(call force,CFG_DT,y)
++$(call force,CFG_EXTERNAL_DTB_OVERLAY,y)
+diff --git a/core/arch/arm/plat-corstone1000/main.c b/core/arch/arm/plat-corstone1000/main.c
+new file mode 100644
+index 00000000..35d89535
+--- /dev/null
++++ b/core/arch/arm/plat-corstone1000/main.c
+@@ -0,0 +1,77 @@
++// SPDX-License-Identifier: BSD-2-Clause
++/*
++ * Copyright (c) 2020, Linaro Limited
++ */
++
++#include <arm64.h>
++#include <console.h>
++#include <drivers/gic.h>
++#include <drivers/pl011.h>
++#include <drivers/tzc400.h>
++#include <initcall.h>
++#include <keep.h>
++#include <kernel/boot.h>
++#include <kernel/interrupt.h>
++#include <kernel/misc.h>
++#include <kernel/panic.h>
++#include <kernel/tee_time.h>
++#include <mm/core_memprot.h>
++#include <mm/core_mmu.h>
++#include <platform_config.h>
++#include <sm/psci.h>
++#include <stdint.h>
++#include <string.h>
++#include <trace.h>
++
++static struct gic_data gic_data __nex_bss;
++static struct pl011_data console_data __nex_bss;
++
++register_phys_mem_pgdir(MEM_AREA_IO_SEC, CONSOLE_UART_BASE, PL011_REG_SIZE);
++#ifdef DRAM0_BASE
++register_ddr(DRAM0_BASE, DRAM0_SIZE);
++#endif
++
++#ifdef GIC_BASE
++register_phys_mem_pgdir(MEM_AREA_IO_SEC, GICD_BASE, GIC_DIST_REG_SIZE);
++register_phys_mem_pgdir(MEM_AREA_IO_SEC, GICC_BASE, GIC_DIST_REG_SIZE);
++
++void main_init_gic(void)
++{
++	vaddr_t gicc_base;
++	vaddr_t gicd_base;
++
++	gicc_base = core_mmu_get_va(GICC_BASE, MEM_AREA_IO_SEC);
++	gicd_base = core_mmu_get_va(GICD_BASE, MEM_AREA_IO_SEC);
++
++	if (!gicc_base || !gicd_base)
++		panic();
++
++	/* Initialize GIC */
++	gic_init(&gic_data, gicc_base, gicd_base);
++	itr_init(&gic_data.chip);
++
++}
++
++void main_secondary_init_gic(void)
++{
++	gic_cpu_init(&gic_data);
++}
++
++void itr_core_handler(void)
++{
++	gic_it_handle(&gic_data);
++}
++#endif
++
++void console_init(void)
++{
++	pl011_init(&console_data, CONSOLE_UART_BASE, CONSOLE_UART_CLK_IN_HZ,
++		   CONSOLE_BAUDRATE);
++	register_serial_console(&console_data.chip);
++}
++
++void ffa_secondary_cpu_boot_req(vaddr_t secondary_ep, uint64_t cookie)
++{
++	DMSG("This is single core platform\n");
++}
++
+diff --git a/core/arch/arm/plat-corstone1000/platform_config.h b/core/arch/arm/plat-corstone1000/platform_config.h
+new file mode 100644
+index 00000000..cfee6fa4
+--- /dev/null
++++ b/core/arch/arm/plat-corstone1000/platform_config.h
+@@ -0,0 +1,46 @@
++/* SPDX-License-Identifier: BSD-2-Clause */
++/*
++ * Copyright (c) 2020, Linaro Limited
++ */
++
++#ifndef PLATFORM_CONFIG_H
++#define PLATFORM_CONFIG_H
++
++#include <mm/generic_ram_layout.h>
++#include <stdint.h>
++
++/* Make stacks aligned to data cache line length */
++#define STACK_ALIGNMENT		64
++
++
++#define GIC_BASE		0x1c000000
++#define UART0_BASE		0x1a510000
++#define UART1_BASE		0x1a520000
++
++#define CONSOLE_UART_BASE	UART1_BASE
++
++#define DRAM0_BASE		0x80000000
++#define DRAM0_SIZE		0x7f000000
++
++#define GICD_OFFSET		0x10000
++#define GICC_OFFSET		0x2f000
++
++#ifdef GIC_BASE
++#define GICD_BASE		(GIC_BASE + GICD_OFFSET)
++#define GICC_BASE		(GIC_BASE + GICC_OFFSET)
++#endif
++
++#ifndef UART_BAUDRATE
++#define UART_BAUDRATE		115200
++#endif
++#ifndef CONSOLE_BAUDRATE
++#define CONSOLE_BAUDRATE	UART_BAUDRATE
++#endif
++
++#ifndef SYS_COUNTER_FREQ_IN_TICKS
++#define SYS_COUNTER_FREQ_IN_TICKS	UL(50000000) /* 32MHz */
++#endif
++
++#define CONSOLE_UART_CLK_IN_HZ	UL(50000000) /* 32MHz*/
++
++#endif /*PLATFORM_CONFIG_H*/
+diff --git a/core/arch/arm/plat-corstone1000/sub.mk b/core/arch/arm/plat-corstone1000/sub.mk
+new file mode 100644
+index 00000000..8ddc2fd4
+--- /dev/null
++++ b/core/arch/arm/plat-corstone1000/sub.mk
+@@ -0,0 +1,2 @@
++global-incdirs-y += .
++srcs-y += main.c
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0002-plat-corstone1000-reserve-3MB-CVM-memory-for-optee.patch b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0002-plat-corstone1000-reserve-3MB-CVM-memory-for-optee.patch
new file mode 100644
index 0000000..ea60fee
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0002-plat-corstone1000-reserve-3MB-CVM-memory-for-optee.patch
@@ -0,0 +1,30 @@
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Arpita S.K <arpita.s.k@arm.com>
+
+From dbaf7a11e686d362eb09e63841eb718ea777dd03 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Wed, 1 Sep 2021 16:46:42 +0100
+Subject: [PATCH] plat-corstone1000: reserve 3MB CVM memory for optee
+
+optee requires 3MB CVM memory to include SecurePartition's into
+it's image
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+diff --git a/core/arch/arm/plat-corstone1000/conf.mk b/core/arch/arm/plat-corstone1000/conf.mk
+index b14dd442..7a4aa13c 100644
+--- a/core/arch/arm/plat-corstone1000/conf.mk
++++ b/core/arch/arm/plat-corstone1000/conf.mk
+@@ -23,8 +23,7 @@ CFG_WITH_LPAE ?=y
+ 
+ CFG_TEE_CORE_NB_CORE = 1
+ CFG_TZDRAM_START ?= 0x02002000
+-CFG_TZDRAM_SIZE  ?= 0x000FE000
+-CFG_TEE_RAM_VA_SIZE ?= 0x00AF000
++CFG_TZDRAM_SIZE  ?= 0x300000 # OPTEE CODE + DATA +TA_RAM = 3MB
+ CFG_SHMEM_START  ?= 0x86000000
+ CFG_SHMEM_SIZE   ?= 0x00200000
+ 
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0003-plat-corstone1000-add-a-rule-in-Makefile-to-SP_MAKEF.patch b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0003-plat-corstone1000-add-a-rule-in-Makefile-to-SP_MAKEF.patch
new file mode 100644
index 0000000..9aec829
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0003-plat-corstone1000-add-a-rule-in-Makefile-to-SP_MAKEF.patch
@@ -0,0 +1,38 @@
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+From 327290faefe16440c4975c9539b11d43c92bcf48 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Thu, 25 Nov 2021 06:20:48 +0000
+Subject: [PATCH] plat-corstone1000: add a rule in Makefile to SP_MAKEFILE_PATH
+
+This change is to provide an option to user to include
+secure manifest into optee-os Makefile so that the secure
+partition is part of optee-os image
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+diff --git a/Makefile b/Makefile
+index f6fcea8b..21722939 100644
+--- a/Makefile
++++ b/Makefile
+@@ -18,6 +18,16 @@ unexport MAKEFILE_LIST
+ # nonzero status). Useful since a few recipes use shell redirection.
+ .DELETE_ON_ERROR:
+ 
++# include secure partition make file and manifest file
++ifdef CFG_SP_MKFILE_PATH
++ifdef CFG_EMBED_DTB_SOURCE_FILE
++include $(CFG_SP_MKFILE_PATH)
++$(info Loading secure partitions manifest ${CFG_EMBED_DTB_SOURCE_FILE})
++CFG_EMBED_DTB_SOURCE_FILE :=${CFG_EMBED_DTB_SOURCE_FILE}
++OPTEE_OS_COMMON_EXTRA_FLAGS+=${CFG_EMBED_DTB_SOURCE_FILE}
++endif
++endif
++
+ include mk/checkconf.mk
+ 
+ .PHONY: all
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0004-plat-corstone1000-increase-OPTEE-core-heap-size.patch b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0004-plat-corstone1000-increase-OPTEE-core-heap-size.patch
new file mode 100644
index 0000000..42babb1
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0004-plat-corstone1000-increase-OPTEE-core-heap-size.patch
@@ -0,0 +1,29 @@
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+From 5fdf7792ead29784718d30d8a8a5383a994e2fef Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Thu, 2 Dec 2021 12:51:11 +0000
+Subject: [PATCH] plat-corstone1000: increase OPTEE core heap size
+
+This change is to increase optee core heap size to 131072
+from its default value to fit openAMP and smm-gateway
+in SEL0
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+diff --git a/core/arch/arm/plat-corstone1000/conf.mk b/core/arch/arm/plat-corstone1000/conf.mk
+index 7a4aa13c..65d652a4 100644
+--- a/core/arch/arm/plat-corstone1000/conf.mk
++++ b/core/arch/arm/plat-corstone1000/conf.mk
+@@ -31,6 +31,7 @@ CFG_DDR_SIZE ?= 0x80000000
+ CFG_DT_ADDR ?= 0x82100000
+ CFG_DTB_MAX_SIZE ?= 0x100000
+ 
++CFG_CORE_HEAP_SIZE ?= 131072
+ $(call force,CFG_PSCI_ARM64,y)
+ $(call force,CFG_DT,y)
+ $(call force,CFG_EXTERNAL_DTB_OVERLAY,y)
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0005-Fix-add-missing-error-check-during-SP-init.patch b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0005-Fix-add-missing-error-check-during-SP-init.patch
new file mode 100644
index 0000000..852067b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/0005-Fix-add-missing-error-check-during-SP-init.patch
@@ -0,0 +1,40 @@
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+From 4617f85b70c7e4206b244f3eaffdc62ac3744a17 Mon Sep 17 00:00:00 2001
+From: Jelle Sels <jelle.sels@arm.com>
+Date: Mon, 10 May 2021 11:37:23 +0200
+Subject: [PATCH] Fix: add missing error check during SP init
+
+Error checking during SP initialization (sp_init_uuid()) was lacking.
+As a result an out of CORE HEAP situation resulted in a panic. This was
+due to lack of memory in the inflate() function of zlib.
+
+Signed-off-by: Jelle Sels <jelle.sels@arm.com>
+Change-Id: I287e2e2dd507feb45991b32ed264b3b6ad6fcd39
+
+diff --git a/core/arch/arm/kernel/sp.c b/core/arch/arm/kernel/sp.c
+index 4acbc2e2..23532a01 100644
+--- a/core/arch/arm/kernel/sp.c
++++ b/core/arch/arm/kernel/sp.c
+@@ -383,10 +383,14 @@ static TEE_Result __attribute__((unused)) sp_init_uuid(const TEE_UUID *uuid)
+ 				  cancel_req_to,
+ 				  &param);
+ 
+-	args.a0 = sp_get_session(sess->id)->sp_regs.x[0];
+-	sp_thread(sess->id, FFA_NW_ID, &args);
++	if (res == TEE_SUCCESS) {
++		args.a0 = sp_get_session(sess->id)->sp_regs.x[0];
++		sp_thread(sess->id, FFA_NW_ID, &args);
+ 
+-	thread_spmc_msg_recv(&args, sess->id);
++		thread_spmc_msg_recv(&args, sess->id);
++	} else {
++		EMSG("SP initialization failed, try increasing CFG_CORE_HEAP_SIZE");
++	}
+ 
+ 	return res;
+ }
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/sp_manifest_combined_se.dts b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/sp_manifest_combined_se.dts
new file mode 100644
index 0000000..b09018e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/corstone1000/sp_manifest_combined_se.dts
@@ -0,0 +1,62 @@
+/*
+* Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+*
+* SPDX-License-Identifier: BSD-3-Clause
+*/
+
+/dts-v1/;
+
+/ {
+    se-proxy{
+        compatible = "arm,ffa-manifest-1.0";
+        ffa-version = <0x00010000>; /* 31:16 - Major, 15:0 - Minor */
+        uuid = <0x46bb39d1 0xb4d945b5 0x88ff0400 0x27dab249>;
+        description = "SE Proxy";
+        execution-ctx-count = <1>;
+        exception-level = <1>; /* S-EL0 */
+        execution-state = <0>; /* AArch64 */
+        xlat-granule = <0>; /* 4KiB */
+        messaging-method = <0>; /* Direct messaging only */
+	device-regions {
+		compatible = "arm,ffa-manifest-device-regions";
+		mhu-sender {
+			/* Armv8 A Foundation Platform values */
+			base-address = <0x00000000 0x1b820000>;
+			pages-count = <16>;
+			attributes = <0x3>; /* read-write */
+		};
+		mhu-receiver {
+			/* Armv8 A Foundation Platform values */
+			base-address = <0x00000000 0x1b830000>;
+			pages-count = <16>;
+			attributes = <0x3>; /* read-write */
+		};
+		openamp-virtio {
+			/* Armv8 A Foundation Platform values */
+			base-address = <0x00000000 0x88000000>;
+			pages-count = <256>;
+			attributes = <0x3>; /* read-write */
+		};
+	};
+    };
+    smm-gateway{
+        compatible = "arm,ffa-manifest-1.0";
+        ffa-version = <0x00010000>; /* 31:16 - Major, 15:0 - Minor */
+        uuid = <0xed32d533 0x99e64209 0x9cc02d72 0xcdd998a7>;
+        description = "SMM Gateway";
+        execution-ctx-count = <1>;
+        exception-level = <1>; /* S-EL0 */
+        execution-state = <0>; /* AArch64 */
+        xlat-granule = <0>; /* 4KiB */
+        messaging-method = <0>; /* Direct messaging only */
+		device-regions {
+          compatible = "arm,ffa-manifest-device-regions";
+          mm-comm-buffer {
+              /* Armv8 A Foundation Platform values */
+              base-address = <0x00000000 0x02000000>;
+              pages-count = <1>;
+              attributes = <0x3>; /* read-write */
+              };
+		};
+    };
+};
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0001-WIP-Enable-managed-exit.patch b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0001-WIP-Enable-managed-exit.patch
new file mode 100644
index 0000000..3a21b39
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0001-WIP-Enable-managed-exit.patch
@@ -0,0 +1,124 @@
+From 14b84786e85483bf3c737ef8b392204e307c0ff1 Mon Sep 17 00:00:00 2001
+From: Olivier Deprez <olivier.deprez@arm.com>
+Date: Mon, 16 Nov 2020 10:14:02 +0100
+Subject: [PATCH] WIP: Enable managed exit
+
+This change declares OP-TEE SP as supporting managed exit in response to
+a NS interrupt triggering while the SWd runs.
+
+At init OP-TEE enables (HF_INTERRUPT_ENABLE) the managed exit virtual
+interrupt through the Hafnium para-virtualized interface.
+
+Physical interrupts are trapped to the SPMC which injects a managed exit
+interrupt to OP-TEE. The managed exit interrupt is acknowledged by
+OP-TEE by HF_INTERUPT_GET hvc call.
+
+Note: this code change is meant with in mind the SPMC runs at SEL2. It
+needs slight refactoring such that it does not break the SEL1 SPMC
+configuration.
+
+Change-Id: I9a95f36cf517c11048ff04680007f40259c4f636
+Signed-off-by: Olivier Deprez <olivier.deprez@arm.com>
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ core/arch/arm/kernel/boot.c                          | 12 ++++++++++++
+ core/arch/arm/kernel/thread_a64.S                    | 11 ++++++++++-
+ core/arch/arm/kernel/thread_spmc.c                   | 11 +++++++++++
+ .../arm/plat-totalcompute/fdts/optee_sp_manifest.dts |  2 +-
+ 4 files changed, 34 insertions(+), 2 deletions(-)
+
+diff --git a/core/arch/arm/kernel/boot.c b/core/arch/arm/kernel/boot.c
+index 09c1b811..d130107f 100644
+--- a/core/arch/arm/kernel/boot.c
++++ b/core/arch/arm/kernel/boot.c
+@@ -1279,6 +1279,18 @@ static void init_secondary_helper(unsigned long nsec_entry)
+ 	init_vfp_sec();
+ 	init_vfp_nsec();
+ 
++	/* Enable managed exit interrupt for secondary core. */
++	__asm__ volatile (
++		"mov x0, %0;"
++		"mov x1, %1;"
++		"mov x2, %2;"
++		"mov x3, %3;"
++		"hvc #0"
++		: : "i" (0xff03), "i" (4), "i" (1), "i" (1));
++
++	IMSG("%s core %lu: enabled managed exit interrupt.",
++		__func__, get_core_pos());
++
+ 	IMSG("Secondary CPU %zu switching to normal world boot", get_core_pos());
+ }
+ 
+diff --git a/core/arch/arm/kernel/thread_a64.S b/core/arch/arm/kernel/thread_a64.S
+index 3e0f5115..63bf396a 100644
+--- a/core/arch/arm/kernel/thread_a64.S
++++ b/core/arch/arm/kernel/thread_a64.S
+@@ -904,6 +904,14 @@ END_FUNC el0_sync_abort
+ 	bl	dcache_op_louis
+ 	ic	iallu
+ #endif
++
++	/* HF_INTERRUPT_GET */
++	mov	x0, #0xff04
++	hvc	#0
++	/* Expect managed exit interrupt */
++	cmp	x0, #4
++	bne	.
++
+ 	/*
+ 	 * Mark current thread as suspended
+ 	 */
+@@ -1021,8 +1029,9 @@ LOCAL_FUNC elx_irq , :
+ #endif
+ END_FUNC elx_irq
+ 
++#define HF_MANAGED_EXIT		1
+ LOCAL_FUNC elx_fiq , :
+-#if defined(CFG_ARM_GICV3)
++#if defined(CFG_ARM_GICV3) || defined (HF_MANAGED_EXIT)
+ 	foreign_intr_handler	fiq
+ #else
+ 	native_intr_handler	fiq
+diff --git a/core/arch/arm/kernel/thread_spmc.c b/core/arch/arm/kernel/thread_spmc.c
+index bd7930e7..89ff82bc 100644
+--- a/core/arch/arm/kernel/thread_spmc.c
++++ b/core/arch/arm/kernel/thread_spmc.c
+@@ -1394,6 +1394,17 @@ static TEE_Result spmc_init(void)
+ 	my_endpoint_id = spmc_get_id();
+ 	DMSG("My endpoint ID %#x", my_endpoint_id);
+ 
++	/* Enable managed exit interrupt for boot core. */
++	__asm__ volatile (
++		"mov x0, %0;"
++		"mov x1, %1;"
++		"mov x2, %2;"
++		"mov x3, %3;"
++		"hvc #0"
++		: : "i" (0xff03), "i" (4), "i" (1), "i" (1));
++
++	IMSG("%s enabled managed exit interrupt.", __func__);
++
+ 	return TEE_SUCCESS;
+ }
+ #endif /*CFG_CORE_SEL2_SPMC*/
+diff --git a/core/arch/arm/plat-totalcompute/fdts/optee_sp_manifest.dts b/core/arch/arm/plat-totalcompute/fdts/optee_sp_manifest.dts
+index 4b8b3681..04847c4d 100644
+--- a/core/arch/arm/plat-totalcompute/fdts/optee_sp_manifest.dts
++++ b/core/arch/arm/plat-totalcompute/fdts/optee_sp_manifest.dts
+@@ -23,7 +23,8 @@
+ 	entrypoint-offset = <0x1000>;
+ 	xlat-granule = <0>; /* 4KiB */
+ 	boot-order = <0>;
+-	messaging-method = <0>; /* Direct messaging only */
++	messaging-method = <3>; /* Direct request/response supported */
++	managed-exit; /* Managed exit supported */
+
+ 	device-regions {
+ 		compatible = "arm,ffa-manifest-device-regions";
+-- 
+2.29.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0002-ffa-Update-function-ID-according-to-FFA-v1.1-spec.patch b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0002-ffa-Update-function-ID-according-to-FFA-v1.1-spec.patch
new file mode 100644
index 0000000..44138cb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0002-ffa-Update-function-ID-according-to-FFA-v1.1-spec.patch
@@ -0,0 +1,29 @@
+From 3a240f6b6c58d70471fd0752b8854c43c7c4df72 Mon Sep 17 00:00:00 2001
+From: Usama Arif <usama.arif@arm.com>
+Date: Wed, 11 Aug 2021 11:00:52 +0100
+Subject: [PATCH 2/2] ffa: Update function ID according to FFA v1.1 spec
+
+This updates function ID FFA_SECONDARY_EP_REGISTER_64.
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Usama Arif <usama.arif@arm.com>
+---
+ core/arch/arm/include/ffa.h | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/core/arch/arm/include/ffa.h b/core/arch/arm/include/ffa.h
+index 20a009ca..b0d68173 100644
+--- a/core/arch/arm/include/ffa.h
++++ b/core/arch/arm/include/ffa.h
+@@ -69,7 +69,7 @@
+ #define FFA_MEM_RECLAIM			U(0x84000077)
+ #define FFA_MEM_FRAG_RX			U(0x8400007A)
+ #define FFA_MEM_FRAG_TX			U(0x8400007B)
+-#define FFA_SECONDARY_EP_REGISTER_64	U(0xC4000084)
++#define FFA_SECONDARY_EP_REGISTER_64	U(0xC4000087)
+ 
+ /* Special value for traffic targeted to the Hypervisor or SPM */
+ #define FFA_TARGET_INFO_MBZ		U(0x0)
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0003-Fix-optee-UUID.patch b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0003-Fix-optee-UUID.patch
new file mode 100644
index 0000000..5374c02
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0003-Fix-optee-UUID.patch
@@ -0,0 +1,27 @@
+From 37fd6f3c18015bcad2c099bf9269e72140e55557 Mon Sep 17 00:00:00 2001
+From: Davidson K <davidson.kumaresan@arm.com>
+Date: Wed, 12 Jan 2022 17:14:03 +0530
+Subject: [PATCH] Fix optee UUID
+
+Upstream-Status: Backport [https://github.com/OP-TEE/optee_os/commit/a9a8e483b6ff7f6e40c5ed95310a18e0bd1993c3#diff-2a310f8cc43d961b2efc05ac1619521653ba8977ff5e6dc5bb89754fd60fe954]
+Signed-off-by: Davidson K <davidson.kumaresan@arm.com>
+---
+ core/arch/arm/plat-totalcompute/fdts/optee_sp_manifest.dts | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/core/arch/arm/plat-totalcompute/fdts/optee_sp_manifest.dts b/core/arch/arm/plat-totalcompute/fdts/optee_sp_manifest.dts
+index 04847c4d..3b76fc7d 100644
+--- a/core/arch/arm/plat-totalcompute/fdts/optee_sp_manifest.dts
++++ b/core/arch/arm/plat-totalcompute/fdts/optee_sp_manifest.dts
+@@ -14,7 +14,7 @@
+ 	/* Properties */
+ 	description = "op-tee";
+ 	ffa-version = <0x00010000>; /* 31:16 - Major, 15:0 - Minor */
+-	uuid = <0x486178e0 0xe7f811e3 0xbc5e0002 0xa5d5c51b>;
++	uuid = <0xe0786148 0xe311f8e7 0x02005ebc 0x1bc5d5a5>;
+ 	id = <1>;
+ 	execution-ctx-count = <8>;
+ 	exception-level = <2>; /* S-EL1 */
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0004-plat-totalcompute-add-support-for-higher-DRAM.patch b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0004-plat-totalcompute-add-support-for-higher-DRAM.patch
new file mode 100644
index 0000000..293ea7d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/0004-plat-totalcompute-add-support-for-higher-DRAM.patch
@@ -0,0 +1,60 @@
+From 7fb6d720a285b6135a9247b2adde833ea90e2549 Mon Sep 17 00:00:00 2001
+From: Usama Arif <usama.arif@arm.com>
+Date: Mon, 27 Sep 2021 19:58:56 +0100
+Subject: [PATCH] plat-totalcompute: add support for higher DRAM
+
+The new 6GB DRAM bank starts at 0x8080000000.
+
+Signed-off-by: Usama Arif <usama.arif@arm.com>
+Acked-by: Jens Wiklander <jens.wiklander@linaro.org>
+
+Upstream-Status: Backport [https://github.com/OP-TEE/optee_os/commit/6d8430f943e091282849b188fbc0847c159e5de4]
+Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
+---
+ core/arch/arm/plat-totalcompute/conf.mk           | 2 ++
+ core/arch/arm/plat-totalcompute/main.c            | 1 +
+ core/arch/arm/plat-totalcompute/platform_config.h | 3 +++
+ 3 files changed, 6 insertions(+)
+
+diff --git a/core/arch/arm/plat-totalcompute/conf.mk b/core/arch/arm/plat-totalcompute/conf.mk
+index 558b7889..e894b1e1 100644
+--- a/core/arch/arm/plat-totalcompute/conf.mk
++++ b/core/arch/arm/plat-totalcompute/conf.mk
+@@ -24,6 +24,8 @@ platform-cflags-debug-info = -gdwarf-2
+ platform-aflags-debug-info = -gdwarf-2
+ endif
+ 
++$(call force,CFG_CORE_ARM64_PA_BITS,40)
++
+ ifneq (,$(filter ${PLATFORM_FLAVOR},tc0 tc1))
+ CFG_TEE_CORE_NB_CORE = 8
+ 
+diff --git a/core/arch/arm/plat-totalcompute/main.c b/core/arch/arm/plat-totalcompute/main.c
+index 42acf8dd..eab237bf 100644
+--- a/core/arch/arm/plat-totalcompute/main.c
++++ b/core/arch/arm/plat-totalcompute/main.c
+@@ -27,6 +27,7 @@ register_phys_mem_pgdir(MEM_AREA_IO_SEC, GICD_BASE, GIC_DIST_REG_SIZE);
+ #endif
+ 
+ register_ddr(DRAM0_BASE, DRAM0_SIZE);
++register_ddr(DRAM1_BASE, DRAM1_SIZE);
+ 
+ #ifndef CFG_CORE_SEL2_SPMC
+ void main_init_gic(void)
+diff --git a/core/arch/arm/plat-totalcompute/platform_config.h b/core/arch/arm/plat-totalcompute/platform_config.h
+index 4255abca..b474a899 100644
+--- a/core/arch/arm/plat-totalcompute/platform_config.h
++++ b/core/arch/arm/plat-totalcompute/platform_config.h
+@@ -26,6 +26,9 @@
+ #define DRAM0_BASE		0x80000000
+ #define DRAM0_SIZE		0x7d000000
+ 
++#define DRAM1_BASE		0x8080000000ULL
++#define DRAM1_SIZE		0x180000000ULL
++
+ #define TZCDRAM_BASE		0xff000000
+ #define TZCDRAM_SIZE		0x01000000
+ 
+-- 
+2.30.2
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/sp_layout.json b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/sp_layout.json
new file mode 100644
index 0000000..d37d902
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-os/tc/sp_layout.json
@@ -0,0 +1,6 @@
+{
+	"op-tee" : {
+		"image": "tee-pager_v2.bin",
+		"pm": "optee_sp_manifest.dts"
+	}
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-test/tc/0001-xtest-Limit-tests-to-a-single-thread.patch b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-test/tc/0001-xtest-Limit-tests-to-a-single-thread.patch
new file mode 100644
index 0000000..370a81c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/files/optee-test/tc/0001-xtest-Limit-tests-to-a-single-thread.patch
@@ -0,0 +1,55 @@
+From 73bef38c5697cd6bd3ddbe9046681087f4f6454e Mon Sep 17 00:00:00 2001
+From: Ben Horgan <ben.horgan@arm.com>
+Date: Thu, 27 Jan 2022 10:33:04 +0000
+Subject: [PATCH] xtest: Limit tests to a single thread
+
+Signed-off-by: Ben Horgan <ben.horgan@arm.com>
+Upstream-Status: Inappropriate [Workaround for intermittent failures]
+---
+ host/xtest/regression_1000.c | 2 +-
+ host/xtest/regression_2000.c | 2 +-
+ host/xtest/regression_6000.c | 2 +-
+ 3 files changed, 3 insertions(+), 3 deletions(-)
+
+diff --git a/host/xtest/regression_1000.c b/host/xtest/regression_1000.c
+index 9ee9d02..82d1def 100644
+--- a/host/xtest/regression_1000.c
++++ b/host/xtest/regression_1000.c
+@@ -1080,7 +1080,7 @@ static void *test_1013_thread(void *arg)
+ 	return NULL;
+ }
+ 
+-#define NUM_THREADS 3
++#define NUM_THREADS 1
+ 
+ static void xtest_tee_test_1013_single(ADBG_Case_t *c, double *mean_concurrency,
+ 				       const TEEC_UUID *uuid)
+diff --git a/host/xtest/regression_2000.c b/host/xtest/regression_2000.c
+index 0591a42..a9f4b95 100644
+--- a/host/xtest/regression_2000.c
++++ b/host/xtest/regression_2000.c
+@@ -499,7 +499,7 @@ out:
+ 	return NULL;
+ }
+ 
+-#define NUM_THREADS	3
++#define NUM_THREADS	1
+ 
+ static void xtest_tee_test_2002(ADBG_Case_t *c)
+ {
+diff --git a/host/xtest/regression_6000.c b/host/xtest/regression_6000.c
+index ca1c254..d67ea7f 100644
+--- a/host/xtest/regression_6000.c
++++ b/host/xtest/regression_6000.c
+@@ -1568,7 +1568,7 @@ exit:
+ }
+ 
+ 
+-#define NUM_THREADS 4
++#define NUM_THREADS 1
+ static void xtest_tee_test_6016_loop(ADBG_Case_t *c, uint32_t storage_id)
+ {
+ 	struct test_6016_thread_arg arg[NUM_THREADS] = { };
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os-generic-tc.inc b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os-generic-tc.inc
new file mode 100644
index 0000000..eeaa59a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os-generic-tc.inc
@@ -0,0 +1,21 @@
+# Total Compute (tc) specific configuration for optee-os and optee-os-tadevkit
+
+# Intermediate SHA with 3.14 baseline version
+# This has TC0 and TC1 platform support
+SRCREV = "e4f34e786135079160697d88212591105a65fbce"
+PV = "3.14.0+git${SRCPV}"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/optee-os/tc:"
+SRC_URI:append:tc = " \
+    file://sp_layout.json \
+    file://0001-WIP-Enable-managed-exit.patch \
+    file://0002-ffa-Update-function-ID-according-to-FFA-v1.1-spec.patch \
+    file://0003-Fix-optee-UUID.patch \
+    file://0004-plat-totalcompute-add-support-for-higher-DRAM.patch \
+    "
+
+COMPATIBLE_MACHINE = "(tc?)"
+
+OPTEEMACHINE:tc0 = "totalcompute-tc0"
+OPTEEMACHINE:tc1 = "totalcompute-tc1"
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os-tadevkit_3.14.0.bbappend b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os-tadevkit_3.14.0.bbappend
new file mode 100644
index 0000000..6a22d47
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os-tadevkit_3.14.0.bbappend
@@ -0,0 +1,6 @@
+# Machine specific configurations
+
+MACHINE_OPTEE_OS_TADEVKIT_REQUIRE ?= ""
+MACHINE_OPTEE_OS_TADEVKIT_REQUIRE:tc = "optee-os-generic-tc.inc"
+
+require ${MACHINE_OPTEE_OS_TADEVKIT_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os-tc.inc b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os-tc.inc
new file mode 100644
index 0000000..7936652
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os-tc.inc
@@ -0,0 +1,23 @@
+# TC0 specific configuration
+
+require optee-os-generic-tc.inc
+
+# Enable optee memory layout and boot logs
+EXTRA_OEMAKE += " CFG_TEE_CORE_LOG_LEVEL=3"
+
+# default disable latency benchmarks (over all OP-TEE layers)
+EXTRA_OEMAKE += " CFG_TEE_BENCHMARK=n"
+
+# Enable stats
+EXTRA_OEMAKE += " CFG_WITH_STATS=y"
+
+EXTRA_OEMAKE += " CFG_CORE_SEL2_SPMC=y"
+
+# Copy optee manifest file
+do_install:append() {
+    install -d ${D}${nonarch_base_libdir}/firmware/
+    install -m 644 ${WORKDIR}/sp_layout.json ${D}${nonarch_base_libdir}/firmware/
+    install -m 644 \
+        ${S}/core/arch/arm/plat-totalcompute/fdts/optee_sp_manifest.dts \
+        ${D}${nonarch_base_libdir}/firmware/
+}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os_3.14.0.bbappend b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os_3.14.0.bbappend
new file mode 100644
index 0000000..7834b64
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os_3.14.0.bbappend
@@ -0,0 +1,7 @@
+# Machine specific configurations
+
+MACHINE_OPTEE_OS_REQUIRE ?= ""
+MACHINE_OPTEE_OS_REQUIRE:corstone1000 = "optee-os_corstone1000.inc"
+MACHINE_OPTEE_OS_REQUIRE:tc = "optee-os-tc.inc"
+
+require ${MACHINE_OPTEE_OS_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os_corstone1000.inc b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os_corstone1000.inc
new file mode 100644
index 0000000..eb5f6df
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os_corstone1000.inc
@@ -0,0 +1,20 @@
+require optee-os_corstone1000_common.inc
+
+DEPENDS += " secure-partitions"
+EXTRA_OEMAKE +="'SP_PACKAGING_METHOD=embedded'"
+
+TS_INSTALL_PREFIX_PATH="${RECIPE_SYSROOT}/firmware/sp/opteesp"
+EXTRA_OEMAKE += "'TS_INSTALL_PREFIX=${TS_INSTALL_PREFIX_PATH}'"
+
+# se-proxy secure partition
+SP_MKFILE_PATH="${TS_INSTALL_PREFIX}/lib/make/se-proxy.mk"
+
+# smm-gateway secure partition
+SP_MKFILE_PATH += "${TS_INSTALL_PREFIX}/lib/make/smm-gateway.mk"
+
+EXTRA_OEMAKE += "'CFG_SP_MKFILE_PATH=${SP_MKFILE_PATH}'"
+
+EXTRA_OEMAKE += "CFG_EMBED_DTB_SOURCE_FILE=${EMBED_DTB_SOURCE_FILE}"
+
+SRC_URI:append = " file://sp_manifest_combined_se.dts;"
+EMBED_DTB_SOURCE_FILE = "${WORKDIR}/sp_manifest_combined_se.dts"
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os_corstone1000_common.inc b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os_corstone1000_common.inc
new file mode 100644
index 0000000..9f37241
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-os_corstone1000_common.inc
@@ -0,0 +1,31 @@
+SRC_URI = "git://git.trustedfirmware.org/OP-TEE/optee_os.git;protocol=https;branch=psa-development"
+SRCREV = "f9de2c9520ed97b89760cc4c99424aae440b63f4"
+PV = "3.10.0+git${SRCPV}"
+
+DEPENDS += "python3-pycryptodomex-native dtc-native"
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/optee-os/corstone1000:"
+
+SRC_URI:append = " \
+                  file://0001-plat-corstone1000-add-corstone1000-platform.patch \
+                  file://0002-plat-corstone1000-reserve-3MB-CVM-memory-for-optee.patch \
+                  file://0003-plat-corstone1000-add-a-rule-in-Makefile-to-SP_MAKEF.patch \
+                  file://0004-plat-corstone1000-increase-OPTEE-core-heap-size.patch \
+                  file://0005-Fix-add-missing-error-check-during-SP-init.patch \
+                   "
+
+COMPATIBLE_MACHINE = "corstone1000"
+
+OPTEEMACHINE = "corstone1000"
+# Enable optee memory layout and boot logs
+EXTRA_OEMAKE += " CFG_TEE_CORE_LOG_LEVEL=4"
+
+# default disable latency benchmarks (over all OP-TEE layers)
+EXTRA_OEMAKE += " CFG_TEE_BENCHMARK=n"
+
+EXTRA_OEMAKE += " CFG_CORE_SEL1_SPMC=y CFG_CORE_FFA=y"
+
+EXTRA_OEMAKE += " CFG_WITH_SP=y"
+
+EXTRA_OEMAKE += " HOST_PREFIX=${HOST_PREFIX}"
+EXTRA_OEMAKE += " CROSS_COMPILE64=${HOST_PREFIX}"
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/optee-spdevkit_corstone1000.inc b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-spdevkit_corstone1000.inc
new file mode 100644
index 0000000..363b0ed
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-spdevkit_corstone1000.inc
@@ -0,0 +1 @@
+require optee-os_corstone1000_common.inc
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/optee-spdevkit_git.bbappend b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-spdevkit_git.bbappend
new file mode 100644
index 0000000..13e109c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-spdevkit_git.bbappend
@@ -0,0 +1,6 @@
+# Machine specific configurations
+
+MACHINE_OPTEE_SPDEVKIT_REQUIRE ?= ""
+MACHINE_OPTEE_SPDEVKIT_REQUIRE:corstone1000 = "optee-spdevkit_corstone1000.inc"
+
+require ${MACHINE_OPTEE_SPDEVKIT_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/optee-test-tc.inc b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-test-tc.inc
new file mode 100644
index 0000000..af73675
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-test-tc.inc
@@ -0,0 +1,8 @@
+# TC specific configuration
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/files/optee-test/tc:"
+SRC_URI:append:tc = " \
+    file://0001-xtest-Limit-tests-to-a-single-thread.patch \
+    "
+
+COMPATIBLE_MACHINE = "(tc?)"
diff --git a/meta-arm/meta-arm-bsp/recipes-security/optee/optee-test_3.14.0.bbappend b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-test_3.14.0.bbappend
new file mode 100644
index 0000000..490b350
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/optee/optee-test_3.14.0.bbappend
@@ -0,0 +1,6 @@
+# Machine specific configurations
+
+MACHINE_OPTEE_TEST_REQUIRE ?= ""
+MACHINE_OPTEE_TEST_REQUIRE:tc = "optee-test-tc.inc"
+
+require ${MACHINE_OPTEE_TEST_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0001-tools-cmake-common-applying-lowercase-project-conven.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0001-tools-cmake-common-applying-lowercase-project-conven.patch
new file mode 100644
index 0000000..83c7b54
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0001-tools-cmake-common-applying-lowercase-project-conven.patch
@@ -0,0 +1,33 @@
+From 73c27b917e15eb04f39eedac9b79e5011e8a754f Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Sat, 13 Nov 2021 07:47:44 +0000
+Subject: [PATCH] tools/cmake/common: applying lowercase project convention
+
+Lowercase convention should only apply on the paths inside TS
+source-code.
+Host build paths should not be lowercased. Otherwise, builds
+with uppercase paths will break.
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+
+
+---
+ tools/cmake/common/AddPlatform.cmake | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/tools/cmake/common/AddPlatform.cmake b/tools/cmake/common/AddPlatform.cmake
+index ae34c6e4..31bcd8ca 100644
+--- a/tools/cmake/common/AddPlatform.cmake
++++ b/tools/cmake/common/AddPlatform.cmake
+@@ -37,8 +37,8 @@ function(add_platform)
+ 	set(TGT ${MY_PARAMS_TARGET} CACHE STRING "")
+ 
+ 	# Ensure file path conforms to lowercase project convention
+-	string(TOLOWER "${TS_PLATFORM_ROOT}/${TS_PLATFORM}/platform.cmake" _platdef)
+-	include(${_platdef})
++	string(TOLOWER "${TS_PLATFORM}/platform.cmake" _platdef)
++	include(${TS_PLATFORM_ROOT}/${_platdef})
+ 	set(CMAKE_CONFIGURE_DEPENDS ${_platdef})
+ 
+ 	unset(TGT CACHE)
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0002-fix-EARLY_TA_PATHS-env-variable.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0002-fix-EARLY_TA_PATHS-env-variable.patch
new file mode 100644
index 0000000..f56b455
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0002-fix-EARLY_TA_PATHS-env-variable.patch
@@ -0,0 +1,32 @@
+From 146f4dfa73aa316d611188f63e3530cffe9200af Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Sat, 13 Nov 2021 07:51:53 +0000
+Subject: [PATCH] fix EARLY_TA_PATHS env variable
+
+Yocto cleans up environment varaibles at build time.
+EARLY_TA_PATHS should be set a separate rule for securepartitions
+to be included into optee-os image
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+
+
+---
+ environments/opteesp/sp.mk.in | 3 ++-
+ 1 file changed, 2 insertions(+), 1 deletion(-)
+
+diff --git a/environments/opteesp/sp.mk.in b/environments/opteesp/sp.mk.in
+index c44ad59e..d67e2dca 100644
+--- a/environments/opteesp/sp.mk.in
++++ b/environments/opteesp/sp.mk.in
+@@ -14,7 +14,8 @@ ifeq (,${@EXPORT_SP_UUID@-included})
+ endif
+ 
+ ifeq (embedded,${SP_PACKAGING_METHOD})
+-OPTEE_OS_COMMON_EXTRA_FLAGS+=EARLY_TA_PATHS+=${TS_INSTALL_PREFIX}/opteesp/bin/@EXPORT_SP_UUID@.stripped.elf
++EARLY_TA_PATHS+=${TS_INSTALL_PREFIX}/bin/@EXPORT_SP_UUID@.stripped.elf
++OPTEE_OS_COMMON_EXTRA_FLAGS+=${EARLY_TA_PATHS}
+ TS_SP_DTSI_LIST+="\\n\#include \"${TS_INSTALL_PREFIX}/opteesp/manifest/@EXPORT_SP_UUID@.dtsi\""
+ else ifeq (fip,${SP_PACKAGING_METHOD})
+ TS_SP_JSON_LIST+=${TS_INSTALL_PREFIX}/opteesp/json/@EXPORT_SP_NAME@.json
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0003-corstone1000-port-crypto-config.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0003-corstone1000-port-crypto-config.patch
new file mode 100644
index 0000000..3b26b77
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0003-corstone1000-port-crypto-config.patch
@@ -0,0 +1,234 @@
+From f86f5b42d853d2a65f6753362361bbb95aac1800 Mon Sep 17 00:00:00 2001
+From: Satish Kumar <satish.kumar01@arm.com>
+Date: Sat, 11 Dec 2021 11:06:57 +0000
+Subject: [PATCH] corstone1000: port crypto config
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+%% original patch: 0003-corstone1000-port-crypto-config.patch
+---
+ .../nspe/pal_crypto_config.h                  | 83 +++++++++++++++----
+ 1 file changed, 66 insertions(+), 17 deletions(-)
+
+diff --git a/api-tests/platform/targets/tgt_dev_apis_linux/nspe/pal_crypto_config.h b/api-tests/platform/targets/tgt_dev_apis_linux/nspe/pal_crypto_config.h
+index 844cd2e..c936bdd 100755
+--- a/api-tests/platform/targets/tgt_dev_apis_linux/nspe/pal_crypto_config.h
++++ b/api-tests/platform/targets/tgt_dev_apis_linux/nspe/pal_crypto_config.h
+@@ -1,5 +1,5 @@
+ /** @file
+- * Copyright (c) 2021, Arm Limited or its affiliates. All rights reserved.
++ * Copyright (c) 2019-2020, Arm Limited or its affiliates. All rights reserved.
+  * SPDX-License-Identifier : Apache-2.0
+  *
+  * Licensed under the Apache License, Version 2.0 (the "License");
+@@ -34,10 +34,14 @@
+  *
+  * Comment macros to disable the types
+  */
++#ifndef TF_M_PROFILE_SMALL
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_RSA
+ #define ARCH_TEST_RSA_1024
+ #define ARCH_TEST_RSA_2048
+ #define ARCH_TEST_RSA_3072
++#endif
++#endif
+ 
+ /**
+  * \def  ARCH_TEST_ECC
+@@ -50,11 +54,17 @@
+  * Requires: ARCH_TEST_ECC
+  * Comment macros to disable the curve
+  */
++#ifndef TF_M_PROFILE_SMALL
+ #define ARCH_TEST_ECC
+ #define ARCH_TEST_ECC_CURVE_SECP192R1
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_ECC_CURVE_SECP224R1
++#endif
+ #define ARCH_TEST_ECC_CURVE_SECP256R1
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_ECC_CURVE_SECP384R1
++#endif
++#endif
+ 
+ /**
+  * \def ARCH_TEST_AES
+@@ -78,10 +88,10 @@
+  *
+  * Comment macros to disable the types
+  */
+-#define ARCH_TEST_DES
+-#define ARCH_TEST_DES_1KEY
+-#define ARCH_TEST_DES_2KEY
+-#define ARCH_TEST_DES_3KEY
++//#define ARCH_TEST_DES
++//#define ARCH_TEST_DES_1KEY
++//#define ARCH_TEST_DES_2KEY
++//#define ARCH_TEST_DES_3KEY
+ 
+ /**
+  * \def  ARCH_TEST_RAW
+@@ -104,7 +114,7 @@
+  *
+  * Enable the ARC4 key type.
+  */
+-#define ARCH_TEST_ARC4
++//#define ARCH_TEST_ARC4
+ 
+ /**
+  * \def ARCH_TEST_CIPHER_MODE_CTR
+@@ -113,7 +123,11 @@
+  *
+  * Requires: ARCH_TEST_CIPHER
+  */
++#ifndef TF_M_PROFILE_SMALL
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_CIPHER_MODE_CTR
++#endif
++#endif
+ 
+ /**
+  * \def ARCH_TEST_CIPHER_MODE_CFB
+@@ -138,7 +152,11 @@
+  *
+  * Requires: ARCH_TEST_CIPHER, ARCH_TEST_AES, ARCH_TEST_CIPHER_MODE_CTR
+  */
++#ifndef TF_M_PROFILE_SMALL
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_CTR_AES
++#endif
++#endif
+ 
+ /**
+  * \def ARCH_TEST_CBC_AES
+@@ -157,7 +175,11 @@
+  *
+  * Comment macros to disable the types
+  */
++#ifndef TF_M_PROFILE_SMALL
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_CBC_NO_PADDING
++#endif
++#endif
+ 
+ /**
+  * \def ARCH_TEST_CFB_AES
+@@ -177,11 +199,15 @@
+  *
+  * Comment macros to disable the types
+  */
++#ifndef TF_M_PROFILE_SMALL
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_PKCS1V15
+ #define ARCH_TEST_RSA_PKCS1V15_SIGN
+ #define ARCH_TEST_RSA_PKCS1V15_SIGN_RAW
+ #define ARCH_TEST_RSA_PKCS1V15_CRYPT
+ #define ARCH_TEST_RSA_OAEP
++#endif
++#endif
+ 
+ /**
+  * \def ARCH_TEST_CBC_PKCS7
+@@ -190,7 +216,11 @@
+  *
+  * Comment macros to disable the types
+  */
++#ifndef TF_M_PROFILE_SMALL
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_CBC_PKCS7
++#endif
++#endif
+ 
+ /**
+  * \def ARCH_TEST_ASYMMETRIC_ENCRYPTION
+@@ -227,21 +257,27 @@
+  *
+  * Comment macros to disable the types
+  */
+-// #define ARCH_TEST_MD2
+-// #define ARCH_TEST_MD4
+-#define ARCH_TEST_MD5
+-#define ARCH_TEST_RIPEMD160
+-#define ARCH_TEST_SHA1
++//#define ARCH_TEST_MD2
++//#define ARCH_TEST_MD4
++//#define ARCH_TEST_MD5
++//#define ARCH_TEST_RIPEMD160
++//#define ARCH_TEST_SHA1
++#ifndef TF_M_PROFILE_SMALL
+ #define ARCH_TEST_SHA224
++#endif
+ #define ARCH_TEST_SHA256
++#ifndef TF_M_PROFILE_SMALL
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_SHA384
+ #define ARCH_TEST_SHA512
+-// #define ARCH_TEST_SHA512_224
+-// #define ARCH_TEST_SHA512_256
+-// #define ARCH_TEST_SHA3_224
+-// #define ARCH_TEST_SHA3_256
+-// #define ARCH_TEST_SHA3_384
+-// #define ARCH_TEST_SHA3_512
++#endif
++#endif
++//#define ARCH_TEST_SHA512_224
++//#define ARCH_TEST_SHA512_256
++//#define ARCH_TEST_SHA3_224
++//#define ARCH_TEST_SHA3_256
++//#define ARCH_TEST_SHA3_384
++//#define ARCH_TEST_SHA3_512
+ 
+ /**
+  * \def ARCH_TEST_HKDF
+@@ -261,7 +297,12 @@
+  *
+  * Comment macros to disable the types
+  */
++#ifndef TF_M_PROFILE_SMALL
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_CMAC
++#endif
++#endif
++//#define ARCH_TEST_GMAC
+ #define ARCH_TEST_HMAC
+ 
+ /**
+@@ -281,7 +322,11 @@
+  * Requires: ARCH_TEST_AES
+  *
+  */
++#ifndef TF_M_PROFILE_SMALL
++#ifndef TF_M_PROFILE_MEDIUM
+ #define ARCH_TEST_GCM
++#endif
++#endif
+ 
+ /**
+  * \def ARCH_TEST_TRUNCATED_MAC
+@@ -300,7 +345,9 @@
+  *
+  * Requires: ARCH_TEST_ECC
+  */
++#ifndef TF_M_PROFILE_SMALL
+ #define ARCH_TEST_ECDH
++#endif
+ 
+ /**
+  * \def ARCH_TEST_ECDSA
+@@ -308,7 +355,9 @@
+  * Enable the elliptic curve DSA library.
+  * Requires: ARCH_TEST_ECC
+  */
++#ifndef TF_M_PROFILE_SMALL
+ #define ARCH_TEST_ECDSA
++#endif
+ 
+ /**
+  * \def ARCH_TEST_DETERMINISTIC_ECDSA
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0003-se-proxy-dts-add-se-proxy-as-child-node.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0003-se-proxy-dts-add-se-proxy-as-child-node.patch
new file mode 100644
index 0000000..bb4fc82
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0003-se-proxy-dts-add-se-proxy-as-child-node.patch
@@ -0,0 +1,46 @@
+From 0ec1b3d20d612325b9c55baa2539d080eb6a72a8 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Sat, 13 Nov 2021 08:34:42 +0000
+Subject: [PATCH] se-proxy:dts: add se-proxy as child node
+
+se-proxy sp string should be added for se-proxy node to be
+read properly.
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+
+
+---
+ .../se-proxy/opteesp/default_se-proxy.dts.in  | 20 ++++++++++---------
+ 1 file changed, 11 insertions(+), 9 deletions(-)
+
+diff --git a/deployments/se-proxy/opteesp/default_se-proxy.dts.in b/deployments/se-proxy/opteesp/default_se-proxy.dts.in
+index 961071ad..9f5cf712 100644
+--- a/deployments/se-proxy/opteesp/default_se-proxy.dts.in
++++ b/deployments/se-proxy/opteesp/default_se-proxy.dts.in
+@@ -7,13 +7,15 @@
+ @DTS_TAG@
+ 
+ @DTS_NODE@ {
+-	compatible = "arm,ffa-manifest-1.0";
+-	ffa-version = <0x00010000>; /* 31:16 - Major, 15:0 - Minor */
+-	uuid = <@EXPORT_SP_UUID_DT@>;
+-	description = "SE Proxy";
+-	execution-ctx-count = <1>;
+-	exception-level = <1>; /* S-EL0 */
+-	execution-state = <0>; /* AArch64 */
+-	xlat-granule = <0>; /* 4KiB */
+-	messaging-method = <0>; /* Direct messaging only */
++	se-proxy {
++		compatible = "arm,ffa-manifest-1.0";
++		ffa-version = <0x00010000>; /* 31:16 - Major, 15:0 - Minor */
++		uuid = <@EXPORT_SP_UUID_DT@>;
++		description = "SE Proxy";
++		execution-ctx-count = <1>;
++		exception-level = <1>; /* S-EL0 */
++		execution-state = <0>; /* AArch64 */
++		xlat-granule = <0>; /* 4KiB */
++		messaging-method = <0>; /* Direct messaging only */
++	};
+ };
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0004-Update-mm-comm-buffer-region-in-dts-file.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0004-Update-mm-comm-buffer-region-in-dts-file.patch
new file mode 100644
index 0000000..9ceb176
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0004-Update-mm-comm-buffer-region-in-dts-file.patch
@@ -0,0 +1,59 @@
+From 97b4f3cd0216c30c39f6ece4f68d8faf3901fded Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Wed, 17 Nov 2021 15:31:09 +0000
+Subject: [PATCH] Update mm-comm-buffer region in dts file
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+
+---
+ .../opteesp/default_smm-gateway.dts.in        | 35 ++++++++++---------
+ 1 file changed, 18 insertions(+), 17 deletions(-)
+
+diff --git a/deployments/smm-gateway/opteesp/default_smm-gateway.dts.in b/deployments/smm-gateway/opteesp/default_smm-gateway.dts.in
+index 0ad7878b..183c38a7 100644
+--- a/deployments/smm-gateway/opteesp/default_smm-gateway.dts.in
++++ b/deployments/smm-gateway/opteesp/default_smm-gateway.dts.in
+@@ -7,23 +7,24 @@
+ @DTS_TAG@
+ 
+ @DTS_NODE@ {
+-	compatible = "arm,ffa-manifest-1.0";
+-	ffa-version = <0x00010000>; /* 31:16 - Major, 15:0 - Minor */
+-	uuid = <@EXPORT_SP_UUID_DT@>;
+-	description = "SMM Gateway";
+-	execution-ctx-count = <1>;
+-	exception-level = <1>; /* S-EL0 */
+-	execution-state = <0>; /* AArch64 */
+-	xlat-granule = <0>; /* 4KiB */
+-	messaging-method = <0>; /* Direct messaging only */
+-
+-	memory-regions {
+-		compatible = "arm,ffa-manifest-memory-regions";
+-
+-		mm-comm-buffer {
+-			base-address = <@MM_COMM_BUFFER_ADDRESS@>;
+-			pages-count = <@MM_COMM_BUFFER_PAGE_COUNT@>;
+-			attributes = <0xb>;  /* ns access-read-write */
++	smm-gateway{
++		compatible = "arm,ffa-manifest-1.0";
++		ffa-version = <0x00010000>; /* 31:16 - Major, 15:0 - Minor */
++		uuid = <@EXPORT_SP_UUID_DT@>;
++		description = "SMM Gateway";
++		execution-ctx-count = <1>;
++		exception-level = <1>; /* S-EL0 */
++		execution-state = <0>; /* AArch64 */
++		xlat-granule = <0>; /* 4KiB */
++		messaging-method = <0>; /* Direct messaging only */
++		device-regions {
++          compatible = "arm,ffa-manifest-device-regions";
++          mm-comm-buffer {
++              /* Armv8 A Foundation Platform values */
++              base-address = <0x00000000 0x02000000>;
++              pages-count = <1>;
++              attributes = <0x3>; /* read-write */
++              };
+ 		};
+ 	};
+ };
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0005-Configure-NV-storage-macro.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0005-Configure-NV-storage-macro.patch
new file mode 100644
index 0000000..8d2d62b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0005-Configure-NV-storage-macro.patch
@@ -0,0 +1,26 @@
+From a11b23dd5f0c4124a5c6c2fcab0ea623bc76f4ba Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Wed, 17 Nov 2021 15:32:04 +0000
+Subject: [PATCH] Configure NV storage macro
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+
+---
+ deployments/smm-gateway/smm_gateway.c | 2 ++
+ 1 file changed, 2 insertions(+)
+
+diff --git a/deployments/smm-gateway/smm_gateway.c b/deployments/smm-gateway/smm_gateway.c
+index 4884a040..7828b3af 100644
+--- a/deployments/smm-gateway/smm_gateway.c
++++ b/deployments/smm-gateway/smm_gateway.c
+@@ -13,6 +13,8 @@
+ 
+ /* Build-time default configuration */
+ 
++#define SMM_GATEWAY_NV_STORE_SN "sn:ffa:46bb39d1-b4d9-45b5-88ff-040027dab249:1"
++
+ /* Default to using the Protected Storage SP */
+ #ifndef SMM_GATEWAY_NV_STORE_SN
+ #define SMM_GATEWAY_NV_STORE_SN		"sn:ffa:751bf801-3dde-4768-a514-0f10aeed1790:0"
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0006-Use-device-region.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0006-Use-device-region.patch
new file mode 100644
index 0000000..eba1338
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0006-Use-device-region.patch
@@ -0,0 +1,53 @@
+From ae22f5077d35e6acf3feb8a84a8ef7f599261b00 Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Wed, 17 Nov 2021 15:32:46 +0000
+Subject: [PATCH] Use device region
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+
+---
+ deployments/smm-gateway/opteesp/smm_gateway_sp.c | 8 ++++----
+ 1 file changed, 4 insertions(+), 4 deletions(-)
+
+diff --git a/deployments/smm-gateway/opteesp/smm_gateway_sp.c b/deployments/smm-gateway/opteesp/smm_gateway_sp.c
+index 6f138850..0bc09023 100644
+--- a/deployments/smm-gateway/opteesp/smm_gateway_sp.c
++++ b/deployments/smm-gateway/opteesp/smm_gateway_sp.c
+@@ -10,7 +10,7 @@
+ #include <config/loader/sp/sp_config_loader.h>
+ #include "components/rpc/mm_communicate/endpoint/sp/mm_communicate_call_ep.h"
+ #include "components/service/smm_variable/frontend/mm_communicate/smm_variable_mm_service.h"
+-#include "platform/interface/memory_region.h"
++#include "platform/interface/device_region.h"
+ #include <ffa_api.h>
+ #include <sp_api.h>
+ #include <sp_messaging.h>
+@@ -25,7 +25,7 @@ static int sp_init(uint16_t *own_sp_id);
+ 
+ void __noreturn sp_main(struct ffa_init_info *init_info)
+ {
+-	struct memory_region mm_comm_buffer_region = { 0 };
++	struct device_region mm_comm_buffer_region = { 0 };
+ 	struct rpc_interface *gateway_iface = NULL;
+ 	struct smm_variable_mm_service smm_var_service = { 0 };
+ 	struct mm_service_interface *smm_var_service_interface = NULL;
+@@ -42,7 +42,7 @@ void __noreturn sp_main(struct ffa_init_info *init_info)
+ 	config_ramstore_init();
+ 	sp_config_load(init_info);
+ 
+-	if (!config_store_query(CONFIG_CLASSIFIER_MEMORY_REGION, CONFIG_NAME_MM_COMM_BUFFER_REGION,
++	if (!config_store_query(CONFIG_CLASSIFIER_DEVICE_REGION, CONFIG_NAME_MM_COMM_BUFFER_REGION,
+ 				0, &mm_comm_buffer_region, sizeof(mm_comm_buffer_region))) {
+ 		EMSG(CONFIG_NAME_MM_COMM_BUFFER_REGION " is not set in SP configuration");
+ 		goto fatal_error;
+@@ -57,7 +57,7 @@ void __noreturn sp_main(struct ffa_init_info *init_info)
+ 	/* Initialize MM communication layer */
+ 	if (!mm_communicate_call_ep_init(&mm_communicate_call_ep,
+ 					 (void *)mm_comm_buffer_region.base_addr,
+-					 mm_comm_buffer_region.region_size))
++					 mm_comm_buffer_region.io_region_size))
+ 		goto fatal_error;
+ 
+ 	/* Attach SMM variable service to MM communication layer */
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0007-Add-openamp-to-SE-proxy-deployment.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0007-Add-openamp-to-SE-proxy-deployment.patch
new file mode 100644
index 0000000..eb51ef7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0007-Add-openamp-to-SE-proxy-deployment.patch
@@ -0,0 +1,259 @@
+From 90712f624c7b676e5b9a2d95cbe97d2b63fddcc9 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 3 Dec 2021 16:36:51 +0000
+Subject: [PATCH] Add openamp to SE proxy deployment
+
+Openamp is required to communicate between secure partitions(running on
+Cortex-A) and trusted-firmware-m(running on Cortex-M).
+These changes are to fetch libmetal and openamp from github repo's
+and build it.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ deployments/se-proxy/opteesp/CMakeLists.txt | 14 ++++
+ deployments/se-proxy/opteesp/lse.S          | 19 +++++
+ external/openamp/libmetal.cmake             | 81 ++++++++++++++++++++
+ external/openamp/openamp.cmake              | 82 +++++++++++++++++++++
+ 4 files changed, 196 insertions(+)
+ create mode 100644 deployments/se-proxy/opteesp/lse.S
+ create mode 100644 external/openamp/libmetal.cmake
+ create mode 100644 external/openamp/openamp.cmake
+
+diff --git a/deployments/se-proxy/opteesp/CMakeLists.txt b/deployments/se-proxy/opteesp/CMakeLists.txt
+index 4e2069a5..248bd7e3 100644
+--- a/deployments/se-proxy/opteesp/CMakeLists.txt
++++ b/deployments/se-proxy/opteesp/CMakeLists.txt
+@@ -89,6 +89,7 @@ add_components(TARGET "se-proxy"
+ target_sources(se-proxy PRIVATE
+ 	se_proxy_sp.c
+ 	service_proxy_factory.c
++	lse.S
+ )
+ 
+ #-------------------------------------------------------------------------------
+@@ -108,6 +109,19 @@ include(../../../external/nanopb/nanopb.cmake)
+ target_link_libraries(se-proxy PRIVATE nanopb::protobuf-nanopb-static)
+ protobuf_generate_all(TGT "se-proxy" NAMESPACE "protobuf" BASE_DIR "${TS_ROOT}/protocols")
+ 
++# libmetal
++list(APPEND LIBMETAL_EXTERNAL_INCLUDE_PATHS ${SP_DEV_KIT_INCLUDE_DIR})
++set(LIBMETAL_EXTRA_INCLUDE_PATHS ${LIBMETAL_EXTRA_INCLUDE_PATHS}
++	CACHE STRING "" FORCE)
++include(../../../external/openamp/libmetal.cmake)
++
++# OpenAMP
++list(APPEND OPENAMP_EXTERNAL_INCLUDE_PATHS ${SP_DEV_KIT_INCLUDE_DIR})
++set(OPENAMP_EXTRA_INCLUDE_PATHS ${OPENAMP_EXTRA_INCLUDE_PATHS}
++	CACHE STRING "" FORCE)
++include(../../../external/openamp/openamp.cmake)
++target_link_libraries(se-proxy PRIVATE openamp libmetal)
++
+ #################################################################
+ 
+ target_compile_definitions(se-proxy PRIVATE
+diff --git a/deployments/se-proxy/opteesp/lse.S b/deployments/se-proxy/opteesp/lse.S
+new file mode 100644
+index 00000000..840683a6
+--- /dev/null
++++ b/deployments/se-proxy/opteesp/lse.S
+@@ -0,0 +1,19 @@
++// SPDX-License-Identifier: BSD-3-Clause
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ */
++
++.text
++.globl __aarch64_cas4_acq_rel
++
++__aarch64_cas4_acq_rel:
++	mov	w16, w0
++	ldaxr	w0, [x2]
++	cmp	w0, w16
++0:	bne	1f
++
++	stlxr	w17, w1, [x2]
++	cbnz	w17, 0b
++1:	ret
++
++
+diff --git a/external/openamp/libmetal.cmake b/external/openamp/libmetal.cmake
+new file mode 100644
+index 00000000..3a647e69
+--- /dev/null
++++ b/external/openamp/libmetal.cmake
+@@ -0,0 +1,81 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021 Linaro Limited
++# Copyright (c) 2021, Arm Limited. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++
++set (LIBMETAL_URL "https://github.com/OpenAMP/libmetal.git" CACHE STRING "libmetal repository URL")
++set (LIBMETAL_INSTALL_PATH "${CMAKE_CURRENT_BINARY_DIR}/libmetal_install" CACHE PATH "libmetal installation directory")
++set (LIBMETAL_PACKAGE_PATH "${LIBMETAL_INSTALL_PATH}/libmetal/cmake" CACHE PATH "libmetal CMake package directory")
++set (LIBMETAL_TARGET_NAME "libmetal")
++set (LIBMETAL_VERSION "f252f0e007fbfb8b3a52b1d5901250ddac96baad"  CACHE STRING "The version of libmetal to use")
++
++if(NOT LIBMETAL_DEBUG)
++	set(LIBMETAL_BUILD_TYPE "Release")
++else()
++	set(LIBMETAL_BUILD_TYPE "Debug")
++endif()
++
++include(FetchContent)
++
++# Checking git
++find_program(GIT_COMMAND "git")
++if (NOT GIT_COMMAND)
++	message(FATAL_ERROR "Please install git")
++endif()
++
++FetchContent_Declare(
++	libmetal
++	GIT_REPOSITORY ${LIBMETAL_URL}
++	GIT_TAG ${LIBMETAL_VERSION}
++)
++
++# FetchContent_GetProperties exports libmetal_SOURCE_DIR and libmetal_BINARY_DIR variables
++FetchContent_GetProperties(libmetal)
++if(NOT libmetal_POPULATED)
++	message(STATUS "Fetching libmetal")
++	FetchContent_Populate(libmetal)
++endif()
++
++# Ensure list of include paths is separated correctly
++string(REPLACE ";" "\\;" LIBMETAL_EXTERNAL_INCLUDE_PATHS "${LIBMETAL_EXTERNAL_INCLUDE_PATHS}")
++
++#Configure the library
++execute_process(COMMAND
++	${CMAKE_COMMAND}
++			-DCMAKE_BUILD_TYPE=${LIBMETAL_BUILD_TYPE}
++			-DCMAKE_C_COMPILER=${CMAKE_C_COMPILER}
++			-DCMAKE_C_COMPILER_ID=${CMAKE_C_COMPILER_ID}
++			-DCMAKE_TOOLCHAIN_FILE=${TS_EXTERNAL_LIB_TOOLCHAIN_FILE}
++			-DCMAKE_INSTALL_PREFIX=${LIBMETAL_INSTALL_PATH}
++			-DCMAKE_C_FLAGS_DEBUG=${CMAKE_C_FLAGS_DEBUG}
++			-DCMAKE_C_FLAGS=${CMAKE_C_FLAGS}
++			-DCMAKE_C_OUTPUT_EXTENSION=.o
++			-DCMAKE_C_COMPILER_WORKS=true
++			-DCMAKE_SYSTEM_PROCESSOR=arm
++			-DWITH_DOC=off
++			-DWITH_TESTS=off
++			-DWITH_EXAMPLES=off
++			-DWITH_DEFAULT_LOGGER=off
++			-DEXTERNAL_INCLUDE_PATHS=${LIBMETAL_EXTERNAL_INCLUDE_PATHS}
++			-DMACHINE=template
++			${libmetal_SOURCE_DIR}
++	WORKING_DIRECTORY
++			${libmetal_BINARY_DIR}
++)
++
++# Build the library
++execute_process(COMMAND
++    ${CMAKE_COMMAND} --build ${libmetal_BINARY_DIR} -- install
++		RESULT_VARIABLE _exec_error
++	)
++if (_exec_error)
++	message(FATAL_ERROR "Build step of libmetal failed with ${_exec_error}.")
++endif()
++
++#Create an imported target to have clean abstraction in the build-system.
++add_library(libmetal STATIC IMPORTED)
++set_property(TARGET libmetal PROPERTY IMPORTED_LOCATION "${LIBMETAL_INSTALL_PATH}/lib/${CMAKE_STATIC_LIBRARY_PREFIX}metal${CMAKE_STATIC_LIBRARY_SUFFIX}")
++set_property(TARGET libmetal PROPERTY INTERFACE_INCLUDE_DIRECTORIES "${LIBMETAL_INSTALL_PATH}/include")
+diff --git a/external/openamp/openamp.cmake b/external/openamp/openamp.cmake
+new file mode 100644
+index 00000000..aae13bad
+--- /dev/null
++++ b/external/openamp/openamp.cmake
+@@ -0,0 +1,82 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021 Linaro Limited
++# Copyright (c) 2021, Arm Limited. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++
++set (OPENAMP_URL "https://github.com/OpenAMP/open-amp.git" CACHE STRING "OpenAMP repository URL")
++set (OPENAMP_INSTALL_PATH "${CMAKE_CURRENT_BINARY_DIR}/openamp_install" CACHE PATH "OpenAMP installation directory")
++set (OPENAMP_PACKAGE_PATH "${OPENAMP_INSTALL_PATH}/openamp/cmake" CACHE PATH "OpenAMP CMake package directory")
++set (OPENAMP_TARGET_NAME "openamp")
++set (OPENAMP_VERSION "347397decaa43372fc4d00f965640ebde042966d"  CACHE STRING "The version of openamp to use")
++
++
++if(NOT OPENAMP_DEBUG)
++	set(OPENAMP_BUILD_TYPE "Release")
++else()
++	set(OPENAMP_BUILD_TYPE "Debug")
++endif()
++
++include(FetchContent)
++
++# Checking git
++find_program(GIT_COMMAND "git")
++if (NOT GIT_COMMAND)
++	message(FATAL_ERROR "Please install git")
++endif()
++
++FetchContent_Declare(
++	openamp
++	GIT_REPOSITORY ${OPENAMP_URL}
++	GIT_TAG ${OPENAMP_VERSION}
++)
++
++# FetchContent_GetProperties exports openamp_SOURCE_DIR and openamp_BINARY_DIR variables
++FetchContent_GetProperties(openamp)
++if(NOT openamp_POPULATED)
++	message(STATUS "Fetching openamp")
++	FetchContent_Populate(openamp)
++endif()
++
++# Ensure list of include paths is separated correctly
++get_target_property(_libmetal_inc libmetal INTERFACE_INCLUDE_DIRECTORIES)
++set (_openam_external_include_paths ${_libmetal_inc} ${OPENAMP_EXTERNAL_INCLUDE_PATHS})
++string(REPLACE ";" "\\;" OPENAMP_EXTERNAL_INCLUDE_PATHS "${_openam_external_include_paths}")
++
++#Configure the library
++execute_process(COMMAND
++	${CMAKE_COMMAND}
++			-DCMAKE_BUILD_TYPE=${OPENAMP_BUILD_TYPE}
++			-DCMAKE_C_COMPILER=${CMAKE_C_COMPILER}
++			-DCMAKE_C_COMPILER_ID=${CMAKE_C_COMPILER_ID}
++			-DCMAKE_TOOLCHAIN_FILE=${TS_EXTERNAL_LIB_TOOLCHAIN_FILE}
++			-DCMAKE_INSTALL_PREFIX=${OPENAMP_INSTALL_PATH}
++			-DCMAKE_C_FLAGS_DEBUG=${CMAKE_C_FLAGS_DEBUG}
++			-DLIBMETAL_INCLUDE_DIR=${CMAKE_CURRENT_BINARY_DIR}/libmetal/lib/include
++			-DLIBMETAL_LIB=${CMAKE_CURRENT_BINARY_DIR}/libmetal/lib
++			-DCMAKE_C_OUTPUT_EXTENSION=.o
++			-DCMAKE_C_COMPILER_WORKS=true
++			-DCMAKE_SYSTEM_PROCESSOR=arm
++			-DEXTERNAL_INCLUDE_PATHS=${OPENAMP_EXTERNAL_INCLUDE_PATHS}
++			-DMACHINE=template
++			-DRPMSG_BUFFER_SIZE=512
++			${openamp_SOURCE_DIR}
++	WORKING_DIRECTORY
++			${openamp_BINARY_DIR}
++)
++
++# Build the library
++execute_process(COMMAND
++    ${CMAKE_COMMAND} --build ${openamp_BINARY_DIR} -- install
++		RESULT_VARIABLE _exec_error
++	)
++if (_exec_error)
++	message(FATAL_ERROR "Build step of OpenAMP failed with ${_exec_error}.")
++endif()
++
++#Create an imported target to have clean abstraction in the build-system.
++add_library(openamp STATIC IMPORTED)
++set_property(TARGET openamp PROPERTY IMPORTED_LOCATION "${OPENAMP_INSTALL_PATH}/lib/${CMAKE_STATIC_LIBRARY_PREFIX}open_amp${CMAKE_STATIC_LIBRARY_SUFFIX}")
++set_property(TARGET openamp PROPERTY INTERFACE_INCLUDE_DIRECTORIES "${OPENAMP_INSTALL_PATH}/include")
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0008-Implement-mhu-driver-and-the-OpenAmp-conversion-laye.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0008-Implement-mhu-driver-and-the-OpenAmp-conversion-laye.patch
new file mode 100644
index 0000000..47ca59f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0008-Implement-mhu-driver-and-the-OpenAmp-conversion-laye.patch
@@ -0,0 +1,1093 @@
+From 993a3c3cb08a723f5b5eb07cf38d70e9acf52e7f Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 3 Dec 2021 18:00:46 +0000
+Subject: [PATCH] Implement mhu driver and the OpenAmp conversion layer.
+
+This commit adds an mhu driver (v2.1 and v2) to the secure
+partition se_proxy and a conversion layer to communicate with
+the secure enclave using OpenAmp.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../se-proxy/opteesp/default_se-proxy.dts.in  |  16 +
+ .../drivers/arm/mhu_driver/component.cmake    |  12 +
+ platform/drivers/arm/mhu_driver/mhu_v2.h      | 391 ++++++++++++
+ platform/drivers/arm/mhu_driver/mhu_v2_x.c    | 602 ++++++++++++++++++
+ .../providers/arm/corstone1000/platform.cmake |  10 +
+ 5 files changed, 1031 insertions(+)
+ create mode 100644 platform/drivers/arm/mhu_driver/component.cmake
+ create mode 100644 platform/drivers/arm/mhu_driver/mhu_v2.h
+ create mode 100644 platform/drivers/arm/mhu_driver/mhu_v2_x.c
+ create mode 100644 platform/providers/arm/corstone1000/platform.cmake
+
+diff --git a/deployments/se-proxy/opteesp/default_se-proxy.dts.in b/deployments/se-proxy/opteesp/default_se-proxy.dts.in
+index 9f5cf712..f351a592 100644
+--- a/deployments/se-proxy/opteesp/default_se-proxy.dts.in
++++ b/deployments/se-proxy/opteesp/default_se-proxy.dts.in
+@@ -17,5 +17,21 @@
+ 		execution-state = <0>; /* AArch64 */
+ 		xlat-granule = <0>; /* 4KiB */
+ 		messaging-method = <0>; /* Direct messaging only */
++
++		device-regions {
++			compatible = "arm,ffa-manifest-device-regions";
++			mhu-sender {
++				/* Armv8 A Foundation Platform values */
++				base-address = <0x00000000 0x1b820000>;
++				pages-count = <16>;
++				attributes = <0x3>; /* read-write */
++			};
++			mhu-receiver {
++				/* Armv8 A Foundation Platform values */
++				base-address = <0x00000000 0x1b830000>;
++				pages-count = <16>;
++				attributes = <0x3>; /* read-write */
++			};
++		};
+ 	};
+ };
+diff --git a/platform/drivers/arm/mhu_driver/component.cmake b/platform/drivers/arm/mhu_driver/component.cmake
+new file mode 100644
+index 00000000..77a5a50b
+--- /dev/null
++++ b/platform/drivers/arm/mhu_driver/component.cmake
+@@ -0,0 +1,12 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++
++# Add source files for using mhu driver
++target_sources(${TGT}
++	PRIVATE
++	"${CMAKE_CURRENT_LIST_DIR}/mhu_v2_x.c"
++)
+diff --git a/platform/drivers/arm/mhu_driver/mhu_v2.h b/platform/drivers/arm/mhu_driver/mhu_v2.h
+new file mode 100644
+index 00000000..2e4ba80f
+--- /dev/null
++++ b/platform/drivers/arm/mhu_driver/mhu_v2.h
+@@ -0,0 +1,391 @@
++/*
++ * Copyright (c) 2021 Arm Limited
++ *
++ * Licensed under the Apache License, Version 2.0 (the "License");
++ * you may not use this file except in compliance with the License.
++ * You may obtain a copy of the License at
++ *
++ *     http://www.apache.org/licenses/LICENSE-2.0
++ *
++ * Unless required by applicable law or agreed to in writing, software
++ * distributed under the License is distributed on an "AS IS" BASIS,
++ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
++ * See the License for the specific language governing permissions and
++ * limitations under the License.
++ */
++
++/**
++ * \file mhu_v2_x.h
++ * \brief Driver for Arm MHU v2.0 and v2.1
++ */
++
++#ifndef __MHU_V2_X_H__
++#define __MHU_V2_X_H__
++
++#include <stdint.h>
++#include <stdbool.h>
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++#define MHU_2_X_INTR_NR2R_OFF             (0x0u)
++#define MHU_2_X_INTR_R2NR_OFF             (0x1u)
++#define MHU_2_1_INTR_CHCOMB_OFF           (0x2u)
++
++#define MHU_2_X_INTR_NR2R_MASK            (0x1u << MHU_2_X_INTR_NR2R_OFF)
++#define MHU_2_X_INTR_R2NR_MASK            (0x1u << MHU_2_X_INTR_R2NR_OFF)
++#define MHU_2_1_INTR_CHCOMB_MASK          (0x1u << MHU_2_1_INTR_CHCOMB_OFF)
++
++enum mhu_v2_x_frame_t {
++    MHU_V2_X_SENDER_FRAME   = 0x0u,
++    MHU_V2_X_RECEIVER_FRAME = 0x1u,
++};
++
++enum mhu_v2_x_supported_revisions {
++     MHU_REV_READ_FROM_HW = 0,
++     MHU_REV_2_0,
++     MHU_REV_2_1,
++};
++
++struct mhu_v2_x_dev_t {
++    uint32_t base;
++    enum mhu_v2_x_frame_t frame;
++    uint32_t subversion;    /*!< Hardware subversion: v2.X */
++    bool is_initialized;    /*!< Indicates if the MHU driver
++                             *   is initialized and enabled
++                             */
++};
++
++/**
++ * \brief MHU v2 error enumeration types.
++ */
++enum mhu_v2_x_error_t {
++    MHU_V_2_X_ERR_NONE                =  0,
++    MHU_V_2_X_ERR_NOT_INIT            = -1,
++    MHU_V_2_X_ERR_ALREADY_INIT        = -2,
++    MHU_V_2_X_ERR_UNSUPPORTED_VERSION = -3,
++    MHU_V_2_X_ERR_INVALID_ARG         = -4,
++    MHU_V_2_X_ERR_GENERAL             = -5
++};
++
++/**
++ * \brief Initializes the driver
++ *
++ * \param[in] dev   MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] rev   MHU revision (if can't be identified from HW)
++ *
++ * Reads the MHU hardware version
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note MHU revision only has to be specified when versions can't be read
++ *       from HW (ARCH_MAJOR_REV reg reads as 0x0).
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_driver_init(struct mhu_v2_x_dev_t *dev,
++     enum mhu_v2_x_supported_revisions rev);
++
++/**
++ * \brief Returns the number of channels implemented.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ *
++ * Returns the number of channels implemented.
++ *
++ * \return Returns the number of channels implemented.
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++uint32_t mhu_v2_x_get_num_channel_implemented(
++         const struct mhu_v2_x_dev_t *dev);
++
++/**
++ * \brief Sends the value over a channel.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] channel     Channel to send the value over.
++ * \param[in] val         Value to send.
++ *
++ * Sends the value over a channel.
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ * \note This function doesn't check if channel is implemented.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_channel_send(const struct mhu_v2_x_dev_t *dev,
++     uint32_t channel, uint32_t val);
++
++/**
++ * \brief Clears the channel after the value is send over it.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] channel     Channel to clear.
++ *
++ * Clears the channel after the value is send over it.
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ * \note This function doesn't check if channel is implemented.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_channel_clear(const struct mhu_v2_x_dev_t *dev,
++     uint32_t channel);
++
++/**
++ * \brief Receives the value over a channel.
++ *
++ * \param[in]  dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in]  channel     Channel to receive the value from.
++ * \param[out] value       Pointer to variable that will store the value.
++ *
++ * Receives the value over a channel.
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ * \note This function doesn't check if channel is implemented.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_channel_receive(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t *value);
++
++/**
++ * \brief Sets bits in the Channel Mask.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] channel     Which channel's mask to set.
++ * \param[in] mask        Mask to be set over a receiver frame.
++ *
++ * Sets bits in the Channel Mask.
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ * \note This function doesn't check if channel is implemented.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_channel_mask_set(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask);
++
++/**
++ * \brief Clears bits in the Channel Mask.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] channel     Which channel's mask to clear.
++ * \param[in] mask        Mask to be clear over a receiver frame.
++ *
++ * Clears bits in the Channel Mask.
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ * \note This function doesn't check if channel is implemented.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_channel_mask_clear(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask);
++
++/**
++ * \brief Enables the Channel interrupt.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] channel     Which channel's interrupt to enable.
++ *
++ * Enables the Channel clear interrupt.
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ * \note This function doesn't check if channel is implemented.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_channel_interrupt_enable(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel);
++
++/**
++ * \brief Disables the Channel interrupt.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] channel     Which channel's interrupt to disable.
++ *
++ * Disables the Channel interrupt.
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ * \note This function doesn't check if channel is implemented.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_channel_interrupt_disable(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel);
++
++/**
++ * \brief Cleares the Channel interrupt.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] channel     Which channel's interrupt to clear.
++ *
++ * Cleares the Channel interrupt.
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ * \note This function doesn't check if channel is implemented.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_channel_interrupt_clear(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel);
++
++/**
++ * \brief Initiates a MHU transfer with the handshake signals.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ *
++ * Initiates a MHU transfer with the handshake signals in a blocking mode.
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_initiate_transfer(
++     const struct mhu_v2_x_dev_t *dev);
++
++/**
++ * \brief Closes a MHU transfer with the handshake signals.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ *
++ * Closes a MHU transfer with the handshake signals in a blocking mode.
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_close_transfer(
++     const struct mhu_v2_x_dev_t *dev);
++
++/**
++ * \brief Returns the value of access request signal.
++ *
++ * \param[in]  dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[out] val         Pointer to variable that will store the value.
++ *
++ * For more information please read the MHU v2 user guide
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_get_access_request(
++     const struct mhu_v2_x_dev_t *dev, uint32_t *val);
++
++/**
++ * \brief Sets the value of access request signal to high.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ *
++ * For more information please read the MHU v2 user guide
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_set_access_request(
++     const struct mhu_v2_x_dev_t *dev);
++
++/**
++ * \brief Sets the value of access request signal to low.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ *
++ * For more information please read the MHU v2 user guide
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_reset_access_request(
++     const struct mhu_v2_x_dev_t *dev);
++
++/**
++ * \brief Returns the value of access ready signal.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[out] val        Pointer to variable that will store the value.
++ *
++ * For more information please read the MHU v2 user guide
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_get_access_ready(
++     const struct mhu_v2_x_dev_t *dev, uint32_t *val);
++
++/**
++ * \brief Returns the MHU interrupt status.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ *
++ * \return Interrupt status register value. Masking is needed for individual
++ *         interrupts.
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++uint32_t mhu_v2_x_get_interrupt_status(const struct mhu_v2_x_dev_t *dev);
++
++/**
++ * \brief Enables MHU interrupts.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] mask        Bit mask for enabling/disabling interrupts
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_interrupt_enable(
++     const struct mhu_v2_x_dev_t *dev, uint32_t mask);
++
++/**
++ * \brief Disables MHU interrupts.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] mask        Bit mask for enabling/disabling interrupts
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_interrupt_disable(
++     const struct mhu_v2_x_dev_t *dev, uint32_t mask);
++
++/**
++ * \brief Clears MHU interrupts.
++ *
++ * \param[in] dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[in] mask        Bit mask for clearing interrupts
++ *
++ * \return Returns mhu_v2_x_error_t error code
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_x_interrupt_clear(
++     const struct mhu_v2_x_dev_t *dev, uint32_t mask);
++
++/**
++ * \brief Returns the first channel number whose interrupt bit is high.
++ *
++ * \param[in]  dev         MHU device struct \ref mhu_v2_x_dev_t
++ * \param[out] channel     Pointer to variable that will have the channel value.
++ *
++ * \return Returns the first channel number whose interrupt bit is high.
++ * \return Returns mhu_v2_x_error_t error code.
++ *
++ * \note This function doesn't check if dev is NULL.
++ */
++enum mhu_v2_x_error_t mhu_v2_1_get_ch_interrupt_num(
++     const struct mhu_v2_x_dev_t *dev, uint32_t *channel);
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* __MHU_V2_X_H__ */
+diff --git a/platform/drivers/arm/mhu_driver/mhu_v2_x.c b/platform/drivers/arm/mhu_driver/mhu_v2_x.c
+new file mode 100644
+index 00000000..01d8f659
+--- /dev/null
++++ b/platform/drivers/arm/mhu_driver/mhu_v2_x.c
+@@ -0,0 +1,602 @@
++/*
++ * Copyright (c) 2021 Arm Limited
++ *
++ * Licensed under the Apache License, Version 2.0 (the "License");
++ * you may not use this file except in compliance with the License.
++ * You may obtain a copy of the License at
++ *
++ *     http://www.apache.org/licenses/LICENSE-2.0
++ *
++ * Unless required by applicable law or agreed to in writing, software
++ * distributed under the License is distributed on an "AS IS" BASIS,
++ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
++ * See the License for the specific language governing permissions and
++ * limitations under the License.
++ */
++#include <stdint.h>
++#include <stdbool.h>
++#include "mhu_v2.h"
++
++#define _MHU_V2_X_MAX_CHANNELS    124
++#define _MHU_V2_1_MAX_CHCOMB_INT  4
++#define ENABLE                    0x1
++#define DISABLE                   0x0
++#define CLEAR_INTR                0x1
++#define CH_PER_CH_COMB            0x20
++#define SEND_FRAME(p_mhu)       ((struct _mhu_v2_x_send_frame_t *)p_mhu)
++#define RECV_FRAME(p_mhu)       ((struct _mhu_v2_x_recv_frame_t *)p_mhu)
++
++#define MHU_MAJOR_REV_V2      0x1u
++#define MHU_MINOR_REV_2_0     0x0u
++#define MHU_MINOR_REV_2_1     0x1u
++
++struct _mhu_v2_x_send_ch_window_t {
++    /* Offset: 0x00 (R/ ) Channel Status */
++    volatile uint32_t ch_st;
++    /* Offset: 0x04 (R/ ) Reserved */
++    volatile uint32_t reserved_0;
++    /* Offset: 0x08 (R/ ) Reserved */
++    volatile uint32_t reserved_1;
++    /* Offset: 0x0C ( /W) Channel Set */
++    volatile uint32_t ch_set;
++    /* Offset: 0x10 (R/ ) Channel Interrupt Status (Reserved in 2.0) */
++    volatile uint32_t ch_int_st;
++    /* Offset: 0x14 ( /W) Channel Interrupt Clear  (Reserved in 2.0) */
++    volatile uint32_t ch_int_clr;
++    /* Offset: 0x18 (R/W) Channel Interrupt Enable (Reserved in 2.0) */
++    volatile uint32_t ch_int_en;
++    /* Offset: 0x1C (R/ ) Reserved */
++    volatile uint32_t reserved_2;
++};
++
++struct _mhu_v2_x_send_frame_t {
++    /* Offset: 0x000 ( / ) Sender Channel Window 0 -123 */
++    struct _mhu_v2_x_send_ch_window_t send_ch_window[_MHU_V2_X_MAX_CHANNELS];
++    /* Offset: 0xF80 (R/ ) Message Handling Unit Configuration */
++    volatile uint32_t mhu_cfg;
++    /* Offset: 0xF84 (R/W) Response Configuration */
++    volatile uint32_t resp_cfg;
++    /* Offset: 0xF88 (R/W) Access Request */
++    volatile uint32_t access_request;
++    /* Offset: 0xF8C (R/ ) Access Ready */
++    volatile uint32_t access_ready;
++    /* Offset: 0xF90 (R/ ) Interrupt Status */
++    volatile uint32_t int_st;
++    /* Offset: 0xF94 ( /W) Interrupt Clear */
++    volatile uint32_t int_clr;
++    /* Offset: 0xF98 (R/W) Interrupt Enable */
++    volatile uint32_t int_en;
++    /* Offset: 0xF9C (R/ ) Reserved */
++    volatile uint32_t reserved_0;
++    /* Offset: 0xFA0 (R/W) Channel Combined Interrupt Stat (Reserved in 2.0) */
++    volatile uint32_t ch_comb_int_st[_MHU_V2_1_MAX_CHCOMB_INT];
++    /* Offset: ‭0xFC4‬ (R/ ) Reserved */
++    volatile uint32_t reserved_1[6];
++    /* Offset: 0xFC8 (R/ ) Implementer Identification Register */
++    volatile uint32_t iidr;
++    /* Offset: 0xFCC (R/ ) Architecture Identification Register */
++    volatile uint32_t aidr;
++    /* Offset: 0xFD0 (R/ )  */
++    volatile uint32_t pid_1[4];
++    /* Offset: 0xFE0 (R/ )  */
++    volatile uint32_t pid_0[4];
++    /* Offset: 0xFF0 (R/ )  */
++    volatile uint32_t cid[4];
++};
++
++struct _mhu_v2_x_rec_ch_window_t {
++    /* Offset: 0x00 (R/ ) Channel Status */
++    volatile uint32_t ch_st;
++    /* Offset: 0x04 (R/ ) Channel Status Masked */
++    volatile uint32_t ch_st_msk;
++    /* Offset: 0x08 ( /W) Channel Clear */
++    volatile uint32_t ch_clr;
++    /* Offset: 0x0C (R/ ) Reserved */
++    volatile uint32_t reserved_0;
++    /* Offset: 0x10 (R/ ) Channel Mask Status */
++    volatile uint32_t ch_msk_st;
++    /* Offset: 0x14 ( /W) Channel Mask Set */
++    volatile uint32_t ch_msk_set;
++    /* Offset: 0x18 ( /W) Channel Mask Clear */
++    volatile uint32_t ch_msk_clr;
++    /* Offset: 0x1C (R/ ) Reserved */
++    volatile uint32_t reserved_1;
++};
++
++struct _mhu_v2_x_recv_frame_t {
++    /* Offset: 0x000 ( / ) Receiver Channel Window 0 -123 */
++    struct _mhu_v2_x_rec_ch_window_t rec_ch_window[_MHU_V2_X_MAX_CHANNELS];
++    /* Offset: 0xF80 (R/ ) Message Handling Unit Configuration */
++    volatile uint32_t mhu_cfg;
++    /* Offset: 0xF84 (R/ ) Reserved */
++    volatile uint32_t reserved_0[3];
++    /* Offset: 0xF90 (R/ ) Interrupt Status (Reserved in 2.0) */
++    volatile uint32_t int_st;
++    /* Offset: 0xF94 (R/ ) Interrupt Clear  (Reserved in 2.0) */
++    volatile uint32_t int_clr;
++    /* Offset: 0xF98 (R/W) Interrupt Enable (Reserved in 2.0) */
++    volatile uint32_t int_en;
++    /* Offset: 0xF9C (R/ ) Reserved  */
++    volatile uint32_t reserved_1;
++    /* Offset: 0xFA0 (R/ ) Channel Combined Interrupt Stat (Reserved in 2.0) */
++    volatile uint32_t ch_comb_int_st[_MHU_V2_1_MAX_CHCOMB_INT];
++    /* Offset: 0xFB0 (R/ ) Reserved */
++    volatile uint32_t reserved_2[6];
++    /* Offset: 0xFC8 (R/ ) Implementer Identification Register */
++    volatile uint32_t iidr;
++    /* Offset: 0xFCC (R/ ) Architecture Identification Register */
++    volatile uint32_t aidr;
++    /* Offset: 0xFD0 (R/ )  */
++    volatile uint32_t pid_1[4];
++    /* Offset: 0xFE0 (R/ )  */
++    volatile uint32_t pid_0[4];
++    /* Offset: 0xFF0 (R/ )  */
++    volatile uint32_t cid[4];
++};
++
++union _mhu_v2_x_frame_t {
++    struct _mhu_v2_x_send_frame_t send_frame;
++    struct _mhu_v2_x_recv_frame_t recv_frame;
++};
++
++enum mhu_v2_x_error_t mhu_v2_x_driver_init(struct mhu_v2_x_dev_t *dev,
++     enum mhu_v2_x_supported_revisions rev)
++{
++    uint32_t AIDR = 0;
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if (dev->is_initialized) {
++        return MHU_V_2_X_ERR_ALREADY_INIT;
++    }
++
++    if (rev == MHU_REV_READ_FROM_HW) {
++        /* Read revision from HW */
++        if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
++            AIDR = p_mhu->recv_frame.aidr;
++        } else {
++            AIDR = p_mhu->send_frame.aidr;
++        }
++
++        /* Get bits 7:4 to read major revision */
++        if ( ((AIDR >> 4) & 0b1111) != MHU_MAJOR_REV_V2) {
++            /* Unsupported MHU version */
++            return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
++        } /* No need to save major version, driver only supports MHUv2 */
++
++        /* Get bits 3:0 to read minor revision */
++        dev->subversion = AIDR & 0b1111;
++
++        if (dev->subversion != MHU_MINOR_REV_2_0 &&
++            dev->subversion != MHU_MINOR_REV_2_1) {
++            /* Unsupported subversion */
++            return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
++        }
++    } else {
++        /* Revisions were provided by caller */
++        if (rev == MHU_REV_2_0) {
++            dev->subversion = MHU_MINOR_REV_2_0;
++        } else if (rev == MHU_REV_2_1) {
++            dev->subversion = MHU_MINOR_REV_2_1;
++        } else {
++            /* Unsupported subversion */
++            return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
++        }/* No need to save major version, driver only supports MHUv2 */
++    }
++
++    dev->is_initialized = true;
++
++    return MHU_V_2_X_ERR_NONE;
++}
++
++uint32_t mhu_v2_x_get_num_channel_implemented(const struct mhu_v2_x_dev_t *dev)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame == MHU_V2_X_SENDER_FRAME) {
++        return (SEND_FRAME(p_mhu))->mhu_cfg;
++    } else {
++        return (RECV_FRAME(p_mhu))->mhu_cfg;
++    }
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_channel_send(const struct mhu_v2_x_dev_t *dev,
++     uint32_t channel, uint32_t val)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame == MHU_V2_X_SENDER_FRAME) {
++        (SEND_FRAME(p_mhu))->send_ch_window[channel].ch_set = val;
++        return MHU_V_2_X_ERR_NONE;
++    } else {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_channel_clear(const struct mhu_v2_x_dev_t *dev,
++     uint32_t channel)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame == MHU_V2_X_RECEIVER_FRAME) {
++        (RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_clr = UINT32_MAX;
++        return MHU_V_2_X_ERR_NONE;
++    } else {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_channel_receive(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t *value)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame == MHU_V2_X_RECEIVER_FRAME) {
++        *value = (RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_st;
++        return MHU_V_2_X_ERR_NONE;
++    } else {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_channel_mask_set(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame == MHU_V2_X_RECEIVER_FRAME) {
++        (RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_msk_set = mask;
++        return MHU_V_2_X_ERR_NONE;
++    } else {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_channel_mask_clear(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame == MHU_V2_X_RECEIVER_FRAME) {
++        (RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_msk_clr = mask;
++        return MHU_V_2_X_ERR_NONE;
++    } else {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_channel_interrupt_enable(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if (dev->subversion == MHU_MINOR_REV_2_1) {
++        return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
++    }
++
++    if(dev->frame == MHU_V2_X_SENDER_FRAME) {
++        (SEND_FRAME(p_mhu))->send_ch_window[channel].ch_int_en = ENABLE;
++        return MHU_V_2_X_ERR_NONE;
++    } else {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_channel_interrupt_disable(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if (dev->subversion == MHU_MINOR_REV_2_1) {
++        return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
++    }
++
++    if(dev->frame == MHU_V2_X_SENDER_FRAME) {
++        (SEND_FRAME(p_mhu))->send_ch_window[channel].ch_int_en = DISABLE;
++        return MHU_V_2_X_ERR_NONE;
++    } else {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_channel_interrupt_clear(
++     const struct mhu_v2_x_dev_t *dev, uint32_t channel)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if (dev->subversion == MHU_MINOR_REV_2_1) {
++        return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
++    }
++
++    if(dev->frame == MHU_V2_X_SENDER_FRAME) {
++        (SEND_FRAME(p_mhu))->send_ch_window[channel].ch_int_clr = CLEAR_INTR;
++        return MHU_V_2_X_ERR_NONE;
++    } else {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_initiate_transfer(
++     const struct mhu_v2_x_dev_t *dev)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame != MHU_V2_X_SENDER_FRAME) {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++
++    (SEND_FRAME(p_mhu))->access_request = ENABLE;
++
++    while ( !((SEND_FRAME(p_mhu))->access_ready) ) {
++        /* Wait in a loop for access ready signal to be high */
++        ;
++    }
++
++    return MHU_V_2_X_ERR_NONE;
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_close_transfer(const struct mhu_v2_x_dev_t *dev)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame != MHU_V2_X_SENDER_FRAME) {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++
++    (SEND_FRAME(p_mhu))->access_request = DISABLE;
++
++    return MHU_V_2_X_ERR_NONE;
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_get_access_request(
++     const struct mhu_v2_x_dev_t *dev, uint32_t *val)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame != MHU_V2_X_SENDER_FRAME) {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++
++    *val = (SEND_FRAME(p_mhu))->access_request;
++
++    return MHU_V_2_X_ERR_NONE;
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_set_access_request(
++     const struct mhu_v2_x_dev_t *dev)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame != MHU_V2_X_SENDER_FRAME) {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++
++    (SEND_FRAME(p_mhu))->access_request = ENABLE;
++
++    return MHU_V_2_X_ERR_NONE;
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_reset_access_request(
++     const struct mhu_v2_x_dev_t *dev)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame != MHU_V2_X_SENDER_FRAME) {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++
++    (SEND_FRAME(p_mhu))->access_request = DISABLE;
++
++    return MHU_V_2_X_ERR_NONE;
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_get_access_ready(
++     const struct mhu_v2_x_dev_t *dev, uint32_t *val)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame != MHU_V2_X_SENDER_FRAME) {
++        return MHU_V_2_X_ERR_INVALID_ARG;
++    }
++
++    *val = (SEND_FRAME(p_mhu))->access_ready;
++
++    return MHU_V_2_X_ERR_NONE;
++}
++
++uint32_t mhu_v2_x_get_interrupt_status(const struct mhu_v2_x_dev_t *dev)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if(dev->frame == MHU_V2_X_SENDER_FRAME) {
++        return (SEND_FRAME(p_mhu))->int_st;
++    } else {
++        return (RECV_FRAME(p_mhu))->int_st;
++    }
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_interrupt_enable(
++     const struct mhu_v2_x_dev_t *dev, uint32_t mask)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if (dev->subversion == MHU_MINOR_REV_2_0) {
++        if (mask & MHU_2_1_INTR_CHCOMB_MASK) {
++            /* Combined channel IRQ is not present in v2.0 */
++            return MHU_V_2_X_ERR_INVALID_ARG;
++        }
++
++        if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
++            /* Only sender frame has these registers */
++            return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
++        }
++    }
++
++    if(dev->frame == MHU_V2_X_SENDER_FRAME) {
++        (SEND_FRAME(p_mhu))->int_en |= mask;
++    } else {
++        (RECV_FRAME(p_mhu))->int_en |= mask;
++    }
++
++    return MHU_V_2_X_ERR_NONE;
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_interrupt_disable(
++     const struct mhu_v2_x_dev_t *dev, uint32_t mask)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if (dev->subversion == MHU_MINOR_REV_2_0) {
++        if (mask & MHU_2_1_INTR_CHCOMB_MASK) {
++            /* Combined channel IRQ is not present in v2.0 */
++            return MHU_V_2_X_ERR_INVALID_ARG;
++        }
++
++        if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
++            /* Only sender frame has these registers */
++            return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
++        }
++    }
++
++    if(dev->frame == MHU_V2_X_SENDER_FRAME) {
++        (SEND_FRAME(p_mhu))->int_en &= ~mask;
++    } else {
++        (RECV_FRAME(p_mhu))->int_en &= ~mask;
++    }
++
++    return MHU_V_2_X_ERR_NONE;
++}
++
++enum mhu_v2_x_error_t mhu_v2_x_interrupt_clear(
++     const struct mhu_v2_x_dev_t *dev, uint32_t mask)
++{
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if (dev->subversion == MHU_MINOR_REV_2_0) {
++        if (mask & MHU_2_1_INTR_CHCOMB_MASK) {
++            /* Combined channel IRQ is not present in v2.0 */
++            return MHU_V_2_X_ERR_INVALID_ARG;
++        }
++
++        if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
++            /* Only sender frame has these registers */
++            return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
++        }
++    }
++
++    if(dev->frame == MHU_V2_X_SENDER_FRAME) {
++        (SEND_FRAME(p_mhu))->int_clr = mask;
++    } else {
++        (RECV_FRAME(p_mhu))->int_clr = mask;
++    }
++
++    return MHU_V_2_X_ERR_NONE;
++}
++
++enum mhu_v2_x_error_t mhu_v2_1_get_ch_interrupt_num(
++     const struct mhu_v2_x_dev_t *dev, uint32_t *channel)
++{
++    uint32_t i, j, status;
++    union _mhu_v2_x_frame_t *p_mhu = (union _mhu_v2_x_frame_t *)dev->base;
++
++    if ( !(dev->is_initialized) ) {
++        return MHU_V_2_X_ERR_NOT_INIT;
++    }
++
++    if (dev->subversion != MHU_MINOR_REV_2_1) {
++        /* Feature is only supported in MHU v2.1 */
++        return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
++    }
++
++    for(i = 0; i < _MHU_V2_1_MAX_CHCOMB_INT; i++) {
++        if(dev->frame == MHU_V2_X_SENDER_FRAME) {
++            status = (SEND_FRAME(p_mhu))->ch_comb_int_st[i];
++        } else {
++            status = (RECV_FRAME(p_mhu))->ch_comb_int_st[i];
++        }
++
++        for(j = 0; j < CH_PER_CH_COMB; j++) {
++            if ((status >> CH_PER_CH_COMB - j - 1) & (ENABLE)) {
++                *channel = (CH_PER_CH_COMB - j -1 + (i * CH_PER_CH_COMB));
++                return MHU_V_2_X_ERR_NONE;
++            }
++        }
++    }
++
++    return MHU_V_2_X_ERR_GENERAL;
++}
+diff --git a/platform/providers/arm/corstone1000/platform.cmake b/platform/providers/arm/corstone1000/platform.cmake
+new file mode 100644
+index 00000000..bb778bb9
+--- /dev/null
++++ b/platform/providers/arm/corstone1000/platform.cmake
+@@ -0,0 +1,10 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++# Platform definition for the 'fvp_base_revc-2xaem8a' virtual platform.
++#-------------------------------------------------------------------------------
++
++# include MHU driver
++include(${TS_ROOT}/platform/drivers/arm/mhu_driver/component.cmake)
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0009-Add-openamp-rpc-caller.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0009-Add-openamp-rpc-caller.patch
new file mode 100644
index 0000000..0f3f2c6
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0009-Add-openamp-rpc-caller.patch
@@ -0,0 +1,1197 @@
+From a6721cc391397f5f999db84e4ebec4c20985996a Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 3 Dec 2021 19:00:54 +0000
+Subject: [PATCH] Add openamp rpc caller
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ components/rpc/common/caller/rpc_caller.c     |  10 +
+ components/rpc/common/interface/rpc_caller.h  |   8 +
+ .../rpc/openamp/caller/sp/component.cmake     |  15 +
+ .../rpc/openamp/caller/sp/openamp_caller.c    | 203 +++++++
+ .../rpc/openamp/caller/sp/openamp_caller.h    |  43 ++
+ .../rpc/openamp/caller/sp/openamp_mhu.c       | 191 ++++++
+ .../rpc/openamp/caller/sp/openamp_mhu.h       |  19 +
+ .../rpc/openamp/caller/sp/openamp_virtio.c    | 554 ++++++++++++++++++
+ .../rpc/openamp/caller/sp/openamp_virtio.h    |  24 +
+ deployments/se-proxy/opteesp/CMakeLists.txt   |   1 +
+ .../se-proxy/opteesp/default_se-proxy.dts.in  |   6 +
+ 11 files changed, 1074 insertions(+)
+ create mode 100644 components/rpc/openamp/caller/sp/component.cmake
+ create mode 100644 components/rpc/openamp/caller/sp/openamp_caller.c
+ create mode 100644 components/rpc/openamp/caller/sp/openamp_caller.h
+ create mode 100644 components/rpc/openamp/caller/sp/openamp_mhu.c
+ create mode 100644 components/rpc/openamp/caller/sp/openamp_mhu.h
+ create mode 100644 components/rpc/openamp/caller/sp/openamp_virtio.c
+ create mode 100644 components/rpc/openamp/caller/sp/openamp_virtio.h
+
+diff --git a/components/rpc/common/caller/rpc_caller.c b/components/rpc/common/caller/rpc_caller.c
+index 2dceabeb..20d889c1 100644
+--- a/components/rpc/common/caller/rpc_caller.c
++++ b/components/rpc/common/caller/rpc_caller.c
+@@ -37,3 +37,13 @@ void rpc_caller_end(struct rpc_caller *s, rpc_call_handle handle)
+ {
+ 	s->call_end(s->context, handle);
+ }
++
++void *rpc_caller_virt_to_phys(struct rpc_caller *s, void *va)
++{
++	return s->virt_to_phys(s->context, va);
++}
++
++void *rpc_caller_phys_to_virt(struct rpc_caller *s, void *pa)
++{
++	return s->phys_to_virt(s->context, pa);
++}
+diff --git a/components/rpc/common/interface/rpc_caller.h b/components/rpc/common/interface/rpc_caller.h
+index 387489cd..ef9bb649 100644
+--- a/components/rpc/common/interface/rpc_caller.h
++++ b/components/rpc/common/interface/rpc_caller.h
+@@ -45,6 +45,10 @@ struct rpc_caller
+ 		     	rpc_opstatus_t *opstatus, uint8_t **resp_buf, size_t *resp_len);
+ 
+ 	void (*call_end)(void *context, rpc_call_handle handle);
++
++	void *(*virt_to_phys)(void *context, void *va);
++
++	void *(*phys_to_virt)(void *context, void *pa);
+ };
+ 
+ /*
+@@ -87,6 +91,10 @@ RPC_CALLER_EXPORTED rpc_status_t rpc_caller_invoke(struct rpc_caller *s, rpc_cal
+  */
+ RPC_CALLER_EXPORTED void rpc_caller_end(struct rpc_caller *s, rpc_call_handle handle);
+ 
++RPC_CALLER_EXPORTED void *rpc_caller_virt_to_phys(struct rpc_caller *s, void *va);
++
++RPC_CALLER_EXPORTED void *rpc_caller_phys_to_virt(struct rpc_caller *s, void *pa);
++
+ #ifdef __cplusplus
+ }
+ #endif
+diff --git a/components/rpc/openamp/caller/sp/component.cmake b/components/rpc/openamp/caller/sp/component.cmake
+new file mode 100644
+index 00000000..fc919529
+--- /dev/null
++++ b/components/rpc/openamp/caller/sp/component.cmake
+@@ -0,0 +1,15 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2020, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++if (NOT DEFINED TGT)
++	message(FATAL_ERROR "mandatory parameter TGT is not defined.")
++endif()
++
++target_sources(${TGT} PRIVATE
++	"${CMAKE_CURRENT_LIST_DIR}/openamp_caller.c"
++	"${CMAKE_CURRENT_LIST_DIR}/openamp_virtio.c"
++	"${CMAKE_CURRENT_LIST_DIR}/openamp_mhu.c"
++	)
+diff --git a/components/rpc/openamp/caller/sp/openamp_caller.c b/components/rpc/openamp/caller/sp/openamp_caller.c
+new file mode 100644
+index 00000000..6cdfb756
+--- /dev/null
++++ b/components/rpc/openamp/caller/sp/openamp_caller.c
+@@ -0,0 +1,203 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021, Linaro Limited. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#include <stddef.h>
++#include <trace.h>
++#include "openamp_caller.h"
++#include "openamp_mhu.h"
++#include "openamp_virtio.h"
++#include <protocols/rpc/common/packed-c/status.h>
++
++#define OPENAMP_TRANSACTION_IDLE	0x0
++#define OPENAMP_TRANSACTION_INPROGRESS	0x1
++#define OPENAMP_TRANSACTION_INVOKED	0x2
++
++static rpc_call_handle openamp_call_begin(void *context, uint8_t **req_buf,
++					  size_t req_len)
++{
++	struct openamp_caller *openamp = context;
++	const struct openamp_platform_ops *ops = openamp->platform_ops;
++	rpc_call_handle handle;
++	int ret;
++
++	if (!req_buf) {
++		EMSG("openamp: call_begin: not req_buf");
++		return NULL;
++	}
++
++	if (req_len > UINT32_MAX || req_len == 0) {
++		EMSG("openamp: call_begin: resp_len invalid: %lu", req_len);
++		return NULL;
++	}
++
++	if (openamp->status != OPENAMP_TRANSACTION_IDLE) {
++		EMSG("openamp: call_begin: transaction not idle");
++		return NULL;
++	}
++
++	ret = ops->platform_call_begin(openamp, req_buf, req_len);
++	if (ret < 0) {
++		EMSG("openamp: call_begin: platform begin failed: %d", ret);
++		return NULL;
++	}
++
++	openamp->status = OPENAMP_TRANSACTION_INPROGRESS;
++	handle = openamp;
++
++	return handle;
++}
++
++static rpc_status_t openamp_call_invoke(void *context, rpc_call_handle handle,
++					uint32_t opcode, int *opstatus,
++					uint8_t **resp_buf, size_t *resp_len)
++{
++	struct openamp_caller *openamp = context;
++	const struct openamp_platform_ops *ops = openamp->platform_ops;
++	rpc_status_t status;
++	int ret;
++
++	(void)opcode;
++
++	if ((handle != openamp) || !opstatus || !resp_buf || !resp_len) {
++		EMSG("openamp: call_invoke: invalid arguments");
++		return TS_RPC_ERROR_INVALID_PARAMETER;
++	}
++
++	if (openamp->status != OPENAMP_TRANSACTION_INPROGRESS) {
++		EMSG("openamp: call_invoke: transaction needed to be started");
++		return TS_RPC_ERROR_NOT_READY;
++	}
++
++	ret = ops->platform_call_invoke(openamp, opstatus, resp_buf, resp_len);
++	if (ret < 0)
++		return TS_RPC_ERROR_INTERNAL;
++
++	openamp->status = OPENAMP_TRANSACTION_INVOKED;
++	*opstatus = 0;
++
++	return TS_RPC_CALL_ACCEPTED;
++}
++
++static void openamp_call_end(void *context, rpc_call_handle handle)
++{
++	struct openamp_caller *openamp = context;
++	const struct openamp_platform_ops *ops = openamp->platform_ops;
++
++	if (handle != openamp) {
++		EMSG("openamp: call_end: invalid arguments");
++		return;
++	}
++
++	if (openamp->status == OPENAMP_TRANSACTION_IDLE) {
++		EMSG("openamp: call_end: transaction idle");
++		return;
++	}
++
++	ops->platform_call_end(openamp);
++
++	openamp->status = OPENAMP_TRANSACTION_IDLE;
++}
++
++static void *openamp_virt_to_phys(void *context, void *va)
++{
++	struct openamp_caller *openamp = context;
++	const struct openamp_platform_ops *ops = openamp->platform_ops;
++
++	return ops->platform_virt_to_phys(openamp, va);
++}
++
++static void *openamp_phys_to_virt(void *context, void *pa)
++{
++	struct openamp_caller *openamp = context;
++	const struct openamp_platform_ops *ops = openamp->platform_ops;
++
++	return ops->platform_phys_to_virt(openamp, pa);
++}
++
++static int openamp_init(struct openamp_caller *openamp)
++{
++	const struct openamp_platform_ops *ops = openamp->platform_ops;
++	int ret;
++
++	ret = ops->transport_init(openamp);
++	if (ret < 0)
++		return ret;
++
++	ret = ops->platform_init(openamp);
++	if (ret < 0)
++		goto denit_transport;
++
++	return 0;
++
++denit_transport:
++	ops->transport_deinit(openamp);
++
++	return ret;
++}
++
++static const struct openamp_platform_ops openamp_virtio_ops = {
++	.transport_init = openamp_mhu_init,
++	.transport_deinit = openamp_mhu_deinit,
++	.transport_notify = openamp_mhu_notify_peer,
++	.transport_receive = openamp_mhu_receive,
++	.platform_init = openamp_virtio_init,
++	.platform_call_begin = openamp_virtio_call_begin,
++	.platform_call_invoke = openamp_virtio_call_invoke,
++	.platform_call_end = openamp_virtio_call_end,
++	.platform_virt_to_phys = openamp_virtio_virt_to_phys,
++	.platform_phys_to_virt = openamp_virtio_phys_to_virt,
++};
++
++struct rpc_caller *openamp_caller_init(struct openamp_caller *openamp)
++{
++	struct rpc_caller *rpc = &openamp->rpc_caller;
++	int ret;
++
++	if (openamp->ref_count)
++		return rpc;
++
++	rpc_caller_init(rpc, openamp);
++
++	rpc->call_begin = openamp_call_begin;
++	rpc->call_invoke = openamp_call_invoke;
++	rpc->call_end = openamp_call_end;
++	rpc->virt_to_phys = openamp_virt_to_phys;
++	rpc->phys_to_virt = openamp_phys_to_virt;
++	openamp->platform_ops = &openamp_virtio_ops;
++
++	ret = openamp_init(openamp);
++	if (ret < 0) {
++		EMSG("openamp_init: failed to start: %d", ret);
++		return rpc;
++	}
++	openamp->ref_count++;
++
++	return rpc;
++}
++
++void openamp_caller_deinit(struct openamp_caller *openamp)
++{
++	struct rpc_caller *rpc = &openamp->rpc_caller;
++
++	if (--openamp->ref_count)
++		return;
++
++	rpc->context = NULL;
++	rpc->call_begin = NULL;
++	rpc->call_invoke = NULL;
++	rpc->call_end = NULL;
++}
++
++int openamp_caller_discover(struct openamp_caller *openamp)
++{
++	return openamp_init(openamp);
++}
++
++int openamp_caller_open(struct openamp_caller *openamp)
++{
++
++}
+diff --git a/components/rpc/openamp/caller/sp/openamp_caller.h b/components/rpc/openamp/caller/sp/openamp_caller.h
+new file mode 100644
+index 00000000..3fb67c56
+--- /dev/null
++++ b/components/rpc/openamp/caller/sp/openamp_caller.h
+@@ -0,0 +1,43 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021, Linaro Limited. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++#ifndef OPENAMP_CALLER_H
++#define OPENAMP_CALLER_H
++
++#include <stddef.h>
++#include <rpc_caller.h>
++
++struct openamp_caller {
++	struct rpc_caller rpc_caller;
++	const struct openamp_platform_ops *platform_ops;
++	uint32_t ref_count;
++	uint8_t status;
++
++	void *transport;
++	void *platform;
++};
++
++struct openamp_platform_ops {
++	int (*transport_init)(struct openamp_caller *openamp);
++	int (*transport_deinit)(struct openamp_caller *openamp);
++	int (*transport_notify)(struct openamp_caller *openamp);
++	int (*transport_receive)(struct openamp_caller *openamp);
++	int (*platform_init)(struct openamp_caller *openamp);
++	int (*platform_deinit)(struct openamp_caller *openamp);
++	int (*platform_call_begin)(struct openamp_caller *openamp,
++				   uint8_t **req_buf, size_t req_len);
++	int (*platform_call_invoke)(struct openamp_caller *openamp,
++				    int *opstatus, uint8_t **resp_buf,
++				    size_t *resp_len);
++	int (*platform_call_end)(struct openamp_caller *openamp);
++	void *(*platform_virt_to_phys)(struct openamp_caller *openamp, void *va);
++	void *(*platform_phys_to_virt)(struct openamp_caller *openamp, void *pa);
++};
++
++struct rpc_caller *openamp_caller_init(struct openamp_caller *openamp);
++void openamp_caller_deinit(struct openamp_caller *openamp);
++
++#endif
+diff --git a/components/rpc/openamp/caller/sp/openamp_mhu.c b/components/rpc/openamp/caller/sp/openamp_mhu.c
+new file mode 100644
+index 00000000..ffdadaf8
+--- /dev/null
++++ b/components/rpc/openamp/caller/sp/openamp_mhu.c
+@@ -0,0 +1,191 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021, Linaro Limited. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#include <config/interface/config_store.h>
++#include <config/interface/config_blob.h>
++#include <platform/interface/device_region.h>
++#include <platform/drivers/arm/mhu_driver/mhu_v2.h>
++#include <trace.h>
++#include <errno.h>
++#include <stdlib.h>
++#include <stdint.h>
++#include <stddef.h>
++#include <limits.h>
++
++#include "openamp_caller.h"
++
++#define MHU_V_2_NOTIFY_CHANNEL	0
++#define MHU_V_2_NOTIFY_VALUE	0xff
++
++struct openamp_mhu {
++	struct device_region rx_region;
++	struct device_region tx_region;
++	struct mhu_v2_x_dev_t rx_dev;
++	struct mhu_v2_x_dev_t tx_dev;
++};
++
++static int openamp_mhu_device_get(const char *dev,
++				  struct device_region *dev_region)
++{
++	bool found;
++
++	found = config_store_query(CONFIG_CLASSIFIER_DEVICE_REGION, dev, 0,
++				   dev_region, sizeof(*dev_region));
++	if (!found)
++		return -EINVAL;
++
++	if (!dev_region->base_addr)
++		return -EINVAL;
++
++	IMSG("mhu: device region found: %s addr: 0x%x size: %d", dev,
++	     dev_region->base_addr, dev_region->io_region_size);
++
++	return 0;
++}
++
++int openamp_mhu_receive(struct openamp_caller *openamp)
++{
++	struct mhu_v2_x_dev_t *rx_dev;
++	enum mhu_v2_x_error_t ret;
++	struct openamp_mhu *mhu;
++	uint32_t channel = 0;
++	uint32_t irq_status;
++
++	if (!openamp->transport) {
++		EMSG("openamp: mhu: receive transport not initialized");
++		return -EINVAL;
++	}
++
++	mhu = openamp->transport;
++	rx_dev = &mhu->rx_dev;
++
++	irq_status = 0;
++
++	do {
++		irq_status = mhu_v2_x_get_interrupt_status(rx_dev);
++	} while(!irq_status);
++
++	ret = mhu_v2_1_get_ch_interrupt_num(rx_dev, &channel);
++
++	ret = mhu_v2_x_channel_clear(rx_dev, channel);
++	if (ret != MHU_V_2_X_ERR_NONE) {
++		EMSG("openamp: mhu: failed to clear channel: %d", channel);
++		return -EPROTO;
++	}
++
++	return 0;
++}
++
++int openamp_mhu_notify_peer(struct openamp_caller *openamp)
++{
++	struct mhu_v2_x_dev_t *tx_dev;
++	enum mhu_v2_x_error_t ret;
++	struct openamp_mhu *mhu;
++	uint32_t access_ready;
++
++	if (!openamp->transport) {
++		EMSG("openamp: mhu: notify transport not initialized");
++		return -EINVAL;
++	}
++
++	mhu = openamp->transport;
++	tx_dev = &mhu->tx_dev;
++
++	ret = mhu_v2_x_set_access_request(tx_dev);
++	if (ret != MHU_V_2_X_ERR_NONE) {
++		EMSG("openamp: mhu: set access request failed");
++		return -EPROTO;
++	}
++
++	do {
++		ret = mhu_v2_x_get_access_ready(tx_dev, &access_ready);
++		if (ret != MHU_V_2_X_ERR_NONE) {
++			EMSG("openamp: mhu: failed to get access_ready");
++			return -EPROTO;
++		}
++	} while (!access_ready);
++
++	ret = mhu_v2_x_channel_send(tx_dev, MHU_V_2_NOTIFY_CHANNEL,
++				    MHU_V_2_NOTIFY_VALUE);
++	if (ret != MHU_V_2_X_ERR_NONE) {
++		EMSG("openamp: mhu: failed send over channel");
++		return -EPROTO;
++	}
++
++	ret = mhu_v2_x_reset_access_request(tx_dev);
++	if (ret != MHU_V_2_X_ERR_NONE) {
++		EMSG("openamp: mhu: failed reset access request");
++		return -EPROTO;
++	}
++
++	return 0;
++}
++
++int openamp_mhu_init(struct openamp_caller *openamp)
++{
++	struct mhu_v2_x_dev_t *rx_dev;
++	struct mhu_v2_x_dev_t *tx_dev;
++	struct openamp_mhu *mhu;
++	int ret;
++
++	/* if we already have initialized skip this */
++	if (openamp->transport)
++		return 0;
++
++	mhu = malloc(sizeof(*mhu));
++	if (!mhu)
++		return -1;
++
++	ret = openamp_mhu_device_get("mhu-sender", &mhu->tx_region);
++	if (ret < 0)
++		goto free_mhu;
++
++	ret = openamp_mhu_device_get("mhu-receiver", &mhu->rx_region);
++	if (ret < 0)
++		goto free_mhu;
++
++	rx_dev = &mhu->rx_dev;
++	tx_dev = &mhu->tx_dev;
++
++	rx_dev->base =  (unsigned int)mhu->rx_region.base_addr;
++	rx_dev->frame = MHU_V2_X_RECEIVER_FRAME;
++
++	tx_dev->base =  (unsigned int)mhu->tx_region.base_addr;
++	tx_dev->frame = MHU_V2_X_SENDER_FRAME;
++
++	ret = mhu_v2_x_driver_init(rx_dev, MHU_REV_READ_FROM_HW);
++	if (ret < 0)
++		goto free_mhu;
++
++	ret = mhu_v2_x_driver_init(tx_dev, MHU_REV_READ_FROM_HW);
++	if (ret < 0)
++		goto free_mhu;
++
++	openamp->transport = (void *)mhu;
++
++	return 0;
++
++free_mhu:
++	free(mhu);
++
++	return ret;
++}
++
++int openamp_mhu_deinit(struct openamp_caller *openamp)
++{
++	struct openamp_mhu *mhu;
++
++	if (!openamp->transport)
++		return 0;
++
++	mhu = openamp->transport;
++	free(mhu);
++
++	openamp->transport = NULL;
++
++	return 0;
++}
+diff --git a/components/rpc/openamp/caller/sp/openamp_mhu.h b/components/rpc/openamp/caller/sp/openamp_mhu.h
+new file mode 100644
+index 00000000..2ae5cb8e
+--- /dev/null
++++ b/components/rpc/openamp/caller/sp/openamp_mhu.h
+@@ -0,0 +1,19 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021, Linaro Limited. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++#ifndef OPENAMP_MHU_H
++#define OPENAMP_MHU_H
++
++#include <stddef.h>
++#include "openamp_caller.h"
++
++int openamp_mhu_init(struct openamp_caller *openamp);
++int openamp_mhu_deinit(struct openamp_caller *openamp);
++
++int openamp_mhu_notify_peer(struct openamp_caller *openamp);
++int openamp_mhu_receive(struct openamp_caller *openamp);
++
++#endif
+diff --git a/components/rpc/openamp/caller/sp/openamp_virtio.c b/components/rpc/openamp/caller/sp/openamp_virtio.c
+new file mode 100644
+index 00000000..06e0735b
+--- /dev/null
++++ b/components/rpc/openamp/caller/sp/openamp_virtio.c
+@@ -0,0 +1,554 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021, Linaro Limited. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#include <metal/device.h>
++#include <metal/spinlock.h>
++#include <openamp/open_amp.h>
++#include <platform/interface/device_region.h>
++#include <config/interface/config_store.h>
++
++#include <stddef.h>
++#include "openamp_caller.h"
++
++#define OPENAMP_SHEM_DEVICE_NAME "openamp-virtio"
++#define OPENAMP_RPMSG_ENDPOINT_NAME OPENAMP_SHEM_DEVICE_NAME
++#define OPENAMP_RPMSG_ENDPOINT_ADDR 1024
++
++#define OPENAMP_SHEM_PHYS 0x88000000
++#define OPENAMP_SHEM_PHYS_PAGES 1
++#define OPENAMP_SHEM_SE_PHYS 0xa8000000
++
++#define OPENAMP_SHEM_VDEV_SIZE (4 * 1024)
++#define OPENAMP_SHEM_VRING_SIZE (4 * 1024)
++
++#define OPENAMP_BUFFER_NO_WAIT  0
++#define OPENAMP_BUFFER_WAIT     1
++
++#define VIRTQUEUE_NR            2
++#define VQ_TX                   0
++#define VQ_RX                   1
++
++#define VRING_DESCRIPTORS       16
++#define VRING_ALIGN             4
++
++#define container_of(ptr, type, member) \
++	((type *)((char *)(ptr) - (unsigned long)(&((type *)0)->member)))
++
++struct openamp_virtio_shm {
++	uintptr_t base_addr;
++	size_t size;
++	uintptr_t vdev_status;
++	size_t vdev_status_size;
++	uintptr_t payload_addr;
++	size_t payload_size;
++	uintptr_t vring_tx;
++	size_t vring_tx_size;
++	uintptr_t vring_rx;
++	size_t vring_rx_size;
++
++	metal_phys_addr_t shm_physmap[OPENAMP_SHEM_PHYS_PAGES];
++};
++
++struct openamp_virtio_metal {
++	struct metal_spinlock lock;
++	struct metal_device shm_dev;
++	struct metal_device *io_dev;
++
++	struct metal_io_region *io;
++	struct openamp_virtio_shm shm;
++};
++
++struct openamp_virtio_device {
++	struct virtio_device virtio_dev;
++	struct virtqueue *vq[VIRTQUEUE_NR];
++	struct virtio_vring_info rvrings[VIRTQUEUE_NR];
++};
++
++struct openamp_virtio_rpmsg {
++	struct rpmsg_virtio_device rpmsg_vdev;
++	struct rpmsg_endpoint ep;
++	uint8_t *req_buf;
++	uint32_t req_len;
++	uint8_t *resp_buf;
++	size_t resp_len;
++};
++
++struct openamp_virtio {
++	struct openamp_caller *openamp;
++	struct openamp_virtio_rpmsg rpmsg;
++	struct openamp_virtio_device vdev;
++	struct openamp_virtio_metal metal;
++};
++
++static struct openamp_virtio *openamp_virtio_from_dev(struct virtio_device *vdev)
++{
++	struct openamp_virtio_device *openamp_vdev;
++
++	openamp_vdev = container_of(vdev, struct openamp_virtio_device,
++			    virtio_dev);
++
++	return container_of(openamp_vdev, struct openamp_virtio, vdev);
++}
++
++static struct openamp_virtio_rpmsg *openamp_virtio_rpmsg_from_dev(struct rpmsg_device *rdev)
++{
++	struct rpmsg_virtio_device *rvdev;
++
++	rvdev = container_of(rdev, struct rpmsg_virtio_device, rdev);
++
++	return container_of(rvdev, struct openamp_virtio_rpmsg, rpmsg_vdev);
++
++}
++
++static void openamp_virtio_metal_device_setup(struct metal_device *shm_dev,
++					      struct openamp_virtio_shm *shm)
++{
++	struct metal_io_region *shm_region;
++
++	shm_region = &shm_dev->regions[0];
++
++	shm_dev->name = OPENAMP_SHEM_DEVICE_NAME;
++	shm_dev->num_regions = 1;
++
++	shm_region->virt = (void *)shm->payload_addr;
++	shm_region->size = shm->payload_size;
++
++	shm_region->physmap = &shm->shm_physmap;
++	shm_region->page_shift = (metal_phys_addr_t)(-1);
++	shm_region->page_mask = (metal_phys_addr_t)(-1);
++}
++
++static int openamp_virtio_metal_init(struct openamp_virtio_metal *metal)
++{
++	struct metal_init_params params = METAL_INIT_DEFAULTS;
++	struct metal_device *shm_dev = &metal->shm_dev;
++	int ret;
++
++	openamp_virtio_metal_device_setup(shm_dev, &metal->shm);
++
++	metal_spinlock_init(&metal->lock);
++
++	ret = metal_init(&params);
++	if (ret < 0)
++		return ret;
++
++	ret = metal_register_generic_device(shm_dev);
++	if (ret < 0)
++		goto metal_finish;
++
++	ret = metal_device_open("generic", OPENAMP_SHEM_DEVICE_NAME,
++				&metal->io_dev);
++	if (ret < 0)
++		goto metal_finish;
++
++	metal->io = metal_device_io_region(metal->io_dev, 0);
++	if (!metal->io) {
++		EMSG("openamp: virtio: failed to init metal io");
++		ret = -EPROTO;
++		goto metal_finish;
++	}
++
++	return 0;
++
++metal_finish:
++	metal_finish();
++	return ret;
++}
++
++static unsigned char openamp_virtio_status_get(struct virtio_device *vdev)
++{
++	struct openamp_virtio *virtio = openamp_virtio_from_dev(vdev);
++	struct openamp_virtio_shm *shm = &virtio->metal.shm;
++
++	uint32_t status = *(volatile uint32_t *)shm->vdev_status;
++
++	return status;
++}
++
++static void openamp_virtio_status_set(struct virtio_device *vdev,
++				      unsigned char status)
++{
++	struct openamp_virtio *virtio = openamp_virtio_from_dev(vdev);
++	struct openamp_virtio_shm *shm = &virtio->metal.shm;
++
++	*(volatile uint32_t *)shm->vdev_status = status;
++}
++
++static int count;
++
++static uint32_t openamp_virtio_features_get(struct virtio_device *vdev)
++{
++	return 1 << VIRTIO_RPMSG_F_NS;
++}
++
++static void openamp_virtio_notify(struct virtqueue *vq)
++{
++	struct openamp_virtio_device *openamp_vdev;
++	struct openamp_caller *openamp;
++	struct openamp_virtio *virtio;
++	int ret;
++
++	openamp_vdev = container_of(vq->vq_dev, struct openamp_virtio_device, virtio_dev);
++	virtio = container_of(openamp_vdev, struct openamp_virtio, vdev);
++	openamp = virtio->openamp;
++
++	ret = openamp->platform_ops->transport_notify(openamp);
++	if (ret < 0)
++		EMSG("openamp: virtio: erro in transport_notify: %d", ret);
++}
++
++const static struct virtio_dispatch openamp_virtio_dispatch = {
++	.get_status = openamp_virtio_status_get,
++	.set_status = openamp_virtio_status_set,
++	.get_features = openamp_virtio_features_get,
++	.notify = openamp_virtio_notify,
++};
++
++static int openamp_virtio_device_setup(struct openamp_virtio *virtio)
++{
++	struct openamp_virtio_metal *metal = &virtio->metal;
++	struct openamp_virtio_device *openamp_vdev = &virtio->vdev;
++	struct virtio_device *vdev = &openamp_vdev->virtio_dev;
++	struct openamp_virtio_shm *shm = &metal->shm;
++	struct virtio_vring_info *rvring;
++
++	rvring = &openamp_vdev->rvrings[0];
++
++	vdev->role = RPMSG_REMOTE;
++	vdev->vrings_num = VIRTQUEUE_NR;
++	vdev->func = &openamp_virtio_dispatch;
++
++	openamp_vdev->vq[VQ_TX] = virtqueue_allocate(VRING_DESCRIPTORS);
++	if (!openamp_vdev->vq[VQ_TX]) {
++		EMSG("openamp: virtio: failed to allocate virtqueue 0");
++		return -ENOMEM;
++	}
++	rvring->io = metal->io;
++	rvring->info.vaddr = (void *)shm->vring_tx;
++	rvring->info.num_descs = VRING_DESCRIPTORS;
++	rvring->info.align = VRING_ALIGN;
++	rvring->vq = openamp_vdev->vq[VQ_TX];
++
++	openamp_vdev->vq[VQ_RX] = virtqueue_allocate(VRING_DESCRIPTORS);
++	if (!openamp_vdev->vq[VQ_RX]) {
++		EMSG("openamp: virtio: failed to allocate virtqueue 1");
++		goto free_vq;
++	}
++	rvring = &openamp_vdev->rvrings[VQ_RX];
++	rvring->io = metal->io;
++	rvring->info.vaddr = (void *)shm->vring_rx;
++	rvring->info.num_descs = VRING_DESCRIPTORS;
++	rvring->info.align = VRING_ALIGN;
++	rvring->vq = openamp_vdev->vq[VQ_RX];
++
++	vdev->vrings_info = &openamp_vdev->rvrings[0];
++
++	return 0;
++
++free_vq:
++	virtqueue_free(openamp_vdev->vq[VQ_TX]);
++	virtqueue_free(openamp_vdev->vq[VQ_RX]);
++
++	return -ENOMEM;
++}
++
++static int openamp_virtio_rpmsg_endpoint_callback(struct rpmsg_endpoint *ep,
++						  void *data, size_t len,
++						  uint32_t src, void *priv)
++{
++	struct openamp_virtio_rpmsg *vrpmsg;
++	struct rpmsg_device *rdev;
++	struct openamp_virtio *virtio;
++
++	rdev = ep->rdev;
++	vrpmsg = openamp_virtio_rpmsg_from_dev(rdev);
++	virtio = container_of(vrpmsg, struct openamp_virtio, rpmsg);
++
++	rpmsg_hold_rx_buffer(ep, data);
++	vrpmsg->resp_buf = data;
++	vrpmsg->resp_len = len;
++
++	return 0;
++}
++
++static void openamp_virtio_rpmsg_service_unbind(struct rpmsg_endpoint *ep)
++{
++	struct openamp_virtio_rpmsg *vrpmsg;
++	struct rpmsg_device *rdev;
++
++	rdev = container_of(ep, struct rpmsg_device, ns_ept);
++	vrpmsg = openamp_virtio_rpmsg_from_dev(rdev);
++
++	rpmsg_destroy_ept(&vrpmsg->ep);
++}
++
++static void openamp_virtio_rpmsg_endpoint_bind(struct rpmsg_device *rdev,
++					       const char *name,
++					       unsigned int dest)
++{
++	struct openamp_virtio_rpmsg *vrpmsg;
++
++	vrpmsg = openamp_virtio_rpmsg_from_dev(rdev);
++
++	rpmsg_create_ept(&vrpmsg->ep, rdev, name, RPMSG_ADDR_ANY, dest,
++			 openamp_virtio_rpmsg_endpoint_callback,
++			 openamp_virtio_rpmsg_service_unbind);
++}
++
++static int openamp_virtio_rpmsg_device_setup(struct openamp_virtio *virtio,
++					     struct device_region *virtio_dev)
++{
++	struct openamp_virtio_rpmsg *vrpmsg = &virtio->rpmsg;
++	struct rpmsg_virtio_device *rpmsg_vdev = &vrpmsg->rpmsg_vdev;
++	struct openamp_virtio_device *openamp_vdev = &virtio->vdev;
++	struct virtio_device *vdev = &openamp_vdev->virtio_dev;
++	struct openamp_virtio_metal *metal = &virtio->metal;
++	int ret;
++
++	/*
++	 * we assume here that we are the client side and do not need to
++	 * initialize the share memory poll (this is done at server side).
++	 */
++	ret = rpmsg_init_vdev(rpmsg_vdev, vdev,
++			      openamp_virtio_rpmsg_endpoint_bind, metal->io,
++			      NULL);
++	if (ret < 0) {
++		EMSG("openamp: virtio: init vdev failed: %d", ret);
++		return ret;
++	}
++
++
++	ret = rpmsg_create_ept(&vrpmsg->ep, &rpmsg_vdev->rdev,
++			       OPENAMP_RPMSG_ENDPOINT_NAME, RPMSG_ADDR_ANY,
++			       RPMSG_ADDR_ANY,
++			       openamp_virtio_rpmsg_endpoint_callback,
++			       openamp_virtio_rpmsg_service_unbind);
++	if (ret < 0) {
++		EMSG("openamp: virtio: failed to create endpoint: %d", ret);
++		return ret;
++	}
++
++	/* set default remote addr */
++	vrpmsg->ep.dest_addr = OPENAMP_RPMSG_ENDPOINT_ADDR;
++
++	return 0;
++}
++
++static void openamp_virtio_shm_set(struct openamp_virtio *virtio,
++				   struct device_region *virtio_region)
++{
++	struct openamp_virtio_shm *shm = &virtio->metal.shm;
++
++	shm->base_addr = virtio_region->base_addr;
++	shm->size = virtio_region->io_region_size;
++
++	shm->vdev_status = shm->base_addr;
++	shm->vdev_status_size = OPENAMP_SHEM_VDEV_SIZE;
++
++	shm->vring_rx = shm->base_addr + shm->size -
++		(2 * OPENAMP_SHEM_VRING_SIZE);
++	shm->vring_rx_size = OPENAMP_SHEM_VRING_SIZE;
++
++	shm->vring_tx = shm->vring_rx + shm->vring_rx_size;
++	shm->vring_tx_size = OPENAMP_SHEM_VRING_SIZE;
++
++	shm->payload_addr = shm->vdev_status + shm->vdev_status_size;
++	shm->payload_size = shm->size - shm->vdev_status_size -
++		shm->vring_rx_size - shm->vring_tx_size;
++
++	shm->shm_physmap[0] = OPENAMP_SHEM_PHYS + shm->vdev_status_size;
++
++	IMSG("SHEM: base: 0x%0x size: 0x%0x size: %d",
++	     shm->base_addr, shm->size, shm->size);
++	IMSG("VDEV: base: 0x%0x size: 0x%0x size: %d",
++	     shm->vdev_status, shm->vdev_status_size, shm->vdev_status_size);
++	IMSG("PAYLOAD: base: 0x%0x size: 0x%0x size: %d",
++	     shm->payload_addr, shm->payload_size, shm->payload_size);
++	IMSG("VRING_TX: base: 0x%0x size: 0x%0x size: %d",
++	     shm->vring_tx, shm->vring_tx_size, shm->vring_tx_size);
++	IMSG("VRING_RX: base: 0x%0x size: 0x%0x size: %d",
++	     shm->vring_rx, shm->vring_rx_size, shm->vring_rx_size);
++	IMSG("PHYMAP: base: 0x%0x", shm->shm_physmap[0]);
++}
++
++static int openamp_virtio_device_get(const char *dev,
++				     struct device_region *dev_region)
++{
++	bool found;
++
++	found = config_store_query(CONFIG_CLASSIFIER_DEVICE_REGION, dev, 0,
++				   dev_region, sizeof(*dev_region));
++	if (!found) {
++		EMSG("openamp: virtio: device region not found: %s", dev);
++		return -EINVAL;
++	}
++
++	if (dev_region->base_addr == 0 || dev_region->io_region_size == 0) {
++		EMSG("openamp: virtio: device region not valid");
++		return -EINVAL;
++	}
++
++	IMSG("openamp: virtio: device region found: %s addr: 0x%x size: %d",
++	     dev, dev_region->base_addr, dev_region->io_region_size);
++
++	return  0;
++}
++
++int openamp_virtio_call_begin(struct openamp_caller *openamp, uint8_t **req_buf,
++			      size_t req_len)
++{
++	struct openamp_virtio *virtio = openamp->platform;
++	struct openamp_virtio_rpmsg *vrpmsg = &virtio->rpmsg;
++	struct rpmsg_endpoint *ep = &vrpmsg->ep;
++
++
++	*req_buf = rpmsg_get_tx_payload_buffer(ep, &vrpmsg->req_len,
++					       OPENAMP_BUFFER_WAIT);
++	if (*req_buf == NULL)
++		return -EINVAL;
++
++	if (vrpmsg->req_len < req_len)
++		return -E2BIG;
++
++	vrpmsg->req_buf = *req_buf;
++
++	return 0;
++}
++
++int openamp_virtio_call_invoke(struct openamp_caller *openamp, int *opstatus,
++			       uint8_t **resp_buf, size_t *resp_len)
++{
++	const struct openamp_platform_ops *ops = openamp->platform_ops;
++	struct openamp_virtio *virtio = openamp->platform;
++	struct openamp_virtio_device *openamp_vdev = &virtio->vdev;
++	struct openamp_virtio_rpmsg *vrpmsg = &virtio->rpmsg;
++	struct rpmsg_endpoint *ep = &vrpmsg->ep;
++	int ret;
++
++	ret = rpmsg_send_nocopy(ep, vrpmsg->req_buf, vrpmsg->req_len);
++	if (ret < 0) {
++		EMSG("openamp: virtio: send nocopy failed: %d", ret);
++		return -EIO;
++	}
++
++	if (ret != vrpmsg->req_len) {
++		EMSG("openamp: virtio: send less bytes %d than requested %d",
++		     ret, vrpmsg->req_len);
++		return -EIO;
++	}
++
++	if (!ops->transport_receive)
++		return 0;
++
++	ret = ops->transport_receive(openamp);
++	if (ret < 0) {
++		EMSG("openamp: virtio: failed transport_receive");
++		return -EIO;
++	}
++
++	virtqueue_notification(openamp_vdev->vq[VQ_RX]);
++
++	*resp_buf = vrpmsg->resp_buf;
++	*resp_len = vrpmsg->resp_len;
++
++	return  0;
++}
++
++void openamp_virtio_call_end(struct openamp_caller *openamp)
++{
++	struct openamp_virtio *virtio = openamp->platform;
++	struct openamp_virtio_rpmsg *vrpmsg = &virtio->rpmsg;
++
++	rpmsg_release_rx_buffer(&vrpmsg->ep, vrpmsg->resp_buf);
++
++	vrpmsg->req_buf = NULL;
++	vrpmsg->req_len = 0;
++	vrpmsg->resp_buf = NULL;
++	vrpmsg->resp_len = 0;
++}
++
++void *openamp_virtio_virt_to_phys(struct openamp_caller *openamp, void *va)
++{
++	struct openamp_virtio *virtio = openamp->platform;
++	struct openamp_virtio_metal *metal = &virtio->metal;
++
++	return metal_io_virt_to_phys(metal->io, va);
++}
++
++void *openamp_virtio_phys_to_virt(struct openamp_caller *openamp, void *pa)
++{
++	struct openamp_virtio *virtio = openamp->platform;
++	struct openamp_virtio_metal *metal = &virtio->metal;
++
++	return metal_io_phys_to_virt(metal->io, pa);
++}
++
++int openamp_virtio_init(struct openamp_caller *openamp)
++{
++	struct device_region virtio_dev;
++	struct openamp_virtio *virtio;
++	int ret;
++
++	if (openamp->platform)
++		return 0;
++
++
++	virtio = malloc(sizeof(*virtio));
++	if (!virtio)
++		return -ENOMEM;
++
++	virtio->openamp = openamp;
++
++	ret = openamp_virtio_device_get(OPENAMP_SHEM_DEVICE_NAME, &virtio_dev);
++	if (ret < 0)
++		goto free_virtio;
++
++	openamp_virtio_shm_set(virtio, &virtio_dev);
++
++	ret = openamp_virtio_metal_init(&virtio->metal);
++	if (ret < 0)
++		goto free_virtio;
++
++	ret = openamp_virtio_device_setup(virtio);
++	if (ret < 0)
++		goto finish_metal;
++
++	ret = openamp_virtio_rpmsg_device_setup(virtio, &virtio_dev);
++	if (ret < 0) {
++		EMSG("openamp: virtio: rpmsg device setup failed: %d", ret);
++		goto finish_metal;
++	}
++
++	openamp->platform = virtio;
++
++	return 0;
++
++finish_metal:
++	metal_finish();
++
++free_virtio:
++	free(virtio);
++
++	return ret;
++}
++
++int openamp_virtio_deinit(struct openamp_caller *openamp)
++{
++	struct openamp_virtio *virtio;
++
++	if (!openamp->platform)
++		return 0;
++
++	virtio = openamp->platform;
++
++	metal_finish();
++	free(virtio);
++
++	openamp->platform = NULL;
++
++	return 0;
++}
+diff --git a/components/rpc/openamp/caller/sp/openamp_virtio.h b/components/rpc/openamp/caller/sp/openamp_virtio.h
+new file mode 100644
+index 00000000..915128ff
+--- /dev/null
++++ b/components/rpc/openamp/caller/sp/openamp_virtio.h
+@@ -0,0 +1,24 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021, Linaro Limited. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++#ifndef OPENAMP_VIRTIO_H
++#define OPENAMP_VIRTIO_H
++
++#include <stddef.h>
++#include "openamp_caller.h"
++
++int openamp_virtio_call_begin(struct openamp_caller *openamp, uint8_t **req_buf,
++			      size_t req_len);
++int openamp_virtio_call_invoke(struct openamp_caller *openamp, int *opstatus,
++			       uint8_t **resp_buf, size_t *resp_len);
++int openamp_virtio_call_end(struct openamp_caller *openamp);
++void *openamp_virtio_virt_to_phys(struct openamp_caller *openamp, void *va);
++void *openamp_virtio_phys_to_virt(struct openamp_caller *openamp, void *pa);
++
++int openamp_virtio_init(struct openamp_caller *openamp);
++int openamp_virtio_deinit(struct openamp_caller *openamp);
++
++#endif
+diff --git a/deployments/se-proxy/opteesp/CMakeLists.txt b/deployments/se-proxy/opteesp/CMakeLists.txt
+index 248bd7e3..1511bbad 100644
+--- a/deployments/se-proxy/opteesp/CMakeLists.txt
++++ b/deployments/se-proxy/opteesp/CMakeLists.txt
+@@ -75,6 +75,7 @@ add_components(TARGET "se-proxy"
+ 		"components/service/attestation/include"
+ 		"components/service/attestation/provider"
+ 		"components/service/attestation/provider/serializer/packed-c"
++		"components/rpc/openamp/caller/sp"
+ 
+ 		# Stub service provider backends
+ 		"components/rpc/dummy"
+diff --git a/deployments/se-proxy/opteesp/default_se-proxy.dts.in b/deployments/se-proxy/opteesp/default_se-proxy.dts.in
+index f351a592..55d49b31 100644
+--- a/deployments/se-proxy/opteesp/default_se-proxy.dts.in
++++ b/deployments/se-proxy/opteesp/default_se-proxy.dts.in
+@@ -32,6 +32,12 @@
+ 				pages-count = <16>;
+ 				attributes = <0x3>; /* read-write */
+ 			};
++			openamp-virtio {
++				/* Armv8 A Foundation Platform values */
++				base-address = <0x00000000 0x88000000>;
++				pages-count = <256>;
++				attributes = <0x3>; /* read-write */
++			};
+ 		};
+ 	};
+ };
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0010-add-psa-client-definitions-for-ff-m.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0010-add-psa-client-definitions-for-ff-m.patch
new file mode 100644
index 0000000..f41556a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0010-add-psa-client-definitions-for-ff-m.patch
@@ -0,0 +1,299 @@
+From 791a1302d7b779f3aeee7d6f7c9fac00b4244c1b Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 3 Dec 2021 19:05:18 +0000
+Subject: [PATCH] add psa client definitions for ff-m
+
+Add PSA client definitions in common include to add future
+ff-m support.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../service/common/include/psa/client.h       | 194 ++++++++++++++++++
+ components/service/common/include/psa/sid.h   |  71 +++++++
+ 2 files changed, 265 insertions(+)
+ create mode 100644 components/service/common/include/psa/client.h
+ create mode 100644 components/service/common/include/psa/sid.h
+
+diff --git a/components/service/common/include/psa/client.h b/components/service/common/include/psa/client.h
+new file mode 100644
+index 00000000..69ccf14f
+--- /dev/null
++++ b/components/service/common/include/psa/client.h
+@@ -0,0 +1,194 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef SERVICE_PSA_IPC_H
++#define SERVICE_PSA_IPC_H
++
++#include <stddef.h>
++#include <stdint.h>
++
++#include <rpc_caller.h>
++#include <psa/error.h>
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++#ifndef IOVEC_LEN
++#define IOVEC_LEN(arr) ((uint32_t)(sizeof(arr)/sizeof(arr[0])))
++#endif
++
++/*********************** PSA Client Macros and Types *************************/
++
++typedef int32_t psa_handle_t;
++
++/**
++ * The version of the PSA Framework API that is being used to build the calling
++ * firmware. Only part of features of FF-M v1.1 have been implemented. FF-M v1.1
++ * is compatible with v1.0.
++ */
++#define PSA_FRAMEWORK_VERSION       (0x0101u)
++
++/**
++ * Return value from psa_version() if the requested RoT Service is not present
++ * in the system.
++ */
++#define PSA_VERSION_NONE            (0u)
++
++/**
++ * The zero-value null handle can be assigned to variables used in clients and
++ * RoT Services, indicating that there is no current connection or message.
++ */
++#define PSA_NULL_HANDLE             ((psa_handle_t)0)
++
++/**
++ * Tests whether a handle value returned by psa_connect() is valid.
++ */
++#define PSA_HANDLE_IS_VALID(handle) ((psa_handle_t)(handle) > 0)
++
++/**
++ * Converts the handle value returned from a failed call psa_connect() into
++ * an error code.
++ */
++#define PSA_HANDLE_TO_ERROR(handle) ((psa_status_t)(handle))
++
++/**
++ * Maximum number of input and output vectors for a request to psa_call().
++ */
++#define PSA_MAX_IOVEC               (4u)
++
++/**
++ * An IPC message type that indicates a generic client request.
++ */
++#define PSA_IPC_CALL                (0)
++
++/**
++ * A read-only input memory region provided to an RoT Service.
++ */
++struct __attribute__ ((__packed__)) psa_invec {
++    uint32_t base;           /*!< the start address of the memory buffer */
++    uint32_t len;                 /*!< the size in bytes                      */
++};
++
++/**
++ * A writable output memory region provided to an RoT Service.
++ */
++struct __attribute__ ((__packed__)) psa_outvec {
++    uint32_t base;                 /*!< the start address of the memory buffer */
++    uint32_t len;                 /*!< the size in bytes                      */
++};
++
++/*************************** PSA Client API **********************************/
++
++/**
++ * \brief Retrieve the version of the PSA Framework API that is implemented.
++ *
++ * \param[in] rpc_caller        RPC caller to use
++ * \return version              The version of the PSA Framework implementation
++ *                              that is providing the runtime services to the
++ *                              caller. The major and minor version are encoded
++ *                              as follows:
++ * \arg                           version[15:8] -- major version number.
++ * \arg                           version[7:0]  -- minor version number.
++ */
++uint32_t psa_framework_version(struct rpc_caller *caller);
++
++/**
++ * \brief Retrieve the version of an RoT Service or indicate that it is not
++ *        present on this system.
++ *
++ * \param[in] rpc_caller        RPC caller to use
++ * \param[in] sid               ID of the RoT Service to query.
++ *
++ * \retval PSA_VERSION_NONE     The RoT Service is not implemented, or the
++ *                              caller is not permitted to access the service.
++ * \retval > 0                  The version of the implemented RoT Service.
++ */
++uint32_t psa_version(struct rpc_caller *caller, uint32_t sid);
++
++/**
++ * \brief Connect to an RoT Service by its SID.
++ *
++ * \param[in] rpc_caller        RPC caller to use
++ * \param[in] sid               ID of the RoT Service to connect to.
++ * \param[in] version           Requested version of the RoT Service.
++ *
++ * \retval > 0                  A handle for the connection.
++ * \retval PSA_ERROR_CONNECTION_REFUSED The SPM or RoT Service has refused the
++ *                              connection.
++ * \retval PSA_ERROR_CONNECTION_BUSY The SPM or RoT Service cannot make the
++ *                              connection at the moment.
++ * \retval "PROGRAMMER ERROR"   The call is a PROGRAMMER ERROR if one or more
++ *                              of the following are true:
++ * \arg                           The RoT Service ID is not present.
++ * \arg                           The RoT Service version is not supported.
++ * \arg                           The caller is not allowed to access the RoT
++ *                                service.
++ */
++psa_handle_t psa_connect(struct rpc_caller *caller, uint32_t sid,
++			 uint32_t version);
++
++/**
++ * \brief Call an RoT Service on an established connection.
++ *
++ * \note  FF-M 1.0 proposes 6 parameters for psa_call but the secure gateway ABI
++ *        support at most 4 parameters. TF-M chooses to encode 'in_len',
++ *        'out_len', and 'type' into a 32-bit integer to improve efficiency.
++ *        Compared with struct-based encoding, this method saves extra memory
++ *        check and memory copy operation. The disadvantage is that the 'type'
++ *        range has to be reduced into a 16-bit integer. So with this encoding,
++ *        the valid range for 'type' is 0-32767.
++ *
++ * \param[in] rpc_caller        RPC caller to use
++ * \param[in] handle            A handle to an established connection.
++ * \param[in] type              The request type.
++ *                              Must be zero( \ref PSA_IPC_CALL) or positive.
++ * \param[in] in_vec            Array of input \ref psa_invec structures.
++ * \param[in] in_len            Number of input \ref psa_invec structures.
++ * \param[in,out] out_vec       Array of output \ref psa_outvec structures.
++ * \param[in] out_len           Number of output \ref psa_outvec structures.
++ *
++ * \retval >=0                  RoT Service-specific status value.
++ * \retval <0                   RoT Service-specific error code.
++ * \retval PSA_ERROR_PROGRAMMER_ERROR The connection has been terminated by the
++ *                              RoT Service. The call is a PROGRAMMER ERROR if
++ *                              one or more of the following are true:
++ * \arg                           An invalid handle was passed.
++ * \arg                           The connection is already handling a request.
++ * \arg                           type < 0.
++ * \arg                           An invalid memory reference was provided.
++ * \arg                           in_len + out_len > PSA_MAX_IOVEC.
++ * \arg                           The message is unrecognized by the RoT
++ *                                Service or incorrectly formatted.
++ */
++psa_status_t psa_call(struct rpc_caller *caller, psa_handle_t handle,
++		      int32_t type, const struct psa_invec *in_vec,
++		      size_t in_len, struct psa_outvec *out_vec, size_t out_len);
++
++/**
++ * \brief Close a connection to an RoT Service.
++ *
++ * \param[in] rpc_caller        RPC caller to use
++ * \param[in] handle            A handle to an established connection, or the
++ *                              null handle.
++ *
++ * \retval void                 Success.
++ * \retval "PROGRAMMER ERROR"   The call is a PROGRAMMER ERROR if one or more
++ *                              of the following are true:
++ * \arg                           An invalid handle was provided that is not
++ *                                the null handle.
++ * \arg                           The connection is currently handling a
++ *                                request.
++ */
++void psa_close(struct rpc_caller *caller, psa_handle_t handle);
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* SERVICE_PSA_IPC_H */
++
++
+diff --git a/components/service/common/include/psa/sid.h b/components/service/common/include/psa/sid.h
+new file mode 100644
+index 00000000..aaa973c6
+--- /dev/null
++++ b/components/service/common/include/psa/sid.h
+@@ -0,0 +1,71 @@
++/*
++ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ *
++ */
++
++#ifndef __PSA_MANIFEST_SID_H__
++#define __PSA_MANIFEST_SID_H__
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++/******** TFM_SP_PS ********/
++#define TFM_PROTECTED_STORAGE_SERVICE_SID                          (0x00000060U)
++#define TFM_PROTECTED_STORAGE_SERVICE_VERSION                      (1U)
++#define TFM_PROTECTED_STORAGE_SERVICE_HANDLE                       (0x40000101U)
++
++/* Invalid UID */
++#define TFM_PS_INVALID_UID 0
++
++/* PS message types that distinguish PS services. */
++#define TFM_PS_SET                1001
++#define TFM_PS_GET                1002
++#define TFM_PS_GET_INFO           1003
++#define TFM_PS_REMOVE             1004
++#define TFM_PS_GET_SUPPORT        1005
++
++/******** TFM_SP_ITS ********/
++#define TFM_INTERNAL_TRUSTED_STORAGE_SERVICE_SID                   (0x00000070U)
++#define TFM_INTERNAL_TRUSTED_STORAGE_SERVICE_VERSION               (1U)
++#define TFM_INTERNAL_TRUSTED_STORAGE_SERVICE_HANDLE                (0x40000102U)
++
++/******** TFM_SP_CRYPTO ********/
++#define TFM_CRYPTO_SID                                             (0x00000080U)
++#define TFM_CRYPTO_VERSION                                         (1U)
++#define TFM_CRYPTO_HANDLE                                          (0x40000100U)
++
++/******** TFM_SP_PLATFORM ********/
++#define TFM_SP_PLATFORM_SYSTEM_RESET_SID                           (0x00000040U)
++#define TFM_SP_PLATFORM_SYSTEM_RESET_VERSION                       (1U)
++#define TFM_SP_PLATFORM_IOCTL_SID                                  (0x00000041U)
++#define TFM_SP_PLATFORM_IOCTL_VERSION                              (1U)
++#define TFM_SP_PLATFORM_NV_COUNTER_SID                             (0x00000042U)
++#define TFM_SP_PLATFORM_NV_COUNTER_VERSION                         (1U)
++
++/******** TFM_SP_INITIAL_ATTESTATION ********/
++#define TFM_ATTESTATION_SERVICE_SID                                (0x00000020U)
++#define TFM_ATTESTATION_SERVICE_VERSION                            (1U)
++#define TFM_ATTESTATION_SERVICE_HANDLE                             (0x40000103U)
++
++/******** TFM_SP_FWU ********/
++#define TFM_FWU_WRITE_SID                                          (0x000000A0U)
++#define TFM_FWU_WRITE_VERSION                                      (1U)
++#define TFM_FWU_INSTALL_SID                                        (0x000000A1U)
++#define TFM_FWU_INSTALL_VERSION                                    (1U)
++#define TFM_FWU_ABORT_SID                                          (0x000000A2U)
++#define TFM_FWU_ABORT_VERSION                                      (1U)
++#define TFM_FWU_QUERY_SID                                          (0x000000A3U)
++#define TFM_FWU_QUERY_VERSION                                      (1U)
++#define TFM_FWU_REQUEST_REBOOT_SID                                 (0x000000A4U)
++#define TFM_FWU_REQUEST_REBOOT_VERSION                             (1U)
++#define TFM_FWU_ACCEPT_SID                                         (0x000000A5U)
++#define TFM_FWU_ACCEPT_VERSION                                     (1U)
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* __PSA_MANIFEST_SID_H__ */
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0011-Add-common-service-component-to-ipc-support.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0011-Add-common-service-component-to-ipc-support.patch
new file mode 100644
index 0000000..7ecb60f
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0011-Add-common-service-component-to-ipc-support.patch
@@ -0,0 +1,295 @@
+From b7e9e6fc59263f5daf4ae79eb758fa7647058338 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 3 Dec 2021 19:13:03 +0000
+Subject: [PATCH] Add common service component to ipc support
+
+Add support for inter processor communication for PSA
+including, the openamp client side structures lib.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../service/common/psa_ipc/component.cmake    |  13 ++
+ .../service/common/psa_ipc/service_psa_ipc.c  |  97 +++++++++++++
+ .../psa_ipc/service_psa_ipc_openamp_lib.h     | 131 ++++++++++++++++++
+ deployments/se-proxy/opteesp/CMakeLists.txt   |   1 +
+ 4 files changed, 242 insertions(+)
+ create mode 100644 components/service/common/psa_ipc/component.cmake
+ create mode 100644 components/service/common/psa_ipc/service_psa_ipc.c
+ create mode 100644 components/service/common/psa_ipc/service_psa_ipc_openamp_lib.h
+
+diff --git a/components/service/common/psa_ipc/component.cmake b/components/service/common/psa_ipc/component.cmake
+new file mode 100644
+index 00000000..5a1c9e62
+--- /dev/null
++++ b/components/service/common/psa_ipc/component.cmake
+@@ -0,0 +1,13 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++if (NOT DEFINED TGT)
++	message(FATAL_ERROR "mandatory parameter TGT is not defined.")
++endif()
++
++target_sources(${TGT} PRIVATE
++	"${CMAKE_CURRENT_LIST_DIR}/service_psa_ipc.c"
++	)
+diff --git a/components/service/common/psa_ipc/service_psa_ipc.c b/components/service/common/psa_ipc/service_psa_ipc.c
+new file mode 100644
+index 00000000..e8093c20
+--- /dev/null
++++ b/components/service/common/psa_ipc/service_psa_ipc.c
+@@ -0,0 +1,97 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#include <stddef.h>
++#include <stdint.h>
++#include <string.h>
++#include <trace.h>
++
++#include <protocols/rpc/common/packed-c/status.h>
++#include <psa/error.h>
++#include <rpc_caller.h>
++
++#include <psa/client.h>
++#include "service_psa_ipc_openamp_lib.h"
++
++psa_handle_t psa_connect(struct rpc_caller *caller, uint32_t sid,
++			 uint32_t version)
++{
++	psa_status_t psa_status = PSA_SUCCESS;
++	struct s_openamp_msg *resp_msg = NULL;
++	struct ns_openamp_msg *req_msg;
++	rpc_call_handle rpc_handle;
++	size_t resp_len;
++	uint8_t *resp;
++	uint8_t *req;
++	int ret;
++
++	rpc_handle = rpc_caller_begin(caller, &req,
++				      sizeof(struct ns_openamp_msg));
++	if (!rpc_handle) {
++		EMSG("psa_connect: could not get handle");
++		return PSA_ERROR_GENERIC_ERROR;
++	}
++
++	req_msg = (struct ns_openamp_msg *)req;
++
++	req_msg->call_type = OPENAMP_PSA_CONNECT;
++	req_msg->params.psa_connect_params.sid = sid;
++	req_msg->params.psa_connect_params.version = version;
++
++	ret = rpc_caller_invoke(caller, rpc_handle, 0, &psa_status, &resp,
++				&resp_len);
++	if (ret != TS_RPC_CALL_ACCEPTED) {
++		EMSG("psa_connect: invoke failed: %d", ret);
++		return PSA_ERROR_GENERIC_ERROR;
++	}
++
++	if (psa_status == PSA_SUCCESS)
++		resp_msg = (struct s_openamp_msg *)resp;
++
++	rpc_caller_end(caller, rpc_handle);
++
++	return resp_msg ? (psa_handle_t)resp_msg->reply : PSA_NULL_HANDLE;
++}
++
++psa_status_t psa_call(struct rpc_caller *caller, psa_handle_t handle,
++		      int32_t type, const struct psa_invec *in_vec,
++		      size_t in_len, struct psa_outvec *out_vec, size_t out_len)
++{
++
++}
++
++void psa_close(struct rpc_caller *caller, psa_handle_t handle)
++{
++	psa_status_t psa_status = PSA_SUCCESS;
++	struct s_openamp_msg *resp_msg = NULL;
++	struct ns_openamp_msg *req_msg;
++	rpc_call_handle rpc_handle;
++	size_t resp_len;
++	uint8_t *resp;
++	uint8_t *req;
++	int ret;
++
++	rpc_handle = rpc_caller_begin(caller, &req,
++				      sizeof(struct ns_openamp_msg));
++	if (!rpc_handle) {
++		EMSG("psa_close: could not get handle");
++		return;
++	}
++
++	req_msg = (struct ns_openamp_msg *)req;
++
++	req_msg->call_type = OPENAMP_PSA_CLOSE;
++	req_msg->params.psa_close_params.handle = handle;
++
++	ret = rpc_caller_invoke(caller, rpc_handle, 0, &psa_status, &resp,
++				&resp_len);
++	if (ret != TS_RPC_CALL_ACCEPTED) {
++		EMSG("psa_close: invoke failed: %d", ret);
++		return;
++	}
++
++	rpc_caller_end(caller, rpc_handle);
++}
+diff --git a/components/service/common/psa_ipc/service_psa_ipc_openamp_lib.h b/components/service/common/psa_ipc/service_psa_ipc_openamp_lib.h
+new file mode 100644
+index 00000000..33ea9666
+--- /dev/null
++++ b/components/service/common/psa_ipc/service_psa_ipc_openamp_lib.h
+@@ -0,0 +1,131 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef SERVICE_PSA_IPC_OPENAMP_LIB_H
++#define SERVICE_PSA_IPC_OPENAMP_LIB_H
++
++#include <stddef.h>
++#include <stdint.h>
++
++#include <compiler.h>
++#include <psa/error.h>
++
++#include <stdint.h>
++#include <psa/client.h>
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++/* PSA client call type value */
++#define OPENAMP_PSA_FRAMEWORK_VERSION       (0x1)
++#define OPENAMP_PSA_VERSION                 (0x2)
++#define OPENAMP_PSA_CONNECT                 (0x3)
++#define OPENAMP_PSA_CALL                    (0x4)
++#define OPENAMP_PSA_CLOSE                   (0x5)
++
++/* Return code of openamp APIs */
++#define OPENAMP_SUCCESS                     (0)
++#define OPENAMP_MAP_FULL                    (INT32_MIN + 1)
++#define OPENAMP_MAP_ERROR                   (INT32_MIN + 2)
++#define OPENAMP_INVAL_PARAMS                (INT32_MIN + 3)
++#define OPENAMP_NO_PERMS                    (INT32_MIN + 4)
++#define OPENAMP_NO_PEND_EVENT               (INT32_MIN + 5)
++#define OPENAMP_CHAN_BUSY                   (INT32_MIN + 6)
++#define OPENAMP_CALLBACK_REG_ERROR          (INT32_MIN + 7)
++#define OPENAMP_INIT_ERROR                  (INT32_MIN + 8)
++
++#define HOLD_INPUT_BUFFER (1) /* IF true, TF-M Library will hold the openamp
++			       * buffer so that openamp shared memory buffer
++			       * does not get freed.
++			       */
++
++/*
++ * This structure holds the parameters used in a PSA client call.
++ */
++typedef struct __packed psa_client_in_params {
++	union {
++		struct __packed {
++			uint32_t        sid;
++		} psa_version_params;
++
++		struct __packed {
++			uint32_t        sid;
++			uint32_t        version;
++		} psa_connect_params;
++
++		struct __packed {
++			psa_handle_t     handle;
++			int32_t          type;
++			uint32_t         in_vec;
++			uint32_t         in_len;
++			uint32_t         out_vec;
++			uint32_t         out_len;
++		} psa_call_params;
++
++		struct __packed {
++			psa_handle_t    handle;
++		} psa_close_params;
++	};
++} psa_client_in_params_t;
++
++/* Openamp message passed from NSPE to SPE to deliver a PSA client call */
++struct __packed ns_openamp_msg {
++	uint32_t                      call_type;   /* PSA client call type */
++	struct psa_client_in_params   params;      /* Contain parameters used in PSA
++						  * client call
++						  */
++
++	int32_t                     client_id;   /* Optional client ID of the
++						  * non-secure caller.
++						  * It is required to identify the
++						  * non-secure task when NSPE OS
++						  * enforces non-secure task
++						  * isolation
++						  */
++	int32_t                     request_id;  /* This is the unique ID for a
++						  * request send to TF-M by the
++						  * non-secure core. TF-M forward
++						  * the ID back to non-secure on the
++						  * reply to a given request. Using
++						  * this id, the non-secure library
++						  * can identify the request for
++						  * which the reply has received.
++						  */
++};
++
++/*
++ * This structure holds the location of the out data of the PSA client call.
++ */
++struct __packed psa_client_out_params {
++	uint32_t              out_vec;
++	uint32_t              out_len;
++};
++
++
++/* Openamp message from SPE to NSPE delivering the reply back for a PSA client
++ * call.
++ */
++struct __packed s_openamp_msg {
++	int32_t                     request_id;  /* Using this id, the non-secure
++						  * library identifies the request.
++						  * TF-M forwards the same
++						  * request-id received on the
++						  * initial request.
++						  */
++	int32_t                     reply;       /* Reply of the PSA client call */
++	struct psa_client_out_params     params;      /* Contain out data result of the
++						       * PSA client call.
++						       */
++};
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* SERVICE_PSA_IPC_OPENAMP_LIB_H */
++
++
+diff --git a/deployments/se-proxy/opteesp/CMakeLists.txt b/deployments/se-proxy/opteesp/CMakeLists.txt
+index 1511bbad..e0e0e12b 100644
+--- a/deployments/se-proxy/opteesp/CMakeLists.txt
++++ b/deployments/se-proxy/opteesp/CMakeLists.txt
+@@ -54,6 +54,7 @@ add_components(TARGET "se-proxy"
+ 		"components/service/common/include"
+ 		"components/service/common/serializer/protobuf"
+ 		"components/service/common/client"
++		"components/service/common/psa_ipc"
+ 		"components/service/common/provider"
+ 		"components/service/discovery/provider"
+ 		"components/service/discovery/provider/serializer/packed-c"
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0012-Add-secure-storage-ipc-backend.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0012-Add-secure-storage-ipc-backend.patch
new file mode 100644
index 0000000..068468b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0012-Add-secure-storage-ipc-backend.patch
@@ -0,0 +1,523 @@
+From 962056a9c8115e9228719d46b09da983678ab024 Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 3 Dec 2021 19:19:24 +0000
+Subject: [PATCH] Add secure storage ipc backend
+
+Add secure storage ipc ff-m implementation which may use
+openamp as rpc to communicate with other processor.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../service/common/psa_ipc/service_psa_ipc.c  | 143 +++++++++++-
+ .../secure_storage_ipc/component.cmake        |  14 ++
+ .../secure_storage_ipc/secure_storage_ipc.c   | 214 ++++++++++++++++++
+ .../secure_storage_ipc/secure_storage_ipc.h   |  52 +++++
+ deployments/se-proxy/opteesp/CMakeLists.txt   |   1 +
+ 5 files changed, 420 insertions(+), 4 deletions(-)
+ create mode 100644 components/service/secure_storage/backend/secure_storage_ipc/component.cmake
+ create mode 100644 components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
+ create mode 100644 components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.h
+
+diff --git a/components/service/common/psa_ipc/service_psa_ipc.c b/components/service/common/psa_ipc/service_psa_ipc.c
+index e8093c20..95a07c13 100644
+--- a/components/service/common/psa_ipc/service_psa_ipc.c
++++ b/components/service/common/psa_ipc/service_psa_ipc.c
+@@ -16,6 +16,52 @@
+ #include <psa/client.h>
+ #include "service_psa_ipc_openamp_lib.h"
+ 
++static struct psa_invec *psa_call_in_vec_param(uint8_t *req)
++{
++	return (struct psa_invec *)(req + sizeof(struct ns_openamp_msg));
++}
++
++static struct psa_outvec *psa_call_out_vec_param(uint8_t *req, size_t in_len)
++{
++	return (struct psa_outvec *)(req + sizeof(struct ns_openamp_msg) +
++				     (in_len * sizeof(struct psa_invec)));
++}
++
++static size_t psa_call_header_len(const struct psa_invec *in_vec, size_t in_len,
++				  struct psa_outvec *out_vec, size_t out_len)
++{
++	return sizeof(struct ns_openamp_msg) + (in_len * sizeof(*in_vec)) +
++		(out_len * sizeof(*out_vec));
++}
++
++static size_t psa_call_in_vec_len(const struct psa_invec *in_vec, size_t in_len)
++{
++	size_t req_len = 0;
++	int i;
++
++	if (!in_vec || !in_len)
++		return 0;
++
++	for (i = 0; i < in_len; i++)
++		req_len += in_vec[i].len;
++
++	return req_len;
++}
++
++static size_t psa_call_out_vec_len(const struct psa_outvec *out_vec, size_t out_len)
++{
++	size_t resp_len = 0;
++	int i;
++
++	if (!out_vec || !out_len)
++		return 0;
++
++	for (i = 0; i < out_len; i++)
++		resp_len += out_vec[i].len;
++
++	return resp_len;
++}
++
+ psa_handle_t psa_connect(struct rpc_caller *caller, uint32_t sid,
+ 			 uint32_t version)
+ {
+@@ -31,7 +77,7 @@ psa_handle_t psa_connect(struct rpc_caller *caller, uint32_t sid,
+ 	rpc_handle = rpc_caller_begin(caller, &req,
+ 				      sizeof(struct ns_openamp_msg));
+ 	if (!rpc_handle) {
+-		EMSG("psa_connect: could not get handle");
++		EMSG("psa_connect: could not get rpc handle");
+ 		return PSA_ERROR_GENERIC_ERROR;
+ 	}
+ 
+@@ -56,14 +102,100 @@ psa_handle_t psa_connect(struct rpc_caller *caller, uint32_t sid,
+ 	return resp_msg ? (psa_handle_t)resp_msg->reply : PSA_NULL_HANDLE;
+ }
+ 
+-psa_status_t psa_call(struct rpc_caller *caller, psa_handle_t handle,
++psa_status_t psa_call(struct rpc_caller *caller, psa_handle_t psa_handle,
+ 		      int32_t type, const struct psa_invec *in_vec,
+ 		      size_t in_len, struct psa_outvec *out_vec, size_t out_len)
+ {
++	psa_status_t psa_status = PSA_SUCCESS;
++	struct s_openamp_msg *resp_msg = NULL;
++	struct psa_outvec *out_vec_param;
++	struct psa_invec *in_vec_param;
++	struct ns_openamp_msg *req_msg;
++	rpc_call_handle rpc_handle;
++	size_t out_vec_len;
++	size_t in_vec_len;
++	size_t header_len;
++	uint8_t *payload;
++	size_t resp_len;
++	uint8_t *resp;
++	uint8_t *req;
++	int ret;
++	int i;
++
++	if ((psa_handle == PSA_NULL_HANDLE) || !caller)
++		return PSA_ERROR_INVALID_ARGUMENT;
++
++	header_len = psa_call_header_len(in_vec, in_len, out_vec, out_len);
++	in_vec_len = psa_call_in_vec_len(in_vec, in_len);
++	out_vec_len = psa_call_out_vec_len(out_vec, out_len);
+ 
++	rpc_handle = rpc_caller_begin(caller, &req, header_len + in_vec_len);
++	if (!rpc_handle) {
++		EMSG("psa_call: could not get handle");
++		return PSA_ERROR_GENERIC_ERROR;
++	}
++
++	payload = req + header_len;
++
++	out_vec_param = psa_call_out_vec_param(req, in_len);
++	in_vec_param = psa_call_in_vec_param(req);
++
++	req_msg = (struct ns_openamp_msg *)req;
++
++	req_msg->call_type = OPENAMP_PSA_CALL;
++	req_msg->request_id = 1234;
++	req_msg->params.psa_call_params.handle = psa_handle;
++	req_msg->params.psa_call_params.type = type;
++	req_msg->params.psa_call_params.in_len = in_len;
++	req_msg->params.psa_call_params.in_vec = rpc_caller_virt_to_phys(caller, in_vec_param);
++	req_msg->params.psa_call_params.out_len = out_len;
++	req_msg->params.psa_call_params.out_vec = rpc_caller_virt_to_phys(caller, out_vec_param);
++
++	for (i = 0; i < in_len; i++) {
++		in_vec_param[i].base = rpc_caller_virt_to_phys(caller, payload);
++		in_vec_param[i].len = in_vec[i].len;
++
++		memcpy(payload, in_vec[i].base, in_vec[i].len);
++		payload += in_vec[i].len;
++	}
++
++	for (i = 0; i < out_len; i++) {
++		out_vec_param[i].base = NULL;
++		out_vec_param[i].len = out_vec[i].len;
++	}
++
++	ret = rpc_caller_invoke(caller, rpc_handle, 0, &psa_status, &resp,
++				&resp_len);
++	if (ret != TS_RPC_CALL_ACCEPTED) {
++		EMSG("psa_call: invoke failed: %d", ret);
++		return PSA_ERROR_GENERIC_ERROR;
++	}
++
++	if (psa_status != PSA_SUCCESS) {
++		EMSG("psa_call: psa_status invoke failed: %d", psa_status);
++		return PSA_ERROR_GENERIC_ERROR;
++	}
++
++	resp_msg = (struct s_openamp_msg *)resp;
++
++	if (!resp_msg || !out_len || resp_msg->reply != PSA_SUCCESS)
++		goto caller_end;
++
++	out_vec_param = (struct psa_outvec *)rpc_caller_phys_to_virt(caller,
++						     resp_msg->params.out_vec);
++
++	for (i = 0; i < resp_msg->params.out_len; i++) {
++		memcpy(out_vec[i].base, rpc_caller_phys_to_virt(caller, out_vec_param[i].base),
++		       out_vec[i].len);
++	}
++
++caller_end:
++	rpc_caller_end(caller, rpc_handle);
++
++	return resp_msg ? resp_msg->reply : PSA_ERROR_COMMUNICATION_FAILURE;
+ }
+ 
+-void psa_close(struct rpc_caller *caller, psa_handle_t handle)
++void psa_close(struct rpc_caller *caller, psa_handle_t psa_handle)
+ {
+ 	psa_status_t psa_status = PSA_SUCCESS;
+ 	struct s_openamp_msg *resp_msg = NULL;
+@@ -74,6 +206,9 @@ void psa_close(struct rpc_caller *caller, psa_handle_t handle)
+ 	uint8_t *req;
+ 	int ret;
+ 
++	if ((psa_handle == PSA_NULL_HANDLE) || !caller)
++		return;
++
+ 	rpc_handle = rpc_caller_begin(caller, &req,
+ 				      sizeof(struct ns_openamp_msg));
+ 	if (!rpc_handle) {
+@@ -84,7 +219,7 @@ void psa_close(struct rpc_caller *caller, psa_handle_t handle)
+ 	req_msg = (struct ns_openamp_msg *)req;
+ 
+ 	req_msg->call_type = OPENAMP_PSA_CLOSE;
+-	req_msg->params.psa_close_params.handle = handle;
++	req_msg->params.psa_close_params.handle = psa_handle;
+ 
+ 	ret = rpc_caller_invoke(caller, rpc_handle, 0, &psa_status, &resp,
+ 				&resp_len);
+diff --git a/components/service/secure_storage/backend/secure_storage_ipc/component.cmake b/components/service/secure_storage/backend/secure_storage_ipc/component.cmake
+new file mode 100644
+index 00000000..5d8f6714
+--- /dev/null
++++ b/components/service/secure_storage/backend/secure_storage_ipc/component.cmake
+@@ -0,0 +1,14 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++if (NOT DEFINED TGT)
++	message(FATAL_ERROR "mandatory parameter TGT is not defined.")
++endif()
++
++target_sources(${TGT} PRIVATE
++	"${CMAKE_CURRENT_LIST_DIR}/secure_storage_ipc.c"
++	)
++
+diff --git a/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
+new file mode 100644
+index 00000000..9b55f77d
+--- /dev/null
++++ b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
+@@ -0,0 +1,214 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#include <protocols/rpc/common/packed-c/status.h>
++#include "secure_storage_ipc.h"
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <rpc_caller.h>
++#include <string.h>
++#include <trace.h>
++
++
++static psa_status_t secure_storage_ipc_set(void *context, uint32_t client_id,
++			 psa_storage_uid_t uid, size_t data_length,
++			 const void *p_data, psa_storage_create_flags_t create_flags)
++{
++	struct secure_storage_ipc *ipc = context;
++	struct rpc_caller *caller = ipc->client.caller;
++	psa_handle_t psa_handle;
++	psa_status_t psa_status;
++	struct psa_invec in_vec[] = {
++		{ .base = &uid, .len = sizeof(uid) },
++		{ .base = p_data, .len = data_length },
++		{ .base = &create_flags, .len = sizeof(create_flags) },
++	};
++
++	(void)client_id;
++
++	ipc->client.rpc_status = TS_RPC_CALL_ACCEPTED;
++
++	/* Validating input parameters */
++	if (p_data == NULL)
++		return PSA_ERROR_INVALID_ARGUMENT;
++
++	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
++			      TFM_PS_SET, in_vec, IOVEC_LEN(in_vec), NULL, 0);
++	if (psa_status < 0)
++		EMSG("ipc_set: psa_call failed: %d", psa_status);
++
++	return psa_status;
++}
++
++static psa_status_t secure_storage_ipc_get(void *context,
++					   uint32_t client_id,
++					   psa_storage_uid_t uid,
++					   size_t data_offset,
++					   size_t data_size,
++					   void *p_data,
++					   size_t *p_data_length)
++{
++	struct secure_storage_ipc *ipc = context;
++	struct rpc_caller *caller = ipc->client.caller;
++	psa_handle_t psa_handle;
++	psa_status_t psa_status;
++	uint32_t offset = (uint32_t)data_offset;
++	struct psa_invec in_vec[] = {
++		{ .base = &uid, .len = sizeof(uid) },
++		{ .base = &offset, .len = sizeof(offset) },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = p_data, .len = data_size },
++	};
++
++	if (!p_data_length) {
++		EMSG("ipc_get: p_data_length not defined");
++		return PSA_ERROR_INVALID_ARGUMENT;
++	}
++
++	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
++			      TFM_PS_GET, in_vec, IOVEC_LEN(in_vec),
++			      out_vec, IOVEC_LEN(out_vec));
++	if (psa_status == PSA_SUCCESS)
++		*p_data_length = out_vec[0].len;
++
++	return psa_status;
++}
++
++static psa_status_t secure_storage_ipc_get_info(void *context,
++						uint32_t client_id,
++						psa_storage_uid_t uid,
++						struct psa_storage_info_t *p_info)
++{
++	struct secure_storage_ipc *ipc = context;
++	struct rpc_caller *caller = ipc->client.caller;
++	psa_handle_t psa_handle;
++	psa_status_t psa_status;
++	struct psa_invec in_vec[] = {
++		{ .base = &uid, .len = sizeof(uid) },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = p_info, .len = sizeof(*p_info) },
++	};
++
++	(void)client_id;
++
++	/* Validating input parameters */
++	if (!p_info)
++		return PSA_ERROR_INVALID_ARGUMENT;
++
++	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
++			      TFM_PS_GET_INFO, in_vec,
++			      IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++	if (psa_status != PSA_SUCCESS)
++		EMSG("ipc_get_info: failed to psa_call: %d", psa_status);
++
++	return psa_status;
++}
++
++static psa_status_t secure_storage_ipc_remove(void *context,
++						uint32_t client_id,
++						psa_storage_uid_t uid)
++{
++	struct secure_storage_ipc *ipc = context;
++	struct rpc_caller *caller = ipc->client.caller;
++	psa_handle_t psa_handle;
++	psa_status_t psa_status;
++	struct psa_invec in_vec[] = {
++		{ .base = &uid, .len = sizeof(uid) },
++	};
++
++	(void)client_id;
++
++	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
++			      TFM_PS_REMOVE, in_vec,
++			      IOVEC_LEN(in_vec), NULL, 0);
++	if (psa_status != PSA_SUCCESS)
++		EMSG("ipc_remove: failed to psa_call: %d", psa_status);
++
++	return psa_status;
++}
++
++static psa_status_t secure_storage_ipc_create(void *context,
++					      uint32_t client_id,
++					      uint64_t uid,
++					      size_t capacity,
++					      uint32_t create_flags)
++{
++	(void)context;
++	(void)uid;
++	(void)client_id;
++	(void)capacity;
++	(void)create_flags;
++
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static psa_status_t secure_storage_set_extended(void *context,
++						uint32_t client_id,
++						uint64_t uid,
++						size_t data_offset,
++						size_t data_length,
++						const void *p_data)
++{
++	(void)context;
++	(void)uid;
++	(void)client_id;
++	(void)data_offset;
++	(void)data_length;
++	(void)p_data;
++
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static uint32_t secure_storage_get_support(void *context, uint32_t client_id)
++{
++	struct secure_storage_ipc *ipc = context;
++	struct rpc_caller *caller = ipc->client.caller;
++	psa_handle_t psa_handle;
++	psa_status_t psa_status;
++	uint32_t support_flags;
++	struct psa_outvec out_vec[] = {
++		{ .base = &support_flags, .len =  sizeof(support_flags) },
++	};
++
++	(void)client_id;
++
++	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
++			      TFM_PS_GET_SUPPORT, NULL, 0,
++			      out_vec, IOVEC_LEN(out_vec));
++	if (psa_status != PSA_SUCCESS)
++		EMSG("ipc_get_support: failed to psa_call: %d", psa_status);
++
++	return psa_status;
++}
++
++struct storage_backend *secure_storage_ipc_init(struct secure_storage_ipc *context,
++						struct rpc_caller *caller)
++{
++	service_client_init(&context->client, caller);
++
++	static const struct storage_backend_interface interface =
++	{
++		.set = secure_storage_ipc_set,
++		.get = secure_storage_ipc_get,
++		.get_info = secure_storage_ipc_get_info,
++		.remove = secure_storage_ipc_remove,
++		.create = secure_storage_ipc_create,
++		.set_extended = secure_storage_set_extended,
++		.get_support = secure_storage_get_support,
++	};
++
++	context->backend.context = context;
++	context->backend.interface = &interface;
++
++	return &context->backend;
++}
++
++void secure_storage_ipc_deinit(struct secure_storage_ipc *context)
++{
++	service_client_deinit(&context->client);
++}
+diff --git a/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.h b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.h
+new file mode 100644
+index 00000000..e8c1e8fd
+--- /dev/null
++++ b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.h
+@@ -0,0 +1,52 @@
++/*
++ * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef SECURE_STORAGE_IPC_H
++#define SECURE_STORAGE_IPC_H
++
++#include <service/secure_storage/backend/storage_backend.h>
++#include <service/common/client/service_client.h>
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++/**
++ * @brief      Secure storage ipc instance
++ */
++struct secure_storage_ipc
++{
++    struct storage_backend backend;
++    struct service_client client;
++};
++
++/**
++ * @brief      Initialize a secure storage ipc client
++ *
++ * A secure storage client is a storage backend that makes RPC calls
++ * to a remote secure storage provider.
++ *
++ * @param[in]  context    Instance data
++ * @param[in]  rpc_caller RPC caller instance
++ *
++ *
++ * @return     Pointer to inialized storage backend or NULL on failure
++ */
++struct storage_backend *secure_storage_ipc_init(struct secure_storage_ipc *context,
++						struct rpc_caller *caller);
++
++/**
++ * @brief      Deinitialize a secure storage ipc client
++ *
++ * @param[in]  context   Instance data
++ */
++void secure_storage_ipc_deinit(struct secure_storage_ipc *context);
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* SECURE_STORAGE_IPC_H */
+diff --git a/deployments/se-proxy/opteesp/CMakeLists.txt b/deployments/se-proxy/opteesp/CMakeLists.txt
+index e0e0e12b..663177b7 100644
+--- a/deployments/se-proxy/opteesp/CMakeLists.txt
++++ b/deployments/se-proxy/opteesp/CMakeLists.txt
+@@ -73,6 +73,7 @@ add_components(TARGET "se-proxy"
+ 		"components/service/crypto/factory/full"
+ 		"components/service/secure_storage/include"
+ 		"components/service/secure_storage/frontend/secure_storage_provider"
++		"components/service/secure_storage/backend/secure_storage_ipc"
+ 		"components/service/attestation/include"
+ 		"components/service/attestation/provider"
+ 		"components/service/attestation/provider/serializer/packed-c"
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0013-Use-secure-storage-ipc-and-openamp-for-se_proxy.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0013-Use-secure-storage-ipc-and-openamp-for-se_proxy.patch
new file mode 100644
index 0000000..56964b5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0013-Use-secure-storage-ipc-and-openamp-for-se_proxy.patch
@@ -0,0 +1,63 @@
+From 12b8b8bb28c96e6f121122939b7d23e6c7055f0f Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 3 Dec 2021 19:25:34 +0000
+Subject: [PATCH] Use secure storage ipc and openamp for se_proxy
+
+Remove mock up backend for secure storage in se proxy
+deployment and use instead the secure storage ipc backend with
+openamp as rpc to secure enclave side.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../se-proxy/opteesp/service_proxy_factory.c     | 16 +++++++++++++---
+ 1 file changed, 13 insertions(+), 3 deletions(-)
+
+diff --git a/deployments/se-proxy/opteesp/service_proxy_factory.c b/deployments/se-proxy/opteesp/service_proxy_factory.c
+index acfb6e88..57290056 100644
+--- a/deployments/se-proxy/opteesp/service_proxy_factory.c
++++ b/deployments/se-proxy/opteesp/service_proxy_factory.c
+@@ -6,15 +6,20 @@
+ 
+ #include <stddef.h>
+ #include <rpc/common/endpoint/rpc_interface.h>
++#include <rpc/openamp/caller/sp/openamp_caller.h>
+ #include <service/attestation/provider/attest_provider.h>
+ #include <service/attestation/provider/serializer/packed-c/packedc_attest_provider_serializer.h>
+ #include <service/crypto/factory/crypto_provider_factory.h>
+ #include <service/secure_storage/frontend/secure_storage_provider/secure_storage_provider.h>
++#include <trace.h>
+ 
+ /* Stub backends */
+ #include <service/crypto/backend/stub/stub_crypto_backend.h>
++#include <service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.h>
+ #include <service/secure_storage/backend/mock_store/mock_store.h>
+ 
++struct openamp_caller openamp;
++
+ struct rpc_interface *attest_proxy_create(void)
+ {
+ 	struct rpc_interface *attest_iface;
+@@ -47,10 +52,15 @@ struct rpc_interface *crypto_proxy_create(void)
+ 
+ struct rpc_interface *ps_proxy_create(void)
+ {
+-	static struct mock_store ps_backend;
+ 	static struct secure_storage_provider ps_provider;
+-
+-	struct storage_backend *backend = mock_store_init(&ps_backend);
++	static struct secure_storage_ipc ps_backend;
++	static struct rpc_caller *storage_caller;
++	struct storage_backend *backend;
++
++	storage_caller = openamp_caller_init(&openamp);
++	if (!storage_caller)
++		return NULL;
++	backend = secure_storage_ipc_init(&ps_backend, &openamp.rpc_caller);
+ 
+ 	return secure_storage_provider_init(&ps_provider, backend);
+ }
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0014-Add-uefi-variable-append-write-support.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0014-Add-uefi-variable-append-write-support.patch
new file mode 100644
index 0000000..cf7357e
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0014-Add-uefi-variable-append-write-support.patch
@@ -0,0 +1,1162 @@
+From 254f564c76320478e7b509faf279c0c493470657 Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Thu, 2 Dec 2021 10:15:54 +0000
+Subject: [PATCH] Add uefi variable append write support
+
+Adds support for extending UEFI variable data handled by the
+smm_variable service provider using the EFI_VARIABLE_APPEND_WRITE
+attribute.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: I7a6562327bc0a5ce5cd0e85276325227b83e9f9e
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../backend/test/variable_index_tests.cpp     |  90 +++---
+ .../backend/test/variable_store_tests.cpp     |  40 ++-
+ .../backend/uefi_variable_store.c             | 263 +++++++++++-------
+ .../smm_variable/backend/variable_index.c     |  95 +++----
+ .../smm_variable/backend/variable_index.h     |  58 ++--
+ .../backend/variable_index_iterator.c         |   4 +-
+ .../backend/variable_index_iterator.h         |   2 +-
+ .../service/smm_variable_service_tests.cpp    |  48 ++++
+ protocols/service/smm_variable/parameters.h   |   3 +
+ 9 files changed, 364 insertions(+), 239 deletions(-)
+
+diff --git a/components/service/smm_variable/backend/test/variable_index_tests.cpp b/components/service/smm_variable/backend/test/variable_index_tests.cpp
+index c8bacf97..8edc0e70 100644
+--- a/components/service/smm_variable/backend/test/variable_index_tests.cpp
++++ b/components/service/smm_variable/backend/test/variable_index_tests.cpp
+@@ -69,34 +69,37 @@ TEST_GROUP(UefiVariableIndexTests)
+ 
+ 	void create_variables()
+ 	{
+-		const struct variable_info *info = NULL;
++		struct variable_info *info = NULL;
+ 
+-		info = variable_index_add_variable(
++		info = variable_index_add_entry(
+ 			&m_variable_index,
+ 			&guid_1,
+ 			name_1.size() * sizeof(int16_t),
+-			name_1.data(),
+-			EFI_VARIABLE_BOOTSERVICE_ACCESS);
+-
++			name_1.data());
+ 		CHECK_TRUE(info);
++		variable_index_set_variable(
++			info,
++			EFI_VARIABLE_BOOTSERVICE_ACCESS);
+ 
+-		info = variable_index_add_variable(
++		info = variable_index_add_entry(
+ 			&m_variable_index,
+ 			&guid_2,
+ 			name_2.size() * sizeof(int16_t),
+-			name_2.data(),
+-			EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS);
+-
++			name_2.data());
+ 		CHECK_TRUE(info);
++		variable_index_set_variable(
++			info,
++			EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS);
+ 
+-		info = variable_index_add_variable(
++		info = variable_index_add_entry(
+ 			&m_variable_index,
+ 			&guid_1,
+ 			name_3.size() * sizeof(int16_t),
+-			name_3.data(),
+-			EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_RUNTIME_ACCESS);
+-
++			name_3.data());
+ 		CHECK_TRUE(info);
++		variable_index_set_variable(
++			info,
++			EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_RUNTIME_ACCESS);
+ 	}
+ 
+ 	static const size_t MAX_VARIABLES = 10;
+@@ -111,7 +114,7 @@ TEST_GROUP(UefiVariableIndexTests)
+ 
+ TEST(UefiVariableIndexTests, emptyIndexOperations)
+ {
+-	const struct variable_info *info = NULL;
++	struct variable_info *info = NULL;
+ 
+ 	/* Expect not to find a variable */
+ 	info = variable_index_find(
+@@ -130,36 +133,34 @@ TEST(UefiVariableIndexTests, emptyIndexOperations)
+ 	POINTERS_EQUAL(NULL, info);
+ 
+ 	/* Remove should silently return */
+-	variable_index_remove_variable(
++	variable_index_clear_variable(
+ 		&m_variable_index,
+ 		info);
+ }
+ 
+ TEST(UefiVariableIndexTests, addWithOversizedName)
+ {
+-	const struct variable_info *info = NULL;
++	struct variable_info *info = NULL;
+ 	std::vector<int16_t> name;
+ 
+ 	name = to_variable_name(L"a long variable name that exceeds the length limit");
+ 
+-	info = variable_index_add_variable(
++	info = variable_index_add_entry(
+ 		&m_variable_index,
+ 		&guid_1,
+ 		name.size() * sizeof(int16_t),
+-		name.data(),
+-		EFI_VARIABLE_BOOTSERVICE_ACCESS);
++		name.data());
+ 
+ 	/* Expect the add to fail because of an oversized name */
+ 	POINTERS_EQUAL(NULL, info);
+ 
+ 	name = to_variable_name(L"a long variable name that fits!");
+ 
+-	info = variable_index_add_variable(
++	info = variable_index_add_entry(
+ 		&m_variable_index,
+ 		&guid_1,
+ 		name.size() * sizeof(int16_t),
+-		name.data(),
+-		EFI_VARIABLE_BOOTSERVICE_ACCESS);
++		name.data());
+ 
+ 	/* Expect the add succeed */
+ 	CHECK_TRUE(info);
+@@ -167,18 +168,17 @@ TEST(UefiVariableIndexTests, addWithOversizedName)
+ 
+ TEST(UefiVariableIndexTests, variableIndexFull)
+ {
+-	const struct variable_info *info = NULL;
++	struct variable_info *info = NULL;
+ 	EFI_GUID guid = guid_1;
+ 
+ 	/* Expect to be able to fill the index */
+ 	for (size_t i = 0; i < MAX_VARIABLES; ++i) {
+ 
+-		info = variable_index_add_variable(
++		info = variable_index_add_entry(
+ 			&m_variable_index,
+ 			&guid,
+ 			name_1.size() * sizeof(int16_t),
+-			name_1.data(),
+-			EFI_VARIABLE_BOOTSERVICE_ACCESS);
++			name_1.data());
+ 
+ 		CHECK_TRUE(info);
+ 
+@@ -187,12 +187,11 @@ TEST(UefiVariableIndexTests, variableIndexFull)
+ 	}
+ 
+ 	/* Variable index should now be full */
+-	info = variable_index_add_variable(
++	info = variable_index_add_entry(
+ 		&m_variable_index,
+ 		&guid,
+ 		name_1.size() * sizeof(int16_t),
+-		name_1.data(),
+-		EFI_VARIABLE_BOOTSERVICE_ACCESS);
++		name_1.data());
+ 
+ 	POINTERS_EQUAL(NULL, info);
+ }
+@@ -323,7 +322,7 @@ TEST(UefiVariableIndexTests, dumpBufferTooSmall)
+ TEST(UefiVariableIndexTests, removeVariable)
+ {
+ 	uint8_t buffer[MAX_VARIABLES * sizeof(struct variable_metadata)];
+-	const struct variable_info *info = NULL;
++	struct variable_info *info = NULL;
+ 
+ 	create_variables();
+ 
+@@ -334,7 +333,7 @@ TEST(UefiVariableIndexTests, removeVariable)
+ 		name_2.size() * sizeof(int16_t),
+ 		name_2.data());
+ 
+-	variable_index_remove_variable(
++	variable_index_clear_variable(
+ 		&m_variable_index,
+ 		info);
+ 
+@@ -352,7 +351,7 @@ TEST(UefiVariableIndexTests, removeVariable)
+ 		name_1.size() * sizeof(int16_t),
+ 		name_1.data());
+ 
+-	variable_index_remove_variable(
++	variable_index_clear_variable(
+ 		&m_variable_index,
+ 		info);
+ 
+@@ -370,7 +369,7 @@ TEST(UefiVariableIndexTests, removeVariable)
+ 		name_3.size() * sizeof(int16_t),
+ 		name_3.data());
+ 
+-	variable_index_remove_variable(
++	variable_index_clear_variable(
+ 		&m_variable_index,
+ 		info);
+ 
+@@ -395,7 +394,7 @@ TEST(UefiVariableIndexTests, removeVariable)
+ 
+ TEST(UefiVariableIndexTests, checkIterator)
+ {
+-	const struct variable_info *info = NULL;
++	struct variable_info *info = NULL;
+ 
+ 	create_variables();
+ 
+@@ -419,7 +418,7 @@ TEST(UefiVariableIndexTests, checkIterator)
+ 	UNSIGNED_LONGS_EQUAL(name_2.size() * sizeof(int16_t), info->metadata.name_size);
+ 	MEMCMP_EQUAL(name_2.data(), info->metadata.name, info->metadata.name_size);
+ 
+-	const struct variable_info *info_to_remove = info;
++	struct variable_info *info_to_remove = info;
+ 
+ 	variable_index_iterator_next(&iter);
+ 	CHECK_FALSE(variable_index_iterator_is_done(&iter));
+@@ -435,7 +434,8 @@ TEST(UefiVariableIndexTests, checkIterator)
+ 	CHECK_TRUE(variable_index_iterator_is_done(&iter));
+ 
+ 	/* Now remove the middle entry */
+-	variable_index_remove_variable(&m_variable_index, info_to_remove);
++	variable_index_clear_variable(&m_variable_index, info_to_remove);
++	variable_index_remove_unused_entry(&m_variable_index, info_to_remove);
+ 
+ 	/* Iterate again but this time there should only be two entries */
+ 	variable_index_iterator_first(&iter, &m_variable_index);
+@@ -478,7 +478,7 @@ TEST(UefiVariableIndexTests, setCheckConstraintsExistingVar)
+ 	constraints.max_size = 100;
+ 
+ 	/* Set check constraints on one of the variables */
+-	const struct variable_info *info = variable_index_find(
++	struct variable_info *info = variable_index_find(
+ 		&m_variable_index,
+ 		&guid_2,
+ 		name_2.size() * sizeof(int16_t),
+@@ -488,7 +488,7 @@ TEST(UefiVariableIndexTests, setCheckConstraintsExistingVar)
+ 	CHECK_TRUE(info->is_variable_set);
+ 	CHECK_FALSE(info->is_constraints_set);
+ 
+-	variable_index_update_constraints(info, &constraints);
++	variable_index_set_constraints(info, &constraints);
+ 
+ 	CHECK_TRUE(info->is_constraints_set);
+ 	CHECK_TRUE(info->is_variable_set);
+@@ -496,7 +496,7 @@ TEST(UefiVariableIndexTests, setCheckConstraintsExistingVar)
+ 	/* Remove the variable but still expect the variable to be indexed
+ 	 * because of the set constraints.
+ 	 */
+-	variable_index_remove_variable(
++	variable_index_clear_variable(
+ 		&m_variable_index,
+ 		info);
+ 
+@@ -588,7 +588,7 @@ TEST(UefiVariableIndexTests, setCheckConstraintsNonExistingVar)
+ 	constraints.max_size = 100;
+ 
+ 	/* Initially expect no variable_info */
+-	const struct variable_info *info = variable_index_find(
++	struct variable_info *info = variable_index_find(
+ 		&m_variable_index,
+ 		&guid_2,
+ 		name_2.size() * sizeof(int16_t),
+@@ -597,19 +597,19 @@ TEST(UefiVariableIndexTests, setCheckConstraintsNonExistingVar)
+ 	CHECK_FALSE(info);
+ 
+ 	/* Adding the check constraints should result in an entry being added */
+-	info = variable_index_add_constraints(
++	info = variable_index_add_entry(
+ 		&m_variable_index,
+ 		&guid_2,
+ 		name_2.size() * sizeof(int16_t),
+-		name_2.data(),
+-		&constraints);
+-
++		name_2.data());
+ 	CHECK_TRUE(info);
++
++	variable_index_set_constraints(info, &constraints);
+ 	CHECK_FALSE(info->is_variable_set);
+ 	CHECK_TRUE(info->is_constraints_set);
+ 
+ 	/* Updating the variable should cause the variable to be marked as set */
+-	variable_index_update_variable(info, EFI_VARIABLE_RUNTIME_ACCESS);
++	variable_index_set_variable(info, EFI_VARIABLE_RUNTIME_ACCESS);
+ 
+ 	CHECK_TRUE(info->is_variable_set);
+ 	CHECK_TRUE(info->is_constraints_set);
+diff --git a/components/service/smm_variable/backend/test/variable_store_tests.cpp b/components/service/smm_variable/backend/test/variable_store_tests.cpp
+index f6aba13a..578f118f 100644
+--- a/components/service/smm_variable/backend/test/variable_store_tests.cpp
++++ b/components/service/smm_variable/backend/test/variable_store_tests.cpp
+@@ -250,6 +250,21 @@ TEST(UefiVariableStoreTests, setGetRoundtrip)
+ 	/* Expect got variable data to be the same as the set value */
+ 	UNSIGNED_LONGLONGS_EQUAL(input_data.size(), output_data.size());
+ 	LONGS_EQUAL(0, input_data.compare(output_data));
++
++	/* Extend the variable using an append write */
++	std::string input_data2 = " jumps over the lazy dog";
++
++	status = set_variable(var_name, input_data2, EFI_VARIABLE_APPEND_WRITE);
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
++
++	status = get_variable(var_name, output_data);
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
++
++	std::string expected_output = input_data + input_data2;
++
++	/* Expect the append write operation to have extended the variable */
++	UNSIGNED_LONGLONGS_EQUAL(expected_output.size(), output_data.size());
++	LONGS_EQUAL(0, expected_output.compare(output_data));
+ }
+ 
+ TEST(UefiVariableStoreTests, persistentSetGet)
+@@ -259,7 +274,8 @@ TEST(UefiVariableStoreTests, persistentSetGet)
+ 	std::string input_data = "quick brown fox";
+ 	std::string output_data;
+ 
+-	status = set_variable(var_name, input_data, EFI_VARIABLE_NON_VOLATILE);
++	status = set_variable(var_name, input_data,
++		EFI_VARIABLE_NON_VOLATILE);
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+ 
+ 	status = get_variable(var_name, output_data);
+@@ -269,6 +285,22 @@ TEST(UefiVariableStoreTests, persistentSetGet)
+ 	UNSIGNED_LONGLONGS_EQUAL(input_data.size(), output_data.size());
+ 	LONGS_EQUAL(0, input_data.compare(output_data));
+ 
++	/* Extend the variable using an append write */
++	std::string input_data2 = " jumps over the lazy dog";
++
++	status = set_variable(var_name, input_data2,
++		EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_APPEND_WRITE);
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
++
++	status = get_variable(var_name, output_data);
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
++
++	std::string expected_output = input_data + input_data2;
++
++	/* Expect the append write operation to have extended the variable */
++	UNSIGNED_LONGLONGS_EQUAL(expected_output.size(), output_data.size());
++	LONGS_EQUAL(0, expected_output.compare(output_data));
++
+ 	/* Expect the variable to survive a power cycle */
+ 	power_cycle();
+ 
+@@ -277,8 +309,8 @@ TEST(UefiVariableStoreTests, persistentSetGet)
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+ 
+ 	/* Still expect got variable data to be the same as the set value */
+-	UNSIGNED_LONGLONGS_EQUAL(input_data.size(), output_data.size());
+-	LONGS_EQUAL(0, input_data.compare(output_data));
++	UNSIGNED_LONGLONGS_EQUAL(expected_output.size(), output_data.size());
++	LONGS_EQUAL(0, expected_output.compare(output_data));
+ }
+ 
+ TEST(UefiVariableStoreTests, removeVolatile)
+@@ -317,7 +349,7 @@ TEST(UefiVariableStoreTests, removePersistent)
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+ 
+ 	/* Remove by setting with zero data length */
+-	status = set_variable(var_name, std::string(), 0);
++	status = set_variable(var_name, std::string(), EFI_VARIABLE_NON_VOLATILE);
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+ 
+ 	/* Expect variable to no loger exist */
+diff --git a/components/service/smm_variable/backend/uefi_variable_store.c b/components/service/smm_variable/backend/uefi_variable_store.c
+index b7091d75..bcb85995 100644
+--- a/components/service/smm_variable/backend/uefi_variable_store.c
++++ b/components/service/smm_variable/backend/uefi_variable_store.c
+@@ -46,6 +46,13 @@ static efi_status_t load_variable_data(
+ 	SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *var,
+ 	size_t max_data_len);
+ 
++static psa_status_t append_write(
++	struct storage_backend *storage_backend,
++	uint32_t client_id,
++	uint64_t uid,
++	size_t data_length,
++	const void *data);
++
+ static void purge_orphan_index_entries(
+ 	struct uefi_variable_store *context);
+ 
+@@ -113,40 +120,45 @@ efi_status_t uefi_variable_store_set_variable(
+ 	struct uefi_variable_store *context,
+ 	const SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *var)
+ {
++	bool should_sync_index = false;
++
++	/* Validate incoming request */
+ 	efi_status_t status = check_name_terminator(var->Name, var->NameSize);
+ 	if (status != EFI_SUCCESS) return status;
+ 
+ 	status = check_capabilities(var);
+-	bool should_sync_index = false;
+-
+ 	if (status != EFI_SUCCESS) return status;
+ 
+-	/* Find in index */
+-	const struct variable_info *info = variable_index_find(
++	/* Find an existing entry in the variable index or add a new one */
++	struct variable_info *info = variable_index_find(
+ 		&context->variable_index,
+ 		&var->Guid,
+ 		var->NameSize,
+ 		var->Name);
+ 
+-	if (info) {
++	if (!info) {
+ 
+-		/* Variable info already exists */
+-		status = check_access_permitted_on_set(context, info, var);
++		info = variable_index_add_entry(
++			&context->variable_index,
++			&var->Guid,
++			var->NameSize,
++			var->Name);
+ 
+-		if (status == EFI_SUCCESS) {
++		if (!info) return EFI_OUT_OF_RESOURCES;
++	}
+ 
+-			should_sync_index =
+-				(var->Attributes & EFI_VARIABLE_NON_VOLATILE) ||
+-				(info->is_variable_set && (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE));
++	/* Control access */
++	status = check_access_permitted_on_set(context, info, var);
+ 
+-			if (var->DataSize) {
++	if (status == EFI_SUCCESS) {
+ 
+-				/* It's a set rather than a remove operation */
+-				variable_index_update_variable(
+-					info,
+-					var->Attributes);
+-			}
+-			else {
++		/* Access permitted */
++		if (info->is_variable_set) {
++
++			/* It's a request to update to an existing variable */
++			if (!(var->Attributes &
++				(EFI_VARIABLE_APPEND_WRITE | EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS_MASK)) &&
++				!var->DataSize) {
+ 
+ 				/* It's a remove operation - for a remove, the variable
+ 				 * data must be removed from the storage backend before
+@@ -155,30 +167,29 @@ efi_status_t uefi_variable_store_set_variable(
+ 				 * the storage backend without a corresponding index entry.
+ 				 */
+ 				remove_variable_data(context, info);
+-				variable_index_remove_variable(&context->variable_index, info);
++				variable_index_clear_variable(&context->variable_index, info);
+ 
+-				/* Variable info no longer valid */
+-				info = NULL;
++				should_sync_index = (var->Attributes & EFI_VARIABLE_NON_VOLATILE);
++			}
++			else {
++
++				/* It's a set operation where variable data is potentially
++				 * being overwritten or extended.
++				 */
++				if ((var->Attributes & ~EFI_VARIABLE_APPEND_WRITE) != info->metadata.attributes) {
++
++					/* Modifying attributes is forbidden */
++					return EFI_INVALID_PARAMETER;
++				}
+ 			}
+ 		}
+ 		else {
+ 
+-			/* Access forbidden */
+-			info = NULL;
+-		}
+-	}
+-	else if (var->DataSize) {
++			/*  It's a request to create a new variable */
++			variable_index_set_variable(info, var->Attributes);
+ 
+-		/* It's a new variable */
+-		info = variable_index_add_variable(
+-			&context->variable_index,
+-			&var->Guid,
+-			var->NameSize,
+-			var->Name,
+-			var->Attributes);
+-
+-		if (!info) status = EFI_OUT_OF_RESOURCES;
+-		should_sync_index = info && (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE);
++			should_sync_index = (var->Attributes & EFI_VARIABLE_NON_VOLATILE);
++		}
+ 	}
+ 
+ 	/* The order of these operations is important. For an update
+@@ -195,11 +206,13 @@ efi_status_t uefi_variable_store_set_variable(
+ 	}
+ 
+ 	/* Store any variable data to the storage backend */
+-	if (info && (status == EFI_SUCCESS)) {
++	if (info->is_variable_set && (status == EFI_SUCCESS)) {
+ 
+ 		status = store_variable_data(context, info, var);
+ 	}
+ 
++	variable_index_remove_unused_entry(&context->variable_index, info);
++
+ 	return status;
+ }
+ 
+@@ -293,54 +306,42 @@ efi_status_t uefi_variable_store_set_var_check_property(
+ 	efi_status_t status = check_name_terminator(property->Name, property->NameSize);
+ 	if (status != EFI_SUCCESS) return status;
+ 
+-	/* Find in index */
+-	const struct variable_info *info = variable_index_find(
++	/* Find in index or create a new entry */
++	struct variable_info *info = variable_index_find(
+ 		&context->variable_index,
+ 		&property->Guid,
+ 		property->NameSize,
+ 		property->Name);
+ 
+-	if (info) {
++	if (!info) {
+ 
+-		/* Applying check constraints to an existing variable that may have
+-		 * constraints already set.  These could constrain the setting of
+-		 * the constraints.
+-		 */
+-		struct variable_constraints constraints = info->check_constraints;
+-
+-		status = variable_checker_set_constraints(
+-			&constraints,
+-			info->is_constraints_set,
+-			&property->VariableProperty);
+-
+-		if (status == EFI_SUCCESS) {
++		info = variable_index_add_entry(
++			&context->variable_index,
++			&property->Guid,
++			property->NameSize,
++			property->Name);
+ 
+-			variable_index_update_constraints(info, &constraints);
+-		}
++		if (!info) return EFI_OUT_OF_RESOURCES;
+ 	}
+-	else {
+-
+-		/* Applying check constraints for a new variable */
+-		struct variable_constraints constraints;
+ 
+-		status = variable_checker_set_constraints(
+-			&constraints,
+-			false,
+-			&property->VariableProperty);
++	/* Applying check constraints to an existing variable that may have
++	 * constraints already set.  These could constrain the setting of
++	 * the constraints.
++	 */
++	struct variable_constraints constraints = info->check_constraints;
+ 
+-		if (status == EFI_SUCCESS) {
++	status = variable_checker_set_constraints(
++		&constraints,
++		info->is_constraints_set,
++		&property->VariableProperty);
+ 
+-			info = variable_index_add_constraints(
+-				&context->variable_index,
+-				&property->Guid,
+-				property->NameSize,
+-				property->Name,
+-				&constraints);
++	if (status == EFI_SUCCESS) {
+ 
+-			if (!info) status = EFI_OUT_OF_RESOURCES;
+-		}
++		variable_index_set_constraints(info, &constraints);
+ 	}
+ 
++	variable_index_remove_unused_entry(&context->variable_index, info);
++
+ 	return status;
+ }
+ 
+@@ -440,7 +441,8 @@ static efi_status_t check_capabilities(
+ 	if (var->Attributes & ~(
+ 		EFI_VARIABLE_NON_VOLATILE |
+ 		EFI_VARIABLE_BOOTSERVICE_ACCESS |
+-		EFI_VARIABLE_RUNTIME_ACCESS)) {
++		EFI_VARIABLE_RUNTIME_ACCESS |
++		EFI_VARIABLE_APPEND_WRITE)) {
+ 
+ 		/* An unsupported attribute has been requested */
+ 		status = EFI_UNSUPPORTED;
+@@ -486,17 +488,6 @@ static efi_status_t check_access_permitted_on_set(
+ 			var->DataSize);
+ 	}
+ 
+-	if ((status == EFI_SUCCESS) && var->DataSize) {
+-
+-		/* Restrict which attributes can be modified for an existing variable */
+-		if ((var->Attributes & EFI_VARIABLE_NON_VOLATILE) !=
+-			(info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE)) {
+-
+-			/* Don't permit change of storage class */
+-			status = EFI_INVALID_PARAMETER;
+-		}
+-	}
+-
+ 	return status;
+ }
+ 
+@@ -518,20 +509,34 @@ static efi_status_t store_variable_data(
+ 
+ 	if (storage_backend) {
+ 
+-		psa_status = storage_backend->interface->set(
+-			storage_backend->context,
+-			context->owner_id,
+-			info->metadata.uid,
+-			data_len,
+-			data,
+-			PSA_STORAGE_FLAG_NONE);
++		if (!(var->Attributes & EFI_VARIABLE_APPEND_WRITE)) {
++
++			/* Create or overwrite variable data */
++			psa_status = storage_backend->interface->set(
++				storage_backend->context,
++				context->owner_id,
++				info->metadata.uid,
++				data_len,
++				data,
++				PSA_STORAGE_FLAG_NONE);
++		}
++		else {
++
++			/* Append new data to existing variable data */
++			psa_status = append_write(
++				storage_backend,
++				context->owner_id,
++				info->metadata.uid,
++				data_len,
++				data);
++		}
+ 	}
+ 
+ 	if ((psa_status != PSA_SUCCESS) && is_nv) {
+ 
+ 		/* A storage failure has occurred so attempt to fix any
+-		* mismatch between the variable index and stored NV variables.
+-		*/
++		 * mismatch between the variable index and stored NV variables.
++		 */
+ 		purge_orphan_index_entries(context);
+ 	}
+ 
+@@ -598,6 +603,76 @@ static efi_status_t load_variable_data(
+ 	return psa_to_efi_storage_status(psa_status);
+ }
+ 
++static psa_status_t append_write(
++	struct storage_backend *storage_backend,
++	uint32_t client_id,
++	uint64_t uid,
++	size_t data_length,
++	const void *data)
++{
++	struct psa_storage_info_t storage_info;
++
++	if (data_length == 0) return PSA_SUCCESS;
++
++	psa_status_t psa_status = storage_backend->interface->get_info(
++		storage_backend->context,
++		client_id,
++		uid,
++		&storage_info);
++
++	if (psa_status != PSA_SUCCESS) return psa_status;
++
++	/* Determine size of appended variable */
++	size_t new_size = storage_info.size + data_length;
++
++	/* Defend against integer overflow */
++	if (new_size < storage_info.size) return PSA_ERROR_INVALID_ARGUMENT;
++
++	/* Storage backend doesn't support an append operation so we need
++	 * need to read the current variable data, extend it and write it back.
++	 */
++	uint8_t *rw_buf = malloc(new_size);
++	if (!rw_buf) return PSA_ERROR_INSUFFICIENT_MEMORY;
++
++	size_t old_size = 0;
++	psa_status = storage_backend->interface->get(
++		storage_backend->context,
++		client_id,
++		uid,
++		0,
++		new_size,
++		rw_buf,
++		&old_size);
++
++	if (psa_status == PSA_SUCCESS) {
++
++		if ((old_size + data_length) <= new_size) {
++
++			/* Extend the variable data */
++			memcpy(&rw_buf[old_size], data, data_length);
++
++			psa_status = storage_backend->interface->set(
++				storage_backend->context,
++				client_id,
++				uid,
++				old_size + data_length,
++				rw_buf,
++				storage_info.flags);
++		}
++		else {
++
++			/* There's a mismatch between the length obtained from
++			 * get_info() and the subsequent length returned by get().
++			 */
++			psa_status = PSA_ERROR_STORAGE_FAILURE;
++		}
++	}
++
++	free(rw_buf);
++
++	return psa_status;
++}
++
+ static void purge_orphan_index_entries(
+ 	struct uefi_variable_store *context)
+ {
+@@ -612,7 +687,7 @@ static void purge_orphan_index_entries(
+ 	 */
+ 	while (!variable_index_iterator_is_done(&iter)) {
+ 
+-		const struct variable_info *info = variable_index_iterator_current(&iter);
++		struct variable_info *info = variable_index_iterator_current(&iter);
+ 
+ 		if (info->is_variable_set && (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE)) {
+ 
+@@ -628,7 +703,7 @@ static void purge_orphan_index_entries(
+ 			if (psa_status != PSA_SUCCESS) {
+ 
+ 				/* Detected a mismatch between the index and storage */
+-				variable_index_remove_variable(&context->variable_index, info);
++				variable_index_clear_variable(&context->variable_index, info);
+ 				any_orphans = true;
+ 			}
+ 		}
+diff --git a/components/service/smm_variable/backend/variable_index.c b/components/service/smm_variable/backend/variable_index.c
+index 99d7c97a..a8a55753 100644
+--- a/components/service/smm_variable/backend/variable_index.c
++++ b/components/service/smm_variable/backend/variable_index.c
+@@ -132,13 +132,13 @@ size_t variable_index_max_dump_size(
+ 	return sizeof(struct variable_metadata) * context->max_variables;
+ }
+ 
+-const struct variable_info *variable_index_find(
+-	const struct variable_index *context,
++struct variable_info *variable_index_find(
++	struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+ 	const int16_t *name)
+ {
+-	const struct variable_info *result = NULL;
++	struct variable_info *result = NULL;
+ 	int pos = find_variable(context, guid, name_size, name);
+ 
+ 	if (pos >= 0) {
+@@ -149,13 +149,13 @@ const struct variable_info *variable_index_find(
+ 	return result;
+ }
+ 
+-const struct variable_info *variable_index_find_next(
++struct variable_info *variable_index_find_next(
+ 	const struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+ 	const int16_t *name)
+ {
+-	const struct variable_info *result = NULL;
++	struct variable_info *result = NULL;
+ 
+ 	if (name_size >= sizeof(int16_t)) {
+ 
+@@ -263,12 +263,11 @@ static struct variable_entry *add_entry(
+ 	return entry;
+ }
+ 
+-const struct variable_info *variable_index_add_variable(
++struct variable_info *variable_index_add_entry(
+ 	struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+-	const int16_t *name,
+-	uint32_t attributes)
++	const int16_t *name)
+ {
+ 	struct variable_info *info = NULL;
+ 	struct variable_entry *entry = add_entry(context, guid, name_size, name);
+@@ -276,40 +275,41 @@ const struct variable_info *variable_index_add_variable(
+ 	if (entry) {
+ 
+ 		info = &entry->info;
+-
+-		info->metadata.attributes = attributes;
+-		info->is_variable_set = true;
+-
+-		mark_dirty(entry);
+ 	}
+ 
+ 	return info;
+ }
+ 
+-const struct variable_info *variable_index_add_constraints(
++void variable_index_remove_unused_entry(
+ 	struct variable_index *context,
+-	const EFI_GUID *guid,
+-	size_t name_size,
+-	const int16_t *name,
+-	const struct variable_constraints *constraints)
++	struct variable_info *info)
+ {
+-	struct variable_info *info = NULL;
+-	struct variable_entry *entry = add_entry(context, guid, name_size, name);
+-
+-	if (entry) {
++	if (info &&
++		!info->is_constraints_set &&
++		!info->is_variable_set) {
+ 
+-		info = &entry->info;
++		struct variable_entry *entry = containing_entry(info);
++		entry->in_use = false;
+ 
+-		info->check_constraints = *constraints;
+-		info->is_constraints_set = true;
++		memset(info, 0, sizeof(struct variable_info));
+ 	}
++}
+ 
+-	return info;
++void variable_index_set_variable(
++	struct variable_info *info,
++	uint32_t attributes)
++{
++	struct variable_entry *entry = containing_entry(info);
++
++	info->metadata.attributes = attributes;
++	info->is_variable_set = true;
++
++	mark_dirty(entry);
+ }
+ 
+-void variable_index_remove_variable(
++void variable_index_clear_variable(
+ 	struct variable_index *context,
+-	const struct variable_info *info)
++	struct variable_info *info)
+ {
+ 	if (info) {
+ 
+@@ -318,48 +318,17 @@ void variable_index_remove_variable(
+ 
+ 		/* Mark variable as no longer set */
+ 		entry->info.is_variable_set = false;
+-
+-		/* Entry may still be needed if check constraints were set */
+-		entry->in_use = info->is_constraints_set;
+-
+-		if (!entry->in_use) {
+-
+-			/* Entry not needed so wipe */
+-			memset(&entry->info, 0, sizeof(struct variable_info));
+-		}
+ 	}
+ }
+ 
+-void variable_index_update_variable(
+-	const struct variable_info *info,
+-	uint32_t attributes)
+-{
+-	if (info) {
+-
+-		struct variable_info *modified_info = (struct variable_info*)info;
+-		struct variable_entry *entry = containing_entry(modified_info);
+-
+-		if (!modified_info->is_variable_set ||
+-			(attributes != modified_info->metadata.attributes)) {
+-
+-			/* The update changes the variable_info state */
+-			modified_info->is_variable_set = true;
+-			modified_info->metadata.attributes = attributes;
+-			mark_dirty(entry);
+-		}
+-	}
+-}
+-
+-void variable_index_update_constraints(
+-	const struct variable_info *info,
++void variable_index_set_constraints(
++	struct variable_info *info,
+ 	const struct variable_constraints *constraints)
+ {
+ 	if (info) {
+ 
+-		struct variable_info *modified_info = (struct variable_info*)info;
+-
+-		modified_info->check_constraints = *constraints;
+-		modified_info->is_constraints_set = true;
++		info->check_constraints = *constraints;
++		info->is_constraints_set = true;
+ 	}
+ }
+ 
+diff --git a/components/service/smm_variable/backend/variable_index.h b/components/service/smm_variable/backend/variable_index.h
+index e109d0d1..63f42ab6 100644
+--- a/components/service/smm_variable/backend/variable_index.h
++++ b/components/service/smm_variable/backend/variable_index.h
+@@ -119,8 +119,8 @@ size_t variable_index_max_dump_size(
+  *
+  * @return     Pointer to variable_info or NULL
+  */
+-const struct variable_info *variable_index_find(
+-	const struct variable_index *context,
++struct variable_info *variable_index_find(
++	struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+ 	const int16_t *name);
+@@ -135,78 +135,76 @@ const struct variable_info *variable_index_find(
+  *
+  * @return     Pointer to variable_info or NULL
+  */
+-const struct variable_info *variable_index_find_next(
++struct variable_info *variable_index_find_next(
+ 	const struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+ 	const int16_t *name);
+ 
+ /**
+- * @brief      Add a new variable to the index
++ * @brief      Add a new entry to the index
++ *
++ * An entry is needed either when a new variable is created or
++ * when variable constraints are set for a variable that doesn't
++ * yet exist.
+  *
+  * @param[in]  context variable_index
+  * @param[in]  guid The variable's guid
+  * @param[in]  name_size The name parameter's size
+  * @param[in]  name The variable's name
+- * @param[in]  attributes The variable's attributes
+  *
+  * @return     Pointer to variable_info or NULL
+  */
+-const struct variable_info *variable_index_add_variable(
++struct variable_info *variable_index_add_entry(
+ 	struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+-	const int16_t *name,
+-	uint32_t attributes);
++	const int16_t *name);
+ 
+ /**
+- * @brief      Remove a variable from the index
++ * @brief      Remove an unused entry from the index
+  *
+- * Removes a variable from the index if it exists.
++ * Removes an entry if it is not in use.
+  *
+  * @param[in]  context variable_index
+  * @param[in]  info The variable info corresponding to the entry to remove
+  */
+-void variable_index_remove_variable(
++void variable_index_remove_unused_entry(
+ 	struct variable_index *context,
+-	const struct variable_info *info);
++	struct variable_info *info);
+ 
+ /**
+- * @brief      Update a variable that's already in the index
++ * @brief      Set a variable to the index
++ *
++ * An entry for the variable must already exist.
+  *
+  * @param[in]  info variable info
+  * @param[in]  attributes The variable's attributes
+  */
+-void variable_index_update_variable(
+-	const struct variable_info *info,
++void variable_index_set_variable(
++	struct variable_info *info,
+ 	uint32_t attributes);
+ 
+ /**
+- * @brief      Add a new check constraints object to the index
++ * @brief      Clear a variable from the index
+  *
+- * @param[in]  context variable_index
+- * @param[in]  guid The variable's guid
+- * @param[in]  name_size The name parameter's size
+- * @param[in]  name The variable's name
+- * @param[in]  constraints The check constraints
++ * Clears a variable from the index
+  *
+- * @return     Pointer to variable_info or NULL
++ * @param[in]  context variable_index
++ * @param[in]  info The variable info corresponding to the variable to clear
+  */
+-const struct variable_info *variable_index_add_constraints(
++void variable_index_clear_variable(
+ 	struct variable_index *context,
+-	const EFI_GUID *guid,
+-	size_t name_size,
+-	const int16_t *name,
+-	const struct variable_constraints *constraints);
++	struct variable_info *info);
+ 
+ /**
+- * @brief      Update variable constraints that are already in the index
++ * @brief      Set a check constraints object associated with a variavle
+  *
+  * @param[in]  info variable info
+  * @param[in]  constraints The check constraints
+  */
+-void variable_index_update_constraints(
+-	const struct variable_info *info,
++void variable_index_set_constraints(
++	struct variable_info *info,
+ 	const struct variable_constraints *constraints);
+ 
+ /**
+diff --git a/components/service/smm_variable/backend/variable_index_iterator.c b/components/service/smm_variable/backend/variable_index_iterator.c
+index 7cc6dc7a..8f8fc741 100644
+--- a/components/service/smm_variable/backend/variable_index_iterator.c
++++ b/components/service/smm_variable/backend/variable_index_iterator.c
+@@ -31,10 +31,10 @@ bool variable_index_iterator_is_done(
+ 	return iter->current_pos >= iter->variable_index->max_variables;
+ }
+ 
+-const struct variable_info *variable_index_iterator_current(
++struct variable_info *variable_index_iterator_current(
+ 	const struct variable_index_iterator *iter)
+ {
+-	const struct variable_info *current = NULL;
++	struct variable_info *current = NULL;
+ 
+ 	if (!variable_index_iterator_is_done(iter)) {
+ 
+diff --git a/components/service/smm_variable/backend/variable_index_iterator.h b/components/service/smm_variable/backend/variable_index_iterator.h
+index f64a2c49..7ff77c50 100644
+--- a/components/service/smm_variable/backend/variable_index_iterator.h
++++ b/components/service/smm_variable/backend/variable_index_iterator.h
+@@ -54,7 +54,7 @@ bool variable_index_iterator_is_done(
+  *
+  * @return     Pointer to variable_info or NULL
+  */
+-const struct variable_info *variable_index_iterator_current(
++struct variable_info *variable_index_iterator_current(
+ 	const struct variable_index_iterator *iter);
+ 
+ /**
+diff --git a/components/service/smm_variable/test/service/smm_variable_service_tests.cpp b/components/service/smm_variable/test/service/smm_variable_service_tests.cpp
+index d76d9cce..088940a8 100644
+--- a/components/service/smm_variable/test/service/smm_variable_service_tests.cpp
++++ b/components/service/smm_variable/test/service/smm_variable_service_tests.cpp
+@@ -249,6 +249,30 @@ TEST(SmmVariableServiceTests, setAndGet)
+ 	UNSIGNED_LONGS_EQUAL(set_data.size(), get_data.size());
+ 	LONGS_EQUAL(0, get_data.compare(set_data));
+ 
++	/* Extend the variable using an append write */
++	std::string append_data = " values added with append write";
++
++	efi_status = m_client->set_variable(
++		m_common_guid,
++		var_name,
++		append_data,
++		EFI_VARIABLE_APPEND_WRITE);
++
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
++
++	efi_status = m_client->get_variable(
++		m_common_guid,
++		var_name,
++		get_data);
++
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
++
++	std::string appended_data = set_data + append_data;
++
++	/* Expect the append write operation to have extended the variable */
++	UNSIGNED_LONGLONGS_EQUAL(appended_data.size(), get_data.size());
++	LONGS_EQUAL(0, appended_data.compare(get_data));
++
+ 	/* Expect remove to be permitted */
+ 	efi_status = m_client->remove_variable(m_common_guid, var_name);
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
+@@ -279,6 +303,30 @@ TEST(SmmVariableServiceTests, setAndGetNv)
+ 	UNSIGNED_LONGS_EQUAL(set_data.size(), get_data.size());
+ 	LONGS_EQUAL(0, get_data.compare(set_data));
+ 
++	/* Extend the variable using an append write */
++	std::string append_data = " values added with append write";
++
++	efi_status = m_client->set_variable(
++		m_common_guid,
++		var_name,
++		append_data,
++		EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_APPEND_WRITE);
++
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
++
++	efi_status = m_client->get_variable(
++		m_common_guid,
++		var_name,
++		get_data);
++
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
++
++	std::string appended_data = set_data + append_data;
++
++	/* Expect the append write operation to have extended the variable */
++	UNSIGNED_LONGLONGS_EQUAL(appended_data.size(), get_data.size());
++	LONGS_EQUAL(0, appended_data.compare(get_data));
++
+ 	/* Expect remove to be permitted */
+ 	efi_status = m_client->remove_variable(m_common_guid, var_name);
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
+diff --git a/protocols/service/smm_variable/parameters.h b/protocols/service/smm_variable/parameters.h
+index 1f795a9b..233f301b 100644
+--- a/protocols/service/smm_variable/parameters.h
++++ b/protocols/service/smm_variable/parameters.h
+@@ -47,6 +47,9 @@ typedef struct {
+ 	 EFI_VARIABLE_HARDWARE_ERROR_RECORD | \
+ 	 EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS | \
+ 	 EFI_VARIABLE_APPEND_WRITE)
++#define	EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS_MASK \
++	(EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS | \
++	 EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS)
+ 
+ /**
+  * Parameter structure for SetVariable and GetVariable.
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0015-Add-UEFI-variable-support-for-QueryVariableInfo.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0015-Add-UEFI-variable-support-for-QueryVariableInfo.patch
new file mode 100644
index 0000000..978600d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0015-Add-UEFI-variable-support-for-QueryVariableInfo.patch
@@ -0,0 +1,830 @@
+From c4eaf83548eed4ed6194ff9e1368d6ae65f4ebf9 Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Thu, 2 Dec 2021 17:27:55 +0000
+Subject: [PATCH] Add UEFI variable support for QueryVariableInfo
+
+Adds support for the UEFI QueryVariableInfo operation. The total
+store size currently relies on pre-configured values, set for a
+particular deployment. Ideally, this information would be read
+from the storage backend. This facility is not however yet
+supported by the storage backend interface or by any PSA
+storage backend storage providers.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: I971252831f7e478914d736c672d184a371e64502
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../backend/test/variable_store_tests.cpp     |  89 +++++++-
+ .../backend/uefi_variable_store.c             | 213 ++++++++++++++----
+ .../backend/uefi_variable_store.h             |  39 +++-
+ .../client/cpp/smm_variable_client.cpp        |  66 ++++++
+ .../client/cpp/smm_variable_client.h          |   7 +
+ .../provider/smm_variable_provider.c          |  31 ++-
+ .../service/smm_variable_service_tests.cpp    |  55 ++++-
+ 7 files changed, 445 insertions(+), 55 deletions(-)
+
+diff --git a/components/service/smm_variable/backend/test/variable_store_tests.cpp b/components/service/smm_variable/backend/test/variable_store_tests.cpp
+index 578f118f..e90c1067 100644
+--- a/components/service/smm_variable/backend/test/variable_store_tests.cpp
++++ b/components/service/smm_variable/backend/test/variable_store_tests.cpp
+@@ -27,6 +27,18 @@ TEST_GROUP(UefiVariableStoreTests)
+ 
+ 		UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+ 
++		uefi_variable_store_set_storage_limits(
++			&m_uefi_variable_store,
++			EFI_VARIABLE_NON_VOLATILE,
++			STORE_CAPACITY,
++			MAX_VARIABLE_SIZE);
++
++		uefi_variable_store_set_storage_limits(
++			&m_uefi_variable_store,
++			0,
++			STORE_CAPACITY,
++			MAX_VARIABLE_SIZE);
++
+ 		setup_common_guid();
+ 	}
+ 
+@@ -152,6 +164,33 @@ TEST_GROUP(UefiVariableStoreTests)
+ 		return status;
+ 	}
+ 
++	efi_status_t query_variable_info(
++		uint32_t attributes,
++		size_t *max_variable_storage_size,
++		size_t *remaining_variable_storage_size,
++		size_t *max_variable_size)
++	{
++		SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO query;
++
++		query.MaximumVariableStorageSize = 0;
++		query.RemainingVariableStorageSize = 0;
++		query.MaximumVariableSize = 0;
++		query.Attributes = attributes;
++
++		efi_status_t status = uefi_variable_store_query_variable_info(
++			&m_uefi_variable_store,
++			&query);
++
++		if (status == EFI_SUCCESS) {
++
++			*max_variable_storage_size = query.MaximumVariableStorageSize;
++			*remaining_variable_storage_size = query.RemainingVariableStorageSize;
++			*max_variable_size = query.MaximumVariableSize;
++		}
++
++		return status;
++	}
++
+ 	efi_status_t set_check_var_property(
+ 		const std::wstring &name,
+ 		const VAR_CHECK_VARIABLE_PROPERTY &check_property)
+@@ -195,7 +234,8 @@ TEST_GROUP(UefiVariableStoreTests)
+ 
+ 		if (info && (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE)) {
+ 
+-			struct storage_backend *storage_backend = m_uefi_variable_store.persistent_store;
++			struct storage_backend *storage_backend =
++				m_uefi_variable_store.persistent_store.storage_backend;
+ 
+ 			storage_backend->interface->remove(
+ 				storage_backend->context,
+@@ -220,9 +260,24 @@ TEST_GROUP(UefiVariableStoreTests)
+ 			m_volatile_backend);
+ 
+ 		UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
++
++		uefi_variable_store_set_storage_limits(
++			&m_uefi_variable_store,
++			EFI_VARIABLE_NON_VOLATILE,
++			STORE_CAPACITY,
++			MAX_VARIABLE_SIZE);
++
++		uefi_variable_store_set_storage_limits(
++			&m_uefi_variable_store,
++			0,
++			STORE_CAPACITY,
++			MAX_VARIABLE_SIZE);
+ 	}
+ 
+ 	static const size_t MAX_VARIABLES = 10;
++	static const size_t MAX_VARIABLE_SIZE = 100;
++	static const size_t STORE_CAPACITY = 1000;
++
+ 	static const uint32_t OWNER_ID = 100;
+ 	static const size_t VARIABLE_BUFFER_SIZE = 1024;
+ 
+@@ -265,6 +320,22 @@ TEST(UefiVariableStoreTests, setGetRoundtrip)
+ 	/* Expect the append write operation to have extended the variable */
+ 	UNSIGNED_LONGLONGS_EQUAL(expected_output.size(), output_data.size());
+ 	LONGS_EQUAL(0, expected_output.compare(output_data));
++
++	/* Expect query_variable_info to return consistent values */
++	size_t max_variable_storage_size = 0;
++	size_t remaining_variable_storage_size = 0;
++	size_t max_variable_size = 0;
++
++	status = query_variable_info(
++		0,
++		&max_variable_storage_size,
++		&remaining_variable_storage_size,
++		&max_variable_size);
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
++
++	UNSIGNED_LONGLONGS_EQUAL(STORE_CAPACITY, max_variable_storage_size);
++	UNSIGNED_LONGLONGS_EQUAL(MAX_VARIABLE_SIZE, max_variable_size);
++	UNSIGNED_LONGLONGS_EQUAL(STORE_CAPACITY - expected_output.size(), remaining_variable_storage_size);
+ }
+ 
+ TEST(UefiVariableStoreTests, persistentSetGet)
+@@ -311,6 +382,22 @@ TEST(UefiVariableStoreTests, persistentSetGet)
+ 	/* Still expect got variable data to be the same as the set value */
+ 	UNSIGNED_LONGLONGS_EQUAL(expected_output.size(), output_data.size());
+ 	LONGS_EQUAL(0, expected_output.compare(output_data));
++
++	/* Expect query_variable_info to return consistent values */
++	size_t max_variable_storage_size = 0;
++	size_t remaining_variable_storage_size = 0;
++	size_t max_variable_size = 0;
++
++	status = query_variable_info(
++		EFI_VARIABLE_NON_VOLATILE,
++		&max_variable_storage_size,
++		&remaining_variable_storage_size,
++		&max_variable_size);
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
++
++	UNSIGNED_LONGLONGS_EQUAL(STORE_CAPACITY, max_variable_storage_size);
++	UNSIGNED_LONGLONGS_EQUAL(MAX_VARIABLE_SIZE, max_variable_size);
++	UNSIGNED_LONGLONGS_EQUAL(STORE_CAPACITY - expected_output.size(), remaining_variable_storage_size);
+ }
+ 
+ TEST(UefiVariableStoreTests, removeVolatile)
+diff --git a/components/service/smm_variable/backend/uefi_variable_store.c b/components/service/smm_variable/backend/uefi_variable_store.c
+index bcb85995..ed50eaf9 100644
+--- a/components/service/smm_variable/backend/uefi_variable_store.c
++++ b/components/service/smm_variable/backend/uefi_variable_store.c
+@@ -46,8 +46,15 @@ static efi_status_t load_variable_data(
+ 	SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *var,
+ 	size_t max_data_len);
+ 
+-static psa_status_t append_write(
+-	struct storage_backend *storage_backend,
++static psa_status_t store_overwrite(
++	struct delegate_variable_store *delegate_store,
++	uint32_t client_id,
++	uint64_t uid,
++	size_t data_length,
++	const void *data);
++
++static psa_status_t store_append_write(
++	struct delegate_variable_store *delegate_store,
+ 	uint32_t client_id,
+ 	uint64_t uid,
+ 	size_t data_length,
+@@ -56,6 +63,15 @@ static psa_status_t append_write(
+ static void purge_orphan_index_entries(
+ 	struct uefi_variable_store *context);
+ 
++static struct delegate_variable_store *select_delegate_store(
++	struct uefi_variable_store *context,
++	uint32_t attributes);
++
++static size_t space_used(
++	struct uefi_variable_store *context,
++	uint32_t attributes,
++	struct storage_backend *storage_backend);
++
+ static efi_status_t psa_to_efi_storage_status(
+ 	psa_status_t psa_status);
+ 
+@@ -66,6 +82,10 @@ static efi_status_t check_name_terminator(
+ /* Private UID for storing the variable index */
+ #define VARIABLE_INDEX_STORAGE_UID			(1)
+ 
++/* Default maximum variable size -
++ * may be overridden using uefi_variable_store_set_storage_limits()
++ */
++#define DEFAULT_MAX_VARIABLE_SIZE			(2048)
+ 
+ efi_status_t uefi_variable_store_init(
+ 	struct uefi_variable_store *context,
+@@ -76,8 +96,17 @@ efi_status_t uefi_variable_store_init(
+ {
+ 	efi_status_t status = EFI_SUCCESS;
+ 
+-	context->persistent_store = persistent_store;
+-	context->volatile_store = volatile_store;
++	/* Initialise persistent store defaults */
++	context->persistent_store.is_nv = true;
++	context->persistent_store.max_variable_size = DEFAULT_MAX_VARIABLE_SIZE;
++	context->persistent_store.total_capacity = DEFAULT_MAX_VARIABLE_SIZE * max_variables;
++	context->persistent_store.storage_backend = persistent_store;
++
++	/* Initialise volatile store defaults */
++	context->volatile_store.is_nv = false;
++	context->volatile_store.max_variable_size = DEFAULT_MAX_VARIABLE_SIZE;
++	context->volatile_store.total_capacity = DEFAULT_MAX_VARIABLE_SIZE * max_variables;
++	context->volatile_store.storage_backend = volatile_store;
+ 
+ 	context->owner_id = owner_id;
+ 	context->is_boot_service = true;
+@@ -116,6 +145,20 @@ void uefi_variable_store_deinit(
+ 	context->index_sync_buffer = NULL;
+ }
+ 
++void uefi_variable_store_set_storage_limits(
++	struct uefi_variable_store *context,
++	uint32_t attributes,
++	size_t total_capacity,
++	size_t max_variable_size)
++{
++	struct delegate_variable_store *delegate_store = select_delegate_store(
++		context,
++		attributes);
++
++	delegate_store->total_capacity = total_capacity;
++	delegate_store->max_variable_size = max_variable_size;
++}
++
+ efi_status_t uefi_variable_store_set_variable(
+ 	struct uefi_variable_store *context,
+ 	const SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *var)
+@@ -284,12 +327,24 @@ efi_status_t uefi_variable_store_get_next_variable_name(
+ 
+ efi_status_t uefi_variable_store_query_variable_info(
+ 	struct uefi_variable_store *context,
+-	SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO *cur)
++	SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO *var_info)
+ {
+-	efi_status_t status = EFI_UNSUPPORTED;
++	struct delegate_variable_store *delegate_store = select_delegate_store(
++		context,
++		var_info->Attributes);
+ 
++	size_t total_used = space_used(
++		context,
++		var_info->Attributes,
++		delegate_store->storage_backend);
+ 
+-	return status;
++	var_info->MaximumVariableSize = delegate_store->max_variable_size;
++	var_info->MaximumVariableStorageSize = delegate_store->total_capacity;
++	var_info->RemainingVariableStorageSize = (total_used < delegate_store->total_capacity) ?
++		delegate_store->total_capacity - total_used :
++		0;
++
++	return EFI_SUCCESS;
+ }
+ 
+ efi_status_t uefi_variable_store_exit_boot_service(
+@@ -375,7 +430,7 @@ efi_status_t uefi_variable_store_get_var_check_property(
+ static void load_variable_index(
+ 	struct uefi_variable_store *context)
+ {
+-	struct storage_backend *persistent_store = context->persistent_store;
++	struct storage_backend *persistent_store = context->persistent_store.storage_backend;
+ 
+ 	if (persistent_store) {
+ 
+@@ -413,7 +468,7 @@ static efi_status_t sync_variable_index(
+ 
+ 	if (is_dirty) {
+ 
+-		struct storage_backend *persistent_store = context->persistent_store;
++		struct storage_backend *persistent_store = context->persistent_store.storage_backend;
+ 
+ 		if (persistent_store) {
+ 
+@@ -501,30 +556,27 @@ static efi_status_t store_variable_data(
+ 	const uint8_t *data = (const uint8_t*)var +
+ 		SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE_DATA_OFFSET(var);
+ 
+-	bool is_nv = (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE);
+-
+-	struct storage_backend *storage_backend = (is_nv) ?
+-		context->persistent_store :
+-		context->volatile_store;
++	struct delegate_variable_store *delegate_store = select_delegate_store(
++		context,
++		info->metadata.attributes);
+ 
+-	if (storage_backend) {
++	if (delegate_store->storage_backend) {
+ 
+ 		if (!(var->Attributes & EFI_VARIABLE_APPEND_WRITE)) {
+ 
+ 			/* Create or overwrite variable data */
+-			psa_status = storage_backend->interface->set(
+-				storage_backend->context,
++			psa_status = store_overwrite(
++				delegate_store,
+ 				context->owner_id,
+ 				info->metadata.uid,
+ 				data_len,
+-				data,
+-				PSA_STORAGE_FLAG_NONE);
++				data);
+ 		}
+ 		else {
+ 
+ 			/* Append new data to existing variable data */
+-			psa_status = append_write(
+-				storage_backend,
++			psa_status = store_append_write(
++				delegate_store,
+ 				context->owner_id,
+ 				info->metadata.uid,
+ 				data_len,
+@@ -532,7 +584,7 @@ static efi_status_t store_variable_data(
+ 		}
+ 	}
+ 
+-	if ((psa_status != PSA_SUCCESS) && is_nv) {
++	if ((psa_status != PSA_SUCCESS) && delegate_store->is_nv) {
+ 
+ 		/* A storage failure has occurred so attempt to fix any
+ 		 * mismatch between the variable index and stored NV variables.
+@@ -551,16 +603,14 @@ static efi_status_t remove_variable_data(
+ 
+ 	if (info->is_variable_set) {
+ 
+-		bool is_nv = (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE);
++		struct delegate_variable_store *delegate_store = select_delegate_store(
++			context,
++			info->metadata.attributes);
+ 
+-		struct storage_backend *storage_backend = (is_nv) ?
+-			context->persistent_store :
+-			context->volatile_store;
++		if (delegate_store->storage_backend) {
+ 
+-		if (storage_backend) {
+-
+-			psa_status = storage_backend->interface->remove(
+-				storage_backend->context,
++			psa_status = delegate_store->storage_backend->interface->remove(
++				delegate_store->storage_backend->context,
+ 				context->owner_id,
+ 				info->metadata.uid);
+ 		}
+@@ -580,16 +630,14 @@ static efi_status_t load_variable_data(
+ 	uint8_t *data = (uint8_t*)var +
+ 		SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE_DATA_OFFSET(var);
+ 
+-	bool is_nv = (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE);
++	struct delegate_variable_store *delegate_store = select_delegate_store(
++		context,
++		info->metadata.attributes);
+ 
+-	struct storage_backend *storage_backend = (is_nv) ?
+-		context->persistent_store :
+-		context->volatile_store;
++	if (delegate_store->storage_backend) {
+ 
+-	if (storage_backend) {
+-
+-		psa_status = storage_backend->interface->get(
+-			storage_backend->context,
++		psa_status = delegate_store->storage_backend->interface->get(
++			delegate_store->storage_backend->context,
+ 			context->owner_id,
+ 			info->metadata.uid,
+ 			0,
+@@ -603,8 +651,29 @@ static efi_status_t load_variable_data(
+ 	return psa_to_efi_storage_status(psa_status);
+ }
+ 
+-static psa_status_t append_write(
+-	struct storage_backend *storage_backend,
++static psa_status_t store_overwrite(
++	struct delegate_variable_store *delegate_store,
++	uint32_t client_id,
++	uint64_t uid,
++	size_t data_length,
++	const void *data)
++{
++	/* Police maximum variable size limit */
++	if (data_length > delegate_store->max_variable_size) return PSA_ERROR_INVALID_ARGUMENT;
++
++	psa_status_t psa_status = delegate_store->storage_backend->interface->set(
++		delegate_store->storage_backend->context,
++		client_id,
++		uid,
++		data_length,
++		data,
++		PSA_STORAGE_FLAG_NONE);
++
++	return psa_status;
++}
++
++static psa_status_t store_append_write(
++	struct delegate_variable_store *delegate_store,
+ 	uint32_t client_id,
+ 	uint64_t uid,
+ 	size_t data_length,
+@@ -614,8 +683,8 @@ static psa_status_t append_write(
+ 
+ 	if (data_length == 0) return PSA_SUCCESS;
+ 
+-	psa_status_t psa_status = storage_backend->interface->get_info(
+-		storage_backend->context,
++	psa_status_t psa_status = delegate_store->storage_backend->interface->get_info(
++		delegate_store->storage_backend->context,
+ 		client_id,
+ 		uid,
+ 		&storage_info);
+@@ -628,6 +697,9 @@ static psa_status_t append_write(
+ 	/* Defend against integer overflow */
+ 	if (new_size < storage_info.size) return PSA_ERROR_INVALID_ARGUMENT;
+ 
++		/* Police maximum variable size limit */
++	if (new_size > delegate_store->max_variable_size) return PSA_ERROR_INVALID_ARGUMENT;
++
+ 	/* Storage backend doesn't support an append operation so we need
+ 	 * need to read the current variable data, extend it and write it back.
+ 	 */
+@@ -635,8 +707,8 @@ static psa_status_t append_write(
+ 	if (!rw_buf) return PSA_ERROR_INSUFFICIENT_MEMORY;
+ 
+ 	size_t old_size = 0;
+-	psa_status = storage_backend->interface->get(
+-		storage_backend->context,
++	psa_status = delegate_store->storage_backend->interface->get(
++		delegate_store->storage_backend->context,
+ 		client_id,
+ 		uid,
+ 		0,
+@@ -651,8 +723,8 @@ static psa_status_t append_write(
+ 			/* Extend the variable data */
+ 			memcpy(&rw_buf[old_size], data, data_length);
+ 
+-			psa_status = storage_backend->interface->set(
+-				storage_backend->context,
++			psa_status = delegate_store->storage_backend->interface->set(
++				delegate_store->storage_backend->context,
+ 				client_id,
+ 				uid,
+ 				old_size + data_length,
+@@ -692,7 +764,7 @@ static void purge_orphan_index_entries(
+ 		if (info->is_variable_set && (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE)) {
+ 
+ 			struct psa_storage_info_t storage_info;
+-			struct storage_backend *storage_backend = context->persistent_store;
++			struct storage_backend *storage_backend = context->persistent_store.storage_backend;
+ 
+ 			psa_status_t psa_status = storage_backend->interface->get_info(
+ 				storage_backend->context,
+@@ -714,6 +786,53 @@ static void purge_orphan_index_entries(
+ 	if (any_orphans) sync_variable_index(context);
+ }
+ 
++static struct delegate_variable_store *select_delegate_store(
++	struct uefi_variable_store *context,
++	uint32_t attributes)
++{
++	bool is_nv = (attributes & EFI_VARIABLE_NON_VOLATILE);
++
++	return (is_nv) ?
++		&context->persistent_store :
++		&context->volatile_store;
++}
++
++static size_t space_used(
++	struct uefi_variable_store *context,
++	uint32_t attributes,
++	struct storage_backend *storage_backend)
++{
++	if (!storage_backend) return 0;
++
++	size_t total_used = 0;
++	struct variable_index_iterator iter;
++	variable_index_iterator_first(&iter, &context->variable_index);
++
++	while (!variable_index_iterator_is_done(&iter)) {
++
++		struct variable_info *info = variable_index_iterator_current(&iter);
++
++		if (info->is_variable_set &&
++		    ((info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE) ==
++			 (attributes & EFI_VARIABLE_NON_VOLATILE))) {
++
++			struct psa_storage_info_t storage_info;
++
++			psa_status_t psa_status = storage_backend->interface->get_info(
++				storage_backend->context,
++				context->owner_id,
++				info->metadata.uid,
++				&storage_info);
++
++			if (psa_status == PSA_SUCCESS) total_used += storage_info.size;
++		}
++
++		variable_index_iterator_next(&iter);
++	}
++
++	return total_used;
++}
++
+ static efi_status_t psa_to_efi_storage_status(
+ 	psa_status_t psa_status)
+ {
+diff --git a/components/service/smm_variable/backend/uefi_variable_store.h b/components/service/smm_variable/backend/uefi_variable_store.h
+index fe0f24af..cc992067 100644
+--- a/components/service/smm_variable/backend/uefi_variable_store.h
++++ b/components/service/smm_variable/backend/uefi_variable_store.h
+@@ -20,6 +20,20 @@
+ extern "C" {
+ #endif
+ 
++/**
++ * \brief delegate_variable_store structure definition
++ *
++ * A delegate_variable_store combines an association with a concrete
++ * storage backend and a set of limits parameters.
++ */
++struct delegate_variable_store
++{
++	bool is_nv;
++	size_t total_capacity;
++	size_t max_variable_size;
++	struct storage_backend *storage_backend;
++};
++
+ /**
+  * \brief uefi_variable_store structure definition
+  *
+@@ -35,8 +49,8 @@ struct uefi_variable_store
+ 	uint8_t *index_sync_buffer;
+ 	size_t index_sync_buffer_size;
+ 	struct variable_index variable_index;
+-	struct storage_backend *persistent_store;
+-	struct storage_backend *volatile_store;
++	struct delegate_variable_store persistent_store;
++	struct delegate_variable_store volatile_store;
+ };
+ 
+ /**
+@@ -69,6 +83,23 @@ efi_status_t uefi_variable_store_init(
+ void uefi_variable_store_deinit(
+ 	struct uefi_variable_store *context);
+ 
++/**
++ * @brief      Set storage limits
++ *
++ * Overrides the default limits for the specified storage space. These
++ * values are reflected in the values returned by QueryVariableInfo.
++ *
++ * @param[in]  context uefi_variable_store instance
++ * @param[in]  attributes EFI_VARIABLE_NON_VOLATILE or 0
++ * @param[in]  total_capacity The total storage capacity in bytes
++ * @param[in]  max_variable_size Variable size limit
++ */
++void uefi_variable_store_set_storage_limits(
++	struct uefi_variable_store *context,
++	uint32_t attributes,
++	size_t total_capacity,
++	size_t max_variable_size);
++
+ /**
+  * @brief      Set variable
+  *
+@@ -123,13 +154,13 @@ efi_status_t uefi_variable_store_get_next_variable_name(
+  * @brief      Query for variable info
+  *
+  * @param[in]  context uefi_variable_store instance
+- * @param[out] info Returns info
++ * @param[inout] var_info Returns info
+  *
+  * @return     EFI_SUCCESS if succesful
+  */
+ efi_status_t uefi_variable_store_query_variable_info(
+ 	struct uefi_variable_store *context,
+-	SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO *cur);
++	SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO *var_info);
+ 
+ /**
+  * @brief      Exit boot service
+diff --git a/components/service/smm_variable/client/cpp/smm_variable_client.cpp b/components/service/smm_variable/client/cpp/smm_variable_client.cpp
+index a68b7ace..8438285b 100644
+--- a/components/service/smm_variable/client/cpp/smm_variable_client.cpp
++++ b/components/service/smm_variable/client/cpp/smm_variable_client.cpp
+@@ -219,6 +219,72 @@ efi_status_t smm_variable_client::get_next_variable_name(
+ 		0);
+ }
+ 
++efi_status_t smm_variable_client::query_variable_info(
++	uint32_t attributes,
++	size_t *max_variable_storage_size,
++	size_t *remaining_variable_storage_size,
++	size_t *max_variable_size)
++{
++	efi_status_t efi_status = EFI_NOT_READY;
++
++	size_t req_len = sizeof(SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO);
++	rpc_call_handle call_handle;
++	uint8_t *req_buf;
++
++	call_handle = rpc_caller_begin(m_caller, &req_buf, req_len);
++
++	if (call_handle) {
++
++		uint8_t *resp_buf;
++        size_t resp_len;
++		rpc_opstatus_t opstatus;
++
++		SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO *query =
++			(SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO*)req_buf;
++
++		query->Attributes = attributes;
++		query->MaximumVariableSize = 0;
++		query->MaximumVariableStorageSize = 0;
++		query->RemainingVariableStorageSize = 0;
++
++		m_err_rpc_status = rpc_caller_invoke(m_caller, call_handle,
++			SMM_VARIABLE_FUNCTION_QUERY_VARIABLE_INFO, &opstatus, &resp_buf, &resp_len);
++
++		if (m_err_rpc_status == TS_RPC_CALL_ACCEPTED) {
++
++			efi_status = opstatus;
++
++			if (efi_status == EFI_SUCCESS) {
++
++				if (resp_len >= sizeof(SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO)) {
++
++					query = (SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO*)resp_buf;
++
++					*max_variable_storage_size = query->MaximumVariableStorageSize;
++					*remaining_variable_storage_size = query->RemainingVariableStorageSize;
++					*max_variable_size = query->MaximumVariableSize;
++				}
++				else {
++
++					efi_status = EFI_PROTOCOL_ERROR;
++				}
++			}
++			else {
++
++				efi_status = EFI_PROTOCOL_ERROR;
++			}
++		}
++		else {
++
++			efi_status = rpc_to_efi_status();
++		}
++
++		rpc_caller_end(m_caller, call_handle);
++	}
++
++	return efi_status;
++}
++
+ efi_status_t smm_variable_client::get_next_variable_name(
+ 	EFI_GUID &guid,
+ 	std::wstring &name,
+diff --git a/components/service/smm_variable/client/cpp/smm_variable_client.h b/components/service/smm_variable/client/cpp/smm_variable_client.h
+index 9c36c4eb..c7973916 100644
+--- a/components/service/smm_variable/client/cpp/smm_variable_client.h
++++ b/components/service/smm_variable/client/cpp/smm_variable_client.h
+@@ -63,6 +63,13 @@ public:
+ 		const EFI_GUID &guid,
+ 		const std::wstring &name);
+ 
++	/* Query variable info */
++	efi_status_t query_variable_info(
++		uint32_t attributes,
++		size_t *max_variable_storage_size,
++		size_t *remaining_variable_storage_size,
++		size_t *max_variable_size);
++
+ 	/* Get the next variable name - for enumerating store contents */
+ 	efi_status_t get_next_variable_name(
+ 		EFI_GUID &guid,
+diff --git a/components/service/smm_variable/provider/smm_variable_provider.c b/components/service/smm_variable/provider/smm_variable_provider.c
+index d239a428..52e68d09 100644
+--- a/components/service/smm_variable/provider/smm_variable_provider.c
++++ b/components/service/smm_variable/provider/smm_variable_provider.c
+@@ -252,11 +252,38 @@ static rpc_status_t set_variable_handler(void *context, struct call_req* req)
+ 
+ static rpc_status_t query_variable_info_handler(void *context, struct call_req* req)
+ {
++	efi_status_t efi_status = EFI_INVALID_PARAMETER;
+ 	struct smm_variable_provider *this_instance = (struct smm_variable_provider*)context;
+ 
+-	/* todo */
++	const struct call_param_buf *req_buf = call_req_get_req_buf(req);
++
++	if (req_buf->data_len >= sizeof(SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO)) {
++
++		struct call_param_buf *resp_buf = call_req_get_resp_buf(req);
++
++		if (resp_buf->size >= req_buf->data_len) {
+ 
+-	return TS_RPC_ERROR_NOT_READY;
++			memmove(resp_buf->data, req_buf->data, req_buf->data_len);
++
++			efi_status = uefi_variable_store_query_variable_info(
++				&this_instance->variable_store,
++				(SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO*)resp_buf->data);
++
++			if (efi_status == EFI_SUCCESS) {
++
++				resp_buf->data_len = sizeof(SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO);
++			}
++		}
++		else {
++
++			/* Reponse buffer not big enough */
++			efi_status = EFI_BAD_BUFFER_SIZE;
++		}
++	}
++
++	call_req_set_opstatus(req, efi_status);
++
++	return TS_RPC_CALL_ACCEPTED;
+ }
+ 
+ static rpc_status_t exit_boot_service_handler(void *context, struct call_req* req)
+diff --git a/components/service/smm_variable/test/service/smm_variable_service_tests.cpp b/components/service/smm_variable/test/service/smm_variable_service_tests.cpp
+index 088940a8..15556e9d 100644
+--- a/components/service/smm_variable/test/service/smm_variable_service_tests.cpp
++++ b/components/service/smm_variable/test/service/smm_variable_service_tests.cpp
+@@ -335,12 +335,38 @@ TEST(SmmVariableServiceTests, setAndGetNv)
+ TEST(SmmVariableServiceTests, enumerateStoreContents)
+ {
+ 	efi_status_t efi_status = EFI_SUCCESS;
++
++	/* Query information about the empty variable store */
++	size_t nv_max_variable_storage_size = 0;
++	size_t nv_max_variable_size = 0;
++	size_t nv_remaining_variable_storage_size = 0;
++
++	efi_status = m_client->query_variable_info(
++		EFI_VARIABLE_NON_VOLATILE,
++		&nv_max_variable_storage_size,
++		&nv_remaining_variable_storage_size,
++		&nv_max_variable_size);
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
++	UNSIGNED_LONGLONGS_EQUAL(nv_max_variable_storage_size, nv_remaining_variable_storage_size);
++
++	size_t v_max_variable_storage_size = 0;
++	size_t v_max_variable_size = 0;
++	size_t v_remaining_variable_storage_size = 0;
++
++	efi_status = m_client->query_variable_info(
++		0,
++		&v_max_variable_storage_size,
++		&v_remaining_variable_storage_size,
++		&v_max_variable_size);
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
++	UNSIGNED_LONGLONGS_EQUAL(v_max_variable_storage_size, v_remaining_variable_storage_size);
++
++	/* Add some variables to the store */
+ 	std::wstring var_name_1 = L"varibale_1";
+ 	std::wstring var_name_2 = L"varibale_2";
+ 	std::wstring var_name_3 = L"varibale_3";
+ 	std::string set_data = "Some variable data";
+ 
+-	/* Add some variables to the store */
+ 	efi_status = m_client->set_variable(
+ 		m_common_guid,
+ 		var_name_1,
+@@ -365,6 +391,33 @@ TEST(SmmVariableServiceTests, enumerateStoreContents)
+ 
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
+ 
++	/* Query variable info again and check it's as expected */
++	size_t max_variable_storage_size = 0;
++	size_t max_variable_size = 0;
++	size_t remaining_variable_storage_size = 0;
++
++	/* Check non-volatile - two variables have been added */
++	efi_status = m_client->query_variable_info(
++		EFI_VARIABLE_NON_VOLATILE,
++		&max_variable_storage_size,
++		&remaining_variable_storage_size,
++		&max_variable_size);
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
++	UNSIGNED_LONGLONGS_EQUAL(
++		(nv_remaining_variable_storage_size - set_data.size() * 2),
++		remaining_variable_storage_size);
++
++	/* Check volatile - one variables have been added */
++	efi_status = m_client->query_variable_info(
++		0,
++		&max_variable_storage_size,
++		&remaining_variable_storage_size,
++		&max_variable_size);
++	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
++	UNSIGNED_LONGLONGS_EQUAL(
++		(v_remaining_variable_storage_size - set_data.size() * 1),
++		remaining_variable_storage_size);
++
+ 	/* Enumerate store contents - expect the values we added */
+ 	std::wstring var_name;
+ 	EFI_GUID guid = {0};
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0016-Add-uefi-test-deployment.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0016-Add-uefi-test-deployment.patch
new file mode 100644
index 0000000..66a4499
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0016-Add-uefi-test-deployment.patch
@@ -0,0 +1,248 @@
+From 22120b4bd64da232e5a4e04a9a15376f34a933a3 Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Mon, 6 Dec 2021 15:20:12 +0000
+Subject: [PATCH] Add uefi-test deployment
+
+Adds a new deployment for building and running service level tests
+for UEFI SMM services. Tests may be run against StMM, smm-gateway
+or any other similar secure-world uefi service provider.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: Ic0e16dff51ef76ddd1f4dea37a4a55b029edd696
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../ts-service-test/arm-linux/CMakeLists.txt  |  3 -
+ .../uefi-test/arm-linux/CMakeLists.txt        | 43 +++++++++++
+ deployments/uefi-test/linux-pc/CMakeLists.txt | 76 +++++++++++++++++++
+ deployments/uefi-test/uefi-test.cmake         | 52 +++++++++++++
+ tools/b-test/test_data.yaml                   | 10 +++
+ 5 files changed, 181 insertions(+), 3 deletions(-)
+ create mode 100644 deployments/uefi-test/arm-linux/CMakeLists.txt
+ create mode 100644 deployments/uefi-test/linux-pc/CMakeLists.txt
+ create mode 100644 deployments/uefi-test/uefi-test.cmake
+
+diff --git a/deployments/ts-service-test/arm-linux/CMakeLists.txt b/deployments/ts-service-test/arm-linux/CMakeLists.txt
+index 6a01d38a..e902cd2f 100644
+--- a/deployments/ts-service-test/arm-linux/CMakeLists.txt
++++ b/deployments/ts-service-test/arm-linux/CMakeLists.txt
+@@ -23,9 +23,6 @@ add_components(
+ 	BASE_DIR ${TS_ROOT}
+ 	COMPONENTS
+ 		"components/app/test-runner"
+-# Running smm_variable tests currently requires kernel built with CONFIG_STRICT_DEVMEM=n
+-#		"components/service/smm_variable/client/cpp"
+-#		"components/service/smm_variable/test/service"
+  )
+ 
+ include(${TS_ROOT}/external/CppUTest/CppUTest.cmake)
+diff --git a/deployments/uefi-test/arm-linux/CMakeLists.txt b/deployments/uefi-test/arm-linux/CMakeLists.txt
+new file mode 100644
+index 00000000..053041ad
+--- /dev/null
++++ b/deployments/uefi-test/arm-linux/CMakeLists.txt
+@@ -0,0 +1,43 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++cmake_minimum_required(VERSION 3.16)
++include(../../deployment.cmake REQUIRED)
++
++#-------------------------------------------------------------------------------
++#  The CMakeLists.txt for building the uefi-test deployment for arm-linux
++#
++#  Used for building and running service level tests from Linux user-space
++#  on an Arm platform with real deployments of UEFI SMM services
++#-------------------------------------------------------------------------------
++include(${TS_ROOT}/environments/arm-linux/env.cmake)
++project(trusted-services LANGUAGES CXX C)
++add_executable(uefi-test)
++target_include_directories(uefi-test PRIVATE "${TOP_LEVEL_INCLUDE_DIRS}")
++
++add_components(
++	TARGET "uefi-test"
++	BASE_DIR ${TS_ROOT}
++	COMPONENTS
++		"components/app/test-runner"
++ )
++
++include(${TS_ROOT}/external/CppUTest/CppUTest.cmake)
++target_link_libraries(uefi-test PRIVATE CppUTest)
++
++#-------------------------------------------------------------------------------
++#  Extend with components that are common across all deployments of
++#  uefi-test
++#
++#-------------------------------------------------------------------------------
++include(../uefi-test.cmake REQUIRED)
++
++#-------------------------------------------------------------------------------
++#  Define library options and dependencies.
++#
++#-------------------------------------------------------------------------------
++env_set_link_options(TGT uefi-test)
++target_link_libraries(uefi-test PRIVATE stdc++ gcc m)
+diff --git a/deployments/uefi-test/linux-pc/CMakeLists.txt b/deployments/uefi-test/linux-pc/CMakeLists.txt
+new file mode 100644
+index 00000000..be6e9840
+--- /dev/null
++++ b/deployments/uefi-test/linux-pc/CMakeLists.txt
+@@ -0,0 +1,76 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++cmake_minimum_required(VERSION 3.16)
++include(../../deployment.cmake REQUIRED)
++
++#-------------------------------------------------------------------------------
++#  The CMakeLists.txt for building the uefi-test deployment for linux-pc
++#
++#  Used for building and running service level tests in a native PC enviroment.
++#  Tests can be run by running the built executable called "uefi-test"
++#-------------------------------------------------------------------------------
++include(${TS_ROOT}/environments/linux-pc/env.cmake)
++project(trusted-services LANGUAGES CXX C)
++
++# Prevents symbols in the uefi-test executable overriding symbols with
++# with same name in libts during dynamic linking performed by the program
++# loader.
++set(CMAKE_C_VISIBILITY_PRESET hidden)
++
++# Preparing firmware-test-build by including it
++include(${TS_ROOT}/external/firmware_test_builder/FirmwareTestBuilder.cmake)
++
++include(CTest)
++include(UnitTest)
++
++set(COVERAGE FALSE CACHE BOOL "Enable code coverage measurement")
++set(UNIT_TEST_PROJECT_PATH ${TS_ROOT} CACHE PATH "Path of the project directory")
++set(CMAKE_CXX_STANDARD 11)
++
++unit_test_init_cpputest()
++
++if (COVERAGE)
++	include(Coverage)
++
++	set(COVERAGE_FILE "coverage.info")
++	set(TS_SERVICE_TEST_COVERAGE_FILE "uefi-test-coverage.info" CACHE PATH "Path of coverage info file")
++	set(TS_SERVICE_TEST_COVERAGE_REPORT_DIR "${CMAKE_CURRENT_BINARY_DIR}/ts-service-coverage-report" CACHE PATH "Directory of coverage report")
++
++	# Collecting coverage
++	coverage_generate(
++		NAME "ts-service test"
++		SOURCE_DIR ${TS_ROOT}
++		BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}
++		OUTPUT_FILE ${COVERAGE_FILE}
++	)
++
++	# Filtering project file coverage
++	coverage_filter(
++		INPUT_FILE ${COVERAGE_FILE}
++		OUTPUT_FILE ${TS_SERVICE_TEST_COVERAGE_FILE}
++		INCLUDE_DIRECTORY ${UNIT_TEST_PROJECT_PATH}/components
++	)
++
++	# Coverage report
++	coverage_generate_report(
++		INPUT_FILE ${TS_SERVICE_TEST_COVERAGE_FILE}
++		OUTPUT_DIRECTORY ${TS_SERVICE_TEST_COVERAGE_REPORT_DIR}
++	)
++endif()
++
++unit_test_add_suite(
++	NAME uefi-test
++)
++
++target_include_directories(uefi-test PRIVATE "${TOP_LEVEL_INCLUDE_DIRS}")
++
++#-------------------------------------------------------------------------------
++#  Extend with components that are common across all deployments of
++#  uefi-test
++#
++#-------------------------------------------------------------------------------
++include(../uefi-test.cmake REQUIRED)
+diff --git a/deployments/uefi-test/uefi-test.cmake b/deployments/uefi-test/uefi-test.cmake
+new file mode 100644
+index 00000000..ea678d0e
+--- /dev/null
++++ b/deployments/uefi-test/uefi-test.cmake
+@@ -0,0 +1,52 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++
++#-------------------------------------------------------------------------------
++#  The base build file shared between deployments of 'uefi-test' for
++#  different environments.  Used for running end-to-end service-level tests
++#  against SMM service providers that implement UEFI services such as smm
++#  variable.
++#-------------------------------------------------------------------------------
++
++#-------------------------------------------------------------------------------
++#  Use libts for locating and accessing services. An appropriate version of
++#  libts will be imported for the enviroment in which service tests are
++#  deployed.
++#-------------------------------------------------------------------------------
++include(${TS_ROOT}/deployments/libts/libts-import.cmake)
++target_link_libraries(uefi-test PRIVATE libts)
++
++#-------------------------------------------------------------------------------
++#  Components that are common accross all deployments
++#
++#-------------------------------------------------------------------------------
++add_components(
++	TARGET "uefi-test"
++	BASE_DIR ${TS_ROOT}
++	COMPONENTS
++		"components/service/smm_variable/client/cpp"
++		"components/service/smm_variable/test/service"
++)
++
++#-------------------------------------------------------------------------------
++#  Components used from external projects
++#
++#-------------------------------------------------------------------------------
++
++# Nanopb
++include(${TS_ROOT}/external/nanopb/nanopb.cmake)
++target_link_libraries(uefi-test PRIVATE nanopb::protobuf-nanopb-static)
++protobuf_generate_all(TGT "uefi-test" NAMESPACE "protobuf" BASE_DIR "${TS_ROOT}/protocols")
++
++#-------------------------------------------------------------------------------
++#  Define install content.
++#
++#-------------------------------------------------------------------------------
++if (CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT)
++	set(CMAKE_INSTALL_PREFIX ${CMAKE_BINARY_DIR}/install CACHE PATH "location to install build output to." FORCE)
++endif()
++install(TARGETS uefi-test RUNTIME DESTINATION ${TS_ENV}/bin)
+diff --git a/tools/b-test/test_data.yaml b/tools/b-test/test_data.yaml
+index 11f8f633..33a85b12 100644
+--- a/tools/b-test/test_data.yaml
++++ b/tools/b-test/test_data.yaml
+@@ -163,3 +163,13 @@ data:
+       os_id : "GNU/Linux"
+       params:
+           - "-GUnix Makefiles"
++    - name: "uefi-test-pc-linux"
++      src: "$TS_ROOT/deployments/uefi-test/linux-pc"
++      os_id : "GNU/Linux"
++      params:
++          - "-GUnix Makefiles"
++    - name: "uefi-test-arm-linux"
++      src: "$TS_ROOT/deployments/uefi-test/arm-linux"
++      os_id : "GNU/Linux"
++      params:
++          - "-GUnix Makefiles"
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0017-Fix-interface-ID-parameter-setting-in-sp-ffarpc_call.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0017-Fix-interface-ID-parameter-setting-in-sp-ffarpc_call.patch
new file mode 100644
index 0000000..9ad506b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0017-Fix-interface-ID-parameter-setting-in-sp-ffarpc_call.patch
@@ -0,0 +1,33 @@
+From 289bec4cacac80cb43c19e4ca7b2c50fc932712e Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Wed, 8 Dec 2021 11:48:28 +0000
+Subject: [PATCH] Fix interface ID parameter setting in sp/ffarpc_caller
+
+When making FFA based RPC calls from one SP to another, the
+destination interface ID parameter was not being set correctly.
+This change fixes this issue.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: Iab520e4c7dc63ee1f5d3bf1bd1de702e4cc6f093
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ components/rpc/ffarpc/caller/sp/ffarpc_caller.c | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/components/rpc/ffarpc/caller/sp/ffarpc_caller.c b/components/rpc/ffarpc/caller/sp/ffarpc_caller.c
+index 250b1781..dabcd90c 100644
+--- a/components/rpc/ffarpc/caller/sp/ffarpc_caller.c
++++ b/components/rpc/ffarpc/caller/sp/ffarpc_caller.c
+@@ -81,7 +81,7 @@ static rpc_status_t call_invoke(void *context, rpc_call_handle handle, uint32_t
+ 	req.destination_id = this_context->dest_partition_id;
+ 	req.source_id = own_id;
+ 	req.args[SP_CALL_ARGS_IFACE_ID_OPCODE] =
+-		FFA_CALL_ARGS_COMBINE_IFACE_ID_OPCODE(this_context->dest_partition_id, opcode);
++		FFA_CALL_ARGS_COMBINE_IFACE_ID_OPCODE(this_context->dest_iface_id, opcode);
+ 	//TODO: downcast problem?
+ 	req.args[SP_CALL_ARGS_REQ_DATA_LEN] = (uint32_t)this_context->req_len;
+ 	req.args[SP_CALL_ARGS_ENCODING] = this_context->rpc_caller.encoding;
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0018-Support-FFARPC-call-requests-with-no-shared-buffer.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0018-Support-FFARPC-call-requests-with-no-shared-buffer.patch
new file mode 100644
index 0000000..6ea473b
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0018-Support-FFARPC-call-requests-with-no-shared-buffer.patch
@@ -0,0 +1,109 @@
+From 18b20dea7cf7e8afc26c5d49d5368d3180bd54d7 Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Wed, 8 Dec 2021 16:05:22 +0000
+Subject: [PATCH] Support FFARPC call requests with no shared buffer
+
+To allow simple clients to make RPC calls for service operations
+that take no request parameters and return no response parameters,
+the ffarpc_call_ep.c has been modified to accept call requests
+when no shared buffer exists, as long as there is no request data.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: I83b1bfb719a005922d6394887492d2d272b74907
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../rpc/ffarpc/endpoint/ffarpc_call_ep.c      | 52 ++++++++++---------
+ 1 file changed, 27 insertions(+), 25 deletions(-)
+
+diff --git a/components/rpc/ffarpc/endpoint/ffarpc_call_ep.c b/components/rpc/ffarpc/endpoint/ffarpc_call_ep.c
+index 17f957c2..a08a250c 100644
+--- a/components/rpc/ffarpc/endpoint/ffarpc_call_ep.c
++++ b/components/rpc/ffarpc/endpoint/ffarpc_call_ep.c
+@@ -150,29 +150,43 @@ out:
+ static void handle_service_msg(struct ffa_call_ep *call_ep, uint16_t source_id,
+ 			       const uint32_t *req_args, uint32_t *resp_args)
+ {
+-	rpc_status_t rpc_status = TS_RPC_ERROR_INTERNAL;
++	rpc_status_t rpc_status = TS_RPC_ERROR_INVALID_PARAMETER;
+ 	struct call_req call_req;
+ 
+ 	uint32_t ifaceid_opcode = req_args[SP_CALL_ARGS_IFACE_ID_OPCODE];
+ 	int idx = find_shm(call_ep, source_id);
+ 
+-	if (idx < 0) {
+-		EMSG("handle service msg error");
+-		goto out;
+-	}
+-
+ 	call_req.caller_id = source_id;
+ 	call_req.interface_id = FFA_CALL_ARGS_EXTRACT_IFACE(ifaceid_opcode);
+ 	call_req.opcode = FFA_CALL_ARGS_EXTRACT_OPCODE(ifaceid_opcode);
+ 	call_req.encoding = req_args[SP_CALL_ARGS_ENCODING];
+ 
+-	call_req.req_buf.data = call_ep->shmem_buf[idx];
+ 	call_req.req_buf.data_len = req_args[SP_CALL_ARGS_REQ_DATA_LEN];
+-	call_req.req_buf.size = call_ep->shmem_buf_size[idx];
+-
+-	call_req.resp_buf.data = call_ep->shmem_buf[idx];
+ 	call_req.resp_buf.data_len = 0;
+-	call_req.resp_buf.size = call_ep->shmem_buf_size[idx];
++
++	if (idx >= 0 && call_ep->shmem_buf[idx]) {
++		/* A shared buffer is available for call parameters */
++		call_req.req_buf.data = call_ep->shmem_buf[idx];
++		call_req.req_buf.size = call_ep->shmem_buf_size[idx];
++
++		call_req.resp_buf.data = call_ep->shmem_buf[idx];
++		call_req.resp_buf.size = call_ep->shmem_buf_size[idx];
++	}
++	else if (call_req.req_buf.data_len == 0) {
++		/* No shared buffer so only allow calls with no request data */
++		call_req.req_buf.data = NULL;
++		call_req.req_buf.size = 0;
++
++		call_req.resp_buf.data = NULL;
++		call_req.resp_buf.size = 0;
++	}
++	else {
++		/*
++		 * Caller has specified non-zero length request data but there is
++		 * no shared buffer to carry the request data.
++		 */
++		goto out;
++	}
+ 
+ 	rpc_status = rpc_interface_receive(call_ep->iface, &call_req);
+ 
+@@ -223,7 +237,6 @@ void ffa_call_ep_receive(struct ffa_call_ep *call_ep,
+ {
+ 	const uint32_t *req_args = req_msg->args;
+ 	uint32_t *resp_args = resp_msg->args;
+-	int idx;
+ 
+ 	uint16_t source_id = req_msg->source_id;
+ 	uint32_t ifaceid_opcode = req_args[SP_CALL_ARGS_IFACE_ID_OPCODE];
+@@ -232,18 +245,7 @@ void ffa_call_ep_receive(struct ffa_call_ep *call_ep,
+ 		/* It's an RPC layer management request */
+ 		handle_mgmt_msg(call_ep, source_id, req_args, resp_args);
+ 	} else {
+-		/*
+-		 * Assume anything else is a service request. Service requests
+-		 * rely on a buffer being shared from the requesting client.
+-		 * If it hasn't been set-up, fail the request.
+-		 */
+-		idx = find_shm(call_ep, source_id);
+-
+-		if (idx >= 0 && call_ep->shmem_buf[idx]) {
+-			handle_service_msg(call_ep, source_id, req_args, resp_args);
+-		} else {
+-			EMSG("shared buffer not found or NULL");
+-			set_mgmt_resp_args(resp_args, ifaceid_opcode, TS_RPC_ERROR_NOT_READY);
+-		}
++		/* Assume anything else is a service request */
++		handle_service_msg(call_ep, source_id, req_args, resp_args);
+ 	}
+ }
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0019-Run-psa-arch-test.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0019-Run-psa-arch-test.patch
new file mode 100644
index 0000000..4f54215
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0019-Run-psa-arch-test.patch
@@ -0,0 +1,86 @@
+From a496978dcf82494c69e600f65adf061f15e565f4 Mon Sep 17 00:00:00 2001
+From: Satish Kumar <satish.kumar01@arm.com>
+Date: Sun, 12 Dec 2021 10:43:48 +0000
+Subject: [PATCH] Run psa-arch-test
+
+Fixes needed to run psa-arch-test
+
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+
+---
+ components/service/common/psa_ipc/service_psa_ipc.c       | 1 +
+ .../backend/secure_storage_ipc/secure_storage_ipc.c       | 8 --------
+ .../service/secure_storage/include/psa/storage_common.h   | 4 ++--
+ external/openamp/openamp.cmake                            | 2 +-
+ 4 files changed, 4 insertions(+), 11 deletions(-)
+
+diff --git a/components/service/common/psa_ipc/service_psa_ipc.c b/components/service/common/psa_ipc/service_psa_ipc.c
+index 95a07c13..5e5815db 100644
+--- a/components/service/common/psa_ipc/service_psa_ipc.c
++++ b/components/service/common/psa_ipc/service_psa_ipc.c
+@@ -185,6 +185,7 @@ psa_status_t psa_call(struct rpc_caller *caller, psa_handle_t psa_handle,
+ 						     resp_msg->params.out_vec);
+ 
+ 	for (i = 0; i < resp_msg->params.out_len; i++) {
++                out_vec[i].len = out_vec_param[i].len;
+ 		memcpy(out_vec[i].base, rpc_caller_phys_to_virt(caller, out_vec_param[i].base),
+ 		       out_vec[i].len);
+ 	}
+diff --git a/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
+index 9b55f77d..a1f369db 100644
+--- a/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
++++ b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
+@@ -31,10 +31,6 @@ static psa_status_t secure_storage_ipc_set(void *context, uint32_t client_id,
+ 
+ 	ipc->client.rpc_status = TS_RPC_CALL_ACCEPTED;
+ 
+-	/* Validating input parameters */
+-	if (p_data == NULL)
+-		return PSA_ERROR_INVALID_ARGUMENT;
+-
+ 	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
+ 			      TFM_PS_SET, in_vec, IOVEC_LEN(in_vec), NULL, 0);
+ 	if (psa_status < 0)
+@@ -96,10 +92,6 @@ static psa_status_t secure_storage_ipc_get_info(void *context,
+ 
+ 	(void)client_id;
+ 
+-	/* Validating input parameters */
+-	if (!p_info)
+-		return PSA_ERROR_INVALID_ARGUMENT;
+-
+ 	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
+ 			      TFM_PS_GET_INFO, in_vec,
+ 			      IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
+diff --git a/components/service/secure_storage/include/psa/storage_common.h b/components/service/secure_storage/include/psa/storage_common.h
+index 4f6ba2a7..1fd6b40d 100644
+--- a/components/service/secure_storage/include/psa/storage_common.h
++++ b/components/service/secure_storage/include/psa/storage_common.h
+@@ -20,8 +20,8 @@ typedef uint64_t psa_storage_uid_t;
+ typedef uint32_t psa_storage_create_flags_t;
+ 
+ struct psa_storage_info_t {
+-	size_t capacity;
+-	size_t size;
++	uint32_t capacity;
++	uint32_t size;
+ 	psa_storage_create_flags_t flags;
+ };
+ 
+diff --git a/external/openamp/openamp.cmake b/external/openamp/openamp.cmake
+index aae13bad..75ab2290 100644
+--- a/external/openamp/openamp.cmake
++++ b/external/openamp/openamp.cmake
+@@ -61,7 +61,7 @@ execute_process(COMMAND
+ 			-DCMAKE_SYSTEM_PROCESSOR=arm
+ 			-DEXTERNAL_INCLUDE_PATHS=${OPENAMP_EXTERNAL_INCLUDE_PATHS}
+ 			-DMACHINE=template
+-			-DRPMSG_BUFFER_SIZE=512
++			-DRPMSG_BUFFER_SIZE=8192
+ 			${openamp_SOURCE_DIR}
+ 	WORKING_DIRECTORY
+ 			${openamp_BINARY_DIR}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0020-Use-address-instead-of-pointers.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0020-Use-address-instead-of-pointers.patch
new file mode 100644
index 0000000..844bca3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0020-Use-address-instead-of-pointers.patch
@@ -0,0 +1,169 @@
+From b417c1124af9d4569ba8871dfd1e43e626dddf12 Mon Sep 17 00:00:00 2001
+From: Satish Kumar <satish.kumar01@arm.com>
+Date: Sun, 12 Dec 2021 10:57:17 +0000
+Subject: [PATCH] Use address instead of pointers
+
+Since secure enclave is 32bit and we 64bit there is an issue
+in the protocol communication design that force us to handle
+on our side the manipulation of address and pointers to make
+this work.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+
+---
+ .../service/common/include/psa/client.h       | 15 ++++++++++++++
+ .../service/common/psa_ipc/service_psa_ipc.c  | 20 ++++++++++++-------
+ .../secure_storage_ipc/secure_storage_ipc.c   | 20 +++++++++----------
+ 3 files changed, 38 insertions(+), 17 deletions(-)
+
+diff --git a/components/service/common/include/psa/client.h b/components/service/common/include/psa/client.h
+index 69ccf14f..12dcd68f 100644
+--- a/components/service/common/include/psa/client.h
++++ b/components/service/common/include/psa/client.h
+@@ -81,6 +81,21 @@ struct __attribute__ ((__packed__)) psa_outvec {
+     uint32_t len;                 /*!< the size in bytes                      */
+ };
+ 
++static void *psa_u32_to_ptr(uint32_t addr)
++{
++	return (void *)(uintptr_t)addr;
++}
++
++static uint32_t psa_ptr_to_u32(void *ptr)
++{
++	return (uintptr_t)ptr;
++}
++
++static uint32_t psa_ptr_const_to_u32(const void *ptr)
++{
++	return (uintptr_t)ptr;
++}
++
+ /*************************** PSA Client API **********************************/
+ 
+ /**
+diff --git a/components/service/common/psa_ipc/service_psa_ipc.c b/components/service/common/psa_ipc/service_psa_ipc.c
+index 5e5815db..435c6c0a 100644
+--- a/components/service/common/psa_ipc/service_psa_ipc.c
++++ b/components/service/common/psa_ipc/service_psa_ipc.c
+@@ -62,6 +62,11 @@ static size_t psa_call_out_vec_len(const struct psa_outvec *out_vec, size_t out_
+ 	return resp_len;
+ }
+ 
++static uint32_t psa_virt_to_phys_u32(struct rpc_caller *caller, void *va)
++{
++	return (uintptr_t)rpc_caller_virt_to_phys(caller, va);
++}
++
+ psa_handle_t psa_connect(struct rpc_caller *caller, uint32_t sid,
+ 			 uint32_t version)
+ {
+@@ -147,20 +152,20 @@ psa_status_t psa_call(struct rpc_caller *caller, psa_handle_t psa_handle,
+ 	req_msg->params.psa_call_params.handle = psa_handle;
+ 	req_msg->params.psa_call_params.type = type;
+ 	req_msg->params.psa_call_params.in_len = in_len;
+-	req_msg->params.psa_call_params.in_vec = rpc_caller_virt_to_phys(caller, in_vec_param);
++	req_msg->params.psa_call_params.in_vec = psa_virt_to_phys_u32(caller, in_vec_param);
+ 	req_msg->params.psa_call_params.out_len = out_len;
+-	req_msg->params.psa_call_params.out_vec = rpc_caller_virt_to_phys(caller, out_vec_param);
++	req_msg->params.psa_call_params.out_vec = psa_virt_to_phys_u32(caller, out_vec_param);
+ 
+ 	for (i = 0; i < in_len; i++) {
+-		in_vec_param[i].base = rpc_caller_virt_to_phys(caller, payload);
++		in_vec_param[i].base = psa_virt_to_phys_u32(caller, payload);
+ 		in_vec_param[i].len = in_vec[i].len;
+ 
+-		memcpy(payload, in_vec[i].base, in_vec[i].len);
++		memcpy(payload, psa_u32_to_ptr(in_vec[i].base), in_vec[i].len);
+ 		payload += in_vec[i].len;
+ 	}
+ 
+ 	for (i = 0; i < out_len; i++) {
+-		out_vec_param[i].base = NULL;
++		out_vec_param[i].base = 0;
+ 		out_vec_param[i].len = out_vec[i].len;
+ 	}
+ 
+@@ -182,11 +187,12 @@ psa_status_t psa_call(struct rpc_caller *caller, psa_handle_t psa_handle,
+ 		goto caller_end;
+ 
+ 	out_vec_param = (struct psa_outvec *)rpc_caller_phys_to_virt(caller,
+-						     resp_msg->params.out_vec);
++				psa_u32_to_ptr(resp_msg->params.out_vec));
+ 
+ 	for (i = 0; i < resp_msg->params.out_len; i++) {
+                 out_vec[i].len = out_vec_param[i].len;
+-		memcpy(out_vec[i].base, rpc_caller_phys_to_virt(caller, out_vec_param[i].base),
++		memcpy(psa_u32_to_ptr(out_vec[i].base),
++		       rpc_caller_phys_to_virt(caller,	psa_u32_to_ptr(out_vec_param[i].base)),
+ 		       out_vec[i].len);
+ 	}
+ 
+diff --git a/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
+index a1f369db..bda442a6 100644
+--- a/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
++++ b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
+@@ -22,9 +22,9 @@ static psa_status_t secure_storage_ipc_set(void *context, uint32_t client_id,
+ 	psa_handle_t psa_handle;
+ 	psa_status_t psa_status;
+ 	struct psa_invec in_vec[] = {
+-		{ .base = &uid, .len = sizeof(uid) },
+-		{ .base = p_data, .len = data_length },
+-		{ .base = &create_flags, .len = sizeof(create_flags) },
++		{ .base = psa_ptr_to_u32(&uid), .len = sizeof(uid) },
++		{ .base = psa_ptr_const_to_u32(p_data), .len = data_length },
++		{ .base = psa_ptr_to_u32(&create_flags), .len = sizeof(create_flags) },
+ 	};
+ 
+ 	(void)client_id;
+@@ -53,11 +53,11 @@ static psa_status_t secure_storage_ipc_get(void *context,
+ 	psa_status_t psa_status;
+ 	uint32_t offset = (uint32_t)data_offset;
+ 	struct psa_invec in_vec[] = {
+-		{ .base = &uid, .len = sizeof(uid) },
+-		{ .base = &offset, .len = sizeof(offset) },
++		{ .base = psa_ptr_to_u32(&uid), .len = sizeof(uid) },
++		{ .base = psa_ptr_to_u32(&offset), .len = sizeof(offset) },
+ 	};
+ 	struct psa_outvec out_vec[] = {
+-		{ .base = p_data, .len = data_size },
++		{ .base = psa_ptr_to_u32(p_data), .len = data_size },
+ 	};
+ 
+ 	if (!p_data_length) {
+@@ -84,10 +84,10 @@ static psa_status_t secure_storage_ipc_get_info(void *context,
+ 	psa_handle_t psa_handle;
+ 	psa_status_t psa_status;
+ 	struct psa_invec in_vec[] = {
+-		{ .base = &uid, .len = sizeof(uid) },
++		{ .base = psa_ptr_to_u32(&uid), .len = sizeof(uid) },
+ 	};
+ 	struct psa_outvec out_vec[] = {
+-		{ .base = p_info, .len = sizeof(*p_info) },
++		{ .base = psa_ptr_to_u32(p_info), .len = sizeof(*p_info) },
+ 	};
+ 
+ 	(void)client_id;
+@@ -110,7 +110,7 @@ static psa_status_t secure_storage_ipc_remove(void *context,
+ 	psa_handle_t psa_handle;
+ 	psa_status_t psa_status;
+ 	struct psa_invec in_vec[] = {
+-		{ .base = &uid, .len = sizeof(uid) },
++		{ .base = psa_ptr_to_u32(&uid), .len = sizeof(uid) },
+ 	};
+ 
+ 	(void)client_id;
+@@ -164,7 +164,7 @@ static uint32_t secure_storage_get_support(void *context, uint32_t client_id)
+ 	psa_status_t psa_status;
+ 	uint32_t support_flags;
+ 	struct psa_outvec out_vec[] = {
+-		{ .base = &support_flags, .len =  sizeof(support_flags) },
++		{ .base = psa_ptr_to_u32(&support_flags), .len =  sizeof(support_flags) },
+ 	};
+ 
+ 	(void)client_id;
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0021-Add-psa-ipc-attestation-to-se-proxy.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0021-Add-psa-ipc-attestation-to-se-proxy.patch
new file mode 100644
index 0000000..5376bb2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0021-Add-psa-ipc-attestation-to-se-proxy.patch
@@ -0,0 +1,267 @@
+From 259300dc81b95ff65cd2e95e0fecd140d76e4b5e Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Tue, 7 Dec 2021 11:50:00 +0000
+Subject: [PATCH] Add psa ipc attestation to se proxy
+
+Implement attestation client API as psa ipc and include it to
+se proxy deployment.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+
+
+---
+ .../client/psa_ipc/component.cmake            | 13 +++
+ .../client/psa_ipc/iat_ipc_client.c           | 86 +++++++++++++++++++
+ .../reporter/psa_ipc/component.cmake          | 13 +++
+ .../reporter/psa_ipc/psa_ipc_attest_report.c  | 45 ++++++++++
+ components/service/common/include/psa/sid.h   |  4 +
+ deployments/se-proxy/opteesp/CMakeLists.txt   |  3 +-
+ .../se-proxy/opteesp/service_proxy_factory.c  |  6 ++
+ 7 files changed, 169 insertions(+), 1 deletion(-)
+ create mode 100644 components/service/attestation/client/psa_ipc/component.cmake
+ create mode 100644 components/service/attestation/client/psa_ipc/iat_ipc_client.c
+ create mode 100644 components/service/attestation/reporter/psa_ipc/component.cmake
+ create mode 100644 components/service/attestation/reporter/psa_ipc/psa_ipc_attest_report.c
+
+diff --git a/components/service/attestation/client/psa_ipc/component.cmake b/components/service/attestation/client/psa_ipc/component.cmake
+new file mode 100644
+index 00000000..a5bc6b4a
+--- /dev/null
++++ b/components/service/attestation/client/psa_ipc/component.cmake
+@@ -0,0 +1,13 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++if (NOT DEFINED TGT)
++	message(FATAL_ERROR "mandatory parameter TGT is not defined.")
++endif()
++
++target_sources(${TGT} PRIVATE
++	"${CMAKE_CURRENT_LIST_DIR}/iat_ipc_client.c"
++	)
+diff --git a/components/service/attestation/client/psa_ipc/iat_ipc_client.c b/components/service/attestation/client/psa_ipc/iat_ipc_client.c
+new file mode 100644
+index 00000000..30bd0a13
+--- /dev/null
++++ b/components/service/attestation/client/psa_ipc/iat_ipc_client.c
+@@ -0,0 +1,86 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#include <stddef.h>
++#include <string.h>
++
++#include "../psa/iat_client.h"
++#include <protocols/rpc/common/packed-c/status.h>
++#include <psa/initial_attestation.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++
++/**
++ * @brief      The singleton psa_iat_client instance
++ *
++ * The psa attestation C API assumes a single backend service provider.
++ */
++static struct service_client instance;
++
++
++psa_status_t psa_iat_client_init(struct rpc_caller *caller)
++{
++	return service_client_init(&instance, caller);
++}
++
++void psa_iat_client_deinit(void)
++{
++	service_client_deinit(&instance);
++}
++
++int psa_iat_client_rpc_status(void)
++{
++	return instance.rpc_status;
++}
++
++psa_status_t psa_initial_attest_get_token(const uint8_t *auth_challenge,
++					  size_t challenge_size,
++					  uint8_t *token_buf,
++					  size_t token_buf_size,
++					  size_t *token_size)
++{
++	psa_status_t status = PSA_ERROR_INVALID_ARGUMENT;
++	struct rpc_caller *caller = instance.caller;
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_const_to_u32(auth_challenge), .len = challenge_size},
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(token_buf), .len = token_buf_size},
++	};
++
++	if (!token_buf || !token_buf_size)
++		return PSA_ERROR_INVALID_ARGUMENT;
++
++	status = psa_call(caller, TFM_ATTESTATION_SERVICE_HANDLE,
++			  TFM_ATTEST_GET_TOKEN, in_vec, IOVEC_LEN(in_vec),
++			  out_vec, IOVEC_LEN(out_vec));
++	if (status == PSA_SUCCESS) {
++		*token_size = out_vec[0].len;
++	}
++
++	return status;
++}
++
++psa_status_t psa_initial_attest_get_token_size(size_t challenge_size,
++						size_t *token_size)
++{
++	struct rpc_caller *caller = instance.caller;
++	psa_status_t status;
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&challenge_size), .len = sizeof(uint32_t)}
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(token_size), .len = sizeof(uint32_t)}
++	};
++
++	status = psa_call(caller, TFM_ATTESTATION_SERVICE_HANDLE,
++			  TFM_ATTEST_GET_TOKEN_SIZE,
++			  in_vec, IOVEC_LEN(in_vec),
++			  out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
+diff --git a/components/service/attestation/reporter/psa_ipc/component.cmake b/components/service/attestation/reporter/psa_ipc/component.cmake
+new file mode 100644
+index 00000000..b37830c6
+--- /dev/null
++++ b/components/service/attestation/reporter/psa_ipc/component.cmake
+@@ -0,0 +1,13 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++if (NOT DEFINED TGT)
++	message(FATAL_ERROR "mandatory parameter TGT is not defined.")
++endif()
++
++target_sources(${TGT} PRIVATE
++	"${CMAKE_CURRENT_LIST_DIR}/psa_ipc_attest_report.c"
++	)
+diff --git a/components/service/attestation/reporter/psa_ipc/psa_ipc_attest_report.c b/components/service/attestation/reporter/psa_ipc/psa_ipc_attest_report.c
+new file mode 100644
+index 00000000..15805e8e
+--- /dev/null
++++ b/components/service/attestation/reporter/psa_ipc/psa_ipc_attest_report.c
+@@ -0,0 +1,45 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++/**
++ * A attestation reporter for psa ipc
++ */
++
++#include <stddef.h>
++#include <psa/error.h>
++#include <service/attestation/reporter/attest_report.h>
++#include <psa/initial_attestation.h>
++
++#define TOKEN_BUF_SIZE	1024
++
++static uint8_t token_buf[TOKEN_BUF_SIZE];
++
++int attest_report_create(int32_t client_id, const uint8_t *auth_challenge_data,
++			 size_t auth_challenge_len, const uint8_t **report,
++			 size_t *report_len)
++{
++	*report = token_buf;
++	psa_status_t ret;
++	size_t token_size = 0;
++
++	ret = psa_initial_attest_get_token(auth_challenge_data,
++					   auth_challenge_len, token_buf,
++					   TOKEN_BUF_SIZE, &token_size);
++	if (ret != PSA_SUCCESS) {
++		*report = NULL;
++		*report_len = 0;
++		return ret;
++	}
++
++	*report_len = token_size;
++
++	return PSA_SUCCESS;
++}
++
++void attest_report_destroy(const uint8_t *report)
++{
++	(void)report;
++}
+diff --git a/components/service/common/include/psa/sid.h b/components/service/common/include/psa/sid.h
+index aaa973c6..833f5039 100644
+--- a/components/service/common/include/psa/sid.h
++++ b/components/service/common/include/psa/sid.h
+@@ -50,6 +50,10 @@ extern "C" {
+ #define TFM_ATTESTATION_SERVICE_VERSION                            (1U)
+ #define TFM_ATTESTATION_SERVICE_HANDLE                             (0x40000103U)
+ 
++/* Initial Attestation message types that distinguish Attest services. */
++#define TFM_ATTEST_GET_TOKEN       1001
++#define TFM_ATTEST_GET_TOKEN_SIZE  1002
++
+ /******** TFM_SP_FWU ********/
+ #define TFM_FWU_WRITE_SID                                          (0x000000A0U)
+ #define TFM_FWU_WRITE_VERSION                                      (1U)
+diff --git a/deployments/se-proxy/opteesp/CMakeLists.txt b/deployments/se-proxy/opteesp/CMakeLists.txt
+index 663177b7..af2225e7 100644
+--- a/deployments/se-proxy/opteesp/CMakeLists.txt
++++ b/deployments/se-proxy/opteesp/CMakeLists.txt
+@@ -77,12 +77,13 @@ add_components(TARGET "se-proxy"
+ 		"components/service/attestation/include"
+ 		"components/service/attestation/provider"
+ 		"components/service/attestation/provider/serializer/packed-c"
++		"components/service/attestation/reporter/psa_ipc"
++		"components/service/attestation/client/psa_ipc"
+ 		"components/rpc/openamp/caller/sp"
+ 
+ 		# Stub service provider backends
+ 		"components/rpc/dummy"
+ 		"components/rpc/common/caller"
+-		"components/service/attestation/reporter/stub"
+ 		"components/service/attestation/key_mngr/stub"
+ 		"components/service/crypto/backend/stub"
+ 		"components/service/crypto/client/psa"
+diff --git a/deployments/se-proxy/opteesp/service_proxy_factory.c b/deployments/se-proxy/opteesp/service_proxy_factory.c
+index 57290056..4b8ccecc 100644
+--- a/deployments/se-proxy/opteesp/service_proxy_factory.c
++++ b/deployments/se-proxy/opteesp/service_proxy_factory.c
+@@ -23,12 +23,18 @@ struct openamp_caller openamp;
+ struct rpc_interface *attest_proxy_create(void)
+ {
+ 	struct rpc_interface *attest_iface;
++	struct rpc_caller *attest_caller;
+ 
+ 	/* Static objects for proxy instance */
+ 	static struct attest_provider attest_provider;
+ 
++	attest_caller = openamp_caller_init(&openamp);
++	if (!attest_caller)
++		return NULL;
++
+ 	/* Initialize the service provider */
+ 	attest_iface = attest_provider_init(&attest_provider);
++	psa_iat_client_init(&openamp.rpc_caller);
+ 
+ 	attest_provider_register_serializer(&attest_provider,
+ 		TS_RPC_ENCODING_PACKED_C, packedc_attest_provider_serializer_instance());
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0022-Setup-its-backend-as-openamp-rpc-using-secure-storag.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0022-Setup-its-backend-as-openamp-rpc-using-secure-storag.patch
new file mode 100644
index 0000000..aa389d8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0022-Setup-its-backend-as-openamp-rpc-using-secure-storag.patch
@@ -0,0 +1,164 @@
+From 90006cecbbba58afee5f51e6bd72f7027a257b5e Mon Sep 17 00:00:00 2001
+From: Satish Kumar <satish.kumar01@arm.com>
+Date: Thu, 9 Dec 2021 14:11:06 +0000
+Subject: [PATCH] Setup its backend as openamp rpc using secure storage ipc
+ implementation.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+
+---
+ components/service/common/include/psa/sid.h   | 12 +++++-----
+ .../secure_storage_ipc/secure_storage_ipc.c   | 20 ++++++++---------
+ .../secure_storage_ipc/secure_storage_ipc.h   |  1 +
+ .../se-proxy/opteesp/service_proxy_factory.c  | 22 +++++++++++++------
+ 4 files changed, 32 insertions(+), 23 deletions(-)
+
+diff --git a/components/service/common/include/psa/sid.h b/components/service/common/include/psa/sid.h
+index 833f5039..4a951d4a 100644
+--- a/components/service/common/include/psa/sid.h
++++ b/components/service/common/include/psa/sid.h
+@@ -20,12 +20,12 @@ extern "C" {
+ /* Invalid UID */
+ #define TFM_PS_INVALID_UID 0
+ 
+-/* PS message types that distinguish PS services. */
+-#define TFM_PS_SET                1001
+-#define TFM_PS_GET                1002
+-#define TFM_PS_GET_INFO           1003
+-#define TFM_PS_REMOVE             1004
+-#define TFM_PS_GET_SUPPORT        1005
++/* PS / ITS message types that distinguish PS services. */
++#define TFM_PS_ITS_SET                1001
++#define TFM_PS_ITS_GET                1002
++#define TFM_PS_ITS_GET_INFO           1003
++#define TFM_PS_ITS_REMOVE             1004
++#define TFM_PS_ITS_GET_SUPPORT        1005
+ 
+ /******** TFM_SP_ITS ********/
+ #define TFM_INTERNAL_TRUSTED_STORAGE_SERVICE_SID                   (0x00000070U)
+diff --git a/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
+index bda442a6..0e1b48c0 100644
+--- a/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
++++ b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.c
+@@ -31,8 +31,8 @@ static psa_status_t secure_storage_ipc_set(void *context, uint32_t client_id,
+ 
+ 	ipc->client.rpc_status = TS_RPC_CALL_ACCEPTED;
+ 
+-	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
+-			      TFM_PS_SET, in_vec, IOVEC_LEN(in_vec), NULL, 0);
++	psa_status = psa_call(caller, ipc->service_handle, TFM_PS_ITS_SET,
++			      in_vec, IOVEC_LEN(in_vec), NULL, 0);
+ 	if (psa_status < 0)
+ 		EMSG("ipc_set: psa_call failed: %d", psa_status);
+ 
+@@ -65,8 +65,8 @@ static psa_status_t secure_storage_ipc_get(void *context,
+ 		return PSA_ERROR_INVALID_ARGUMENT;
+ 	}
+ 
+-	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
+-			      TFM_PS_GET, in_vec, IOVEC_LEN(in_vec),
++	psa_status = psa_call(caller, ipc->service_handle,
++			      TFM_PS_ITS_GET, in_vec, IOVEC_LEN(in_vec),
+ 			      out_vec, IOVEC_LEN(out_vec));
+ 	if (psa_status == PSA_SUCCESS)
+ 		*p_data_length = out_vec[0].len;
+@@ -92,8 +92,8 @@ static psa_status_t secure_storage_ipc_get_info(void *context,
+ 
+ 	(void)client_id;
+ 
+-	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
+-			      TFM_PS_GET_INFO, in_vec,
++	psa_status = psa_call(caller, ipc->service_handle,
++			      TFM_PS_ITS_GET_INFO, in_vec,
+ 			      IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
+ 	if (psa_status != PSA_SUCCESS)
+ 		EMSG("ipc_get_info: failed to psa_call: %d", psa_status);
+@@ -115,8 +115,8 @@ static psa_status_t secure_storage_ipc_remove(void *context,
+ 
+ 	(void)client_id;
+ 
+-	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
+-			      TFM_PS_REMOVE, in_vec,
++	psa_status = psa_call(caller, ipc->service_handle,
++			      TFM_PS_ITS_REMOVE, in_vec,
+ 			      IOVEC_LEN(in_vec), NULL, 0);
+ 	if (psa_status != PSA_SUCCESS)
+ 		EMSG("ipc_remove: failed to psa_call: %d", psa_status);
+@@ -169,8 +169,8 @@ static uint32_t secure_storage_get_support(void *context, uint32_t client_id)
+ 
+ 	(void)client_id;
+ 
+-	psa_status = psa_call(caller, TFM_PROTECTED_STORAGE_SERVICE_HANDLE,
+-			      TFM_PS_GET_SUPPORT, NULL, 0,
++	psa_status = psa_call(caller, ipc->service_handle,
++			      TFM_PS_ITS_GET_SUPPORT, NULL, 0,
+ 			      out_vec, IOVEC_LEN(out_vec));
+ 	if (psa_status != PSA_SUCCESS)
+ 		EMSG("ipc_get_support: failed to psa_call: %d", psa_status);
+diff --git a/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.h b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.h
+index e8c1e8fd..d9949f6a 100644
+--- a/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.h
++++ b/components/service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.h
+@@ -21,6 +21,7 @@ struct secure_storage_ipc
+ {
+     struct storage_backend backend;
+     struct service_client client;
++    int32_t service_handle;
+ };
+ 
+ /**
+diff --git a/deployments/se-proxy/opteesp/service_proxy_factory.c b/deployments/se-proxy/opteesp/service_proxy_factory.c
+index 4b8ccecc..1110ac46 100644
+--- a/deployments/se-proxy/opteesp/service_proxy_factory.c
++++ b/deployments/se-proxy/opteesp/service_proxy_factory.c
+@@ -5,6 +5,7 @@
+  */
+ 
+ #include <stddef.h>
++#include <psa/sid.h>
+ #include <rpc/common/endpoint/rpc_interface.h>
+ #include <rpc/openamp/caller/sp/openamp_caller.h>
+ #include <service/attestation/provider/attest_provider.h>
+@@ -60,23 +61,30 @@ struct rpc_interface *ps_proxy_create(void)
+ {
+ 	static struct secure_storage_provider ps_provider;
+ 	static struct secure_storage_ipc ps_backend;
+-	static struct rpc_caller *storage_caller;
++	struct rpc_caller *storage_caller;
+ 	struct storage_backend *backend;
+ 
+ 	storage_caller = openamp_caller_init(&openamp);
+ 	if (!storage_caller)
+ 		return NULL;
+ 	backend = secure_storage_ipc_init(&ps_backend, &openamp.rpc_caller);
++	ps_backend.service_handle = TFM_PROTECTED_STORAGE_SERVICE_HANDLE;
+ 
+ 	return secure_storage_provider_init(&ps_provider, backend);
+ }
+ 
+ struct rpc_interface *its_proxy_create(void)
+ {
+-	static struct mock_store its_backend;
+-	static struct secure_storage_provider its_provider;
+-
+-	struct storage_backend *backend = mock_store_init(&its_backend);
+-
+-	return secure_storage_provider_init(&its_provider, backend);
++        static struct secure_storage_provider its_provider;
++        static struct secure_storage_ipc its_backend;
++        struct rpc_caller *storage_caller;
++        struct storage_backend *backend;
++ 
++        storage_caller = openamp_caller_init(&openamp);
++        if (!storage_caller)
++        	return NULL;
++        backend = secure_storage_ipc_init(&its_backend, &openamp.rpc_caller);
++        its_backend.service_handle = TFM_INTERNAL_TRUSTED_STORAGE_SERVICE_HANDLE;
++ 
++        return secure_storage_provider_init(&its_provider, backend);
+ }
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0023-add-psa-ipc-crypto-backend.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0023-add-psa-ipc-crypto-backend.patch
new file mode 100644
index 0000000..a20a6a4
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0023-add-psa-ipc-crypto-backend.patch
@@ -0,0 +1,2586 @@
+From 5af98a77887c2aa60bc93dbdddb174e03501b733 Mon Sep 17 00:00:00 2001
+From: Rui Miguel Silva <rui.silva@linaro.org>
+Date: Thu, 9 Dec 2021 14:17:39 +0000
+Subject: [PATCH] add psa ipc crypto backend
+
+Add psa ipc crypto backend and attach it to se proxy
+deployment.
+
+Signed-off-by: Rui Miguel Silva <rui.silva@arm.com>
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
+
+
+---
+ components/service/common/include/psa/sid.h   |  73 +++++
+ .../crypto/backend/psa_ipc/component.cmake    |  21 ++
+ .../backend/psa_ipc/crypto_ipc_backend.c      |  26 ++
+ .../backend/psa_ipc/crypto_ipc_backend.h      |  70 ++++
+ .../client/caller/psa_ipc/crypto_caller.h     |  34 ++
+ .../caller/psa_ipc/crypto_caller_aead.h       | 252 +++++++++++++++
+ .../crypto_caller_asymmetric_decrypt.h        |  76 +++++
+ .../crypto_caller_asymmetric_encrypt.h        |  76 +++++
+ .../caller/psa_ipc/crypto_caller_cipher.h     | 246 +++++++++++++++
+ .../caller/psa_ipc/crypto_caller_copy_key.h   |  57 ++++
+ .../psa_ipc/crypto_caller_destroy_key.h       |  51 +++
+ .../caller/psa_ipc/crypto_caller_export_key.h |  59 ++++
+ .../psa_ipc/crypto_caller_export_public_key.h |  59 ++++
+ .../psa_ipc/crypto_caller_generate_key.h      |  55 ++++
+ .../psa_ipc/crypto_caller_generate_random.h   |  57 ++++
+ .../crypto_caller_get_key_attributes.h        |  56 ++++
+ .../caller/psa_ipc/crypto_caller_hash.h       | 220 +++++++++++++
+ .../caller/psa_ipc/crypto_caller_import_key.h |  57 ++++
+ .../psa_ipc/crypto_caller_key_attributes.h    |  51 +++
+ .../psa_ipc/crypto_caller_key_derivation.h    | 298 ++++++++++++++++++
+ .../client/caller/psa_ipc/crypto_caller_mac.h | 207 ++++++++++++
+ .../caller/psa_ipc/crypto_caller_purge_key.h  |  51 +++
+ .../caller/psa_ipc/crypto_caller_sign_hash.h  |  64 ++++
+ .../psa_ipc/crypto_caller_verify_hash.h       |  59 ++++
+ .../crypto/include/psa/crypto_client_struct.h |   8 +-
+ .../service/crypto/include/psa/crypto_sizes.h |   2 +-
+ deployments/se-proxy/opteesp/CMakeLists.txt   |   2 +-
+ .../se-proxy/opteesp/service_proxy_factory.c  |  15 +-
+ .../providers/arm/corstone1000/platform.cmake |   2 +
+ 29 files changed, 2293 insertions(+), 11 deletions(-)
+ create mode 100644 components/service/crypto/backend/psa_ipc/component.cmake
+ create mode 100644 components/service/crypto/backend/psa_ipc/crypto_ipc_backend.c
+ create mode 100644 components/service/crypto/backend/psa_ipc/crypto_ipc_backend.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_aead.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_asymmetric_decrypt.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_asymmetric_encrypt.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_cipher.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_copy_key.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_destroy_key.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_export_key.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_export_public_key.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_generate_key.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_generate_random.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_get_key_attributes.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_hash.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_import_key.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_key_attributes.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_key_derivation.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_mac.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_purge_key.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_sign_hash.h
+ create mode 100644 components/service/crypto/client/caller/psa_ipc/crypto_caller_verify_hash.h
+
+diff --git a/components/service/common/include/psa/sid.h b/components/service/common/include/psa/sid.h
+index 4a951d4a..7a29cc25 100644
+--- a/components/service/common/include/psa/sid.h
++++ b/components/service/common/include/psa/sid.h
+@@ -37,6 +37,79 @@ extern "C" {
+ #define TFM_CRYPTO_VERSION                                         (1U)
+ #define TFM_CRYPTO_HANDLE                                          (0x40000100U)
+ 
++/**
++ * \brief Define a progressive numerical value for each SID which can be used
++ *        when dispatching the requests to the service
++ */
++enum {
++    TFM_CRYPTO_GET_KEY_ATTRIBUTES_SID = (0u),
++    TFM_CRYPTO_RESET_KEY_ATTRIBUTES_SID,
++    TFM_CRYPTO_OPEN_KEY_SID,
++    TFM_CRYPTO_CLOSE_KEY_SID,
++    TFM_CRYPTO_IMPORT_KEY_SID,
++    TFM_CRYPTO_DESTROY_KEY_SID,
++    TFM_CRYPTO_EXPORT_KEY_SID,
++    TFM_CRYPTO_EXPORT_PUBLIC_KEY_SID,
++    TFM_CRYPTO_PURGE_KEY_SID,
++    TFM_CRYPTO_COPY_KEY_SID,
++    TFM_CRYPTO_HASH_COMPUTE_SID,
++    TFM_CRYPTO_HASH_COMPARE_SID,
++    TFM_CRYPTO_HASH_SETUP_SID,
++    TFM_CRYPTO_HASH_UPDATE_SID,
++    TFM_CRYPTO_HASH_FINISH_SID,
++    TFM_CRYPTO_HASH_VERIFY_SID,
++    TFM_CRYPTO_HASH_ABORT_SID,
++    TFM_CRYPTO_HASH_CLONE_SID,
++    TFM_CRYPTO_MAC_COMPUTE_SID,
++    TFM_CRYPTO_MAC_VERIFY_SID,
++    TFM_CRYPTO_MAC_SIGN_SETUP_SID,
++    TFM_CRYPTO_MAC_VERIFY_SETUP_SID,
++    TFM_CRYPTO_MAC_UPDATE_SID,
++    TFM_CRYPTO_MAC_SIGN_FINISH_SID,
++    TFM_CRYPTO_MAC_VERIFY_FINISH_SID,
++    TFM_CRYPTO_MAC_ABORT_SID,
++    TFM_CRYPTO_CIPHER_ENCRYPT_SID,
++    TFM_CRYPTO_CIPHER_DECRYPT_SID,
++    TFM_CRYPTO_CIPHER_ENCRYPT_SETUP_SID,
++    TFM_CRYPTO_CIPHER_DECRYPT_SETUP_SID,
++    TFM_CRYPTO_CIPHER_GENERATE_IV_SID,
++    TFM_CRYPTO_CIPHER_SET_IV_SID,
++    TFM_CRYPTO_CIPHER_UPDATE_SID,
++    TFM_CRYPTO_CIPHER_FINISH_SID,
++    TFM_CRYPTO_CIPHER_ABORT_SID,
++    TFM_CRYPTO_AEAD_ENCRYPT_SID,
++    TFM_CRYPTO_AEAD_DECRYPT_SID,
++    TFM_CRYPTO_AEAD_ENCRYPT_SETUP_SID,
++    TFM_CRYPTO_AEAD_DECRYPT_SETUP_SID,
++    TFM_CRYPTO_AEAD_GENERATE_NONCE_SID,
++    TFM_CRYPTO_AEAD_SET_NONCE_SID,
++    TFM_CRYPTO_AEAD_SET_LENGTHS_SID,
++    TFM_CRYPTO_AEAD_UPDATE_AD_SID,
++    TFM_CRYPTO_AEAD_UPDATE_SID,
++    TFM_CRYPTO_AEAD_FINISH_SID,
++    TFM_CRYPTO_AEAD_VERIFY_SID,
++    TFM_CRYPTO_AEAD_ABORT_SID,
++    TFM_CRYPTO_SIGN_MESSAGE_SID,
++    TFM_CRYPTO_VERIFY_MESSAGE_SID,
++    TFM_CRYPTO_SIGN_HASH_SID,
++    TFM_CRYPTO_VERIFY_HASH_SID,
++    TFM_CRYPTO_ASYMMETRIC_ENCRYPT_SID,
++    TFM_CRYPTO_ASYMMETRIC_DECRYPT_SID,
++    TFM_CRYPTO_KEY_DERIVATION_SETUP_SID,
++    TFM_CRYPTO_KEY_DERIVATION_GET_CAPACITY_SID,
++    TFM_CRYPTO_KEY_DERIVATION_SET_CAPACITY_SID,
++    TFM_CRYPTO_KEY_DERIVATION_INPUT_BYTES_SID,
++    TFM_CRYPTO_KEY_DERIVATION_INPUT_KEY_SID,
++    TFM_CRYPTO_KEY_DERIVATION_KEY_AGREEMENT_SID,
++    TFM_CRYPTO_KEY_DERIVATION_OUTPUT_BYTES_SID,
++    TFM_CRYPTO_KEY_DERIVATION_OUTPUT_KEY_SID,
++    TFM_CRYPTO_KEY_DERIVATION_ABORT_SID,
++    TFM_CRYPTO_RAW_KEY_AGREEMENT_SID,
++    TFM_CRYPTO_GENERATE_RANDOM_SID,
++    TFM_CRYPTO_GENERATE_KEY_SID,
++    TFM_CRYPTO_SID_MAX,
++};
++
+ /******** TFM_SP_PLATFORM ********/
+ #define TFM_SP_PLATFORM_SYSTEM_RESET_SID                           (0x00000040U)
+ #define TFM_SP_PLATFORM_SYSTEM_RESET_VERSION                       (1U)
+diff --git a/components/service/crypto/backend/psa_ipc/component.cmake b/components/service/crypto/backend/psa_ipc/component.cmake
+new file mode 100644
+index 00000000..93c297a8
+--- /dev/null
++++ b/components/service/crypto/backend/psa_ipc/component.cmake
+@@ -0,0 +1,21 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++if (NOT DEFINED TGT)
++	message(FATAL_ERROR "mandatory parameter TGT is not defined.")
++endif()
++
++target_sources(${TGT} PRIVATE
++	"${CMAKE_CURRENT_LIST_DIR}/crypto_ipc_backend.c"
++	)
++
++# The ipc crypto backend uses the psa crypto client to realize the
++# psa crypto API that the crypto provider depends on.  This define
++# configures the psa crypto client to be built with the ipc crypto
++# caller.
++target_compile_definitions(${TGT} PRIVATE
++	PSA_CRYPTO_CLIENT_CALLER_SELECTION_H="service/crypto/client/caller/psa_ipc/crypto_caller.h"
++)
+diff --git a/components/service/crypto/backend/psa_ipc/crypto_ipc_backend.c b/components/service/crypto/backend/psa_ipc/crypto_ipc_backend.c
+new file mode 100644
+index 00000000..e47cd4ff
+--- /dev/null
++++ b/components/service/crypto/backend/psa_ipc/crypto_ipc_backend.c
+@@ -0,0 +1,26 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#include <stddef.h>
++#include <psa/crypto.h>
++#include <service/crypto/client/psa/psa_crypto_client.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include "crypto_ipc_backend.h"
++
++psa_status_t crypto_ipc_backend_init(struct rpc_caller *caller)
++{
++	psa_status_t status = psa_crypto_client_init(caller);
++
++	if (status == PSA_SUCCESS)
++		status = psa_crypto_init();
++
++	return status;
++}
++
++void crypto_ipc_backend_deinit(void)
++{
++	psa_crypto_client_deinit();
++}
+diff --git a/components/service/crypto/backend/psa_ipc/crypto_ipc_backend.h b/components/service/crypto/backend/psa_ipc/crypto_ipc_backend.h
+new file mode 100644
+index 00000000..c13c20e8
+--- /dev/null
++++ b/components/service/crypto/backend/psa_ipc/crypto_ipc_backend.h
+@@ -0,0 +1,70 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef CRYPTO_IPC_BACKEND_H
++#define CRYPTO_IPC_BACKEND_H
++
++#include <service/crypto/client/psa/psa_crypto_client.h>
++#include <psa/error.h>
++#include <rpc_caller.h>
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++/**
++ * \brief This type is used to overcome a limitation in the number of maximum
++ *        IOVECs that can be used especially in psa_aead_encrypt and
++ *        psa_aead_decrypt. To be removed in case the AEAD APIs number of
++ *        parameters passed gets restructured
++ */
++#define TFM_CRYPTO_MAX_NONCE_LENGTH (16u)
++struct psa_ipc_crypto_aead_pack_input {
++	uint8_t nonce[TFM_CRYPTO_MAX_NONCE_LENGTH];
++	uint32_t nonce_length;
++};
++
++struct psa_ipc_crypto_pack_iovec {
++	uint32_t sfn_id;             /*!< Secure function ID used to dispatch the
++				      *   request
++				      */
++	uint16_t step;               /*!< Key derivation step */
++	psa_key_id_t key_id;         /*!< Key id */
++	psa_algorithm_t alg;         /*!< Algorithm */
++	uint32_t op_handle;          /*!< Frontend context handle associated to a
++				      *   multipart operation
++				      */
++	uint32_t capacity;             /*!< Key derivation capacity */
++
++	struct psa_ipc_crypto_aead_pack_input aead_in; /*!< FixMe: Temporarily used for
++							    *   AEAD until the API is
++							    *   restructured
++							    */
++};
++
++#define iov_size sizeof(struct psa_ipc_crypto_pack_iovec)
++
++/**
++ * \brief Initialize the psa ipc crypto backend
++ *
++ * Initializes a crypto backend that uses the psa API client with a
++ * psa_ipc_backend caller to realize the PSA crypto API used by the crypto
++ * service proviser.
++ *
++ * \return PSA_SUCCESS if backend initialized successfully
++ */
++psa_status_t crypto_ipc_backend_init(struct rpc_caller *caller);
++
++/**
++ * \brief Clean-up to free any resource used by the crypto backend
++ */
++void crypto_ipc_backend_deinit(void);
++
++#ifdef __cplusplus
++} /* extern "C" */
++#endif
++
++#endif /* CRYPTO_IPC_BACKEND_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller.h
+new file mode 100644
+index 00000000..0a972187
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller.h
+@@ -0,0 +1,34 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_H
++#define PSA_IPC_CRYPTO_CALLER_H
++
++/**
++ * Includes all header files that form the psa ipc crypto caller
++ * interface.  May be used by a client that needs to call operations
++ * provided by a crypto service instance using the psa ipc interface.
++ */
++#include "crypto_caller_aead.h"
++#include "crypto_caller_asymmetric_decrypt.h"
++#include "crypto_caller_asymmetric_encrypt.h"
++#include "crypto_caller_cipher.h"
++#include "crypto_caller_copy_key.h"
++#include "crypto_caller_destroy_key.h"
++#include "crypto_caller_export_key.h"
++#include "crypto_caller_export_public_key.h"
++#include "crypto_caller_generate_key.h"
++#include "crypto_caller_generate_random.h"
++#include "crypto_caller_get_key_attributes.h"
++#include "crypto_caller_hash.h"
++#include "crypto_caller_import_key.h"
++#include "crypto_caller_key_derivation.h"
++#include "crypto_caller_mac.h"
++#include "crypto_caller_purge_key.h"
++#include "crypto_caller_sign_hash.h"
++#include "crypto_caller_verify_hash.h"
++
++#endif /* PSA_IPC_CRYPTO_CALLER_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_aead.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_aead.h
+new file mode 100644
+index 00000000..78517fe3
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_aead.h
+@@ -0,0 +1,252 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_AEAD_H
++#define PSA_IPC_CRYPTO_CALLER_AEAD_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_aead_encrypt(
++					      struct service_client *context,
++					      psa_key_id_t key,
++					      psa_algorithm_t alg,
++					      const uint8_t *nonce,
++					      size_t nonce_length,
++					      const uint8_t *additional_data,
++					      size_t additional_data_length,
++					      const uint8_t *plaintext,
++					      size_t plaintext_length,
++					      uint8_t *aeadtext,
++					      size_t aeadtext_size,
++					      size_t *aeadtext_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	size_t in_len;
++	int i;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_AEAD_ENCRYPT_SID,
++		.key_id = key,
++		.alg = alg,
++		.aead_in = { .nonce = {0}, .nonce_length = nonce_length },
++	};
++
++	if (!additional_data && additional_data_length)
++		return PSA_ERROR_INVALID_ARGUMENT;
++
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(plaintext),
++			.len = plaintext_length },
++		{ .base = psa_ptr_const_to_u32(additional_data),
++			.len = additional_data_length},
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(aeadtext), .len = aeadtext_size },
++	};
++
++	if (nonce_length > TFM_CRYPTO_MAX_NONCE_LENGTH)
++		return PSA_ERROR_INVALID_ARGUMENT;
++
++	if (nonce) {
++		for (i = 0; i < nonce_length; i++)
++			iov.aead_in.nonce[i] = nonce[i];
++	}
++
++	in_len = IOVEC_LEN(in_vec);
++
++	if (!additional_data)
++		in_len--;
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  in_len, out_vec, IOVEC_LEN(out_vec));
++
++	*aeadtext_length = out_vec[0].len;
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_aead_decrypt(
++					      struct service_client *context,
++					      psa_key_id_t key,
++					      psa_algorithm_t alg,
++					      const uint8_t *nonce,
++					      size_t nonce_length,
++					      const uint8_t *additional_data,
++					      size_t additional_data_length,
++					      const uint8_t *aeadtext,
++					      size_t aeadtext_length,
++					      uint8_t *plaintext,
++					      size_t plaintext_size,
++					      size_t *plaintext_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	size_t in_len;
++	int i;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_AEAD_DECRYPT_SID,
++		.key_id = key,
++		.alg = alg,
++		.aead_in = { .nonce = {0}, .nonce_length = nonce_length },
++	};
++
++	if (!additional_data && additional_data_length)
++		return PSA_ERROR_INVALID_ARGUMENT;
++
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(aeadtext),
++			.len = aeadtext_length },
++		{ .base = psa_ptr_const_to_u32(additional_data),
++			.len = additional_data_length},
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(plaintext), .len = plaintext_size },
++	};
++
++	if (nonce_length > TFM_CRYPTO_MAX_NONCE_LENGTH)
++		return PSA_ERROR_INVALID_ARGUMENT;
++
++	if (nonce) {
++		for (i = 0; i < nonce_length; i++)
++			iov.aead_in.nonce[i] = nonce[i];
++	}
++
++	in_len = IOVEC_LEN(in_vec);
++
++	if (!additional_data)
++		in_len--;
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  in_len, out_vec, IOVEC_LEN(out_vec));
++
++	*plaintext_length = out_vec[0].len;
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_aead_encrypt_setup(
++					    struct service_client *context,
++					    uint32_t *op_handle,
++					    psa_key_id_t key,
++					    psa_algorithm_t alg)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline psa_status_t crypto_caller_aead_decrypt_setup(
++					    struct service_client *context,
++					    uint32_t *op_handle,
++					    psa_key_id_t key,
++					    psa_algorithm_t alg)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline psa_status_t crypto_caller_aead_generate_nonce(
++					     struct service_client *context,
++					     uint32_t op_handle,
++					     uint8_t *nonce,
++					     size_t nonce_size,
++					     size_t *nonce_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline psa_status_t crypto_caller_aead_set_nonce(
++						struct service_client *context,
++						uint32_t op_handle,
++						const uint8_t *nonce,
++						size_t nonce_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline psa_status_t crypto_caller_aead_set_lengths(
++					  struct service_client *context,
++					  uint32_t op_handle,
++					  size_t ad_length,
++					  size_t plaintext_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline psa_status_t crypto_caller_aead_update_ad(
++						struct service_client *context,
++						uint32_t op_handle,
++						const uint8_t *input,
++						size_t input_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline psa_status_t crypto_caller_aead_update(
++					     struct service_client *context,
++					     uint32_t op_handle,
++					     const uint8_t *input,
++					     size_t input_length,
++					     uint8_t *output,
++					     size_t output_size,
++					     size_t *output_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline psa_status_t crypto_caller_aead_finish(
++					     struct service_client *context,
++					     uint32_t op_handle,
++					     uint8_t *aeadtext,
++					     size_t aeadtext_size,
++					     size_t *aeadtext_length,
++					     uint8_t *tag,
++					     size_t tag_size,
++					     size_t *tag_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline psa_status_t crypto_caller_aead_verify(
++					     struct service_client *context,
++					     uint32_t op_handle,
++					     uint8_t *plaintext,
++					     size_t plaintext_size,
++					     size_t *plaintext_length,
++					     const uint8_t *tag,
++					     size_t tag_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline psa_status_t crypto_caller_aead_abort(
++					    struct service_client *context,
++					    uint32_t op_handle)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_AEAD_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_asymmetric_decrypt.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_asymmetric_decrypt.h
+new file mode 100644
+index 00000000..ff01815c
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_asymmetric_decrypt.h
+@@ -0,0 +1,76 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_ASYMMETRIC_DECRYPT_H
++#define PSA_IPC_CRYPTO_CALLER_ASYMMETRIC_DECRYPT_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_asymmetric_decrypt(
++				    struct service_client *context,
++				    psa_key_id_t id,
++				    psa_algorithm_t alg,
++				    const uint8_t *input, size_t input_length,
++				    const uint8_t *salt, size_t salt_length,
++				    uint8_t *output, size_t output_size,
++				    size_t *output_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	size_t in_len;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_ASYMMETRIC_DECRYPT_SID,
++		.key_id = id,
++		.alg = alg,
++	};
++
++	/* Sanitize optional input */
++	if (!salt && salt_length)
++		return PSA_ERROR_INVALID_ARGUMENT;
++
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(input), .len = input_length },
++		{ .base = psa_ptr_const_to_u32(salt), .len = salt_length },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(output), .len = output_size },
++	};
++
++
++	in_len = IOVEC_LEN(in_vec);
++	if (!salt)
++		in_len--;
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  in_len, out_vec, IOVEC_LEN(out_vec));
++
++	*output_length = out_vec[0].len;
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_ASYMMETRIC_DECRYPT_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_asymmetric_encrypt.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_asymmetric_encrypt.h
+new file mode 100644
+index 00000000..1daf1689
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_asymmetric_encrypt.h
+@@ -0,0 +1,76 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_ASYMMETRIC_ENCRYPT_H
++#define PSA_IPC_CRYPTO_CALLER_ASYMMETRIC_ENCRYPT_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_asymmetric_encrypt(
++				    struct service_client *context,
++				    psa_key_id_t id,
++				    psa_algorithm_t alg,
++				    const uint8_t *input, size_t input_length,
++				    const uint8_t *salt, size_t salt_length,
++				    uint8_t *output, size_t output_size,
++				    size_t *output_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	size_t in_len;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_ASYMMETRIC_ENCRYPT_SID,
++		.key_id = id,
++		.alg = alg,
++	};
++
++	/* Sanitize optional input */
++	if (!salt && salt_length)
++		return PSA_ERROR_INVALID_ARGUMENT;
++
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(input), .len = input_length },
++		{ .base = psa_ptr_const_to_u32(salt), .len = salt_length },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(output), .len = output_size },
++	};
++
++
++	in_len = IOVEC_LEN(in_vec);
++	if (!salt)
++		in_len--;
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  in_len, out_vec, IOVEC_LEN(out_vec));
++
++	*output_length = out_vec[0].len;
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_ASYMMETRIC_ENCRYPT_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_cipher.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_cipher.h
+new file mode 100644
+index 00000000..fbefb28d
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_cipher.h
+@@ -0,0 +1,246 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_CIPHER_H
++#define PSA_IPC_CRYPTO_CALLER_CIPHER_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_cipher_encrypt_setup(
++					      struct service_client *context,
++					      uint32_t *op_handle,
++					      psa_key_id_t key,
++					      psa_algorithm_t alg)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_CIPHER_ENCRYPT_SETUP_SID,
++		.key_id = key,
++		.alg = alg,
++		.op_handle = *op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(op_handle), .len = sizeof(uint32_t) }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_cipher_decrypt_setup(
++					      struct service_client *context,
++					      uint32_t *op_handle,
++					      psa_key_id_t key,
++					      psa_algorithm_t alg)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_CIPHER_DECRYPT_SETUP_SID,
++		.key_id = key,
++		.alg = alg,
++		.op_handle = *op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(op_handle), .len = sizeof(uint32_t) }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_cipher_generate_iv(
++					    struct service_client *context,
++					    uint32_t op_handle,
++					    uint8_t *iv,
++					    size_t iv_size,
++					    size_t *iv_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_CIPHER_GENERATE_IV_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++		{ .base = psa_ptr_to_u32(iv), .len = iv_size },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*iv_length = out_vec[1].len;
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_cipher_set_iv(
++					       struct service_client *context,
++					       uint32_t op_handle,
++					       const uint8_t *iv,
++					       size_t iv_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_CIPHER_SET_IV_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(iv), .len = iv_length },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_cipher_update(
++					       struct service_client *context,
++					       uint32_t op_handle,
++					       const uint8_t *input,
++					       size_t input_length,
++					       uint8_t *output,
++					       size_t output_size,
++					       size_t *output_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_CIPHER_UPDATE_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(input), .len = input_length },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++		{ .base = psa_ptr_to_u32(output), .len = output_size },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*output_length = out_vec[1].len;
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_cipher_finish(
++					       struct service_client *context,
++					       uint32_t op_handle,
++					       uint8_t *output,
++					       size_t output_size,
++					       size_t *output_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_CIPHER_FINISH_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++		{ .base = psa_ptr_to_u32(output), .len = output_size },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*output_length = out_vec[1].len;
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_cipher_abort(
++					      struct service_client *context,
++					      uint32_t op_handle)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_CIPHER_ABORT_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline size_t crypto_caller_cipher_max_update_size(const struct service_client *context)
++{
++	/* Returns the maximum number of bytes that may be
++	 * carried as a parameter of the cipher_update operation
++	 * using the ipc encoding.
++	 */
++	size_t payload_space = context->service_info.max_payload;
++	size_t overhead = iov_size;
++
++	/* Allow for output to be a whole number of blocks */
++	overhead += PSA_BLOCK_CIPHER_BLOCK_MAX_SIZE;
++
++	return (payload_space > overhead) ? payload_space - overhead : 0;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_CIPHER_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_copy_key.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_copy_key.h
+new file mode 100644
+index 00000000..9a988171
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_copy_key.h
+@@ -0,0 +1,57 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_COPY_KEY_H
++#define PSA_IPC_CRYPTO_CALLER_COPY_KEY_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_copy_key(struct service_client *context,
++						  psa_key_id_t source_key,
++						  const psa_key_attributes_t *attributes,
++						  psa_key_id_t *target_key)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_COPY_KEY_SID,
++		.key_id = source_key,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec) },
++		{ .base = psa_ptr_const_to_u32(attributes), .len = sizeof(psa_key_attributes_t) },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(target_key), .len = sizeof(psa_key_id_t) }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_COPY_KEY_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_destroy_key.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_destroy_key.h
+new file mode 100644
+index 00000000..d00f4faa
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_destroy_key.h
+@@ -0,0 +1,51 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_DESTROY_KEY_H
++#define PSA_IPC_CRYPTO_CALLER_DESTROY_KEY_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_destroy_key(struct service_client *context,
++						     psa_key_id_t id)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_DESTROY_KEY_SID,
++		.key_id = id,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), NULL, 0);
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_DESTROY_KEY_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_export_key.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_export_key.h
+new file mode 100644
+index 00000000..8ac5477f
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_export_key.h
+@@ -0,0 +1,59 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_EXPORT_KEY_H
++#define PSA_IPC_CRYPTO_CALLER_EXPORT_KEY_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_export_key(struct service_client *context,
++						    psa_key_id_t id,
++						    uint8_t *data,
++						    size_t data_size,
++						    size_t *data_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_EXPORT_KEY_SID,
++		.key_id = id,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec) },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(data), .len = data_size }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*data_length = out_vec[0].len;
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_EXPORT_KEY_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_export_public_key.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_export_public_key.h
+new file mode 100644
+index 00000000..b24c47f1
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_export_public_key.h
+@@ -0,0 +1,59 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_EXPORT_PUBLIC_KEY_H
++#define PSA_IPC_CRYPTO_CALLER_EXPORT_PUBLIC_KEY_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_export_public_key(struct service_client *context,
++							   psa_key_id_t id,
++							   uint8_t *data,
++							   size_t data_size,
++							   size_t *data_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_EXPORT_PUBLIC_KEY_SID,
++		.key_id = id,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec) },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(data), .len = data_size }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*data_length = out_vec[0].len;
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_EXPORT_PUBLIC_KEY_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_generate_key.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_generate_key.h
+new file mode 100644
+index 00000000..1b66ed40
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_generate_key.h
+@@ -0,0 +1,55 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_GENERATE_KEY_H
++#define PSA_IPC_CRYPTO_CALLER_GENERATE_KEY_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_generate_key(struct service_client *context,
++						      const psa_key_attributes_t *attributes,
++						      psa_key_id_t *id)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_GENERATE_KEY_SID,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec) },
++		{ .base = psa_ptr_const_to_u32(attributes), .len = sizeof(psa_key_attributes_t) },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(id), .len = sizeof(psa_key_id_t) }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_GENERATE_KEY_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_generate_random.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_generate_random.h
+new file mode 100644
+index 00000000..7c538237
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_generate_random.h
+@@ -0,0 +1,57 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_GENERATE_RANDOM_H
++#define PSA_IPC_CRYPTO_CALLER_GENERATE_RANDOM_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_generate_random(struct service_client *context,
++							 uint8_t *output,
++							 size_t output_size)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_GENERATE_RANDOM_SID,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec) },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(output), .len = output_size }
++	};
++
++	if (!output_size)
++		return PSA_SUCCESS;
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_GENERATE_RANDOM_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_get_key_attributes.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_get_key_attributes.h
+new file mode 100644
+index 00000000..22f1d18f
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_get_key_attributes.h
+@@ -0,0 +1,56 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_GET_KEY_ATTRIBUTES_H
++#define PSA_IPC_CRYPTO_CALLER_GET_KEY_ATTRIBUTES_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_get_key_attributes(
++					    struct service_client *context,
++					    psa_key_id_t key,
++					    psa_key_attributes_t *attributes)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_GET_KEY_ATTRIBUTES_SID,
++		.key_id = key,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec) },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(attributes), .len = sizeof(psa_key_attributes_t) }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_GET_KEY_ATTRIBUTES_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_hash.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_hash.h
+new file mode 100644
+index 00000000..9f37908a
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_hash.h
+@@ -0,0 +1,220 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_HASH_H
++#define PSA_IPC_CRYPTO_CALLER_HASH_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_hash_setup(
++					    struct service_client *context,
++					    uint32_t *op_handle,
++					    psa_algorithm_t alg)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_HASH_SETUP_SID,
++		.alg = alg,
++		.op_handle = *op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(op_handle), .len = sizeof(uint32_t) }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_hash_update(
++					     struct service_client *context,
++					     uint32_t op_handle,
++					     const uint8_t *input,
++					     size_t input_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_HASH_UPDATE_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(input), .len = input_length },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_hash_finish(
++					     struct service_client *context,
++					     uint32_t op_handle,
++					     uint8_t *hash,
++					     size_t hash_size,
++					     size_t *hash_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_HASH_FINISH_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++		{ .base = psa_ptr_to_u32(hash), .len = hash_size},
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*hash_length = out_vec[1].len;
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_hash_abort(
++					    struct service_client *context,
++					    uint32_t op_handle)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_HASH_ABORT_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_hash_verify(
++					     struct service_client *context,
++					     uint32_t op_handle,
++					     const uint8_t *hash,
++					     size_t hash_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_HASH_VERIFY_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(hash), .len = hash_length},
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_hash_clone(
++					    struct service_client *context,
++					    uint32_t source_op_handle,
++					    uint32_t *target_op_handle)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_HASH_CLONE_SID,
++		.op_handle = source_op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(target_op_handle),
++			.len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_hash_suspend(struct service_client *context,
++	uint32_t op_handle,
++	uint8_t *hash_state,
++	size_t hash_state_size,
++	size_t *hash_state_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline psa_status_t crypto_caller_hash_resume(struct service_client *context,
++	uint32_t op_handle,
++	const uint8_t *hash_state,
++	size_t hash_state_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
++static inline size_t crypto_caller_hash_max_update_size(const struct service_client *context)
++{
++	/* Returns the maximum number of bytes that may be
++	 * carried as a parameter of the hash_update operation
++	 * using the packed-c encoding.
++	 */
++	size_t payload_space = context->service_info.max_payload;
++	size_t overhead = iov_size;
++
++	return (payload_space > overhead) ? payload_space - overhead : 0;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_HASH_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_import_key.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_import_key.h
+new file mode 100644
+index 00000000..d4703366
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_import_key.h
+@@ -0,0 +1,57 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_IMPORT_KEY_H
++#define PSA_IPC_CRYPTO_CALLER_IMPORT_KEY_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_import_key(struct service_client *context,
++				    const psa_key_attributes_t *attributes,
++				    const uint8_t *data, size_t data_length,
++				    psa_key_id_t *id)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_IMPORT_KEY_SID,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec) },
++		{ .base = psa_ptr_const_to_u32(attributes), .len = sizeof(psa_key_attributes_t) },
++		{ .base = psa_ptr_const_to_u32(data), .len = data_length }
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(id), .len = sizeof(psa_key_id_t) }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PACKEDC_CRYPTO_CALLER_IMPORT_KEY_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_key_attributes.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_key_attributes.h
+new file mode 100644
+index 00000000..2fad2f0a
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_key_attributes.h
+@@ -0,0 +1,51 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PACKEDC_CRYPTO_CALLER_KEY_ATTRIBUTES_H
++#define PACKEDC_CRYPTO_CALLER_KEY_ATTRIBUTES_H
++
++#include <psa/crypto.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline void packedc_crypto_caller_translate_key_attributes_to_proto(
++	struct ts_crypto_key_attributes *proto_attributes,
++	const psa_key_attributes_t *psa_attributes)
++{
++	proto_attributes->type = psa_get_key_type(psa_attributes);
++	proto_attributes->key_bits = psa_get_key_bits(psa_attributes);
++	proto_attributes->lifetime = psa_get_key_lifetime(psa_attributes);
++	proto_attributes->id = psa_get_key_id(psa_attributes);
++
++	proto_attributes->policy.usage = psa_get_key_usage_flags(psa_attributes);
++	proto_attributes->policy.alg = psa_get_key_algorithm(psa_attributes);
++ }
++
++static inline void packedc_crypto_caller_translate_key_attributes_from_proto(
++	psa_key_attributes_t *psa_attributes,
++	const struct ts_crypto_key_attributes *proto_attributes)
++{
++	psa_set_key_type(psa_attributes, proto_attributes->type);
++	psa_set_key_bits(psa_attributes, proto_attributes->key_bits);
++	psa_set_key_lifetime(psa_attributes, proto_attributes->lifetime);
++
++	if (proto_attributes->lifetime == PSA_KEY_LIFETIME_PERSISTENT) {
++
++		psa_set_key_id(psa_attributes, proto_attributes->id);
++	}
++
++	psa_set_key_usage_flags(psa_attributes, proto_attributes->policy.usage);
++	psa_set_key_algorithm(psa_attributes, proto_attributes->policy.alg);
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PACKEDC_CRYPTO_CALLER_KEY_ATTRIBUTES_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_key_derivation.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_key_derivation.h
+new file mode 100644
+index 00000000..5ce4fb6c
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_key_derivation.h
+@@ -0,0 +1,298 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_KEY_DERIVATION_H
++#define PSA_IPC_CRYPTO_CALLER_KEY_DERIVATION_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_key_derivation_setup(
++					      struct service_client *context,
++					      uint32_t *op_handle,
++					      psa_algorithm_t alg)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_KEY_DERIVATION_SETUP_SID,
++		.alg = alg,
++		.op_handle = *op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(op_handle), .len = sizeof(uint32_t) }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_key_derivation_get_capacity(
++					     struct service_client *context,
++					     const uint32_t op_handle,
++					     size_t *capacity)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_KEY_DERIVATION_GET_CAPACITY_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(capacity), .len = sizeof(uint32_t) }
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_key_derivation_set_capacity(
++					     struct service_client *context,
++					     uint32_t op_handle,
++					     size_t capacity)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_KEY_DERIVATION_SET_CAPACITY_SID,
++		.capacity = capacity,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), NULL, 0);
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_key_derivation_input_bytes(
++					    struct service_client *context,
++					    uint32_t op_handle,
++					    psa_key_derivation_step_t step,
++					    const uint8_t *data,
++					    size_t data_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_KEY_DERIVATION_INPUT_BYTES_SID,
++		.step = step,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(data), .len = data_length },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), NULL, 0);
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_key_derivation_input_key(
++					  struct service_client *context,
++					  uint32_t op_handle,
++					  psa_key_derivation_step_t step,
++					  psa_key_id_t key)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_KEY_DERIVATION_INPUT_KEY_SID,
++		.key_id = key,
++		.step = step,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), NULL, 0);
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_key_derivation_output_bytes(
++					     struct service_client *context,
++					     uint32_t op_handle,
++					     uint8_t *output,
++					     size_t output_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_KEY_DERIVATION_OUTPUT_BYTES_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(output), .len = output_length },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_key_derivation_output_key(
++				   struct service_client *context,
++				   const psa_key_attributes_t *attributes,
++				   uint32_t op_handle,
++				   psa_key_id_t *key)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_KEY_DERIVATION_OUTPUT_KEY_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(attributes),
++			.len = sizeof(psa_key_attributes_t) },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(key), .len = sizeof(psa_key_id_t)},
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_key_derivation_abort(
++					      struct service_client *context,
++					      uint32_t op_handle)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_KEY_DERIVATION_ABORT_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_key_derivation_key_agreement(
++				      struct service_client *context,
++				      uint32_t op_handle,
++				      psa_key_derivation_step_t step,
++				      psa_key_id_t private_key,
++				      const uint8_t *peer_key,
++				      size_t peer_key_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_KEY_DERIVATION_KEY_AGREEMENT_SID,
++		.key_id = private_key,
++		.step = step,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(peer_key),
++			.len = peer_key_length},
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), NULL, 0);
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_raw_key_agreement(
++					   struct service_client *context,
++					   psa_algorithm_t alg,
++					   psa_key_id_t private_key,
++					   const uint8_t *peer_key,
++					   size_t peer_key_length,
++					   uint8_t *output,
++					   size_t output_size,
++					   size_t *output_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_RAW_KEY_AGREEMENT_SID,
++		.alg = alg,
++		.key_id = private_key,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(peer_key),
++			.len = peer_key_length},
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(output), .len = output_size },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*output_length = out_vec[0].len;
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_KEY_DERIVATION_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_mac.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_mac.h
+new file mode 100644
+index 00000000..3a820192
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_mac.h
+@@ -0,0 +1,207 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_MAC_H
++#define PSA_IPC_CRYPTO_CALLER_MAC_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_mac_sign_setup(
++						struct service_client *context,
++						uint32_t *op_handle,
++						psa_key_id_t key,
++						psa_algorithm_t alg)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_MAC_SIGN_SETUP_SID,
++		.key_id = key,
++		.alg = alg,
++		.op_handle = *op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_mac_verify_setup(
++					  struct service_client *context,
++					  uint32_t *op_handle,
++					  psa_key_id_t key,
++					  psa_algorithm_t alg)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_MAC_VERIFY_SETUP_SID,
++		.key_id = key,
++		.alg = alg,
++		.op_handle = *op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_mac_update(
++					    struct service_client *context,
++					    uint32_t op_handle,
++					    const uint8_t *input,
++					    size_t input_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_MAC_UPDATE_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(input), .len = input_length },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_mac_sign_finish(
++						 struct service_client *context,
++						 uint32_t op_handle,
++						 uint8_t *mac,
++						 size_t mac_size,
++						 size_t *mac_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_MAC_SIGN_FINISH_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++		{ .base = psa_ptr_to_u32(mac), .len = mac_size },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*mac_length = out_vec[1].len;
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_mac_verify_finish(
++					   struct service_client *context,
++					   uint32_t op_handle,
++					   const uint8_t *mac,
++					   size_t mac_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_MAC_VERIFY_FINISH_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(mac), .len = mac_length },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline psa_status_t crypto_caller_mac_abort(
++					   struct service_client *context,
++					   uint32_t op_handle)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_MAC_ABORT_SID,
++		.op_handle = op_handle,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
++}
++
++static inline size_t crypto_caller_mac_max_update_size(const struct service_client *context)
++{
++	/* Returns the maximum number of bytes that may be
++	 * carried as a parameter of the mac_update operation
++	 * using the packed-c encoding.
++	 */
++	size_t payload_space = context->service_info.max_payload;
++	size_t overhead = iov_size;
++
++	return (payload_space > overhead) ? payload_space - overhead : 0;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_MAC_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_purge_key.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_purge_key.h
+new file mode 100644
+index 00000000..a3a796e2
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_purge_key.h
+@@ -0,0 +1,51 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PACKEDC_CRYPTO_CALLER_PURGE_KEY_H
++#define PACKEDC_CRYPTO_CALLER_PURGE_KEY_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_purge_key(struct service_client *context,
++						   psa_key_id_t id)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_PURGE_KEY_SID,
++		.key_id = id,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec) },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), NULL, 0);
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PACKEDC_CRYPTO_CALLER_PURGE_KEY_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_sign_hash.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_sign_hash.h
+new file mode 100644
+index 00000000..71d88ced
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_sign_hash.h
+@@ -0,0 +1,64 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_SIGN_HASH_H
++#define PSA_IPC_CRYPTO_CALLER_SIGN_HASH_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_sign_hash(struct service_client *context,
++						   psa_key_id_t id,
++						   psa_algorithm_t alg,
++						   const uint8_t *hash,
++						   size_t hash_length,
++						   uint8_t *signature,
++						   size_t signature_size,
++						   size_t *signature_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_SIGN_HASH_SID,
++		.key_id = id,
++		.alg = alg,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(hash), .len = hash_length },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(signature), .len = signature_size },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*signature_length = out_vec[0].len;
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_SIGN_HASH_H */
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_verify_hash.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_verify_hash.h
+new file mode 100644
+index 00000000..e16f6e54
+--- /dev/null
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_verify_hash.h
+@@ -0,0 +1,59 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef PSA_IPC_CRYPTO_CALLER_VERIFY_HASH_H
++#define PSA_IPC_CRYPTO_CALLER_VERIFY_HASH_H
++
++#include <string.h>
++#include <stdlib.h>
++#include <psa/crypto.h>
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <service/common/client/service_client.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include <protocols/service/crypto/packed-c/opcodes.h>
++#include <protocols/service/crypto/packed-c/key_attributes.h>
++#include <protocols/service/crypto/packed-c/import_key.h>
++#include "crypto_caller_key_attributes.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++static inline psa_status_t crypto_caller_verify_hash(struct service_client *context,
++						     psa_key_id_t id,
++						     psa_algorithm_t alg,
++						     const uint8_t *hash,
++						     size_t hash_length,
++						     const uint8_t *signature,
++						     size_t signature_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_VERIFY_HASH_SID,
++		.key_id = id,
++		.alg = alg,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec) },
++		{ .base = psa_ptr_const_to_u32(hash), .len = hash_length },
++		{ .base = psa_ptr_const_to_u32(signature), .len = signature_length},
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), NULL, 0);
++
++	return status;
++}
++
++#ifdef __cplusplus
++}
++#endif
++
++#endif /* PSA_IPC_CRYPTO_CALLER_VERIFY_HASH_H */
+diff --git a/components/service/crypto/include/psa/crypto_client_struct.h b/components/service/crypto/include/psa/crypto_client_struct.h
+index abd420c8..bf95c982 100644
+--- a/components/service/crypto/include/psa/crypto_client_struct.h
++++ b/components/service/crypto/include/psa/crypto_client_struct.h
+@@ -31,12 +31,12 @@ extern "C" {
+  * data structure internally. */
+ struct psa_client_key_attributes_s
+ {
++    uint16_t type;
++    uint16_t bits;
+     uint32_t lifetime;
+-    uint32_t id;
+-    uint32_t alg;
++    psa_key_id_t id;
+     uint32_t usage;
+-    size_t bits;
+-    uint16_t type;
++    uint32_t alg;
+ };
+ 
+ #define PSA_CLIENT_KEY_ATTRIBUTES_INIT {0, 0, 0, 0, 0, 0}
+diff --git a/components/service/crypto/include/psa/crypto_sizes.h b/components/service/crypto/include/psa/crypto_sizes.h
+index 7a0149bb..4d7bf6e9 100644
+--- a/components/service/crypto/include/psa/crypto_sizes.h
++++ b/components/service/crypto/include/psa/crypto_sizes.h
+@@ -81,7 +81,7 @@
+ #define PSA_HASH_MAX_SIZE 64
+ #define PSA_HMAC_MAX_HASH_BLOCK_SIZE 128
+ #else
+-#define PSA_HASH_MAX_SIZE 32
++#define PSA_HASH_MAX_SIZE 64
+ #define PSA_HMAC_MAX_HASH_BLOCK_SIZE 64
+ #endif
+ 
+diff --git a/deployments/se-proxy/opteesp/CMakeLists.txt b/deployments/se-proxy/opteesp/CMakeLists.txt
+index af2225e7..21904283 100644
+--- a/deployments/se-proxy/opteesp/CMakeLists.txt
++++ b/deployments/se-proxy/opteesp/CMakeLists.txt
+@@ -85,7 +85,7 @@ add_components(TARGET "se-proxy"
+ 		"components/rpc/dummy"
+ 		"components/rpc/common/caller"
+ 		"components/service/attestation/key_mngr/stub"
+-		"components/service/crypto/backend/stub"
++		"components/service/crypto/backend/psa_ipc"
+ 		"components/service/crypto/client/psa"
+ 		"components/service/secure_storage/backend/mock_store"
+ )
+diff --git a/deployments/se-proxy/opteesp/service_proxy_factory.c b/deployments/se-proxy/opteesp/service_proxy_factory.c
+index 1110ac46..7edeef8b 100644
+--- a/deployments/se-proxy/opteesp/service_proxy_factory.c
++++ b/deployments/se-proxy/opteesp/service_proxy_factory.c
+@@ -15,7 +15,7 @@
+ #include <trace.h>
+ 
+ /* Stub backends */
+-#include <service/crypto/backend/stub/stub_crypto_backend.h>
++#include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
+ #include <service/secure_storage/backend/secure_storage_ipc/secure_storage_ipc.h>
+ #include <service/secure_storage/backend/mock_store/mock_store.h>
+ 
+@@ -47,12 +47,17 @@ struct rpc_interface *crypto_proxy_create(void)
+ {
+ 	struct rpc_interface *crypto_iface = NULL;
+ 	struct crypto_provider *crypto_provider;
++	struct rpc_caller *crypto_caller;
+ 
+-	if (stub_crypto_backend_init() == PSA_SUCCESS) {
++	crypto_caller = openamp_caller_init(&openamp);
++	if (!crypto_caller)
++		return NULL;
++
++	if (crypto_ipc_backend_init(&openamp.rpc_caller) != PSA_SUCCESS)
++		return NULL;
+ 
+-		crypto_provider = crypto_provider_factory_create();
+-		crypto_iface = service_provider_get_rpc_interface(&crypto_provider->base_provider);
+-	}
++	crypto_provider = crypto_provider_factory_create();
++	crypto_iface = service_provider_get_rpc_interface(&crypto_provider->base_provider);
+ 
+ 	return crypto_iface;
+ }
+diff --git a/platform/providers/arm/corstone1000/platform.cmake b/platform/providers/arm/corstone1000/platform.cmake
+index bb778bb9..51e5faa3 100644
+--- a/platform/providers/arm/corstone1000/platform.cmake
++++ b/platform/providers/arm/corstone1000/platform.cmake
+@@ -8,3 +8,5 @@
+ 
+ # include MHU driver
+ include(${TS_ROOT}/platform/drivers/arm/mhu_driver/component.cmake)
++
++add_compile_definitions(MBEDTLS_ECP_DP_SECP521R1_ENABLED)
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0024-Increase-SMM-gateway-UEFI-variable-macro-value.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0024-Increase-SMM-gateway-UEFI-variable-macro-value.patch
new file mode 100644
index 0000000..91207bd
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0024-Increase-SMM-gateway-UEFI-variable-macro-value.patch
@@ -0,0 +1,34 @@
+From 8fca2a8eed6ebc1cbf9f7972c6a9bb137ebafe1a Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Sun, 12 Dec 2021 17:07:03 +0000
+Subject: [PATCH] Increase SMM gateway UEFI variable macro value
+
+The maximum number of UEFI variables that be supported by SMM
+gateway is currently 40. When more than 40 variables are written,
+or read SMM gateway returns error code. Currently this value is
+increased to 100 to support more UEFI variables.
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+
+---
+ deployments/smm-gateway/smm_gateway.c | 3 +++
+ 1 file changed, 3 insertions(+)
+
+diff --git a/deployments/smm-gateway/smm_gateway.c b/deployments/smm-gateway/smm_gateway.c
+index 7828b3af..7e6729ee 100644
+--- a/deployments/smm-gateway/smm_gateway.c
++++ b/deployments/smm-gateway/smm_gateway.c
+@@ -20,6 +20,9 @@
+ #define SMM_GATEWAY_NV_STORE_SN		"sn:ffa:751bf801-3dde-4768-a514-0f10aeed1790:0"
+ #endif
+ 
++/* Maximum number of UEFI variables set to 100 */
++#define SMM_GATEWAY_MAX_UEFI_VARIABLES		(100)
++
+ /* Default maximum number of UEFI variables */
+ #ifndef SMM_GATEWAY_MAX_UEFI_VARIABLES
+ #define SMM_GATEWAY_MAX_UEFI_VARIABLES		(40)
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0025-Add-stub-capsule-update-service-components.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0025-Add-stub-capsule-update-service-components.patch
new file mode 100644
index 0000000..943568c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0025-Add-stub-capsule-update-service-components.patch
@@ -0,0 +1,436 @@
+From eb1beb0f4f3a0d97a1ee941b068fb1f3b7ba7d7b Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Tue, 12 Oct 2021 15:45:41 +0100
+Subject: [PATCH] Add stub capsule update service components
+
+To facilitate development of a capsule update service provider,
+stub components are added to provide a starting point for an
+implementation. The capsule update service provider is integrated
+into the se-proxy/opteesp deployment.
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: I0d4049bb4de5af7ca80806403301692507085d28
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../backend/capsule_update_backend.h          |  24 ++++
+ .../provider/capsule_update_provider.c        | 133 ++++++++++++++++++
+ .../provider/capsule_update_provider.h        |  51 +++++++
+ .../capsule_update/provider/component.cmake   |  13 ++
+ deployments/se-proxy/opteesp/CMakeLists.txt   |   1 +
+ deployments/se-proxy/opteesp/se_proxy_sp.c    |   3 +
+ .../se-proxy/opteesp/service_proxy_factory.c  |  16 +++
+ .../se-proxy/opteesp/service_proxy_factory.h  |   1 +
+ deployments/se-proxy/se_proxy_interfaces.h    |   9 +-
+ .../capsule_update/capsule_update_proto.h     |  13 ++
+ protocols/service/capsule_update/opcodes.h    |  17 +++
+ protocols/service/capsule_update/parameters.h |  15 ++
+ 12 files changed, 292 insertions(+), 4 deletions(-)
+ create mode 100644 components/service/capsule_update/backend/capsule_update_backend.h
+ create mode 100644 components/service/capsule_update/provider/capsule_update_provider.c
+ create mode 100644 components/service/capsule_update/provider/capsule_update_provider.h
+ create mode 100644 components/service/capsule_update/provider/component.cmake
+ create mode 100644 protocols/service/capsule_update/capsule_update_proto.h
+ create mode 100644 protocols/service/capsule_update/opcodes.h
+ create mode 100644 protocols/service/capsule_update/parameters.h
+
+diff --git a/components/service/capsule_update/backend/capsule_update_backend.h b/components/service/capsule_update/backend/capsule_update_backend.h
+new file mode 100644
+index 00000000..f3144ff1
+--- /dev/null
++++ b/components/service/capsule_update/backend/capsule_update_backend.h
+@@ -0,0 +1,24 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef CAPSULE_UPDATE_BACKEND_H
++#define CAPSULE_UPDATE_BACKEND_H
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++/**
++ * Defines the common capsule update backend interface.  Concrete backends
++ * implement this interface for different types of platform.
++ */
++
++
++#ifdef __cplusplus
++} /* extern "C" */
++#endif
++
++#endif /* CAPSULE_UPDATE_BACKEND_H */
+diff --git a/components/service/capsule_update/provider/capsule_update_provider.c b/components/service/capsule_update/provider/capsule_update_provider.c
+new file mode 100644
+index 00000000..9bbd7abc
+--- /dev/null
++++ b/components/service/capsule_update/provider/capsule_update_provider.c
+@@ -0,0 +1,133 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#include <psa/client.h>
++#include <psa/sid.h>
++#include <trace.h>
++
++#include <protocols/service/capsule_update/capsule_update_proto.h>
++#include <protocols/rpc/common/packed-c/status.h>
++#include "capsule_update_provider.h"
++
++
++#define CAPSULE_UPDATE_REQUEST (0x1)
++#define KERNEL_STARTED_EVENT   (0x2)
++
++enum corstone1000_ioctl_id_t {
++	IOCTL_CORSTONE1000_FWU_FLASH_IMAGES = 0,
++	IOCTL_CORSTONE1000_FWU_HOST_ACK,
++};
++
++/* Service request handlers */
++static rpc_status_t update_capsule_handler(void *context, struct call_req *req);
++static rpc_status_t boot_confirmed_handler(void *context, struct call_req *req);
++
++/* Handler mapping table for service */
++static const struct service_handler handler_table[] = {
++	{CAPSULE_UPDATE_OPCODE_UPDATE_CAPSULE,			update_capsule_handler},
++	{CAPSULE_UPDATE_OPCODE_BOOT_CONFIRMED,			boot_confirmed_handler}
++};
++
++struct rpc_interface *capsule_update_provider_init(
++	struct capsule_update_provider *context)
++{
++	struct rpc_interface *rpc_interface = NULL;
++
++	if (context) {
++
++		service_provider_init(
++			&context->base_provider,
++			context,
++			handler_table,
++			sizeof(handler_table)/sizeof(struct service_handler));
++
++		rpc_interface = service_provider_get_rpc_interface(&context->base_provider);
++	}
++
++	return rpc_interface;
++}
++
++void capsule_update_provider_deinit(struct capsule_update_provider *context)
++{
++	(void)context;
++}
++
++static rpc_status_t event_handler(uint32_t opcode, struct rpc_caller *caller)
++{
++	uint32_t ioctl_id;
++	psa_handle_t handle;
++	rpc_status_t rpc_status = TS_RPC_CALL_ACCEPTED;
++
++	struct psa_invec in_vec[] = {
++			{ .base = &ioctl_id, .len = sizeof(ioctl_id) }
++	};
++
++	if(!caller) {
++		EMSG("event_handler rpc_caller is NULL");
++		rpc_status = TS_RPC_ERROR_RESOURCE_FAILURE;
++		return rpc_status;
++	}
++
++	MSG("event handler opcode %x", opcode);
++	switch(opcode) {
++		case CAPSULE_UPDATE_REQUEST:
++		/* Openamp call with IOCTL for firmware update*/
++		ioctl_id = IOCTL_CORSTONE1000_FWU_FLASH_IMAGES;
++		handle = psa_connect(caller, TFM_SP_PLATFORM_IOCTL_SID,
++				TFM_SP_PLATFORM_IOCTL_VERSION);
++		if (handle <= 0) {
++			EMSG("%s Invalid handle", __func__);
++			rpc_status = TS_RPC_ERROR_INVALID_PARAMETER;
++			return rpc_status;
++		}
++		psa_call(caller,handle, PSA_IPC_CALL,
++			in_vec,IOVEC_LEN(in_vec), NULL, 0);
++		break;
++
++		case KERNEL_STARTED_EVENT:
++		ioctl_id = IOCTL_CORSTONE1000_FWU_HOST_ACK;
++		/*openamp call with IOCTL for kernel start*/
++		handle = psa_connect(caller, TFM_SP_PLATFORM_IOCTL_SID,
++				TFM_SP_PLATFORM_IOCTL_VERSION);
++		if (handle <= 0) {
++			EMSG("%s Invalid handle", __func__);
++			rpc_status = TS_RPC_ERROR_INVALID_PARAMETER;
++			return rpc_status;
++		}
++		psa_call(caller,handle, PSA_IPC_CALL,
++			in_vec,IOVEC_LEN(in_vec), NULL, 0);
++		break;
++		default:
++			EMSG("%s unsupported opcode", __func__);
++			rpc_status = TS_RPC_ERROR_INVALID_PARAMETER;
++			return rpc_status;
++	}
++	return rpc_status;
++
++}
++
++static rpc_status_t update_capsule_handler(void *context, struct call_req *req)
++{
++	struct capsule_update_provider *this_instance = (struct capsule_update_provider*)context;
++	struct rpc_caller *caller = this_instance->client.caller;
++	uint32_t opcode = req->opcode;
++	rpc_status_t rpc_status = TS_RPC_ERROR_NOT_READY;
++
++	rpc_status = event_handler(opcode, caller);
++	return rpc_status;
++}
++
++static rpc_status_t boot_confirmed_handler(void *context, struct call_req *req)
++{
++	struct capsule_update_provider *this_instance = (struct capsule_update_provider*)context;
++	struct rpc_caller *caller = this_instance->client.caller;
++	uint32_t opcode = req->opcode;
++	rpc_status_t rpc_status = TS_RPC_ERROR_NOT_READY;
++
++	rpc_status = event_handler(opcode, caller);
++
++	return rpc_status;
++}
+diff --git a/components/service/capsule_update/provider/capsule_update_provider.h b/components/service/capsule_update/provider/capsule_update_provider.h
+new file mode 100644
+index 00000000..3de49854
+--- /dev/null
++++ b/components/service/capsule_update/provider/capsule_update_provider.h
+@@ -0,0 +1,51 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef CAPSULE_UPDATE_PROVIDER_H
++#define CAPSULE_UPDATE_PROVIDER_H
++
++#include <rpc/common/endpoint/rpc_interface.h>
++#include <service/common/provider/service_provider.h>
++#include <service/common/client/service_client.h>
++#include <service/capsule_update/backend/capsule_update_backend.h>
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
++/**
++ * The capsule_update_provider is a service provider that accepts update capsule
++ * requests and delegates them to a suitable backend that applies the update.
++ */
++struct capsule_update_provider
++{
++	struct service_provider base_provider;
++	struct service_client client;
++};
++
++/**
++ * \brief Initialize an instance of the capsule update service provider
++ *
++ * @param[in] context The instance to initialize
++ *
++ * \return An rpc_interface or NULL on failure
++ */
++struct rpc_interface *capsule_update_provider_init(
++	struct capsule_update_provider *context);
++
++/**
++ * \brief Cleans up when the instance is no longer needed
++ *
++ * \param[in] context   The instance to de-initialize
++ */
++void capsule_update_provider_deinit(
++	struct capsule_update_provider *context);
++
++#ifdef __cplusplus
++} /* extern "C" */
++#endif
++
++#endif /* CAPSULE_UPDATE_PROVIDER_H */
+diff --git a/components/service/capsule_update/provider/component.cmake b/components/service/capsule_update/provider/component.cmake
+new file mode 100644
+index 00000000..1d412eb2
+--- /dev/null
++++ b/components/service/capsule_update/provider/component.cmake
+@@ -0,0 +1,13 @@
++#-------------------------------------------------------------------------------
++# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++#
++# SPDX-License-Identifier: BSD-3-Clause
++#
++#-------------------------------------------------------------------------------
++if (NOT DEFINED TGT)
++	message(FATAL_ERROR "mandatory parameter TGT is not defined.")
++endif()
++
++target_sources(${TGT} PRIVATE
++	"${CMAKE_CURRENT_LIST_DIR}/capsule_update_provider.c"
++	)
+diff --git a/deployments/se-proxy/opteesp/CMakeLists.txt b/deployments/se-proxy/opteesp/CMakeLists.txt
+index 21904283..953bb716 100644
+--- a/deployments/se-proxy/opteesp/CMakeLists.txt
++++ b/deployments/se-proxy/opteesp/CMakeLists.txt
+@@ -80,6 +80,7 @@ add_components(TARGET "se-proxy"
+ 		"components/service/attestation/reporter/psa_ipc"
+ 		"components/service/attestation/client/psa_ipc"
+ 		"components/rpc/openamp/caller/sp"
++		"components/service/capsule_update/provider"
+ 
+ 		# Stub service provider backends
+ 		"components/rpc/dummy"
+diff --git a/deployments/se-proxy/opteesp/se_proxy_sp.c b/deployments/se-proxy/opteesp/se_proxy_sp.c
+index ef90d9ee..11b014b2 100644
+--- a/deployments/se-proxy/opteesp/se_proxy_sp.c
++++ b/deployments/se-proxy/opteesp/se_proxy_sp.c
+@@ -48,6 +48,9 @@ void __noreturn sp_main(struct ffa_init_info *init_info)
+ 	rpc_iface = attest_proxy_create();
+ 	rpc_demux_attach(&rpc_demux, SE_PROXY_INTERFACE_ID_ATTEST, rpc_iface);
+ 
++	rpc_iface = capsule_update_proxy_create();
++	rpc_demux_attach(&rpc_demux, SE_PROXY_INTERFACE_ID_CAPSULE_UPDATE, rpc_iface);
++
+ 	/* End of boot phase */
+ 	sp_msg_wait(&req_msg);
+ 
+diff --git a/deployments/se-proxy/opteesp/service_proxy_factory.c b/deployments/se-proxy/opteesp/service_proxy_factory.c
+index 7edeef8b..591cc9ee 100644
+--- a/deployments/se-proxy/opteesp/service_proxy_factory.c
++++ b/deployments/se-proxy/opteesp/service_proxy_factory.c
+@@ -13,6 +13,7 @@
+ #include <service/crypto/factory/crypto_provider_factory.h>
+ #include <service/secure_storage/frontend/secure_storage_provider/secure_storage_provider.h>
+ #include <trace.h>
++#include <service/capsule_update/provider/capsule_update_provider.h>
+ 
+ /* Stub backends */
+ #include <service/crypto/backend/psa_ipc/crypto_ipc_backend.h>
+@@ -93,3 +94,18 @@ struct rpc_interface *its_proxy_create(void)
+  
+         return secure_storage_provider_init(&its_provider, backend);
+ }
++
++struct rpc_interface *capsule_update_proxy_create(void)
++{
++	static struct capsule_update_provider capsule_update_provider;
++	static struct rpc_caller *capsule_update_caller;
++
++	capsule_update_caller = openamp_caller_init(&openamp);
++
++	if (!capsule_update_caller)
++	return NULL;
++
++	capsule_update_provider.client.caller = capsule_update_caller;
++
++	return capsule_update_provider_init(&capsule_update_provider);
++}
+diff --git a/deployments/se-proxy/opteesp/service_proxy_factory.h b/deployments/se-proxy/opteesp/service_proxy_factory.h
+index 298d407a..02aa7fe2 100644
+--- a/deployments/se-proxy/opteesp/service_proxy_factory.h
++++ b/deployments/se-proxy/opteesp/service_proxy_factory.h
+@@ -17,6 +17,7 @@ struct rpc_interface *attest_proxy_create(void);
+ struct rpc_interface *crypto_proxy_create(void);
+ struct rpc_interface *ps_proxy_create(void);
+ struct rpc_interface *its_proxy_create(void);
++struct rpc_interface *capsule_update_proxy_create(void);
+ 
+ #ifdef __cplusplus
+ }
+diff --git a/deployments/se-proxy/se_proxy_interfaces.h b/deployments/se-proxy/se_proxy_interfaces.h
+index 48908f84..3d4a7c20 100644
+--- a/deployments/se-proxy/se_proxy_interfaces.h
++++ b/deployments/se-proxy/se_proxy_interfaces.h
+@@ -8,9 +8,10 @@
+ #define SE_PROXY_INTERFACES_H
+ 
+ /* Interface IDs from service endpoints available from an se-proxy deployment */
+-#define SE_PROXY_INTERFACE_ID_ITS			(0)
+-#define SE_PROXY_INTERFACE_ID_PS			(1)
+-#define SE_PROXY_INTERFACE_ID_CRYPTO		(2)
+-#define SE_PROXY_INTERFACE_ID_ATTEST		(3)
++#define SE_PROXY_INTERFACE_ID_ITS			    (0)
++#define SE_PROXY_INTERFACE_ID_PS			    (1)
++#define SE_PROXY_INTERFACE_ID_CRYPTO		    (2)
++#define SE_PROXY_INTERFACE_ID_ATTEST		    (3)
++#define SE_PROXY_INTERFACE_ID_CAPSULE_UPDATE    (4)
+ 
+ #endif /* SE_PROXY_INTERFACES_H */
+diff --git a/protocols/service/capsule_update/capsule_update_proto.h b/protocols/service/capsule_update/capsule_update_proto.h
+new file mode 100644
+index 00000000..8f326cd3
+--- /dev/null
++++ b/protocols/service/capsule_update/capsule_update_proto.h
+@@ -0,0 +1,13 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef CAPSULE_UPDATE_PROTO_H
++#define CAPSULE_UPDATE_PROTO_H
++
++#include <protocols/service/capsule_update/opcodes.h>
++#include <protocols/service/capsule_update/parameters.h>
++
++#endif /* CAPSULE_UPDATE_PROTO_H */
+diff --git a/protocols/service/capsule_update/opcodes.h b/protocols/service/capsule_update/opcodes.h
+new file mode 100644
+index 00000000..8185a090
+--- /dev/null
++++ b/protocols/service/capsule_update/opcodes.h
+@@ -0,0 +1,17 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef CAPSULE_UPDATE_OPCODES_H
++#define CAPSULE_UPDATE_OPCODES_H
++
++/**
++ * Opcode definitions for the capsule update service
++ */
++
++#define CAPSULE_UPDATE_OPCODE_UPDATE_CAPSULE		1
++#define CAPSULE_UPDATE_OPCODE_BOOT_CONFIRMED		2
++
++#endif /* CAPSULE_UPDATE_OPCODES_H */
+diff --git a/protocols/service/capsule_update/parameters.h b/protocols/service/capsule_update/parameters.h
+new file mode 100644
+index 00000000..285d9241
+--- /dev/null
++++ b/protocols/service/capsule_update/parameters.h
+@@ -0,0 +1,15 @@
++/*
++ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ *
++ * SPDX-License-Identifier: BSD-3-Clause
++ */
++
++#ifndef CAPSULE_UPDATE_PARAMETERS_H
++#define CAPSULE_UPDATE_PARAMETERS_H
++
++/**
++ * Operation parameter definitions for the capsule update service access protocol.
++ */
++
++
++#endif /* CAPSULE_UPDATE_PARAMETERS_H */
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0026-Add-logs-to-functions-in-SMM-gateway-SP.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0026-Add-logs-to-functions-in-SMM-gateway-SP.patch
new file mode 100644
index 0000000..38ce243
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0026-Add-logs-to-functions-in-SMM-gateway-SP.patch
@@ -0,0 +1,248 @@
+From c9188e59fd27d208a975187da285a9b5938bb00d Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Thu, 16 Dec 2021 13:29:58 +0000
+Subject: [PATCH] Add logs to functions in SMM gateway SP
+
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+
+---
+ .../backend/uefi_variable_store.c             | 29 +++++++++++++++++--
+ .../provider/smm_variable_provider.c          |  7 +++--
+ 2 files changed, 32 insertions(+), 4 deletions(-)
+
+diff --git a/components/service/smm_variable/backend/uefi_variable_store.c b/components/service/smm_variable/backend/uefi_variable_store.c
+index ed50eaf9..0c371e94 100644
+--- a/components/service/smm_variable/backend/uefi_variable_store.c
++++ b/components/service/smm_variable/backend/uefi_variable_store.c
+@@ -11,6 +11,7 @@
+ #include "uefi_variable_store.h"
+ #include "variable_index_iterator.h"
+ #include "variable_checker.h"
++#include <trace.h>
+ 
+ /* Private functions */
+ static void load_variable_index(
+@@ -151,12 +152,15 @@ void uefi_variable_store_set_storage_limits(
+ 	size_t total_capacity,
+ 	size_t max_variable_size)
+ {
++	EMSG("In func %s\n", __func__);
+ 	struct delegate_variable_store *delegate_store = select_delegate_store(
+ 		context,
+ 		attributes);
+ 
+ 	delegate_store->total_capacity = total_capacity;
+ 	delegate_store->max_variable_size = max_variable_size;
++	EMSG("In func %s total_capacity is %d\n", __func__, total_capacity);
++	EMSG("In func %s max_variable_size is %d\n", __func__, max_variable_size);
+ }
+ 
+ efi_status_t uefi_variable_store_set_variable(
+@@ -265,6 +269,7 @@ efi_status_t uefi_variable_store_get_variable(
+ 	size_t max_data_len,
+ 	size_t *total_length)
+ {
++	EMSG("In func %s\n", __func__);
+ 	efi_status_t status = check_name_terminator(var->Name, var->NameSize);
+ 	if (status != EFI_SUCCESS) return status;
+ 
+@@ -299,6 +304,7 @@ efi_status_t uefi_variable_store_get_next_variable_name(
+ 	size_t max_name_len,
+ 	size_t *total_length)
+ {
++	EMSG("In func %s\n", __func__);
+ 	efi_status_t status = check_name_terminator(cur->Name, cur->NameSize);
+ 	if (status != EFI_SUCCESS) return status;
+ 
+@@ -329,6 +335,8 @@ efi_status_t uefi_variable_store_query_variable_info(
+ 	struct uefi_variable_store *context,
+ 	SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO *var_info)
+ {
++
++	EMSG("In func %s\n", __func__);
+ 	struct delegate_variable_store *delegate_store = select_delegate_store(
+ 		context,
+ 		var_info->Attributes);
+@@ -337,13 +345,15 @@ efi_status_t uefi_variable_store_query_variable_info(
+ 		context,
+ 		var_info->Attributes,
+ 		delegate_store->storage_backend);
+-
++	EMSG("In func %s total_used is %d\n", __func__, total_used);
+ 	var_info->MaximumVariableSize = delegate_store->max_variable_size;
+ 	var_info->MaximumVariableStorageSize = delegate_store->total_capacity;
+ 	var_info->RemainingVariableStorageSize = (total_used < delegate_store->total_capacity) ?
+ 		delegate_store->total_capacity - total_used :
+ 		0;
+-
++	EMSG("In func %s var_info->MaximumVariableSize is %d\n", __func__, var_info->MaximumVariableSize);
++	EMSG("In func %s var_info->MaximumVariableStorageSize is %d\n", __func__, var_info->MaximumVariableStorageSize);
++	EMSG("In func %s var_info->RemainingVariableStorageSize is %d\n", __func__, var_info->RemainingVariableStorageSize);
+ 	return EFI_SUCCESS;
+ }
+ 
+@@ -358,6 +368,7 @@ efi_status_t uefi_variable_store_set_var_check_property(
+ 	struct uefi_variable_store *context,
+ 	const SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY *property)
+ {
++	EMSG("In func %s\n", __func__);
+ 	efi_status_t status = check_name_terminator(property->Name, property->NameSize);
+ 	if (status != EFI_SUCCESS) return status;
+ 
+@@ -404,6 +415,7 @@ efi_status_t uefi_variable_store_get_var_check_property(
+ 	struct uefi_variable_store *context,
+ 	SMM_VARIABLE_COMMUNICATE_VAR_CHECK_VARIABLE_PROPERTY *property)
+ {
++	EMSG("In func %s\n", __func__);
+ 	efi_status_t status = check_name_terminator(property->Name, property->NameSize);
+ 	if (status != EFI_SUCCESS) return status;
+ 
+@@ -430,6 +442,7 @@ efi_status_t uefi_variable_store_get_var_check_property(
+ static void load_variable_index(
+ 	struct uefi_variable_store *context)
+ {
++	EMSG("In func %s\n", __func__);
+ 	struct storage_backend *persistent_store = context->persistent_store.storage_backend;
+ 
+ 	if (persistent_store) {
+@@ -444,6 +457,7 @@ static void load_variable_index(
+ 			context->index_sync_buffer_size,
+ 			context->index_sync_buffer,
+ 			&data_len);
++		EMSG("In func %s get status is %d\n", __func__, psa_status);
+ 
+ 		if (psa_status == PSA_SUCCESS) {
+ 
+@@ -455,6 +469,7 @@ static void load_variable_index(
+ static efi_status_t sync_variable_index(
+ 	struct uefi_variable_store *context)
+ {
++	EMSG("In func %s\n", __func__);
+ 	efi_status_t status = EFI_SUCCESS;
+ 
+ 	/* Sync the varibale index to storage if anything is dirty */
+@@ -479,6 +494,7 @@ static efi_status_t sync_variable_index(
+ 				data_len,
+ 				context->index_sync_buffer,
+ 				PSA_STORAGE_FLAG_NONE);
++			EMSG("In func %s set status is %d\n", __func__, psa_status);
+ 
+ 			status = psa_to_efi_storage_status(psa_status);
+ 		}
+@@ -490,6 +506,7 @@ static efi_status_t sync_variable_index(
+ static efi_status_t check_capabilities(
+ 	const SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *var)
+ {
++	EMSG("In func %s\n", __func__);
+ 	efi_status_t status = EFI_SUCCESS;
+ 
+ 	/* Check if any unsupported variable attributes have been requested */
+@@ -551,6 +568,7 @@ static efi_status_t store_variable_data(
+ 	const struct variable_info *info,
+ 	const SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *var)
+ {
++	EMSG("In func %s\n", __func__);
+ 	psa_status_t psa_status = PSA_SUCCESS;
+ 	size_t data_len = var->DataSize;
+ 	const uint8_t *data = (const uint8_t*)var +
+@@ -599,6 +617,7 @@ static efi_status_t remove_variable_data(
+ 	struct uefi_variable_store *context,
+ 	const struct variable_info *info)
+ {
++	EMSG("In func %s\n", __func__);
+ 	psa_status_t psa_status = PSA_SUCCESS;
+ 
+ 	if (info->is_variable_set) {
+@@ -613,6 +632,7 @@ static efi_status_t remove_variable_data(
+ 				delegate_store->storage_backend->context,
+ 				context->owner_id,
+ 				info->metadata.uid);
++			EMSG("In func %s status is %d\n", __func__, psa_status);
+ 		}
+ 	}
+ 
+@@ -625,6 +645,7 @@ static efi_status_t load_variable_data(
+ 	SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *var,
+ 	size_t max_data_len)
+ {
++	EMSG("In func %s\n", __func__);
+ 	psa_status_t psa_status = PSA_SUCCESS;
+ 	size_t data_len = 0;
+ 	uint8_t *data = (uint8_t*)var +
+@@ -644,6 +665,7 @@ static efi_status_t load_variable_data(
+ 			max_data_len,
+ 			data,
+ 			&data_len);
++		EMSG("In func %s get status is %d\n", __func__, psa_status);
+ 
+ 		var->DataSize = data_len;
+ 	}
+@@ -771,6 +793,7 @@ static void purge_orphan_index_entries(
+ 				context->owner_id,
+ 				info->metadata.uid,
+ 				&storage_info);
++			EMSG("In func %s get status is %d\n", __func__, psa_status);
+ 
+ 			if (psa_status != PSA_SUCCESS) {
+ 
+@@ -802,6 +825,7 @@ static size_t space_used(
+ 	uint32_t attributes,
+ 	struct storage_backend *storage_backend)
+ {
++	EMSG("In func %s\n", __func__);
+ 	if (!storage_backend) return 0;
+ 
+ 	size_t total_used = 0;
+@@ -823,6 +847,7 @@ static size_t space_used(
+ 				context->owner_id,
+ 				info->metadata.uid,
+ 				&storage_info);
++			EMSG("In func %s get status is %d\n", __func__, psa_status);
+ 
+ 			if (psa_status == PSA_SUCCESS) total_used += storage_info.size;
+ 		}
+diff --git a/components/service/smm_variable/provider/smm_variable_provider.c b/components/service/smm_variable/provider/smm_variable_provider.c
+index 52e68d09..1f362c17 100644
+--- a/components/service/smm_variable/provider/smm_variable_provider.c
++++ b/components/service/smm_variable/provider/smm_variable_provider.c
+@@ -9,6 +9,7 @@
+ #include <protocols/service/smm_variable/smm_variable_proto.h>
+ #include <protocols/rpc/common/packed-c/status.h>
+ #include "smm_variable_provider.h"
++#include <trace.h>
+ 
+ /* Service request handlers */
+ static rpc_status_t get_variable_handler(void *context, struct call_req *req);
+@@ -252,17 +253,18 @@ static rpc_status_t set_variable_handler(void *context, struct call_req* req)
+ 
+ static rpc_status_t query_variable_info_handler(void *context, struct call_req* req)
+ {
++	EMSG("In func %s \n", __func__);
+ 	efi_status_t efi_status = EFI_INVALID_PARAMETER;
+ 	struct smm_variable_provider *this_instance = (struct smm_variable_provider*)context;
+ 
+ 	const struct call_param_buf *req_buf = call_req_get_req_buf(req);
+-
++	EMSG("In func %s sizeof(SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO) is %d\n", __func__, sizeof(SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO));
+ 	if (req_buf->data_len >= sizeof(SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO)) {
+ 
+ 		struct call_param_buf *resp_buf = call_req_get_resp_buf(req);
+ 
+ 		if (resp_buf->size >= req_buf->data_len) {
+-
++			
+ 			memmove(resp_buf->data, req_buf->data, req_buf->data_len);
+ 
+ 			efi_status = uefi_variable_store_query_variable_info(
+@@ -272,6 +274,7 @@ static rpc_status_t query_variable_info_handler(void *context, struct call_req*
+ 			if (efi_status == EFI_SUCCESS) {
+ 
+ 				resp_buf->data_len = sizeof(SMM_VARIABLE_COMMUNICATE_QUERY_VARIABLE_INFO);
++				EMSG("In func %s resp_buf->data_len is %d\n", __func__, resp_buf->data_len);
+ 			}
+ 		}
+ 		else {
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0027-Configure-storage-size.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0027-Configure-storage-size.patch
new file mode 100644
index 0000000..ddf9503
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0027-Configure-storage-size.patch
@@ -0,0 +1,41 @@
+From 3e472452bca64ed90071b61416460f1a69382293 Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Thu, 16 Dec 2021 21:31:40 +0000
+Subject: [PATCH] Configure storage size
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+
+---
+ .../service/smm_variable/backend/uefi_variable_store.c       | 5 +++--
+ 1 file changed, 3 insertions(+), 2 deletions(-)
+
+diff --git a/components/service/smm_variable/backend/uefi_variable_store.c b/components/service/smm_variable/backend/uefi_variable_store.c
+index 0c371e94..b7cfff40 100644
+--- a/components/service/smm_variable/backend/uefi_variable_store.c
++++ b/components/service/smm_variable/backend/uefi_variable_store.c
+@@ -87,6 +87,7 @@ static efi_status_t check_name_terminator(
+  * may be overridden using uefi_variable_store_set_storage_limits()
+  */
+ #define DEFAULT_MAX_VARIABLE_SIZE			(2048)
++#define CONFIGURE_STORAGE_SIZE			    (50)
+ 
+ efi_status_t uefi_variable_store_init(
+ 	struct uefi_variable_store *context,
+@@ -100,13 +101,13 @@ efi_status_t uefi_variable_store_init(
+ 	/* Initialise persistent store defaults */
+ 	context->persistent_store.is_nv = true;
+ 	context->persistent_store.max_variable_size = DEFAULT_MAX_VARIABLE_SIZE;
+-	context->persistent_store.total_capacity = DEFAULT_MAX_VARIABLE_SIZE * max_variables;
++	context->persistent_store.total_capacity = CONFIGURE_STORAGE_SIZE * max_variables;
+ 	context->persistent_store.storage_backend = persistent_store;
+ 
+ 	/* Initialise volatile store defaults */
+ 	context->volatile_store.is_nv = false;
+ 	context->volatile_store.max_variable_size = DEFAULT_MAX_VARIABLE_SIZE;
+-	context->volatile_store.total_capacity = DEFAULT_MAX_VARIABLE_SIZE * max_variables;
++	context->volatile_store.total_capacity = CONFIGURE_STORAGE_SIZE * max_variables;
+ 	context->volatile_store.storage_backend = volatile_store;
+ 
+ 	context->owner_id = owner_id;
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0028-Revert-Add-uefi-variable-append-write-support.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0028-Revert-Add-uefi-variable-append-write-support.patch
new file mode 100644
index 0000000..9bb3f91
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0028-Revert-Add-uefi-variable-append-write-support.patch
@@ -0,0 +1,1219 @@
+From da3bd0721f2403562b6ae6d1939f5f331fd141bb Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Wed, 15 Dec 2021 17:23:25 +0000
+Subject: [PATCH] Revert "Add uefi variable append write support"
+
+This reverts commit e8758d9aff0eddae81a74b0191cd027bcdc92c04.
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+
+---
+ .../backend/test/variable_index_tests.cpp     |  90 +++---
+ .../backend/test/variable_store_tests.cpp     |  72 +----
+ .../backend/uefi_variable_store.c             | 293 ++++++------------
+ .../smm_variable/backend/variable_index.c     |  95 ++++--
+ .../smm_variable/backend/variable_index.h     |  58 ++--
+ .../backend/variable_index_iterator.c         |   4 +-
+ .../backend/variable_index_iterator.h         |   2 +-
+ .../service/smm_variable_service_tests.cpp    |  48 ---
+ protocols/service/smm_variable/parameters.h   |   3 -
+ 9 files changed, 239 insertions(+), 426 deletions(-)
+
+diff --git a/components/service/smm_variable/backend/test/variable_index_tests.cpp b/components/service/smm_variable/backend/test/variable_index_tests.cpp
+index 8edc0e70..c8bacf97 100644
+--- a/components/service/smm_variable/backend/test/variable_index_tests.cpp
++++ b/components/service/smm_variable/backend/test/variable_index_tests.cpp
+@@ -69,37 +69,34 @@ TEST_GROUP(UefiVariableIndexTests)
+ 
+ 	void create_variables()
+ 	{
+-		struct variable_info *info = NULL;
++		const struct variable_info *info = NULL;
+ 
+-		info = variable_index_add_entry(
++		info = variable_index_add_variable(
+ 			&m_variable_index,
+ 			&guid_1,
+ 			name_1.size() * sizeof(int16_t),
+-			name_1.data());
+-		CHECK_TRUE(info);
+-		variable_index_set_variable(
+-			info,
++			name_1.data(),
+ 			EFI_VARIABLE_BOOTSERVICE_ACCESS);
+ 
+-		info = variable_index_add_entry(
++		CHECK_TRUE(info);
++
++		info = variable_index_add_variable(
+ 			&m_variable_index,
+ 			&guid_2,
+ 			name_2.size() * sizeof(int16_t),
+-			name_2.data());
+-		CHECK_TRUE(info);
+-		variable_index_set_variable(
+-			info,
++			name_2.data(),
+ 			EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_BOOTSERVICE_ACCESS);
+ 
+-		info = variable_index_add_entry(
++		CHECK_TRUE(info);
++
++		info = variable_index_add_variable(
+ 			&m_variable_index,
+ 			&guid_1,
+ 			name_3.size() * sizeof(int16_t),
+-			name_3.data());
+-		CHECK_TRUE(info);
+-		variable_index_set_variable(
+-			info,
++			name_3.data(),
+ 			EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_RUNTIME_ACCESS);
++
++		CHECK_TRUE(info);
+ 	}
+ 
+ 	static const size_t MAX_VARIABLES = 10;
+@@ -114,7 +111,7 @@ TEST_GROUP(UefiVariableIndexTests)
+ 
+ TEST(UefiVariableIndexTests, emptyIndexOperations)
+ {
+-	struct variable_info *info = NULL;
++	const struct variable_info *info = NULL;
+ 
+ 	/* Expect not to find a variable */
+ 	info = variable_index_find(
+@@ -133,34 +130,36 @@ TEST(UefiVariableIndexTests, emptyIndexOperations)
+ 	POINTERS_EQUAL(NULL, info);
+ 
+ 	/* Remove should silently return */
+-	variable_index_clear_variable(
++	variable_index_remove_variable(
+ 		&m_variable_index,
+ 		info);
+ }
+ 
+ TEST(UefiVariableIndexTests, addWithOversizedName)
+ {
+-	struct variable_info *info = NULL;
++	const struct variable_info *info = NULL;
+ 	std::vector<int16_t> name;
+ 
+ 	name = to_variable_name(L"a long variable name that exceeds the length limit");
+ 
+-	info = variable_index_add_entry(
++	info = variable_index_add_variable(
+ 		&m_variable_index,
+ 		&guid_1,
+ 		name.size() * sizeof(int16_t),
+-		name.data());
++		name.data(),
++		EFI_VARIABLE_BOOTSERVICE_ACCESS);
+ 
+ 	/* Expect the add to fail because of an oversized name */
+ 	POINTERS_EQUAL(NULL, info);
+ 
+ 	name = to_variable_name(L"a long variable name that fits!");
+ 
+-	info = variable_index_add_entry(
++	info = variable_index_add_variable(
+ 		&m_variable_index,
+ 		&guid_1,
+ 		name.size() * sizeof(int16_t),
+-		name.data());
++		name.data(),
++		EFI_VARIABLE_BOOTSERVICE_ACCESS);
+ 
+ 	/* Expect the add succeed */
+ 	CHECK_TRUE(info);
+@@ -168,17 +167,18 @@ TEST(UefiVariableIndexTests, addWithOversizedName)
+ 
+ TEST(UefiVariableIndexTests, variableIndexFull)
+ {
+-	struct variable_info *info = NULL;
++	const struct variable_info *info = NULL;
+ 	EFI_GUID guid = guid_1;
+ 
+ 	/* Expect to be able to fill the index */
+ 	for (size_t i = 0; i < MAX_VARIABLES; ++i) {
+ 
+-		info = variable_index_add_entry(
++		info = variable_index_add_variable(
+ 			&m_variable_index,
+ 			&guid,
+ 			name_1.size() * sizeof(int16_t),
+-			name_1.data());
++			name_1.data(),
++			EFI_VARIABLE_BOOTSERVICE_ACCESS);
+ 
+ 		CHECK_TRUE(info);
+ 
+@@ -187,11 +187,12 @@ TEST(UefiVariableIndexTests, variableIndexFull)
+ 	}
+ 
+ 	/* Variable index should now be full */
+-	info = variable_index_add_entry(
++	info = variable_index_add_variable(
+ 		&m_variable_index,
+ 		&guid,
+ 		name_1.size() * sizeof(int16_t),
+-		name_1.data());
++		name_1.data(),
++		EFI_VARIABLE_BOOTSERVICE_ACCESS);
+ 
+ 	POINTERS_EQUAL(NULL, info);
+ }
+@@ -322,7 +323,7 @@ TEST(UefiVariableIndexTests, dumpBufferTooSmall)
+ TEST(UefiVariableIndexTests, removeVariable)
+ {
+ 	uint8_t buffer[MAX_VARIABLES * sizeof(struct variable_metadata)];
+-	struct variable_info *info = NULL;
++	const struct variable_info *info = NULL;
+ 
+ 	create_variables();
+ 
+@@ -333,7 +334,7 @@ TEST(UefiVariableIndexTests, removeVariable)
+ 		name_2.size() * sizeof(int16_t),
+ 		name_2.data());
+ 
+-	variable_index_clear_variable(
++	variable_index_remove_variable(
+ 		&m_variable_index,
+ 		info);
+ 
+@@ -351,7 +352,7 @@ TEST(UefiVariableIndexTests, removeVariable)
+ 		name_1.size() * sizeof(int16_t),
+ 		name_1.data());
+ 
+-	variable_index_clear_variable(
++	variable_index_remove_variable(
+ 		&m_variable_index,
+ 		info);
+ 
+@@ -369,7 +370,7 @@ TEST(UefiVariableIndexTests, removeVariable)
+ 		name_3.size() * sizeof(int16_t),
+ 		name_3.data());
+ 
+-	variable_index_clear_variable(
++	variable_index_remove_variable(
+ 		&m_variable_index,
+ 		info);
+ 
+@@ -394,7 +395,7 @@ TEST(UefiVariableIndexTests, removeVariable)
+ 
+ TEST(UefiVariableIndexTests, checkIterator)
+ {
+-	struct variable_info *info = NULL;
++	const struct variable_info *info = NULL;
+ 
+ 	create_variables();
+ 
+@@ -418,7 +419,7 @@ TEST(UefiVariableIndexTests, checkIterator)
+ 	UNSIGNED_LONGS_EQUAL(name_2.size() * sizeof(int16_t), info->metadata.name_size);
+ 	MEMCMP_EQUAL(name_2.data(), info->metadata.name, info->metadata.name_size);
+ 
+-	struct variable_info *info_to_remove = info;
++	const struct variable_info *info_to_remove = info;
+ 
+ 	variable_index_iterator_next(&iter);
+ 	CHECK_FALSE(variable_index_iterator_is_done(&iter));
+@@ -434,8 +435,7 @@ TEST(UefiVariableIndexTests, checkIterator)
+ 	CHECK_TRUE(variable_index_iterator_is_done(&iter));
+ 
+ 	/* Now remove the middle entry */
+-	variable_index_clear_variable(&m_variable_index, info_to_remove);
+-	variable_index_remove_unused_entry(&m_variable_index, info_to_remove);
++	variable_index_remove_variable(&m_variable_index, info_to_remove);
+ 
+ 	/* Iterate again but this time there should only be two entries */
+ 	variable_index_iterator_first(&iter, &m_variable_index);
+@@ -478,7 +478,7 @@ TEST(UefiVariableIndexTests, setCheckConstraintsExistingVar)
+ 	constraints.max_size = 100;
+ 
+ 	/* Set check constraints on one of the variables */
+-	struct variable_info *info = variable_index_find(
++	const struct variable_info *info = variable_index_find(
+ 		&m_variable_index,
+ 		&guid_2,
+ 		name_2.size() * sizeof(int16_t),
+@@ -488,7 +488,7 @@ TEST(UefiVariableIndexTests, setCheckConstraintsExistingVar)
+ 	CHECK_TRUE(info->is_variable_set);
+ 	CHECK_FALSE(info->is_constraints_set);
+ 
+-	variable_index_set_constraints(info, &constraints);
++	variable_index_update_constraints(info, &constraints);
+ 
+ 	CHECK_TRUE(info->is_constraints_set);
+ 	CHECK_TRUE(info->is_variable_set);
+@@ -496,7 +496,7 @@ TEST(UefiVariableIndexTests, setCheckConstraintsExistingVar)
+ 	/* Remove the variable but still expect the variable to be indexed
+ 	 * because of the set constraints.
+ 	 */
+-	variable_index_clear_variable(
++	variable_index_remove_variable(
+ 		&m_variable_index,
+ 		info);
+ 
+@@ -588,7 +588,7 @@ TEST(UefiVariableIndexTests, setCheckConstraintsNonExistingVar)
+ 	constraints.max_size = 100;
+ 
+ 	/* Initially expect no variable_info */
+-	struct variable_info *info = variable_index_find(
++	const struct variable_info *info = variable_index_find(
+ 		&m_variable_index,
+ 		&guid_2,
+ 		name_2.size() * sizeof(int16_t),
+@@ -597,19 +597,19 @@ TEST(UefiVariableIndexTests, setCheckConstraintsNonExistingVar)
+ 	CHECK_FALSE(info);
+ 
+ 	/* Adding the check constraints should result in an entry being added */
+-	info = variable_index_add_entry(
++	info = variable_index_add_constraints(
+ 		&m_variable_index,
+ 		&guid_2,
+ 		name_2.size() * sizeof(int16_t),
+-		name_2.data());
+-	CHECK_TRUE(info);
++		name_2.data(),
++		&constraints);
+ 
+-	variable_index_set_constraints(info, &constraints);
++	CHECK_TRUE(info);
+ 	CHECK_FALSE(info->is_variable_set);
+ 	CHECK_TRUE(info->is_constraints_set);
+ 
+ 	/* Updating the variable should cause the variable to be marked as set */
+-	variable_index_set_variable(info, EFI_VARIABLE_RUNTIME_ACCESS);
++	variable_index_update_variable(info, EFI_VARIABLE_RUNTIME_ACCESS);
+ 
+ 	CHECK_TRUE(info->is_variable_set);
+ 	CHECK_TRUE(info->is_constraints_set);
+diff --git a/components/service/smm_variable/backend/test/variable_store_tests.cpp b/components/service/smm_variable/backend/test/variable_store_tests.cpp
+index e90c1067..235642e6 100644
+--- a/components/service/smm_variable/backend/test/variable_store_tests.cpp
++++ b/components/service/smm_variable/backend/test/variable_store_tests.cpp
+@@ -305,37 +305,6 @@ TEST(UefiVariableStoreTests, setGetRoundtrip)
+ 	/* Expect got variable data to be the same as the set value */
+ 	UNSIGNED_LONGLONGS_EQUAL(input_data.size(), output_data.size());
+ 	LONGS_EQUAL(0, input_data.compare(output_data));
+-
+-	/* Extend the variable using an append write */
+-	std::string input_data2 = " jumps over the lazy dog";
+-
+-	status = set_variable(var_name, input_data2, EFI_VARIABLE_APPEND_WRITE);
+-	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+-
+-	status = get_variable(var_name, output_data);
+-	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+-
+-	std::string expected_output = input_data + input_data2;
+-
+-	/* Expect the append write operation to have extended the variable */
+-	UNSIGNED_LONGLONGS_EQUAL(expected_output.size(), output_data.size());
+-	LONGS_EQUAL(0, expected_output.compare(output_data));
+-
+-	/* Expect query_variable_info to return consistent values */
+-	size_t max_variable_storage_size = 0;
+-	size_t remaining_variable_storage_size = 0;
+-	size_t max_variable_size = 0;
+-
+-	status = query_variable_info(
+-		0,
+-		&max_variable_storage_size,
+-		&remaining_variable_storage_size,
+-		&max_variable_size);
+-	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+-
+-	UNSIGNED_LONGLONGS_EQUAL(STORE_CAPACITY, max_variable_storage_size);
+-	UNSIGNED_LONGLONGS_EQUAL(MAX_VARIABLE_SIZE, max_variable_size);
+-	UNSIGNED_LONGLONGS_EQUAL(STORE_CAPACITY - expected_output.size(), remaining_variable_storage_size);
+ }
+ 
+ TEST(UefiVariableStoreTests, persistentSetGet)
+@@ -345,8 +314,7 @@ TEST(UefiVariableStoreTests, persistentSetGet)
+ 	std::string input_data = "quick brown fox";
+ 	std::string output_data;
+ 
+-	status = set_variable(var_name, input_data,
+-		EFI_VARIABLE_NON_VOLATILE);
++	status = set_variable(var_name, input_data, EFI_VARIABLE_NON_VOLATILE);
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+ 
+ 	status = get_variable(var_name, output_data);
+@@ -356,22 +324,6 @@ TEST(UefiVariableStoreTests, persistentSetGet)
+ 	UNSIGNED_LONGLONGS_EQUAL(input_data.size(), output_data.size());
+ 	LONGS_EQUAL(0, input_data.compare(output_data));
+ 
+-	/* Extend the variable using an append write */
+-	std::string input_data2 = " jumps over the lazy dog";
+-
+-	status = set_variable(var_name, input_data2,
+-		EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_APPEND_WRITE);
+-	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+-
+-	status = get_variable(var_name, output_data);
+-	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+-
+-	std::string expected_output = input_data + input_data2;
+-
+-	/* Expect the append write operation to have extended the variable */
+-	UNSIGNED_LONGLONGS_EQUAL(expected_output.size(), output_data.size());
+-	LONGS_EQUAL(0, expected_output.compare(output_data));
+-
+ 	/* Expect the variable to survive a power cycle */
+ 	power_cycle();
+ 
+@@ -380,24 +332,8 @@ TEST(UefiVariableStoreTests, persistentSetGet)
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+ 
+ 	/* Still expect got variable data to be the same as the set value */
+-	UNSIGNED_LONGLONGS_EQUAL(expected_output.size(), output_data.size());
+-	LONGS_EQUAL(0, expected_output.compare(output_data));
+-
+-	/* Expect query_variable_info to return consistent values */
+-	size_t max_variable_storage_size = 0;
+-	size_t remaining_variable_storage_size = 0;
+-	size_t max_variable_size = 0;
+-
+-	status = query_variable_info(
+-		EFI_VARIABLE_NON_VOLATILE,
+-		&max_variable_storage_size,
+-		&remaining_variable_storage_size,
+-		&max_variable_size);
+-	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+-
+-	UNSIGNED_LONGLONGS_EQUAL(STORE_CAPACITY, max_variable_storage_size);
+-	UNSIGNED_LONGLONGS_EQUAL(MAX_VARIABLE_SIZE, max_variable_size);
+-	UNSIGNED_LONGLONGS_EQUAL(STORE_CAPACITY - expected_output.size(), remaining_variable_storage_size);
++	UNSIGNED_LONGLONGS_EQUAL(input_data.size(), output_data.size());
++	LONGS_EQUAL(0, input_data.compare(output_data));
+ }
+ 
+ TEST(UefiVariableStoreTests, removeVolatile)
+@@ -436,7 +372,7 @@ TEST(UefiVariableStoreTests, removePersistent)
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+ 
+ 	/* Remove by setting with zero data length */
+-	status = set_variable(var_name, std::string(), EFI_VARIABLE_NON_VOLATILE);
++	status = set_variable(var_name, std::string(), 0);
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, status);
+ 
+ 	/* Expect variable to no loger exist */
+diff --git a/components/service/smm_variable/backend/uefi_variable_store.c b/components/service/smm_variable/backend/uefi_variable_store.c
+index b7cfff40..6a90f46a 100644
+--- a/components/service/smm_variable/backend/uefi_variable_store.c
++++ b/components/service/smm_variable/backend/uefi_variable_store.c
+@@ -47,20 +47,6 @@ static efi_status_t load_variable_data(
+ 	SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *var,
+ 	size_t max_data_len);
+ 
+-static psa_status_t store_overwrite(
+-	struct delegate_variable_store *delegate_store,
+-	uint32_t client_id,
+-	uint64_t uid,
+-	size_t data_length,
+-	const void *data);
+-
+-static psa_status_t store_append_write(
+-	struct delegate_variable_store *delegate_store,
+-	uint32_t client_id,
+-	uint64_t uid,
+-	size_t data_length,
+-	const void *data);
+-
+ static void purge_orphan_index_entries(
+ 	struct uefi_variable_store *context);
+ 
+@@ -168,45 +154,40 @@ efi_status_t uefi_variable_store_set_variable(
+ 	struct uefi_variable_store *context,
+ 	const SMM_VARIABLE_COMMUNICATE_ACCESS_VARIABLE *var)
+ {
+-	bool should_sync_index = false;
+-
+-	/* Validate incoming request */
+ 	efi_status_t status = check_name_terminator(var->Name, var->NameSize);
+ 	if (status != EFI_SUCCESS) return status;
+ 
+ 	status = check_capabilities(var);
++	bool should_sync_index = false;
++
+ 	if (status != EFI_SUCCESS) return status;
+ 
+-	/* Find an existing entry in the variable index or add a new one */
+-	struct variable_info *info = variable_index_find(
++	/* Find in index */
++	const struct variable_info *info = variable_index_find(
+ 		&context->variable_index,
+ 		&var->Guid,
+ 		var->NameSize,
+ 		var->Name);
+ 
+-	if (!info) {
++	if (info) {
+ 
+-		info = variable_index_add_entry(
+-			&context->variable_index,
+-			&var->Guid,
+-			var->NameSize,
+-			var->Name);
++		/* Variable info already exists */
++		status = check_access_permitted_on_set(context, info, var);
+ 
+-		if (!info) return EFI_OUT_OF_RESOURCES;
+-	}
++		if (status == EFI_SUCCESS) {
+ 
+-	/* Control access */
+-	status = check_access_permitted_on_set(context, info, var);
++			should_sync_index =
++				(var->Attributes & EFI_VARIABLE_NON_VOLATILE) ||
++				(info->is_variable_set && (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE));
+ 
+-	if (status == EFI_SUCCESS) {
++			if (var->DataSize) {
+ 
+-		/* Access permitted */
+-		if (info->is_variable_set) {
+-
+-			/* It's a request to update to an existing variable */
+-			if (!(var->Attributes &
+-				(EFI_VARIABLE_APPEND_WRITE | EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS_MASK)) &&
+-				!var->DataSize) {
++				/* It's a set rather than a remove operation */
++				variable_index_update_variable(
++					info,
++					var->Attributes);
++			}
++			else {
+ 
+ 				/* It's a remove operation - for a remove, the variable
+ 				 * data must be removed from the storage backend before
+@@ -215,30 +196,31 @@ efi_status_t uefi_variable_store_set_variable(
+ 				 * the storage backend without a corresponding index entry.
+ 				 */
+ 				remove_variable_data(context, info);
+-				variable_index_clear_variable(&context->variable_index, info);
++				variable_index_remove_variable(&context->variable_index, info);
+ 
+-				should_sync_index = (var->Attributes & EFI_VARIABLE_NON_VOLATILE);
+-			}
+-			else {
+-
+-				/* It's a set operation where variable data is potentially
+-				 * being overwritten or extended.
+-				 */
+-				if ((var->Attributes & ~EFI_VARIABLE_APPEND_WRITE) != info->metadata.attributes) {
+-
+-					/* Modifying attributes is forbidden */
+-					return EFI_INVALID_PARAMETER;
+-				}
++				/* Variable info no longer valid */
++				info = NULL;
+ 			}
+ 		}
+ 		else {
+ 
+-			/*  It's a request to create a new variable */
+-			variable_index_set_variable(info, var->Attributes);
+-
+-			should_sync_index = (var->Attributes & EFI_VARIABLE_NON_VOLATILE);
++			/* Access forbidden */
++			info = NULL;
+ 		}
+ 	}
++	else if (var->DataSize) {
++
++		/* It's a new variable */
++		info = variable_index_add_variable(
++			&context->variable_index,
++			&var->Guid,
++			var->NameSize,
++			var->Name,
++			var->Attributes);
++
++		if (!info) status = EFI_OUT_OF_RESOURCES;
++		should_sync_index = info && (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE);
++	}
+ 
+ 	/* The order of these operations is important. For an update
+ 	 * or create operation, The variable index is always synchronized
+@@ -254,13 +236,11 @@ efi_status_t uefi_variable_store_set_variable(
+ 	}
+ 
+ 	/* Store any variable data to the storage backend */
+-	if (info->is_variable_set && (status == EFI_SUCCESS)) {
++	if (info && (status == EFI_SUCCESS)) {
+ 
+ 		status = store_variable_data(context, info, var);
+ 	}
+ 
+-	variable_index_remove_unused_entry(&context->variable_index, info);
+-
+ 	return status;
+ }
+ 
+@@ -373,41 +353,53 @@ efi_status_t uefi_variable_store_set_var_check_property(
+ 	efi_status_t status = check_name_terminator(property->Name, property->NameSize);
+ 	if (status != EFI_SUCCESS) return status;
+ 
+-	/* Find in index or create a new entry */
+-	struct variable_info *info = variable_index_find(
++	/* Find in index */
++	const struct variable_info *info = variable_index_find(
+ 		&context->variable_index,
+ 		&property->Guid,
+ 		property->NameSize,
+ 		property->Name);
+ 
+-	if (!info) {
++	if (info) {
+ 
+-		info = variable_index_add_entry(
+-			&context->variable_index,
+-			&property->Guid,
+-			property->NameSize,
+-			property->Name);
++		/* Applying check constraints to an existing variable that may have
++		 * constraints already set.  These could constrain the setting of
++		 * the constraints.
++		 */
++		struct variable_constraints constraints = info->check_constraints;
++
++		status = variable_checker_set_constraints(
++			&constraints,
++			info->is_constraints_set,
++			&property->VariableProperty);
++
++		if (status == EFI_SUCCESS) {
+ 
+-		if (!info) return EFI_OUT_OF_RESOURCES;
++			variable_index_update_constraints(info, &constraints);
++		}
+ 	}
++	else {
+ 
+-	/* Applying check constraints to an existing variable that may have
+-	 * constraints already set.  These could constrain the setting of
+-	 * the constraints.
+-	 */
+-	struct variable_constraints constraints = info->check_constraints;
++		/* Applying check constraints for a new variable */
++		struct variable_constraints constraints;
+ 
+-	status = variable_checker_set_constraints(
+-		&constraints,
+-		info->is_constraints_set,
+-		&property->VariableProperty);
++		status = variable_checker_set_constraints(
++			&constraints,
++			false,
++			&property->VariableProperty);
+ 
+-	if (status == EFI_SUCCESS) {
++		if (status == EFI_SUCCESS) {
+ 
+-		variable_index_set_constraints(info, &constraints);
+-	}
++			info = variable_index_add_constraints(
++				&context->variable_index,
++				&property->Guid,
++				property->NameSize,
++				property->Name,
++				&constraints);
+ 
+-	variable_index_remove_unused_entry(&context->variable_index, info);
++			if (!info) status = EFI_OUT_OF_RESOURCES;
++		}
++	}
+ 
+ 	return status;
+ }
+@@ -514,8 +506,7 @@ static efi_status_t check_capabilities(
+ 	if (var->Attributes & ~(
+ 		EFI_VARIABLE_NON_VOLATILE |
+ 		EFI_VARIABLE_BOOTSERVICE_ACCESS |
+-		EFI_VARIABLE_RUNTIME_ACCESS |
+-		EFI_VARIABLE_APPEND_WRITE)) {
++		EFI_VARIABLE_RUNTIME_ACCESS)) {
+ 
+ 		/* An unsupported attribute has been requested */
+ 		status = EFI_UNSUPPORTED;
+@@ -561,6 +552,17 @@ static efi_status_t check_access_permitted_on_set(
+ 			var->DataSize);
+ 	}
+ 
++	if ((status == EFI_SUCCESS) && var->DataSize) {
++
++		/* Restrict which attributes can be modified for an existing variable */
++		if ((var->Attributes & EFI_VARIABLE_NON_VOLATILE) !=
++			(info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE)) {
++
++			/* Don't permit change of storage class */
++			status = EFI_INVALID_PARAMETER;
++		}
++	}
++
+ 	return status;
+ }
+ 
+@@ -581,33 +583,20 @@ static efi_status_t store_variable_data(
+ 
+ 	if (delegate_store->storage_backend) {
+ 
+-		if (!(var->Attributes & EFI_VARIABLE_APPEND_WRITE)) {
+-
+-			/* Create or overwrite variable data */
+-			psa_status = store_overwrite(
+-				delegate_store,
+-				context->owner_id,
+-				info->metadata.uid,
+-				data_len,
+-				data);
+-		}
+-		else {
+-
+-			/* Append new data to existing variable data */
+-			psa_status = store_append_write(
+-				delegate_store,
+-				context->owner_id,
+-				info->metadata.uid,
+-				data_len,
+-				data);
+-		}
++		psa_status = delegate_store->storage_backend->interface->set(
++			delegate_store->storage_backend->context,
++			context->owner_id,
++			info->metadata.uid,
++			data_len,
++			data,
++			PSA_STORAGE_FLAG_NONE);
+ 	}
+ 
+ 	if ((psa_status != PSA_SUCCESS) && delegate_store->is_nv) {
+ 
+ 		/* A storage failure has occurred so attempt to fix any
+-		 * mismatch between the variable index and stored NV variables.
+-		 */
++		* mismatch between the variable index and stored NV variables.
++		*/
+ 		purge_orphan_index_entries(context);
+ 	}
+ 
+@@ -674,100 +663,6 @@ static efi_status_t load_variable_data(
+ 	return psa_to_efi_storage_status(psa_status);
+ }
+ 
+-static psa_status_t store_overwrite(
+-	struct delegate_variable_store *delegate_store,
+-	uint32_t client_id,
+-	uint64_t uid,
+-	size_t data_length,
+-	const void *data)
+-{
+-	/* Police maximum variable size limit */
+-	if (data_length > delegate_store->max_variable_size) return PSA_ERROR_INVALID_ARGUMENT;
+-
+-	psa_status_t psa_status = delegate_store->storage_backend->interface->set(
+-		delegate_store->storage_backend->context,
+-		client_id,
+-		uid,
+-		data_length,
+-		data,
+-		PSA_STORAGE_FLAG_NONE);
+-
+-	return psa_status;
+-}
+-
+-static psa_status_t store_append_write(
+-	struct delegate_variable_store *delegate_store,
+-	uint32_t client_id,
+-	uint64_t uid,
+-	size_t data_length,
+-	const void *data)
+-{
+-	struct psa_storage_info_t storage_info;
+-
+-	if (data_length == 0) return PSA_SUCCESS;
+-
+-	psa_status_t psa_status = delegate_store->storage_backend->interface->get_info(
+-		delegate_store->storage_backend->context,
+-		client_id,
+-		uid,
+-		&storage_info);
+-
+-	if (psa_status != PSA_SUCCESS) return psa_status;
+-
+-	/* Determine size of appended variable */
+-	size_t new_size = storage_info.size + data_length;
+-
+-	/* Defend against integer overflow */
+-	if (new_size < storage_info.size) return PSA_ERROR_INVALID_ARGUMENT;
+-
+-		/* Police maximum variable size limit */
+-	if (new_size > delegate_store->max_variable_size) return PSA_ERROR_INVALID_ARGUMENT;
+-
+-	/* Storage backend doesn't support an append operation so we need
+-	 * need to read the current variable data, extend it and write it back.
+-	 */
+-	uint8_t *rw_buf = malloc(new_size);
+-	if (!rw_buf) return PSA_ERROR_INSUFFICIENT_MEMORY;
+-
+-	size_t old_size = 0;
+-	psa_status = delegate_store->storage_backend->interface->get(
+-		delegate_store->storage_backend->context,
+-		client_id,
+-		uid,
+-		0,
+-		new_size,
+-		rw_buf,
+-		&old_size);
+-
+-	if (psa_status == PSA_SUCCESS) {
+-
+-		if ((old_size + data_length) <= new_size) {
+-
+-			/* Extend the variable data */
+-			memcpy(&rw_buf[old_size], data, data_length);
+-
+-			psa_status = delegate_store->storage_backend->interface->set(
+-				delegate_store->storage_backend->context,
+-				client_id,
+-				uid,
+-				old_size + data_length,
+-				rw_buf,
+-				storage_info.flags);
+-		}
+-		else {
+-
+-			/* There's a mismatch between the length obtained from
+-			 * get_info() and the subsequent length returned by get().
+-			 */
+-			psa_status = PSA_ERROR_STORAGE_FAILURE;
+-		}
+-	}
+-
+-	free(rw_buf);
+-
+-	return psa_status;
+-}
+-
+ static void purge_orphan_index_entries(
+ 	struct uefi_variable_store *context)
+ {
+@@ -782,7 +677,7 @@ static void purge_orphan_index_entries(
+ 	 */
+ 	while (!variable_index_iterator_is_done(&iter)) {
+ 
+-		struct variable_info *info = variable_index_iterator_current(&iter);
++		const struct variable_info *info = variable_index_iterator_current(&iter);
+ 
+ 		if (info->is_variable_set && (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE)) {
+ 
+@@ -799,7 +694,7 @@ static void purge_orphan_index_entries(
+ 			if (psa_status != PSA_SUCCESS) {
+ 
+ 				/* Detected a mismatch between the index and storage */
+-				variable_index_clear_variable(&context->variable_index, info);
++				variable_index_remove_variable(&context->variable_index, info);
+ 				any_orphans = true;
+ 			}
+ 		}
+diff --git a/components/service/smm_variable/backend/variable_index.c b/components/service/smm_variable/backend/variable_index.c
+index a8a55753..99d7c97a 100644
+--- a/components/service/smm_variable/backend/variable_index.c
++++ b/components/service/smm_variable/backend/variable_index.c
+@@ -132,13 +132,13 @@ size_t variable_index_max_dump_size(
+ 	return sizeof(struct variable_metadata) * context->max_variables;
+ }
+ 
+-struct variable_info *variable_index_find(
+-	struct variable_index *context,
++const struct variable_info *variable_index_find(
++	const struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+ 	const int16_t *name)
+ {
+-	struct variable_info *result = NULL;
++	const struct variable_info *result = NULL;
+ 	int pos = find_variable(context, guid, name_size, name);
+ 
+ 	if (pos >= 0) {
+@@ -149,13 +149,13 @@ struct variable_info *variable_index_find(
+ 	return result;
+ }
+ 
+-struct variable_info *variable_index_find_next(
++const struct variable_info *variable_index_find_next(
+ 	const struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+ 	const int16_t *name)
+ {
+-	struct variable_info *result = NULL;
++	const struct variable_info *result = NULL;
+ 
+ 	if (name_size >= sizeof(int16_t)) {
+ 
+@@ -263,11 +263,12 @@ static struct variable_entry *add_entry(
+ 	return entry;
+ }
+ 
+-struct variable_info *variable_index_add_entry(
++const struct variable_info *variable_index_add_variable(
+ 	struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+-	const int16_t *name)
++	const int16_t *name,
++	uint32_t attributes)
+ {
+ 	struct variable_info *info = NULL;
+ 	struct variable_entry *entry = add_entry(context, guid, name_size, name);
+@@ -275,41 +276,40 @@ struct variable_info *variable_index_add_entry(
+ 	if (entry) {
+ 
+ 		info = &entry->info;
++
++		info->metadata.attributes = attributes;
++		info->is_variable_set = true;
++
++		mark_dirty(entry);
+ 	}
+ 
+ 	return info;
+ }
+ 
+-void variable_index_remove_unused_entry(
++const struct variable_info *variable_index_add_constraints(
+ 	struct variable_index *context,
+-	struct variable_info *info)
++	const EFI_GUID *guid,
++	size_t name_size,
++	const int16_t *name,
++	const struct variable_constraints *constraints)
+ {
+-	if (info &&
+-		!info->is_constraints_set &&
+-		!info->is_variable_set) {
+-
+-		struct variable_entry *entry = containing_entry(info);
+-		entry->in_use = false;
++	struct variable_info *info = NULL;
++	struct variable_entry *entry = add_entry(context, guid, name_size, name);
+ 
+-		memset(info, 0, sizeof(struct variable_info));
+-	}
+-}
++	if (entry) {
+ 
+-void variable_index_set_variable(
+-	struct variable_info *info,
+-	uint32_t attributes)
+-{
+-	struct variable_entry *entry = containing_entry(info);
++		info = &entry->info;
+ 
+-	info->metadata.attributes = attributes;
+-	info->is_variable_set = true;
++		info->check_constraints = *constraints;
++		info->is_constraints_set = true;
++	}
+ 
+-	mark_dirty(entry);
++	return info;
+ }
+ 
+-void variable_index_clear_variable(
++void variable_index_remove_variable(
+ 	struct variable_index *context,
+-	struct variable_info *info)
++	const struct variable_info *info)
+ {
+ 	if (info) {
+ 
+@@ -318,17 +318,48 @@ void variable_index_clear_variable(
+ 
+ 		/* Mark variable as no longer set */
+ 		entry->info.is_variable_set = false;
++
++		/* Entry may still be needed if check constraints were set */
++		entry->in_use = info->is_constraints_set;
++
++		if (!entry->in_use) {
++
++			/* Entry not needed so wipe */
++			memset(&entry->info, 0, sizeof(struct variable_info));
++		}
+ 	}
+ }
+ 
+-void variable_index_set_constraints(
+-	struct variable_info *info,
++void variable_index_update_variable(
++	const struct variable_info *info,
++	uint32_t attributes)
++{
++	if (info) {
++
++		struct variable_info *modified_info = (struct variable_info*)info;
++		struct variable_entry *entry = containing_entry(modified_info);
++
++		if (!modified_info->is_variable_set ||
++			(attributes != modified_info->metadata.attributes)) {
++
++			/* The update changes the variable_info state */
++			modified_info->is_variable_set = true;
++			modified_info->metadata.attributes = attributes;
++			mark_dirty(entry);
++		}
++	}
++}
++
++void variable_index_update_constraints(
++	const struct variable_info *info,
+ 	const struct variable_constraints *constraints)
+ {
+ 	if (info) {
+ 
+-		info->check_constraints = *constraints;
+-		info->is_constraints_set = true;
++		struct variable_info *modified_info = (struct variable_info*)info;
++
++		modified_info->check_constraints = *constraints;
++		modified_info->is_constraints_set = true;
+ 	}
+ }
+ 
+diff --git a/components/service/smm_variable/backend/variable_index.h b/components/service/smm_variable/backend/variable_index.h
+index 63f42ab6..e109d0d1 100644
+--- a/components/service/smm_variable/backend/variable_index.h
++++ b/components/service/smm_variable/backend/variable_index.h
+@@ -119,8 +119,8 @@ size_t variable_index_max_dump_size(
+  *
+  * @return     Pointer to variable_info or NULL
+  */
+-struct variable_info *variable_index_find(
+-	struct variable_index *context,
++const struct variable_info *variable_index_find(
++	const struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+ 	const int16_t *name);
+@@ -135,76 +135,78 @@ struct variable_info *variable_index_find(
+  *
+  * @return     Pointer to variable_info or NULL
+  */
+-struct variable_info *variable_index_find_next(
++const struct variable_info *variable_index_find_next(
+ 	const struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+ 	const int16_t *name);
+ 
+ /**
+- * @brief      Add a new entry to the index
+- *
+- * An entry is needed either when a new variable is created or
+- * when variable constraints are set for a variable that doesn't
+- * yet exist.
++ * @brief      Add a new variable to the index
+  *
+  * @param[in]  context variable_index
+  * @param[in]  guid The variable's guid
+  * @param[in]  name_size The name parameter's size
+  * @param[in]  name The variable's name
++ * @param[in]  attributes The variable's attributes
+  *
+  * @return     Pointer to variable_info or NULL
+  */
+-struct variable_info *variable_index_add_entry(
++const struct variable_info *variable_index_add_variable(
+ 	struct variable_index *context,
+ 	const EFI_GUID *guid,
+ 	size_t name_size,
+-	const int16_t *name);
++	const int16_t *name,
++	uint32_t attributes);
+ 
+ /**
+- * @brief      Remove an unused entry from the index
++ * @brief      Remove a variable from the index
+  *
+- * Removes an entry if it is not in use.
++ * Removes a variable from the index if it exists.
+  *
+  * @param[in]  context variable_index
+  * @param[in]  info The variable info corresponding to the entry to remove
+  */
+-void variable_index_remove_unused_entry(
++void variable_index_remove_variable(
+ 	struct variable_index *context,
+-	struct variable_info *info);
++	const struct variable_info *info);
+ 
+ /**
+- * @brief      Set a variable to the index
+- *
+- * An entry for the variable must already exist.
++ * @brief      Update a variable that's already in the index
+  *
+  * @param[in]  info variable info
+  * @param[in]  attributes The variable's attributes
+  */
+-void variable_index_set_variable(
+-	struct variable_info *info,
++void variable_index_update_variable(
++	const struct variable_info *info,
+ 	uint32_t attributes);
+ 
+ /**
+- * @brief      Clear a variable from the index
+- *
+- * Clears a variable from the index
++ * @brief      Add a new check constraints object to the index
+  *
+  * @param[in]  context variable_index
+- * @param[in]  info The variable info corresponding to the variable to clear
++ * @param[in]  guid The variable's guid
++ * @param[in]  name_size The name parameter's size
++ * @param[in]  name The variable's name
++ * @param[in]  constraints The check constraints
++ *
++ * @return     Pointer to variable_info or NULL
+  */
+-void variable_index_clear_variable(
++const struct variable_info *variable_index_add_constraints(
+ 	struct variable_index *context,
+-	struct variable_info *info);
++	const EFI_GUID *guid,
++	size_t name_size,
++	const int16_t *name,
++	const struct variable_constraints *constraints);
+ 
+ /**
+- * @brief      Set a check constraints object associated with a variavle
++ * @brief      Update variable constraints that are already in the index
+  *
+  * @param[in]  info variable info
+  * @param[in]  constraints The check constraints
+  */
+-void variable_index_set_constraints(
+-	struct variable_info *info,
++void variable_index_update_constraints(
++	const struct variable_info *info,
+ 	const struct variable_constraints *constraints);
+ 
+ /**
+diff --git a/components/service/smm_variable/backend/variable_index_iterator.c b/components/service/smm_variable/backend/variable_index_iterator.c
+index 8f8fc741..7cc6dc7a 100644
+--- a/components/service/smm_variable/backend/variable_index_iterator.c
++++ b/components/service/smm_variable/backend/variable_index_iterator.c
+@@ -31,10 +31,10 @@ bool variable_index_iterator_is_done(
+ 	return iter->current_pos >= iter->variable_index->max_variables;
+ }
+ 
+-struct variable_info *variable_index_iterator_current(
++const struct variable_info *variable_index_iterator_current(
+ 	const struct variable_index_iterator *iter)
+ {
+-	struct variable_info *current = NULL;
++	const struct variable_info *current = NULL;
+ 
+ 	if (!variable_index_iterator_is_done(iter)) {
+ 
+diff --git a/components/service/smm_variable/backend/variable_index_iterator.h b/components/service/smm_variable/backend/variable_index_iterator.h
+index 7ff77c50..f64a2c49 100644
+--- a/components/service/smm_variable/backend/variable_index_iterator.h
++++ b/components/service/smm_variable/backend/variable_index_iterator.h
+@@ -54,7 +54,7 @@ bool variable_index_iterator_is_done(
+  *
+  * @return     Pointer to variable_info or NULL
+  */
+-struct variable_info *variable_index_iterator_current(
++const struct variable_info *variable_index_iterator_current(
+ 	const struct variable_index_iterator *iter);
+ 
+ /**
+diff --git a/components/service/smm_variable/test/service/smm_variable_service_tests.cpp b/components/service/smm_variable/test/service/smm_variable_service_tests.cpp
+index 15556e9d..38c08ebe 100644
+--- a/components/service/smm_variable/test/service/smm_variable_service_tests.cpp
++++ b/components/service/smm_variable/test/service/smm_variable_service_tests.cpp
+@@ -249,30 +249,6 @@ TEST(SmmVariableServiceTests, setAndGet)
+ 	UNSIGNED_LONGS_EQUAL(set_data.size(), get_data.size());
+ 	LONGS_EQUAL(0, get_data.compare(set_data));
+ 
+-	/* Extend the variable using an append write */
+-	std::string append_data = " values added with append write";
+-
+-	efi_status = m_client->set_variable(
+-		m_common_guid,
+-		var_name,
+-		append_data,
+-		EFI_VARIABLE_APPEND_WRITE);
+-
+-	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
+-
+-	efi_status = m_client->get_variable(
+-		m_common_guid,
+-		var_name,
+-		get_data);
+-
+-	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
+-
+-	std::string appended_data = set_data + append_data;
+-
+-	/* Expect the append write operation to have extended the variable */
+-	UNSIGNED_LONGLONGS_EQUAL(appended_data.size(), get_data.size());
+-	LONGS_EQUAL(0, appended_data.compare(get_data));
+-
+ 	/* Expect remove to be permitted */
+ 	efi_status = m_client->remove_variable(m_common_guid, var_name);
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
+@@ -303,30 +279,6 @@ TEST(SmmVariableServiceTests, setAndGetNv)
+ 	UNSIGNED_LONGS_EQUAL(set_data.size(), get_data.size());
+ 	LONGS_EQUAL(0, get_data.compare(set_data));
+ 
+-	/* Extend the variable using an append write */
+-	std::string append_data = " values added with append write";
+-
+-	efi_status = m_client->set_variable(
+-		m_common_guid,
+-		var_name,
+-		append_data,
+-		EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_APPEND_WRITE);
+-
+-	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
+-
+-	efi_status = m_client->get_variable(
+-		m_common_guid,
+-		var_name,
+-		get_data);
+-
+-	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
+-
+-	std::string appended_data = set_data + append_data;
+-
+-	/* Expect the append write operation to have extended the variable */
+-	UNSIGNED_LONGLONGS_EQUAL(appended_data.size(), get_data.size());
+-	LONGS_EQUAL(0, appended_data.compare(get_data));
+-
+ 	/* Expect remove to be permitted */
+ 	efi_status = m_client->remove_variable(m_common_guid, var_name);
+ 	UNSIGNED_LONGLONGS_EQUAL(EFI_SUCCESS, efi_status);
+diff --git a/protocols/service/smm_variable/parameters.h b/protocols/service/smm_variable/parameters.h
+index 233f301b..1f795a9b 100644
+--- a/protocols/service/smm_variable/parameters.h
++++ b/protocols/service/smm_variable/parameters.h
+@@ -47,9 +47,6 @@ typedef struct {
+ 	 EFI_VARIABLE_HARDWARE_ERROR_RECORD | \
+ 	 EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS | \
+ 	 EFI_VARIABLE_APPEND_WRITE)
+-#define	EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS_MASK \
+-	(EFI_VARIABLE_AUTHENTICATED_WRITE_ACCESS | \
+-	 EFI_VARIABLE_TIME_BASED_AUTHENTICATED_WRITE_ACCESS)
+ 
+ /**
+  * Parameter structure for SetVariable and GetVariable.
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0029-Change-UID-of-variable-index-in-SMM.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0029-Change-UID-of-variable-index-in-SMM.patch
new file mode 100644
index 0000000..caa1d9a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0029-Change-UID-of-variable-index-in-SMM.patch
@@ -0,0 +1,31 @@
+From 12e9b977e4c7515ce90fecc62630be394fd7da62 Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Mon, 20 Dec 2021 19:54:39 +0000
+Subject: [PATCH] Change UID of variable index in SMM
+
+This patch fixes the os_indications setVariable() failure. The variable
+index UID in SMM gateway which was 1 is changed in this patch. TFM has a
+special usage for variable with UID 1, which makes it write once only.
+This is not required for SMM variable index.
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+
+---
+ components/service/smm_variable/backend/uefi_variable_store.c | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/components/service/smm_variable/backend/uefi_variable_store.c b/components/service/smm_variable/backend/uefi_variable_store.c
+index 6a90f46a..1bb869ae 100644
+--- a/components/service/smm_variable/backend/uefi_variable_store.c
++++ b/components/service/smm_variable/backend/uefi_variable_store.c
+@@ -67,7 +67,7 @@ static efi_status_t check_name_terminator(
+ 	size_t name_size);
+ 
+ /* Private UID for storing the variable index */
+-#define VARIABLE_INDEX_STORAGE_UID			(1)
++#define VARIABLE_INDEX_STORAGE_UID			(0x787)
+ 
+ /* Default maximum variable size -
+  * may be overridden using uefi_variable_store_set_storage_limits()
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0030-Add-missing-features-to-setVariable.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0030-Add-missing-features-to-setVariable.patch
new file mode 100644
index 0000000..244146a
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0030-Add-missing-features-to-setVariable.patch
@@ -0,0 +1,73 @@
+From 55fc3dbfb0ec21b1239808d0dddae14fbb8bb5f3 Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Mon, 20 Dec 2021 19:56:30 +0000
+Subject: [PATCH] Add missing features to setVariable()
+
+This patch resolves the failing tests in SCT related to
+setVariable() function. The existing implementation is
+missing few cases where error codes are returned when called
+with certain paramters. These conditions are implemented in
+this patch based on the explanation provided in uefi spec.
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+
+---
+ .../backend/uefi_variable_store.c             | 29 ++++++++++++++++---
+ 1 file changed, 25 insertions(+), 4 deletions(-)
+
+diff --git a/components/service/smm_variable/backend/uefi_variable_store.c b/components/service/smm_variable/backend/uefi_variable_store.c
+index 1bb869ae..a1671074 100644
+--- a/components/service/smm_variable/backend/uefi_variable_store.c
++++ b/components/service/smm_variable/backend/uefi_variable_store.c
+@@ -161,6 +161,17 @@ efi_status_t uefi_variable_store_set_variable(
+ 	bool should_sync_index = false;
+ 
+ 	if (status != EFI_SUCCESS) return status;
++	
++	/*
++	* Runtime access to a data variable implies boot service access. Attributes that have
++	* EFI_VARIABLE_RUNTIME_ACCESS set must also have EFI_VARIABLE_BOOTSERVICE_ACCESS set.
++	* The caller is responsible for following this rule.
++	*/
++	if((var->Attributes & EFI_VARIABLE_RUNTIME_ACCESS))
++	{
++			if((var->Attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS) != EFI_VARIABLE_BOOTSERVICE_ACCESS )
++			return EFI_INVALID_PARAMETER;
++	}
+ 
+ 	/* Find in index */
+ 	const struct variable_info *info = variable_index_find(
+@@ -221,6 +232,13 @@ efi_status_t uefi_variable_store_set_variable(
+ 		if (!info) status = EFI_OUT_OF_RESOURCES;
+ 		should_sync_index = info && (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE);
+ 	}
++	else 
++    {
++		/* Return EFI_NOT_FOUND when a remove operation is performed
++		 * on variable that is not existing.
++		*/
++        status = EFI_NOT_FOUND;
++    }
+ 
+ 	/* The order of these operations is important. For an update
+ 	 * or create operation, The variable index is always synchronized
+@@ -555,10 +573,13 @@ static efi_status_t check_access_permitted_on_set(
+ 	if ((status == EFI_SUCCESS) && var->DataSize) {
+ 
+ 		/* Restrict which attributes can be modified for an existing variable */
+-		if ((var->Attributes & EFI_VARIABLE_NON_VOLATILE) !=
+-			(info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE)) {
+-
+-			/* Don't permit change of storage class */
++        if (((var->Attributes & EFI_VARIABLE_NON_VOLATILE) !=
++            (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE)) ||
++            ((var->Attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS) !=
++            (info->metadata.attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS)) ||
++            ((var->Attributes & EFI_VARIABLE_RUNTIME_ACCESS) !=
++            (info->metadata.attributes & EFI_VARIABLE_RUNTIME_ACCESS))) {
++			/* Don't permit change of attributes */
+ 			status = EFI_INVALID_PARAMETER;
+ 		}
+ 	}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0031-Add-invalid-parameter-check-in-getNextVariableName.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0031-Add-invalid-parameter-check-in-getNextVariableName.patch
new file mode 100644
index 0000000..3990d82
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0031-Add-invalid-parameter-check-in-getNextVariableName.patch
@@ -0,0 +1,55 @@
+From dc3f134436ad6852f1bad9542232e84166843a7e Mon Sep 17 00:00:00 2001
+From: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+Date: Mon, 20 Dec 2021 20:01:10 +0000
+Subject: [PATCH] Add invalid parameter check in getNextVariableName()
+
+This patch resolves the failing tests in SCT related to
+getNextVariableName() function. The existing implementation is
+missing few cases where error codes are returned when called
+with certain paramters. These conditions are implemented in
+this patch based on the explanation provided in uefi spec.
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Gowtham Suresh Kumar <gowtham.sureshkumar@arm.com>
+
+
+---
+ .../smm_variable/backend/uefi_variable_store.c | 18 +++++++++++++++++-
+ 1 file changed, 17 insertions(+), 1 deletion(-)
+
+diff --git a/components/service/smm_variable/backend/uefi_variable_store.c b/components/service/smm_variable/backend/uefi_variable_store.c
+index a1671074..a57b3346 100644
+--- a/components/service/smm_variable/backend/uefi_variable_store.c
++++ b/components/service/smm_variable/backend/uefi_variable_store.c
+@@ -161,7 +161,7 @@ efi_status_t uefi_variable_store_set_variable(
+ 	bool should_sync_index = false;
+ 
+ 	if (status != EFI_SUCCESS) return status;
+-	
++
+ 	/*
+ 	* Runtime access to a data variable implies boot service access. Attributes that have
+ 	* EFI_VARIABLE_RUNTIME_ACCESS set must also have EFI_VARIABLE_BOOTSERVICE_ACCESS set.
+@@ -310,6 +310,22 @@ efi_status_t uefi_variable_store_get_next_variable_name(
+ 	status = EFI_NOT_FOUND;
+ 	*total_length = 0;
+ 
++	/*
++	 *	If input values of VariableName and VendorGuid are not a name and GUID of an 
++	 *	existing variable, EFI_INVALID_PARAMETER is returned.	
++	 */
++	if (cur->NameSize >= sizeof(int16_t)) {
++		/*
++		* Name must be at least one character long to accommodate
++		* the mandatory null terminator.
++		*/
++		if (cur->Name[0] != 0) {
++			const struct variable_info *var_info = variable_index_find(&context->variable_index,&cur->Guid,cur->NameSize,cur->Name);
++			if(var_info == NULL)
++					return EFI_INVALID_PARAMETER;
++		}
++	}
++
+ 	const struct variable_info *info = variable_index_find_next(
+ 		&context->variable_index,
+ 		&cur->Guid,
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0032-smm_gateway-add-checks-for-null-attributes.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0032-smm_gateway-add-checks-for-null-attributes.patch
new file mode 100644
index 0000000..da3ddaf
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0032-smm_gateway-add-checks-for-null-attributes.patch
@@ -0,0 +1,81 @@
+From 571ddac16048dfba4b25b04fe5cbd706c392b5ba Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Fri, 24 Dec 2021 19:17:17 +0000
+Subject: [PATCH] smm_gateway: add checks for null attributes
+
+As par EDK-2 and EDK-2 test code, when a user issue's
+setVariable() with 0 in attributes field, it means a variable
+delete request. Currently, smm gatway doesn't handle this scenario.
+This change is to add that support
+
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
+
+
+---
+ .../backend/uefi_variable_store.c             | 28 ++++++++++++-------
+ 1 file changed, 18 insertions(+), 10 deletions(-)
+
+diff --git a/components/service/smm_variable/backend/uefi_variable_store.c b/components/service/smm_variable/backend/uefi_variable_store.c
+index a57b3346..e8771c21 100644
+--- a/components/service/smm_variable/backend/uefi_variable_store.c
++++ b/components/service/smm_variable/backend/uefi_variable_store.c
+@@ -167,7 +167,9 @@ efi_status_t uefi_variable_store_set_variable(
+ 	* EFI_VARIABLE_RUNTIME_ACCESS set must also have EFI_VARIABLE_BOOTSERVICE_ACCESS set.
+ 	* The caller is responsible for following this rule.
+ 	*/
+-	if((var->Attributes & EFI_VARIABLE_RUNTIME_ACCESS))
++	if (!var->Attributes)
++		EMSG("It might be a delete variable request\n");
++	else if((var->Attributes & EFI_VARIABLE_RUNTIME_ACCESS))
+ 	{
+ 			if((var->Attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS) != EFI_VARIABLE_BOOTSERVICE_ACCESS )
+ 			return EFI_INVALID_PARAMETER;
+@@ -191,7 +193,7 @@ efi_status_t uefi_variable_store_set_variable(
+ 				(var->Attributes & EFI_VARIABLE_NON_VOLATILE) ||
+ 				(info->is_variable_set && (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE));
+ 
+-			if (var->DataSize) {
++			if (var->DataSize && var->Attributes) {
+ 
+ 				/* It's a set rather than a remove operation */
+ 				variable_index_update_variable(
+@@ -206,7 +208,9 @@ efi_status_t uefi_variable_store_set_variable(
+ 				 * that it's never possible for an object to exist within
+ 				 * the storage backend without a corresponding index entry.
+ 				 */
+-				remove_variable_data(context, info);
++				EMSG("  deleting variable %s \n",var->Name);
++				if (remove_variable_data(context, info) != PSA_SUCCESS)
++					EMSG("  deleting variable %s FAILED\n",var->Name);
+ 				variable_index_remove_variable(&context->variable_index, info);
+ 
+ 				/* Variable info no longer valid */
+@@ -587,14 +591,18 @@ static efi_status_t check_access_permitted_on_set(
+ 	}
+ 
+ 	if ((status == EFI_SUCCESS) && var->DataSize) {
+-
++		/* Delete the variable with Attributes is 0 */
++		if (!var->Attributes) {
++			EMSG("Null attributes, may be a delete variable request\n");
++			status = EFI_SUCCESS;
++		}
+ 		/* Restrict which attributes can be modified for an existing variable */
+-        if (((var->Attributes & EFI_VARIABLE_NON_VOLATILE) !=
+-            (info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE)) ||
+-            ((var->Attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS) !=
+-            (info->metadata.attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS)) ||
+-            ((var->Attributes & EFI_VARIABLE_RUNTIME_ACCESS) !=
+-            (info->metadata.attributes & EFI_VARIABLE_RUNTIME_ACCESS))) {
++		else if (((var->Attributes & EFI_VARIABLE_NON_VOLATILE) !=
++			(info->metadata.attributes & EFI_VARIABLE_NON_VOLATILE)) ||
++			((var->Attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS) !=
++			(info->metadata.attributes & EFI_VARIABLE_BOOTSERVICE_ACCESS)) ||
++			((var->Attributes & EFI_VARIABLE_RUNTIME_ACCESS) !=
++			(info->metadata.attributes & EFI_VARIABLE_RUNTIME_ACCESS))) {
+ 			/* Don't permit change of attributes */
+ 			status = EFI_INVALID_PARAMETER;
+ 		}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0033-Enhance-mbedtls-fetch-process.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0033-Enhance-mbedtls-fetch-process.patch
new file mode 100644
index 0000000..02130b5
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0033-Enhance-mbedtls-fetch-process.patch
@@ -0,0 +1,258 @@
+From 47db072e9ec749c6be7c0a27d64d7fbd75748d60 Mon Sep 17 00:00:00 2001
+From: Gyorgy Szing <Gyorgy.Szing@arm.com>
+Date: Wed, 8 Dec 2021 04:20:34 +0100
+Subject: [PATCH] Enhance mbedtls fetch process
+
+Update management of MbedTLS external component to be optimized
+for download speed insted of availability.
+The updated process is:
+  - check if binary is available. If yes configure build to use it
+    and stop.
+  - if not, check is source is available. If yes, build it and use
+    the resulting binary.
+  - if not, then download the source using git, compile it and use
+    the resulting binary
+
+The following variables can be set on the command line to alter the
+behavior of the module:
+  - MBEDTLS_URL git repo URL to fetch from.
+  - MBEDTLS_REFSPEC revision to fetch
+  - MBEDTLS_SOURCE_DIR to specify location of source code in
+    local file syetem.
+  - MBEDTLS_INSTALL_DIR to specify location of binary.
+
+I.e. cmake -S <...> -B <...> -DMBEDTLS_INSTALL_DIR=~/mbedtls
+will make the resulting binary installed to ~/mbedtls. This can be
+used later to speed up a clean build an use the prebuilt binary.
+
+Change-Id: I8a9ad8b3303e6dfa0a7c9c3d7e4b4787b94d925a
+Signed-off-by: Gyorgy Szing <Gyorgy.Szing@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ external/MbedTLS/MbedTLS.cmake | 192 ++++++++++++++++++++-------------
+ 1 file changed, 119 insertions(+), 73 deletions(-)
+
+diff --git a/external/MbedTLS/MbedTLS.cmake b/external/MbedTLS/MbedTLS.cmake
+index 3cbaed15..935be765 100644
+--- a/external/MbedTLS/MbedTLS.cmake
++++ b/external/MbedTLS/MbedTLS.cmake
+@@ -1,96 +1,142 @@
+ #-------------------------------------------------------------------------------
+-# Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++# Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+ #
+ # SPDX-License-Identifier: BSD-3-Clause
+ #
+ #-------------------------------------------------------------------------------
+ 
+-# Determine the number of processes to run while running parallel builds.
+-# Pass -DPROCESSOR_COUNT=<n> to cmake to override.
+-if(NOT DEFINED PROCESSOR_COUNT)
+-	include(ProcessorCount)
+-	ProcessorCount(PROCESSOR_COUNT)
+-	set(PROCESSOR_COUNT ${PROCESSOR_COUNT} CACHE STRING "Number of cores to use for parallel builds.")
+-endif()
++set(MBEDTLS_URL "https://github.com/ARMmbed/mbedtls.git"
++		CACHE STRING "Mbed TLS repository URL")
++set(MBEDTLS_REFSPEC "mbedtls-3.0.0"
++		CACHE STRING "Mbed TLS git refspec")
++set(MBEDTLS_SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/_deps/mbedtls-src"
++		CACHE PATH "MbedTLS source directory")
++set(MBEDTLS_INSTALL_DIR "${CMAKE_CURRENT_BINARY_DIR}/mbedtls_install"
++		CACHE PATH "Mbed TLS installation directory")
+ 
+-set(MBEDTLS_URL "https://github.com/ARMmbed/mbedtls.git" CACHE STRING "Mbed TLS repository URL")
+-set(MBEDTLS_REFSPEC "mbedtls-3.0.0" CACHE STRING "Mbed TLS git refspec")
+-set(MBEDTLS_INSTALL_PATH "${CMAKE_CURRENT_BINARY_DIR}/mbedtls_install" CACHE PATH "Mbed TLS installation directory")
+-set(MBEDTLS_PACKAGE_PATH "${MBEDTLS_INSTALL_PATH}/lib/mbedtls/cmake" CACHE PATH "Mbed TLS CMake package directory")
++find_library(MBEDCRYPTO_LIB_FILE
++				NAMES libmbedcrypto.a mbedcrypto.a libmbedcrypto.lib mbedcrypto.lib
++				PATHS ${MBEDTLS_INSTALL_DIR}
++				PATH_SUFFIXES "lib"
++				DOC "Location of mberdrypto library."
++				NO_DEFAULT_PATH
++)
+ 
+-include(FetchContent)
++set(MBEDCRYPTO_LIB_FILE ${MBEDCRYPTO_LIB_FILE})
++unset(MBEDCRYPTO_LIB_FILE CACHE)
+ 
+-# Checking git
+-find_program(GIT_COMMAND "git")
+-if (NOT GIT_COMMAND)
+-	message(FATAL_ERROR "Please install git")
+-endif()
++set(MBEDTLS_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/_deps/mbedtls-build")
+ 
+-# Fetching Mbed TLS
+-FetchContent_Declare(
+-	mbedtls
+-	GIT_REPOSITORY ${MBEDTLS_URL}
+-	GIT_TAG ${MBEDTLS_REFSPEC}
+-	GIT_SHALLOW TRUE
+-)
++# Binary not found and it needs to be built.
++if (NOT MBEDCRYPTO_LIB_FILE)
++	# Determine the number of processes to run while running parallel builds.
++	# Pass -DPROCESSOR_COUNT=<n> to cmake to override.
++	if(NOT DEFINED PROCESSOR_COUNT)
++		include(ProcessorCount)
++		ProcessorCount(PROCESSOR_COUNT)
++		set(PROCESSOR_COUNT ${PROCESSOR_COUNT}
++				CACHE STRING "Number of cores to use for parallel builds.")
++	endif()
+ 
+-# FetchContent_GetProperties exports mbedtls_SOURCE_DIR and mbedtls_BINARY_DIR variables
+-FetchContent_GetProperties(mbedtls)
+-if(NOT mbedtls_POPULATED)
+-	message(STATUS "Fetching Mbed TLS")
+-	FetchContent_Populate(mbedtls)
+-endif()
++	# See if the source is available locally
++	find_file(MBEDCRYPTO_HEADER_FILE
++		NAMES crypto.h
++		PATHS ${MBEDTLS_SOURCE_DIR}
++		PATH_SUFFIXES "include/psa"
++		NO_DEFAULT_PATH
++	)
++	set(MBEDCRYPTO_HEADER_FILE ${MBEDCRYPTO_HEADER_FILE})
++	unset(MBEDCRYPTO_HEADER_FILE CACHE)
+ 
+-# Convert the include path list to a string. Needed to make parameter passing to
+-# Mbed TLS build work fine.
+-string(REPLACE ";" "\\;" MBEDTLS_EXTRA_INCLUDES "${MBEDTLS_EXTRA_INCLUDES}")
++	# Source not found, fetch it.
++	if (NOT MBEDCRYPTO_HEADER_FILE)
++		include(FetchContent)
+ 
+-find_package(Python3 COMPONENTS Interpreter)
+-if (NOT Python3_Interpreter_FOUND)
+-	message(FATAL_ERROR "Python 3 interpreter not found.")
+-endif()
++		# Checking git
++		find_program(GIT_COMMAND "git")
++		if (NOT GIT_COMMAND)
++			message(FATAL_ERROR "Please install git")
++		endif()
+ 
+-#Configure Mbed TLS to build only mbedcrypto lib
+-execute_process(COMMAND ${Python3_EXECUTABLE} scripts/config.py crypto WORKING_DIRECTORY ${mbedtls_SOURCE_DIR})
+-
+-# Advertise Mbed TLS as the provider of the psa crypto API
+-set(PSA_CRYPTO_API_INCLUDE "${MBEDTLS_INSTALL_PATH}/include" CACHE STRING "PSA Crypto API include path")
+-
+-#Configure the library
+-execute_process(COMMAND
+-	${CMAKE_COMMAND}
+-		-DENABLE_PROGRAMS=OFF
+-		-DENABLE_TESTING=OFF
+-		-DUNSAFE_BUILD=ON
+-		-DCMAKE_INSTALL_PREFIX=${MBEDTLS_INSTALL_PATH}
+-		-DCMAKE_TOOLCHAIN_FILE=${TS_EXTERNAL_LIB_TOOLCHAIN_FILE}
+-		-DCMAKE_TRY_COMPILE_TARGET_TYPE=STATIC_LIBRARY
+-		-DEXTERNAL_DEFINITIONS=-DMBEDTLS_USER_CONFIG_FILE="${MBEDTLS_USER_CONFIG_FILE}"
+-		-DEXTERNAL_INCLUDE_PATHS=${MBEDTLS_EXTRA_INCLUDES}
+-		-GUnix\ Makefiles
+-		${mbedtls_SOURCE_DIR}
+-	WORKING_DIRECTORY
+-		${mbedtls_BINARY_DIR}
+-	RESULT_VARIABLE _exec_error
+-)
++		# Fetching Mbed TLS
++		FetchContent_Declare(
++			mbedtls
++			SOURCE_DIR ${MBEDTLS_SOURCE_DIR}
++			BINARY_DIR ${MBEDTLS_BINARY_DIR}
++			GIT_REPOSITORY ${MBEDTLS_URL}
++			GIT_TAG ${MBEDTLS_REFSPEC}
++			GIT_SHALLOW TRUE
++		)
+ 
+-if (_exec_error)
+-	message(FATAL_ERROR "Configuration step of Mbed TLS failed with ${_exec_error}.")
+-endif()
++		# FetchContent_GetProperties exports mbedtls_SOURCE_DIR and mbedtls_BINARY_DIR variables
++		FetchContent_GetProperties(mbedtls)
++		# FetchContent_Populate will fail if the source directory is removed since it will try to
++		# do an "update" and not a "populate" action. As a workaround, remove the subbuild directory.
++		# Note: this fix assumes, the default subbuild location is used.
++		file(REMOVE_RECURSE "${CMAKE_CURRENT_BINARY_DIR}/_deps/mbedtls-subbuild")
++
++		# If the source directory has been moved, the binary dir must be regenerated from scratch.
++		file(REMOVE_RECURSE "${MBEDTLS_BINARY_DIR}")
+ 
+-#TODO: add dependency to generated project on this file!
+-#TODO: add custom target to rebuild Mbed TLS
++		if (NOT mbedtls_POPULATED)
++			message(STATUS "Fetching Mbed TLS")
++			FetchContent_Populate(mbedtls)
++		endif()
++		set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${MBEDTLS_SOURCE_DIR})
++	endif()
+ 
+-#Build the library
+-execute_process(COMMAND
+-		${CMAKE_COMMAND} --build ${mbedtls_BINARY_DIR} --parallel ${PROCESSOR_COUNT} --target install
++	# Build mbedcrypto library
++
++	# Convert the include path list to a string. Needed to make parameter passing to
++	# Mbed TLS build work fine.
++	string(REPLACE ";" "\\;" MBEDTLS_EXTRA_INCLUDES "${MBEDTLS_EXTRA_INCLUDES}")
++
++	find_package(Python3 REQUIRED COMPONENTS Interpreter)
++
++	#Configure Mbed TLS to build only mbedcrypto lib
++	execute_process(COMMAND ${Python3_EXECUTABLE} scripts/config.py crypto WORKING_DIRECTORY ${MBEDTLS_SOURCE_DIR})
++
++	# Advertise Mbed TLS as the provider of the psa crypto API
++	set(PSA_CRYPTO_API_INCLUDE "${MBEDTLS_INSTALL_DIR}/include" CACHE STRING "PSA Crypto API include path")
++
++	#Configure the library
++	execute_process(COMMAND
++		${CMAKE_COMMAND} -E env CROSS_COMPILE=${CROSS_COMPILE}
++			${CMAKE_COMMAND}
++				-DENABLE_PROGRAMS=OFF
++				-DENABLE_TESTING=OFF
++				-DUNSAFE_BUILD=ON
++				-DCMAKE_INSTALL_PREFIX=${MBEDTLS_INSTALL_DIR}
++				-DCMAKE_TOOLCHAIN_FILE=${TS_EXTERNAL_LIB_TOOLCHAIN_FILE}
++				-DCMAKE_TRY_COMPILE_TARGET_TYPE=STATIC_LIBRARY
++				-DEXTERNAL_DEFINITIONS=-DMBEDTLS_USER_CONFIG_FILE="${MBEDTLS_USER_CONFIG_FILE}"
++				-DEXTERNAL_INCLUDE_PATHS=${MBEDTLS_EXTRA_INCLUDES}
++				-GUnix\ Makefiles
++				${MBEDTLS_SOURCE_DIR}
++		WORKING_DIRECTORY
++			${MBEDTLS_BINARY_DIR}
+ 		RESULT_VARIABLE _exec_error
+ 	)
+-if (_exec_error)
+-	message(FATAL_ERROR "Build step of Mbed TLS failed with ${_exec_error}.")
++
++	if (_exec_error)
++		message(FATAL_ERROR "Configuration step of Mbed TLS failed with ${_exec_error}.")
++	endif()
++
++	#Build the library
++	execute_process(COMMAND
++			${CMAKE_COMMAND} --build ${MBEDTLS_BINARY_DIR} --parallel ${PROCESSOR_COUNT} --target install
++			RESULT_VARIABLE _exec_error
++		)
++
++	if (_exec_error)
++		message(FATAL_ERROR "Build step of Mbed TLS failed with ${_exec_error}.")
++	endif()
++
++	set(MBEDCRYPTO_LIB_FILE "${MBEDTLS_INSTALL_DIR}/lib/${CMAKE_STATIC_LIBRARY_PREFIX}mbedcrypto${CMAKE_STATIC_LIBRARY_SUFFIX}")
+ endif()
+ 
+ #Create an imported target to have clean abstraction in the build-system.
+ add_library(mbedcrypto STATIC IMPORTED)
+-set_property(TARGET mbedcrypto PROPERTY IMPORTED_LOCATION "${MBEDTLS_INSTALL_PATH}/lib/${CMAKE_STATIC_LIBRARY_PREFIX}mbedcrypto${CMAKE_STATIC_LIBRARY_SUFFIX}")
+-set_property(TARGET mbedcrypto PROPERTY INTERFACE_INCLUDE_DIRECTORIES "${MBEDTLS_INSTALL_PATH}/include")
++set_property(DIRECTORY ${CMAKE_SOURCE_DIR} APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${MBEDCRYPTO_LIB_FILE})
++set_property(TARGET mbedcrypto PROPERTY IMPORTED_LOCATION ${MBEDCRYPTO_LIB_FILE})
++set_property(TARGET mbedcrypto PROPERTY INTERFACE_INCLUDE_DIRECTORIES "${MBEDTLS_INSTALL_DIR}/include")
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0034-Fix-format-specifier-in-logging_caller.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0034-Fix-format-specifier-in-logging_caller.patch
new file mode 100644
index 0000000..75fa7c2
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0034-Fix-format-specifier-in-logging_caller.patch
@@ -0,0 +1,41 @@
+From 131bb3c577fff93ff9ba6f5e7d450f727fec0e62 Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Fri, 11 Feb 2022 12:30:45 +0000
+Subject: [PATCH] Fix format specifier in logging_caller
+
+A previous change increased the width of the opstatus value
+returned by an rpc endpoint from 32 to 64 bits. This change
+corrects the printf format specifier in the rpc logging_caller
+that corresponds to logging the opstatus value.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: Ie695a6bf8cf8014317b85196d7b933d344782b2c
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ components/rpc/common/logging/logging_caller.c | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/components/rpc/common/logging/logging_caller.c b/components/rpc/common/logging/logging_caller.c
+index 07c33de5..cac03f2f 100644
+--- a/components/rpc/common/logging/logging_caller.c
++++ b/components/rpc/common/logging/logging_caller.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -87,7 +87,7 @@ static rpc_status_t call_invoke(void *context, rpc_call_handle handle, uint32_t
+ 
+ 	if (status == TS_RPC_CALL_ACCEPTED) {
+ 
+-		fprintf(this_instance->log_file, "op_status: %d\n", *opstatus);
++		fprintf(this_instance->log_file, "op_status: %ld\n", *opstatus);
+ 		fprintf(this_instance->log_file, "resp_len: %ld\n", *resp_len);
+ 	}
+ 
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0035-Update-refspecs-for-mbedtls-and-psa-arch-tests-for-v.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0035-Update-refspecs-for-mbedtls-and-psa-arch-tests-for-v.patch
new file mode 100644
index 0000000..01b99d3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0035-Update-refspecs-for-mbedtls-and-psa-arch-tests-for-v.patch
@@ -0,0 +1,64 @@
+From 7aa9796020487ce32746c25934ce20829acc462c Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Fri, 11 Feb 2022 13:42:59 +0000
+Subject: [PATCH] Update refspecs for mbedtls and psa-arch-tests for v3.1.0
+
+Updates external component refspecs to use mbedtls 3.1.0 and
+compatible API tests from psa-arch-test.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: I1b5cebd7de3c1885f5f8a8ea21ba5e4c52aefaf4
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ external/MbedTLS/MbedTLS.cmake               |  2 +-
+ external/psa_arch_tests/psa_arch_tests.cmake | 17 ++++++-----------
+ 2 files changed, 7 insertions(+), 12 deletions(-)
+
+diff --git a/external/MbedTLS/MbedTLS.cmake b/external/MbedTLS/MbedTLS.cmake
+index 935be765..3350d8a0 100644
+--- a/external/MbedTLS/MbedTLS.cmake
++++ b/external/MbedTLS/MbedTLS.cmake
+@@ -7,7 +7,7 @@
+ 
+ set(MBEDTLS_URL "https://github.com/ARMmbed/mbedtls.git"
+ 		CACHE STRING "Mbed TLS repository URL")
+-set(MBEDTLS_REFSPEC "mbedtls-3.0.0"
++set(MBEDTLS_REFSPEC "mbedtls-3.1.0"
+ 		CACHE STRING "Mbed TLS git refspec")
+ set(MBEDTLS_SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/_deps/mbedtls-src"
+ 		CACHE PATH "MbedTLS source directory")
+diff --git a/external/psa_arch_tests/psa_arch_tests.cmake b/external/psa_arch_tests/psa_arch_tests.cmake
+index e6ab73f7..f6d2fb9f 100644
+--- a/external/psa_arch_tests/psa_arch_tests.cmake
++++ b/external/psa_arch_tests/psa_arch_tests.cmake
+@@ -5,20 +5,15 @@
+ #
+ #-------------------------------------------------------------------------------
+ 
+-# Determine the number of processes to run while running parallel builds.
+-# Pass -DPROCESSOR_COUNT=<n> to cmake to override.
+-if(NOT DEFINED PROCESSOR_COUNT)
+-	include(ProcessorCount)
+-	ProcessorCount(PROCESSOR_COUNT)
+-	set(PROCESSOR_COUNT ${PROCESSOR_COUNT} CACHE STRING "Number of cores to use for parallel builds.")
+-endif()
++# Temporarily using modified tests used for tf-m verification
++set(PSA_ARCH_TESTS_URL "https://github.com/bensze01/psa-arch-tests.git" CACHE STRING "psa-arch-tests repository URL")
++set(PSA_ARCH_TESTS_REFSPEC "fix-multipart-aead" CACHE STRING "psa-arch-tests git refspec")
+ 
+-set(PSA_ARCH_TESTS_URL "https://github.com/ARM-software/psa-arch-tests.git" CACHE STRING "psa-arch-tests repository URL")
+-set(PSA_ARCH_TESTS_REFSPEC "master" CACHE STRING "psa-arch-tests git refspec")
++#set(PSA_ARCH_TESTS_URL "https://github.com/ARM-software/psa-arch-tests.git" CACHE STRING "psa-arch-tests repository URL")
++#set(PSA_ARCH_TESTS_REFSPEC "2a1852252a9b9af655cbe02d5d3c930952d0d798" CACHE STRING "psa-arch-tests v22.01_API1.4_ADAC_BETA")
+ set(PSA_ARCH_TESTS_INSTALL_PATH "${CMAKE_CURRENT_BINARY_DIR}/psa-arch-tests_install" CACHE PATH "psa-arch-tests installation directory")
+ set(PSA_ARCH_TESTS_PACKAGE_PATH "${PSA_ARCH_TESTS_INSTALL_PATH}/libpsa-arch-tests/cmake" CACHE PATH "psa-arch-tests CMake package directory")
+-
+-include(FetchContent)
++set(PSA_ARCH_TESTS_SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/_deps/psa_arch_tests-src" CACHE PATH "psa-arch-tests source.")
+ 
+ # Checking git
+ find_program(GIT_COMMAND "git")
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0036-Separate-sign-verify-message-and-hash-operations.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0036-Separate-sign-verify-message-and-hash-operations.patch
new file mode 100644
index 0000000..ae78f41
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0036-Separate-sign-verify-message-and-hash-operations.patch
@@ -0,0 +1,1080 @@
+From b160f734006f4959d92377dc3aa8eabc3ac7c1da Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Fri, 11 Feb 2022 14:08:13 +0000
+Subject: [PATCH] Separate sign/verify message and hash operations
+
+Previous versions of mbedtls didn't distinguish between
+asymmetric sign and verify operations on a hash or message.
+They are now treated as separate operations from a usage
+control perspective. This change makes the corresponding
+hash/message sepration in client and service provider
+components.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: Ic0041c694c026522c9b00c974d22261e9e2feadd
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ .../caller/packed-c/crypto_caller_sign_hash.h | 29 +++++++-
+ .../packed-c/crypto_caller_verify_hash.h      | 33 ++++++++-
+ .../caller/stub/crypto_caller_sign_hash.h     | 11 ++-
+ .../caller/stub/crypto_caller_verify_hash.h   | 11 ++-
+ .../service/crypto/client/cpp/crypto_client.h | 17 ++++-
+ .../packed-c/packedc_crypto_client.cpp        | 22 +++++-
+ .../protocol/packed-c/packedc_crypto_client.h | 17 ++++-
+ .../protobuf/protobuf_crypto_client.cpp       | 43 ++++++++++-
+ .../protobuf/protobuf_crypto_client.h         | 27 ++++++-
+ .../crypto/client/psa/psa_sign_message.c      | 24 +++---
+ .../crypto/client/psa/psa_verify_message.c    | 24 +++---
+ .../service/crypto/provider/crypto_provider.c | 40 ++++++----
+ .../serializer/crypto_provider_serializer.h   |  6 +-
+ .../packedc_crypto_provider_serializer.c      | 12 +--
+ .../protobuf/pb_crypto_provider_serializer.c  | 74 +++++++++----------
+ .../check_crypto_opcode_alignment.cpp         | 25 ++++---
+ .../test/service/crypto_service_scenarios.cpp | 56 +++++++++++++-
+ .../test/service/crypto_service_scenarios.h   |  3 +-
+ .../packed-c/crypto_service_packedc_tests.cpp |  7 +-
+ .../crypto_service_protobuf_tests.cpp         |  7 +-
+ protocols/service/crypto/packed-c/opcodes.h   |  4 +-
+ .../service/crypto/protobuf/opcodes.proto     |  4 +-
+ 22 files changed, 366 insertions(+), 130 deletions(-)
+
+diff --git a/components/service/crypto/client/caller/packed-c/crypto_caller_sign_hash.h b/components/service/crypto/client/caller/packed-c/crypto_caller_sign_hash.h
+index e807773e..4a9ed20d 100644
+--- a/components/service/crypto/client/caller/packed-c/crypto_caller_sign_hash.h
++++ b/components/service/crypto/client/caller/packed-c/crypto_caller_sign_hash.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -20,7 +20,8 @@
+ extern "C" {
+ #endif
+ 
+-static inline psa_status_t crypto_caller_sign_hash(struct service_client *context,
++static inline psa_status_t crypto_caller_asym_sign_commom(struct service_client *context,
++	uint32_t opcode,
+ 	psa_key_id_t id,
+ 	psa_algorithm_t alg,
+ 	const uint8_t *hash, size_t hash_length,
+@@ -60,7 +61,7 @@ static inline psa_status_t crypto_caller_sign_hash(struct service_client *contex
+ 
+ 		context->rpc_status =
+ 			rpc_caller_invoke(context->caller, call_handle,
+-						TS_CRYPTO_OPCODE_SIGN_HASH, &opstatus, &resp_buf, &resp_len);
++						opcode, &opstatus, &resp_buf, &resp_len);
+ 
+ 		if (context->rpc_status == TS_RPC_CALL_ACCEPTED) {
+ 
+@@ -98,6 +99,28 @@ static inline psa_status_t crypto_caller_sign_hash(struct service_client *contex
+ 	return psa_status;
+ }
+ 
++static inline psa_status_t crypto_caller_sign_hash(struct service_client *context,
++	psa_key_id_t id,
++	psa_algorithm_t alg,
++	const uint8_t *hash, size_t hash_length,
++	uint8_t *signature, size_t signature_size, size_t *signature_length)
++{
++	return crypto_caller_asym_sign_commom(context, TS_CRYPTO_OPCODE_SIGN_HASH,
++		id, alg, hash, hash_length,
++		signature, signature_size, signature_length);
++}
++
++static inline psa_status_t crypto_caller_sign_message(struct service_client *context,
++	psa_key_id_t id,
++	psa_algorithm_t alg,
++	const uint8_t *hash, size_t hash_length,
++	uint8_t *signature, size_t signature_size, size_t *signature_length)
++{
++	return crypto_caller_asym_sign_commom(context, TS_CRYPTO_OPCODE_SIGN_MESSAGE,
++		id, alg, hash, hash_length,
++		signature, signature_size, signature_length);
++}
++
+ #ifdef __cplusplus
+ }
+ #endif
+diff --git a/components/service/crypto/client/caller/packed-c/crypto_caller_verify_hash.h b/components/service/crypto/client/caller/packed-c/crypto_caller_verify_hash.h
+index 47152946..daa11330 100644
+--- a/components/service/crypto/client/caller/packed-c/crypto_caller_verify_hash.h
++++ b/components/service/crypto/client/caller/packed-c/crypto_caller_verify_hash.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -20,7 +20,8 @@
+ extern "C" {
+ #endif
+ 
+-static inline psa_status_t crypto_caller_verify_hash(struct service_client *context,
++static inline psa_status_t crypto_caller_asym_verify_common(struct service_client *context,
++	uint32_t opcode,
+ 	psa_key_id_t id,
+ 	psa_algorithm_t alg,
+ 	const uint8_t *hash, size_t hash_length,
+@@ -65,7 +66,7 @@ static inline psa_status_t crypto_caller_verify_hash(struct service_client *cont
+ 
+ 		context->rpc_status =
+ 			rpc_caller_invoke(context->caller, call_handle,
+-					TS_CRYPTO_OPCODE_VERIFY_HASH, &opstatus, &resp_buf, &resp_len);
++					opcode, &opstatus, &resp_buf, &resp_len);
+ 
+ 		if (context->rpc_status == TS_RPC_CALL_ACCEPTED) psa_status = opstatus;
+ 
+@@ -75,6 +76,32 @@ static inline psa_status_t crypto_caller_verify_hash(struct service_client *cont
+ 	return psa_status;
+ }
+ 
++static inline psa_status_t crypto_caller_verify_hash(struct service_client *context,
++	psa_key_id_t id,
++	psa_algorithm_t alg,
++	const uint8_t *hash, size_t hash_length,
++	const uint8_t *signature, size_t signature_length)
++{
++	return crypto_caller_asym_verify_common(context,
++		TS_CRYPTO_OPCODE_VERIFY_HASH,
++		id, alg,
++		hash, hash_length,
++		signature, signature_length);
++}
++
++static inline psa_status_t crypto_caller_verify_message(struct service_client *context,
++	psa_key_id_t id,
++	psa_algorithm_t alg,
++	const uint8_t *input, size_t input_length,
++	const uint8_t *signature, size_t signature_length)
++{
++	return crypto_caller_asym_verify_common(context,
++		TS_CRYPTO_OPCODE_VERIFY_MESSAGE,
++		id, alg,
++		input, input_length,
++		signature, signature_length);
++}
++
+ #ifdef __cplusplus
+ }
+ #endif
+diff --git a/components/service/crypto/client/caller/stub/crypto_caller_sign_hash.h b/components/service/crypto/client/caller/stub/crypto_caller_sign_hash.h
+index d09369a2..09049f5c 100644
+--- a/components/service/crypto/client/caller/stub/crypto_caller_sign_hash.h
++++ b/components/service/crypto/client/caller/stub/crypto_caller_sign_hash.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -23,6 +23,15 @@ static inline psa_status_t crypto_caller_sign_hash(struct service_client *contex
+ 	return PSA_ERROR_NOT_SUPPORTED;
+ }
+ 
++static inline psa_status_t crypto_caller_sign_message(struct service_client *context,
++	psa_key_id_t id,
++	psa_algorithm_t alg,
++	const uint8_t *hash, size_t hash_length,
++	uint8_t *signature, size_t signature_size, size_t *signature_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
+ #ifdef __cplusplus
+ }
+ #endif
+diff --git a/components/service/crypto/client/caller/stub/crypto_caller_verify_hash.h b/components/service/crypto/client/caller/stub/crypto_caller_verify_hash.h
+index 20d11dcf..3f3eb878 100644
+--- a/components/service/crypto/client/caller/stub/crypto_caller_verify_hash.h
++++ b/components/service/crypto/client/caller/stub/crypto_caller_verify_hash.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -23,6 +23,15 @@ static inline psa_status_t crypto_caller_verify_hash(struct service_client *cont
+ 	return PSA_ERROR_NOT_SUPPORTED;
+ }
+ 
++static inline psa_status_t crypto_caller_verify_message(struct service_client *context,
++	psa_key_id_t id,
++	psa_algorithm_t alg,
++	const uint8_t *input, size_t input_length,
++	const uint8_t *signature, size_t signature_length)
++{
++	return PSA_ERROR_NOT_SUPPORTED;
++}
++
+ #ifdef __cplusplus
+ }
+ #endif
+diff --git a/components/service/crypto/client/cpp/crypto_client.h b/components/service/crypto/client/cpp/crypto_client.h
+index 2a5e5b99..ccb0714a 100644
+--- a/components/service/crypto/client/cpp/crypto_client.h
++++ b/components/service/crypto/client/cpp/crypto_client.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -57,7 +57,7 @@ public:
+ 		psa_key_id_t id,
+ 		uint8_t *data, size_t data_size, size_t *data_length) = 0;
+ 
+-	/* Sign/verify methods */
++	/* Sign/verify hash methods */
+ 	virtual psa_status_t sign_hash(
+ 		psa_key_id_t id,
+ 		psa_algorithm_t alg,
+@@ -70,6 +70,19 @@ public:
+ 		const uint8_t *hash, size_t hash_length,
+ 		const uint8_t *signature, size_t signature_length) = 0;
+ 
++	/* Sign/verify message methods */
++	virtual psa_status_t sign_message(
++		psa_key_id_t id,
++		psa_algorithm_t alg,
++		const uint8_t *message, size_t message_length,
++		uint8_t *signature, size_t signature_size, size_t *signature_length) = 0;
++
++	virtual psa_status_t verify_message(
++		psa_key_id_t id,
++		psa_algorithm_t alg,
++		const uint8_t *message, size_t message_length,
++		const uint8_t *signature, size_t signature_length) = 0;
++
+ 	/* Asymmetric encrypt/decrypt */
+ 	virtual psa_status_t asymmetric_encrypt(
+ 		psa_key_id_t id,
+diff --git a/components/service/crypto/client/cpp/protocol/packed-c/packedc_crypto_client.cpp b/components/service/crypto/client/cpp/protocol/packed-c/packedc_crypto_client.cpp
+index 4d9d8f41..4e10f9be 100644
+--- a/components/service/crypto/client/cpp/protocol/packed-c/packedc_crypto_client.cpp
++++ b/components/service/crypto/client/cpp/protocol/packed-c/packedc_crypto_client.cpp
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -107,6 +107,26 @@ psa_status_t packedc_crypto_client::verify_hash(
+ 		signature, signature_length);
+ }
+ 
++psa_status_t packedc_crypto_client::sign_message(
++	psa_key_id_t id, psa_algorithm_t alg,
++	const uint8_t *message, size_t message_length,
++	uint8_t *signature, size_t signature_size, size_t *signature_length)
++{
++	return crypto_caller_sign_message(&m_client, id, alg,
++		message, message_length,
++		signature, signature_size, signature_length);
++}
++
++psa_status_t packedc_crypto_client::verify_message(
++	psa_key_id_t id, psa_algorithm_t alg,
++	const uint8_t *message, size_t message_length,
++	const uint8_t *signature, size_t signature_length)
++{
++	return crypto_caller_verify_message(&m_client, id, alg,
++		message, message_length,
++		signature, signature_length);
++}
++
+ psa_status_t packedc_crypto_client::asymmetric_encrypt(
+ 	psa_key_id_t id, psa_algorithm_t alg,
+ 	const uint8_t *input, size_t input_length,
+diff --git a/components/service/crypto/client/cpp/protocol/packed-c/packedc_crypto_client.h b/components/service/crypto/client/cpp/protocol/packed-c/packedc_crypto_client.h
+index 377b51d1..d74ba609 100644
+--- a/components/service/crypto/client/cpp/protocol/packed-c/packedc_crypto_client.h
++++ b/components/service/crypto/client/cpp/protocol/packed-c/packedc_crypto_client.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -54,7 +54,7 @@ public:
+ 		psa_key_id_t id,
+ 		uint8_t *data, size_t data_size, size_t *data_length);
+ 
+-	/* Sign/verify methods */
++	/* Sign/verify hash methods */
+ 	psa_status_t sign_hash(
+ 		psa_key_id_t id,
+ 		psa_algorithm_t alg,
+@@ -67,6 +67,19 @@ public:
+ 		const uint8_t *hash, size_t hash_length,
+ 		const uint8_t *signature, size_t signature_length);
+ 
++	/* Sign/verify message methods */
++	psa_status_t sign_message(
++		psa_key_id_t id,
++		psa_algorithm_t alg,
++		const uint8_t *message, size_t message_length,
++		uint8_t *signature, size_t signature_size, size_t *signature_length);
++
++	psa_status_t verify_message(
++		psa_key_id_t id,
++		psa_algorithm_t alg,
++		const uint8_t *message, size_t message_length,
++		const uint8_t *signature, size_t signature_length);
++
+ 	/* Asymmetric encrypt/decrypt */
+ 	psa_status_t asymmetric_encrypt(
+ 		psa_key_id_t id,
+diff --git a/components/service/crypto/client/cpp/protocol/protobuf/protobuf_crypto_client.cpp b/components/service/crypto/client/cpp/protocol/protobuf/protobuf_crypto_client.cpp
+index 17780351..28c8f6fb 100644
+--- a/components/service/crypto/client/cpp/protocol/protobuf/protobuf_crypto_client.cpp
++++ b/components/service/crypto/client/cpp/protocol/protobuf/protobuf_crypto_client.cpp
+@@ -386,6 +386,25 @@ psa_status_t protobuf_crypto_client::export_public_key(psa_key_id_t id,
+ psa_status_t protobuf_crypto_client::sign_hash(psa_key_id_t id, psa_algorithm_t alg,
+ 							const uint8_t *hash, size_t hash_length,
+ 							uint8_t *signature, size_t signature_size, size_t *signature_length)
++{
++	return asym_sign(ts_crypto_Opcode_SIGN_HASH, id, alg,
++				hash, hash_length,
++				signature, signature_size, signature_length);
++}
++
++psa_status_t protobuf_crypto_client::sign_message(psa_key_id_t id, psa_algorithm_t alg,
++							const uint8_t *message, size_t message_length,
++							uint8_t *signature, size_t signature_size, size_t *signature_length)
++{
++	return asym_sign(ts_crypto_Opcode_SIGN_MESSAGE, id, alg,
++				message, message_length,
++				signature, signature_size, signature_length);
++}
++
++psa_status_t protobuf_crypto_client::asym_sign(uint32_t opcode,
++							psa_key_id_t id, psa_algorithm_t alg,
++							const uint8_t *hash, size_t hash_length,
++							uint8_t *signature, size_t signature_size, size_t *signature_length)
+ {
+ 	size_t req_len;
+ 	pb_bytes_array_t *hash_byte_array =
+@@ -416,7 +435,7 @@ psa_status_t protobuf_crypto_client::sign_hash(psa_key_id_t id, psa_algorithm_t
+ 			pb_encode(&ostream, ts_crypto_SignHashIn_fields, &req_msg);
+ 
+ 			m_client.rpc_status = rpc_caller_invoke(m_client.caller, call_handle,
+-						ts_crypto_Opcode_SIGN_HASH, &opstatus, &resp_buf, &resp_len);
++						opcode, &opstatus, &resp_buf, &resp_len);
+ 
+ 			if (m_client.rpc_status == TS_RPC_CALL_ACCEPTED) {
+ 
+@@ -462,10 +481,28 @@ psa_status_t protobuf_crypto_client::sign_hash(psa_key_id_t id, psa_algorithm_t
+ 	return psa_status;
+ }
+ 
+-
+ psa_status_t protobuf_crypto_client::verify_hash(psa_key_id_t id, psa_algorithm_t alg,
+ 						const uint8_t *hash, size_t hash_length,
+ 						const uint8_t *signature, size_t signature_length)
++{
++	return asym_verify(ts_crypto_Opcode_VERIFY_HASH, id, alg,
++				hash, hash_length,
++				signature, signature_length);
++}
++
++psa_status_t protobuf_crypto_client::verify_message(psa_key_id_t id, psa_algorithm_t alg,
++						const uint8_t *message, size_t message_length,
++						const uint8_t *signature, size_t signature_length)
++{
++	return asym_verify(ts_crypto_Opcode_VERIFY_MESSAGE, id, alg,
++				message, message_length,
++				signature, signature_length);
++}
++
++psa_status_t protobuf_crypto_client::asym_verify(uint32_t opcode,
++						psa_key_id_t id, psa_algorithm_t alg,
++						const uint8_t *hash, size_t hash_length,
++						const uint8_t *signature, size_t signature_length)
+ {
+ 	size_t req_len;
+ 	pb_bytes_array_t *hash_byte_array =
+@@ -497,7 +534,7 @@ psa_status_t protobuf_crypto_client::verify_hash(psa_key_id_t id, psa_algorithm_
+ 			pb_encode(&ostream, ts_crypto_VerifyHashIn_fields, &req_msg);
+ 
+ 			m_client.rpc_status = rpc_caller_invoke(m_client.caller, call_handle,
+-						ts_crypto_Opcode_VERIFY_HASH, &opstatus, &resp_buf, &resp_len);
++						opcode, &opstatus, &resp_buf, &resp_len);
+ 
+ 			if (m_client.rpc_status == TS_RPC_CALL_ACCEPTED) psa_status = opstatus;
+ 
+diff --git a/components/service/crypto/client/cpp/protocol/protobuf/protobuf_crypto_client.h b/components/service/crypto/client/cpp/protocol/protobuf/protobuf_crypto_client.h
+index 085d9cfa..abe4439e 100644
+--- a/components/service/crypto/client/cpp/protocol/protobuf/protobuf_crypto_client.h
++++ b/components/service/crypto/client/cpp/protocol/protobuf/protobuf_crypto_client.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -54,7 +54,7 @@ public:
+ 		psa_key_id_t id,
+ 		uint8_t *data, size_t data_size, size_t *data_length);
+ 
+-	/* Sign/verify methods */
++	/* Sign/verify hash methods */
+ 	psa_status_t sign_hash(
+ 		psa_key_id_t id,
+ 		psa_algorithm_t alg,
+@@ -67,6 +67,19 @@ public:
+ 		const uint8_t *hash, size_t hash_length,
+ 		const uint8_t *signature, size_t signature_length);
+ 
++	/* Sign/verify message methods */
++	psa_status_t sign_message(
++		psa_key_id_t id,
++		psa_algorithm_t alg,
++		const uint8_t *message, size_t message_length,
++		uint8_t *signature, size_t signature_size, size_t *signature_length);
++
++	psa_status_t verify_message(
++		psa_key_id_t id,
++		psa_algorithm_t alg,
++		const uint8_t *message, size_t message_length,
++		const uint8_t *signature, size_t signature_length);
++
+ 	/* Asymmetric encrypt/decrypt */
+ 	psa_status_t asymmetric_encrypt(
+ 		psa_key_id_t id,
+@@ -221,6 +234,16 @@ public:
+ 
+ private:
+ 
++	psa_status_t asym_sign(uint32_t opcode,
++		psa_key_id_t id, psa_algorithm_t alg,
++		const uint8_t *hash, size_t hash_length,
++		uint8_t *signature, size_t signature_size, size_t *signature_length);
++
++	psa_status_t asym_verify(uint32_t opcode,
++		psa_key_id_t id, psa_algorithm_t alg,
++		const uint8_t *hash, size_t hash_length,
++		const uint8_t *signature, size_t signature_length);
++
+ 	void translate_key_attributes(
+ 		ts_crypto_KeyAttributes &proto_attributes,
+ 		const psa_key_attributes_t &psa_attributes);
+diff --git a/components/service/crypto/client/psa/psa_sign_message.c b/components/service/crypto/client/psa/psa_sign_message.c
+index dc2f7e80..b6446253 100644
+--- a/components/service/crypto/client/psa/psa_sign_message.c
++++ b/components/service/crypto/client/psa/psa_sign_message.c
+@@ -1,13 +1,15 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+ 
+ #include <psa/crypto.h>
++#include "psa_crypto_client.h"
++#include "crypto_caller_selector.h"
+ 
+ psa_status_t psa_sign_message(
+-	psa_key_id_t key,
++	psa_key_id_t id,
+ 	psa_algorithm_t alg,
+ 	const uint8_t *input,
+ 	size_t input_length,
+@@ -15,19 +17,11 @@ psa_status_t psa_sign_message(
+ 	size_t signature_size,
+ 	size_t *signature_length)
+ {
+-	size_t hash_len;
+-	uint8_t hash[PSA_HASH_MAX_SIZE];
++	if (psa_crypto_client_instance.init_status != PSA_SUCCESS)
++		return psa_crypto_client_instance.init_status;
+ 
+-	psa_status_t psa_status = psa_hash_compute(PSA_ALG_SIGN_GET_HASH(alg),
++	return crypto_caller_sign_message(&psa_crypto_client_instance.base,
++		id, alg,
+ 		input, input_length,
+-		hash, sizeof(hash), &hash_len);
+-
+-	if (psa_status == PSA_SUCCESS) {
+-
+-		psa_status = psa_sign_hash(key, alg,
+-			hash, hash_len,
+-			signature, signature_size, signature_length);
+-	}
+-
+-	return psa_status;
++		signature, signature_size, signature_length);
+ }
+diff --git a/components/service/crypto/client/psa/psa_verify_message.c b/components/service/crypto/client/psa/psa_verify_message.c
+index d0fbc7c8..57c2c5e8 100644
+--- a/components/service/crypto/client/psa/psa_verify_message.c
++++ b/components/service/crypto/client/psa/psa_verify_message.c
+@@ -1,32 +1,26 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+ 
+ #include <psa/crypto.h>
++#include "psa_crypto_client.h"
++#include "crypto_caller_selector.h"
+ 
+ psa_status_t psa_verify_message(
+-	psa_key_id_t key,
++	psa_key_id_t id,
+ 	psa_algorithm_t alg,
+ 	const uint8_t *input,
+ 	size_t input_length,
+ 	const uint8_t * signature,
+ 	size_t signature_length)
+ {
+-	size_t hash_len;
+-	uint8_t hash[PSA_HASH_MAX_SIZE];
++	if (psa_crypto_client_instance.init_status != PSA_SUCCESS)
++		return psa_crypto_client_instance.init_status;
+ 
+-	psa_status_t psa_status = psa_hash_compute(PSA_ALG_SIGN_GET_HASH(alg),
++	return crypto_caller_verify_message(&psa_crypto_client_instance.base,
++		id, alg,
+ 		input, input_length,
+-		hash, sizeof(hash), &hash_len);
+-
+-	if (psa_status == PSA_SUCCESS) {
+-
+-		psa_status = psa_verify_hash(key, alg,
+-			hash, hash_len,
+-			signature, signature_length);
+-	}
+-
+-	return psa_status;
++		signature, signature_length);
+ }
+diff --git a/components/service/crypto/provider/crypto_provider.c b/components/service/crypto/provider/crypto_provider.c
+index d0fc7cac..67a5b340 100644
+--- a/components/service/crypto/provider/crypto_provider.c
++++ b/components/service/crypto/provider/crypto_provider.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -16,8 +16,8 @@ static rpc_status_t destroy_key_handler(void *context, struct call_req* req);
+ static rpc_status_t export_key_handler(void *context, struct call_req* req);
+ static rpc_status_t export_public_key_handler(void *context, struct call_req* req);
+ static rpc_status_t import_key_handler(void *context, struct call_req* req);
+-static rpc_status_t sign_hash_handler(void *context, struct call_req* req);
+-static rpc_status_t verify_hash_handler(void *context, struct call_req* req);
++static rpc_status_t asymmetric_sign_handler(void *context, struct call_req* req);
++static rpc_status_t asymmetric_verify_handler(void *context, struct call_req* req);
+ static rpc_status_t asymmetric_decrypt_handler(void *context, struct call_req* req);
+ static rpc_status_t asymmetric_encrypt_handler(void *context, struct call_req* req);
+ static rpc_status_t generate_random_handler(void *context, struct call_req* req);
+@@ -32,14 +32,16 @@ static const struct service_handler handler_table[] = {
+ 	{TS_CRYPTO_OPCODE_EXPORT_KEY,           export_key_handler},
+ 	{TS_CRYPTO_OPCODE_EXPORT_PUBLIC_KEY,    export_public_key_handler},
+ 	{TS_CRYPTO_OPCODE_IMPORT_KEY,           import_key_handler},
+-	{TS_CRYPTO_OPCODE_SIGN_HASH,            sign_hash_handler},
+-	{TS_CRYPTO_OPCODE_VERIFY_HASH,          verify_hash_handler},
++	{TS_CRYPTO_OPCODE_SIGN_HASH,            asymmetric_sign_handler},
++	{TS_CRYPTO_OPCODE_VERIFY_HASH,          asymmetric_verify_handler},
+ 	{TS_CRYPTO_OPCODE_ASYMMETRIC_DECRYPT,   asymmetric_decrypt_handler},
+ 	{TS_CRYPTO_OPCODE_ASYMMETRIC_ENCRYPT,   asymmetric_encrypt_handler},
+ 	{TS_CRYPTO_OPCODE_GENERATE_RANDOM,      generate_random_handler},
+ 	{TS_CRYPTO_OPCODE_COPY_KEY,          	copy_key_handler},
+ 	{TS_CRYPTO_OPCODE_PURGE_KEY,          	purge_key_handler},
+ 	{TS_CRYPTO_OPCODE_GET_KEY_ATTRIBUTES, 	get_key_attributes_handler},
++	{TS_CRYPTO_OPCODE_SIGN_MESSAGE,         asymmetric_sign_handler},
++	{TS_CRYPTO_OPCODE_VERIFY_MESSAGE,       asymmetric_verify_handler},
+ };
+ 
+ struct rpc_interface *crypto_provider_init(struct crypto_provider *context)
+@@ -272,7 +274,7 @@ static rpc_status_t import_key_handler(void *context, struct call_req* req)
+ 	return rpc_status;
+ }
+ 
+-static rpc_status_t sign_hash_handler(void *context, struct call_req* req)
++static rpc_status_t asymmetric_sign_handler(void *context, struct call_req* req)
+ {
+ 	rpc_status_t rpc_status = TS_RPC_ERROR_SERIALIZATION_NOT_SUPPORTED;
+ 	struct call_param_buf *req_buf = call_req_get_req_buf(req);
+@@ -284,7 +286,7 @@ static rpc_status_t sign_hash_handler(void *context, struct call_req* req)
+ 	uint8_t hash_buffer[PSA_HASH_MAX_SIZE];
+ 
+ 	if (serializer)
+-		rpc_status = serializer->deserialize_sign_hash_req(req_buf, &id, &alg, hash_buffer, &hash_len);
++		rpc_status = serializer->deserialize_asymmetric_sign_req(req_buf, &id, &alg, hash_buffer, &hash_len);
+ 
+ 	if (rpc_status == TS_RPC_CALL_ACCEPTED) {
+ 
+@@ -292,14 +294,16 @@ static rpc_status_t sign_hash_handler(void *context, struct call_req* req)
+ 		size_t sig_len;
+ 		uint8_t sig_buffer[PSA_SIGNATURE_MAX_SIZE];
+ 
+-		psa_status = psa_sign_hash(id, alg,
+-					hash_buffer, hash_len,
+-					sig_buffer, sizeof(sig_buffer), &sig_len);
++		psa_status = (call_req_get_opcode(req) == TS_CRYPTO_OPCODE_SIGN_HASH) ?
++			psa_sign_hash(id, alg, hash_buffer, hash_len,
++				sig_buffer, sizeof(sig_buffer), &sig_len) :
++			psa_sign_message(id, alg, hash_buffer, hash_len,
++				sig_buffer, sizeof(sig_buffer), &sig_len);
+ 
+ 		if (psa_status == PSA_SUCCESS) {
+ 
+ 			struct call_param_buf *resp_buf = call_req_get_resp_buf(req);
+-			rpc_status = serializer->serialize_sign_hash_resp(resp_buf, sig_buffer, sig_len);
++			rpc_status = serializer->serialize_asymmetric_sign_resp(resp_buf, sig_buffer, sig_len);
+ 		}
+ 
+ 		call_req_set_opstatus(req, psa_status);
+@@ -308,7 +312,7 @@ static rpc_status_t sign_hash_handler(void *context, struct call_req* req)
+ 	return rpc_status;
+ }
+ 
+-static rpc_status_t verify_hash_handler(void *context, struct call_req* req)
++static rpc_status_t asymmetric_verify_handler(void *context, struct call_req* req)
+ {
+ 	rpc_status_t rpc_status = TS_RPC_ERROR_SERIALIZATION_NOT_SUPPORTED;
+ 	struct call_param_buf *req_buf = call_req_get_req_buf(req);
+@@ -322,7 +326,7 @@ static rpc_status_t verify_hash_handler(void *context, struct call_req* req)
+ 	uint8_t sig_buffer[PSA_SIGNATURE_MAX_SIZE];
+ 
+ 	if (serializer)
+-		rpc_status = serializer->deserialize_verify_hash_req(req_buf, &id, &alg,
++		rpc_status = serializer->deserialize_asymmetric_verify_req(req_buf, &id, &alg,
+ 											hash_buffer, &hash_len,
+ 											sig_buffer, &sig_len);
+ 
+@@ -330,9 +334,13 @@ static rpc_status_t verify_hash_handler(void *context, struct call_req* req)
+ 
+ 		psa_status_t psa_status;
+ 
+-		psa_status = psa_verify_hash(id, alg,
+-					hash_buffer, hash_len,
+-					sig_buffer, sig_len);
++		psa_status = (call_req_get_opcode(req) == TS_CRYPTO_OPCODE_VERIFY_HASH) ?
++			psa_verify_hash(id, alg,
++				hash_buffer, hash_len,
++				sig_buffer, sig_len) :
++			psa_verify_message(id, alg,
++				hash_buffer, hash_len,
++				sig_buffer, sig_len);
+ 
+ 		call_req_set_opstatus(req, psa_status);
+ 	}
+diff --git a/components/service/crypto/provider/serializer/crypto_provider_serializer.h b/components/service/crypto/provider/serializer/crypto_provider_serializer.h
+index 68940cae..57364f24 100644
+--- a/components/service/crypto/provider/serializer/crypto_provider_serializer.h
++++ b/components/service/crypto/provider/serializer/crypto_provider_serializer.h
+@@ -79,15 +79,15 @@ struct crypto_provider_serializer {
+                                         const psa_key_attributes_t *attributes);
+ 
+     /* Operation: sign_hash */
+-    rpc_status_t (*deserialize_sign_hash_req)(const struct call_param_buf *req_buf,
++    rpc_status_t (*deserialize_asymmetric_sign_req)(const struct call_param_buf *req_buf,
+                                         psa_key_id_t *id, psa_algorithm_t *alg,
+                                         uint8_t *hash, size_t *hash_len);
+ 
+-    rpc_status_t (*serialize_sign_hash_resp)(struct call_param_buf *resp_buf,
++    rpc_status_t (*serialize_asymmetric_sign_resp)(struct call_param_buf *resp_buf,
+                                         const uint8_t *sig, size_t sig_len);
+ 
+     /* Operation: verify_hash */
+-    rpc_status_t (*deserialize_verify_hash_req)(const struct call_param_buf *req_buf,
++    rpc_status_t (*deserialize_asymmetric_verify_req)(const struct call_param_buf *req_buf,
+                                         psa_key_id_t *id, psa_algorithm_t *alg,
+                                         uint8_t *hash, size_t *hash_len,
+                                         uint8_t *sig, size_t *sig_len);
+diff --git a/components/service/crypto/provider/serializer/packed-c/packedc_crypto_provider_serializer.c b/components/service/crypto/provider/serializer/packed-c/packedc_crypto_provider_serializer.c
+index c70db865..4a7e59f0 100644
+--- a/components/service/crypto/provider/serializer/packed-c/packedc_crypto_provider_serializer.c
++++ b/components/service/crypto/provider/serializer/packed-c/packedc_crypto_provider_serializer.c
+@@ -333,7 +333,7 @@ static rpc_status_t serialize_get_key_attributes_resp(struct call_param_buf *res
+ }
+ 
+ /* Operation: sign_hash */
+-static rpc_status_t deserialize_sign_hash_req(const struct call_param_buf *req_buf,
++static rpc_status_t deserialize_asymmetric_sign_req(const struct call_param_buf *req_buf,
+                             psa_key_id_t *id, psa_algorithm_t *alg,
+                             uint8_t *hash, size_t *hash_len)
+ {
+@@ -378,7 +378,7 @@ static rpc_status_t deserialize_sign_hash_req(const struct call_param_buf *req_b
+     return rpc_status;
+ }
+ 
+-static rpc_status_t serialize_sign_hash_resp(struct call_param_buf *resp_buf,
++static rpc_status_t serialize_asymmetric_sign_resp(struct call_param_buf *resp_buf,
+                             const uint8_t *sig, size_t sig_len)
+ {
+     rpc_status_t rpc_status = TS_RPC_ERROR_INTERNAL;
+@@ -401,7 +401,7 @@ static rpc_status_t serialize_sign_hash_resp(struct call_param_buf *resp_buf,
+ }
+ 
+ /* Operation: verify_hash */
+-static rpc_status_t deserialize_verify_hash_req(const struct call_param_buf *req_buf,
++static rpc_status_t deserialize_asymmetric_verify_req(const struct call_param_buf *req_buf,
+                                 psa_key_id_t *id, psa_algorithm_t *alg,
+                                 uint8_t *hash, size_t *hash_len,
+                                 uint8_t *sig, size_t *sig_len)
+@@ -695,9 +695,9 @@ const struct crypto_provider_serializer *packedc_crypto_provider_serializer_inst
+         deserialize_purge_key_req,
+         deserialize_get_key_attributes_req,
+         serialize_get_key_attributes_resp,
+-        deserialize_sign_hash_req,
+-        serialize_sign_hash_resp,
+-        deserialize_verify_hash_req,
++        deserialize_asymmetric_sign_req,
++        serialize_asymmetric_sign_resp,
++        deserialize_asymmetric_verify_req,
+         deserialize_asymmetric_decrypt_req,
+         serialize_asymmetric_decrypt_resp,
+         deserialize_asymmetric_encrypt_req,
+diff --git a/components/service/crypto/provider/serializer/protobuf/pb_crypto_provider_serializer.c b/components/service/crypto/provider/serializer/protobuf/pb_crypto_provider_serializer.c
+index 7767d20a..083a581a 100644
+--- a/components/service/crypto/provider/serializer/protobuf/pb_crypto_provider_serializer.c
++++ b/components/service/crypto/provider/serializer/protobuf/pb_crypto_provider_serializer.c
+@@ -267,9 +267,9 @@ static rpc_status_t serialize_get_key_attributes_resp(struct call_param_buf *res
+ }
+ 
+ /* Operation: sign_hash */
+-static rpc_status_t deserialize_sign_hash_req(const struct call_param_buf *req_buf,
+-                            psa_key_id_t *id, psa_algorithm_t *alg,
+-                            uint8_t *hash, size_t *hash_len)
++static rpc_status_t deserialize_asymmetric_sign_req(const struct call_param_buf *req_buf,
++							psa_key_id_t *id, psa_algorithm_t *alg,
++							uint8_t *hash, size_t *hash_len)
+ {
+     rpc_status_t rpc_status = TS_RPC_ERROR_INVALID_REQ_BODY;
+     ts_crypto_SignHashIn recv_msg = ts_crypto_SignHashIn_init_default;
+@@ -295,8 +295,8 @@ static rpc_status_t deserialize_sign_hash_req(const struct call_param_buf *req_b
+     return rpc_status;
+ }
+ 
+-static rpc_status_t serialize_sign_hash_resp(struct call_param_buf *resp_buf,
+-                            const uint8_t *sig, size_t sig_len)
++static rpc_status_t serialize_asymmetric_sign_resp(struct call_param_buf *resp_buf,
++							const uint8_t *sig, size_t sig_len)
+ {
+     size_t packed_resp_size;
+     rpc_status_t rpc_status = TS_RPC_ERROR_INTERNAL;
+@@ -323,10 +323,10 @@ static rpc_status_t serialize_sign_hash_resp(struct call_param_buf *resp_buf,
+ }
+ 
+ /* Operation: verify_hash */
+-static rpc_status_t deserialize_verify_hash_req(const struct call_param_buf *req_buf,
+-                                psa_key_id_t *id, psa_algorithm_t *alg,
+-                                uint8_t *hash, size_t *hash_len,
+-                                uint8_t *sig, size_t *sig_len)
++static rpc_status_t deserialize_asymmetric_verify_req(const struct call_param_buf *req_buf,
++								psa_key_id_t *id, psa_algorithm_t *alg,
++								uint8_t *hash, size_t *hash_len,
++								uint8_t *sig, size_t *sig_len)
+ {
+     rpc_status_t rpc_status = TS_RPC_ERROR_INVALID_REQ_BODY;
+     ts_crypto_VerifyHashIn recv_msg = ts_crypto_VerifyHashIn_init_default;
+@@ -538,32 +538,32 @@ static rpc_status_t serialize_generate_random_resp(struct call_param_buf *resp_b
+ /* Singleton method to provide access to the serializer instance */
+ const struct crypto_provider_serializer *pb_crypto_provider_serializer_instance(void)
+ {
+-    static const struct crypto_provider_serializer instance = {
+-        max_deserialised_parameter_size,
+-        deserialize_generate_key_req,
+-        serialize_generate_key_resp,
+-        deserialize_destroy_key_req,
+-        deserialize_export_key_req,
+-        serialize_export_key_resp,
+-        deserialize_export_public_key_req,
+-        serialize_export_public_key_resp,
+-        deserialize_import_key_req,
+-        serialize_import_key_resp,
+-        deserialize_copy_key_req,
+-        serialize_copy_key_resp,
+-        deserialize_purge_key_req,
+-        deserialize_get_key_attributes_req,
+-        serialize_get_key_attributes_resp,
+-        deserialize_sign_hash_req,
+-        serialize_sign_hash_resp,
+-        deserialize_verify_hash_req,
+-        deserialize_asymmetric_decrypt_req,
+-        serialize_asymmetric_decrypt_resp,
+-        deserialize_asymmetric_encrypt_req,
+-        serialize_asymmetric_encrypt_resp,
+-        deserialize_generate_random_req,
+-        serialize_generate_random_resp
+-    };
+-
+-    return &instance;
++	static const struct crypto_provider_serializer instance = {
++		max_deserialised_parameter_size,
++		deserialize_generate_key_req,
++		serialize_generate_key_resp,
++		deserialize_destroy_key_req,
++		deserialize_export_key_req,
++		serialize_export_key_resp,
++		deserialize_export_public_key_req,
++		serialize_export_public_key_resp,
++		deserialize_import_key_req,
++		serialize_import_key_resp,
++		deserialize_copy_key_req,
++		serialize_copy_key_resp,
++		deserialize_purge_key_req,
++		deserialize_get_key_attributes_req,
++		serialize_get_key_attributes_resp,
++		deserialize_asymmetric_sign_req,
++		serialize_asymmetric_sign_resp,
++		deserialize_asymmetric_verify_req,
++		deserialize_asymmetric_decrypt_req,
++		serialize_asymmetric_decrypt_resp,
++		deserialize_asymmetric_encrypt_req,
++		serialize_asymmetric_encrypt_resp,
++		deserialize_generate_random_req,
++		serialize_generate_random_resp
++	};
++
++	return &instance;
+ }
+diff --git a/components/service/crypto/test/protocol/check_crypto_opcode_alignment.cpp b/components/service/crypto/test/protocol/check_crypto_opcode_alignment.cpp
+index bd6c66ee..da01abf4 100644
+--- a/components/service/crypto/test/protocol/check_crypto_opcode_alignment.cpp
++++ b/components/service/crypto/test/protocol/check_crypto_opcode_alignment.cpp
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -18,15 +18,16 @@ TEST_GROUP(CryptoProtocolOpcodeChecks)
+ 
+ TEST(CryptoProtocolOpcodeChecks, checkPackedcToProtobuf)
+ {
+-    CHECK_EQUAL(TS_CRYPTO_OPCODE_GENERATE_KEY, ts_crypto_Opcode_GENERATE_KEY);
+-    CHECK_EQUAL(TS_CRYPTO_OPCODE_DESTROY_KEY, ts_crypto_Opcode_DESTROY_KEY);
+-    CHECK_EQUAL(TS_CRYPTO_OPCODE_EXPORT_KEY, ts_crypto_Opcode_EXPORT_KEY);
+-    CHECK_EQUAL(TS_CRYPTO_OPCODE_EXPORT_PUBLIC_KEY, ts_crypto_Opcode_EXPORT_PUBLIC_KEY);
+-    CHECK_EQUAL(TS_CRYPTO_OPCODE_IMPORT_KEY, ts_crypto_Opcode_IMPORT_KEY);
+-    CHECK_EQUAL(TS_CRYPTO_OPCODE_SIGN_HASH, ts_crypto_Opcode_SIGN_HASH);
+-    CHECK_EQUAL(TS_CRYPTO_OPCODE_VERIFY_HASH, ts_crypto_Opcode_VERIFY_HASH);
+-    CHECK_EQUAL(TS_CRYPTO_OPCODE_ASYMMETRIC_DECRYPT, ts_crypto_Opcode_ASYMMETRIC_DECRYPT);
+-    CHECK_EQUAL(TS_CRYPTO_OPCODE_ASYMMETRIC_ENCRYPT, ts_crypto_Opcode_ASYMMETRIC_ENCRYPT);
+-    CHECK_EQUAL(TS_CRYPTO_OPCODE_GENERATE_RANDOM, ts_crypto_Opcode_GENERATE_RANDOM);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_GENERATE_KEY, ts_crypto_Opcode_GENERATE_KEY);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_DESTROY_KEY, ts_crypto_Opcode_DESTROY_KEY);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_EXPORT_KEY, ts_crypto_Opcode_EXPORT_KEY);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_EXPORT_PUBLIC_KEY, ts_crypto_Opcode_EXPORT_PUBLIC_KEY);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_IMPORT_KEY, ts_crypto_Opcode_IMPORT_KEY);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_SIGN_HASH, ts_crypto_Opcode_SIGN_HASH);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_VERIFY_HASH, ts_crypto_Opcode_VERIFY_HASH);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_ASYMMETRIC_DECRYPT, ts_crypto_Opcode_ASYMMETRIC_DECRYPT);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_ASYMMETRIC_ENCRYPT, ts_crypto_Opcode_ASYMMETRIC_ENCRYPT);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_GENERATE_RANDOM, ts_crypto_Opcode_GENERATE_RANDOM);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_SIGN_MESSAGE, ts_crypto_Opcode_SIGN_MESSAGE);
++	CHECK_EQUAL(TS_CRYPTO_OPCODE_VERIFY_MESSAGE, ts_crypto_Opcode_VERIFY_MESSAGE);
+ }
+-
+diff --git a/components/service/crypto/test/service/crypto_service_scenarios.cpp b/components/service/crypto/test/service/crypto_service_scenarios.cpp
+index ec2c6736..b3345551 100644
+--- a/components/service/crypto/test/service/crypto_service_scenarios.cpp
++++ b/components/service/crypto/test/service/crypto_service_scenarios.cpp
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -290,6 +290,56 @@ void crypto_service_scenarios::signAndVerifyHash()
+ 	CHECK_EQUAL(PSA_SUCCESS, status);
+ }
+ 
++void crypto_service_scenarios::signAndVerifyMessage()
++{
++	psa_status_t status;
++	psa_key_attributes_t attributes = PSA_KEY_ATTRIBUTES_INIT;
++	psa_key_id_t key_id;
++
++	psa_set_key_id(&attributes, 14);
++	psa_set_key_usage_flags(&attributes, PSA_KEY_USAGE_SIGN_MESSAGE | PSA_KEY_USAGE_VERIFY_MESSAGE);
++	psa_set_key_algorithm(&attributes, PSA_ALG_DETERMINISTIC_ECDSA(PSA_ALG_SHA_256));
++	psa_set_key_type(&attributes, PSA_KEY_TYPE_ECC_KEY_PAIR(PSA_ECC_FAMILY_SECP_R1));
++	psa_set_key_bits(&attributes, 256);
++
++	/* Generate a key */
++	status = m_crypto_client->generate_key(&attributes, &key_id);
++	CHECK_EQUAL(PSA_SUCCESS, status);
++
++	psa_reset_key_attributes(&attributes);
++
++	/* Sign a message */
++	uint8_t message[21];
++	uint8_t signature[PSA_SIGNATURE_MAX_SIZE];
++	size_t signature_length;
++
++	memset(message, 0x99, sizeof(message));
++
++	status = m_crypto_client->sign_message(key_id,
++		PSA_ALG_DETERMINISTIC_ECDSA(PSA_ALG_SHA_256), message, sizeof(message),
++		signature, sizeof(signature), &signature_length);
++
++	CHECK_EQUAL(PSA_SUCCESS, status);
++	CHECK(signature_length > 0);
++
++	/* Verify the signature */
++	status = m_crypto_client->verify_message(key_id,
++		PSA_ALG_DETERMINISTIC_ECDSA(PSA_ALG_SHA_256), message, sizeof(message),
++		signature, signature_length);
++	CHECK_EQUAL(PSA_SUCCESS, status);
++
++	/* Change the message and expect verify to fail */
++	message[0] = 0x72;
++	status = m_crypto_client->verify_message(key_id,
++		PSA_ALG_DETERMINISTIC_ECDSA(PSA_ALG_SHA_256), message, sizeof(message),
++		signature, signature_length);
++	CHECK_EQUAL(PSA_ERROR_INVALID_SIGNATURE, status);
++
++	/* Remove the key */
++	status = m_crypto_client->destroy_key(key_id);
++	CHECK_EQUAL(PSA_SUCCESS, status);
++}
++
+ void crypto_service_scenarios::signAndVerifyEat()
+ {
+ 	/* Sign and verify a hash using EAT key type and algorithm */
+@@ -348,7 +398,7 @@ void crypto_service_scenarios::asymEncryptDecrypt()
+ 	psa_key_attributes_t attributes = PSA_KEY_ATTRIBUTES_INIT;
+ 	psa_key_id_t key_id;
+ 
+-	psa_set_key_id(&attributes, 14);
++	psa_set_key_id(&attributes, 15);
+ 	psa_set_key_usage_flags(&attributes, PSA_KEY_USAGE_ENCRYPT | PSA_KEY_USAGE_DECRYPT);
+ 	psa_set_key_algorithm(&attributes, PSA_ALG_RSA_PKCS1V15_CRYPT);
+ 	psa_set_key_type(&attributes, PSA_KEY_TYPE_RSA_KEY_PAIR);
+@@ -394,7 +444,7 @@ void crypto_service_scenarios::asymEncryptDecryptWithSalt()
+ 	psa_key_attributes_t attributes = PSA_KEY_ATTRIBUTES_INIT;
+ 	psa_key_id_t key_id;
+ 
+-	psa_set_key_id(&attributes, 15);
++	psa_set_key_id(&attributes, 16);
+ 	psa_set_key_usage_flags(&attributes, PSA_KEY_USAGE_ENCRYPT | PSA_KEY_USAGE_DECRYPT);
+ 	psa_set_key_algorithm(&attributes,  PSA_ALG_RSA_OAEP(PSA_ALG_SHA_256));
+ 	psa_set_key_type(&attributes, PSA_KEY_TYPE_RSA_KEY_PAIR);
+diff --git a/components/service/crypto/test/service/crypto_service_scenarios.h b/components/service/crypto/test/service/crypto_service_scenarios.h
+index c65eba26..23671644 100644
+--- a/components/service/crypto/test/service/crypto_service_scenarios.h
++++ b/components/service/crypto/test/service/crypto_service_scenarios.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -24,6 +24,7 @@ public:
+ 	void asymEncryptDecrypt();
+ 	void asymEncryptDecryptWithSalt();
+ 	void signAndVerifyHash();
++	void signAndVerifyMessage();
+ 	void signAndVerifyEat();
+ 	void exportAndImportKeyPair();
+ 	void exportPublicKey();
+diff --git a/components/service/crypto/test/service/packed-c/crypto_service_packedc_tests.cpp b/components/service/crypto/test/service/packed-c/crypto_service_packedc_tests.cpp
+index 79eddfbb..ea238432 100644
+--- a/components/service/crypto/test/service/packed-c/crypto_service_packedc_tests.cpp
++++ b/components/service/crypto/test/service/packed-c/crypto_service_packedc_tests.cpp
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -87,6 +87,11 @@ TEST(CryptoServicePackedcTests, signAndVerifyHash)
+ 	m_scenarios->signAndVerifyHash();
+ }
+ 
++TEST(CryptoServicePackedcTests, signAndVerifyMessage)
++{
++	m_scenarios->signAndVerifyMessage();
++}
++
+ TEST(CryptoServicePackedcTests, signAndVerifyEat)
+ {
+ 	m_scenarios->signAndVerifyEat();
+diff --git a/components/service/crypto/test/service/protobuf/crypto_service_protobuf_tests.cpp b/components/service/crypto/test/service/protobuf/crypto_service_protobuf_tests.cpp
+index 1230752c..c172ad4a 100644
+--- a/components/service/crypto/test/service/protobuf/crypto_service_protobuf_tests.cpp
++++ b/components/service/crypto/test/service/protobuf/crypto_service_protobuf_tests.cpp
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -77,6 +77,11 @@ TEST(CryptoServiceProtobufTests, signAndVerifyHash)
+     m_scenarios->signAndVerifyHash();
+ }
+ 
++TEST(CryptoServiceProtobufTests, signAndVerifyMessage)
++{
++    m_scenarios->signAndVerifyMessage();
++}
++
+ TEST(CryptoServiceProtobufTests, asymEncryptDecrypt)
+ {
+     m_scenarios->asymEncryptDecrypt();
+diff --git a/protocols/service/crypto/packed-c/opcodes.h b/protocols/service/crypto/packed-c/opcodes.h
+index a07bd57e..5aebf2fa 100644
+--- a/protocols/service/crypto/packed-c/opcodes.h
++++ b/protocols/service/crypto/packed-c/opcodes.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -25,6 +25,8 @@
+ #define TS_CRYPTO_OPCODE_COPY_KEY               (TS_CRYPTO_OPCODE_BASE + 13)
+ #define TS_CRYPTO_OPCODE_PURGE_KEY              (TS_CRYPTO_OPCODE_BASE + 14)
+ #define TS_CRYPTO_OPCODE_GET_KEY_ATTRIBUTES     (TS_CRYPTO_OPCODE_BASE + 15)
++#define TS_CRYPTO_OPCODE_SIGN_MESSAGE           (TS_CRYPTO_OPCODE_BASE + 16)
++#define TS_CRYPTO_OPCODE_VERIFY_MESSAGE         (TS_CRYPTO_OPCODE_BASE + 17)
+ 
+ /* Hash operations */
+ #define TS_CRYPTO_OPCODE_HASH_BASE              (0x0200)
+diff --git a/protocols/service/crypto/protobuf/opcodes.proto b/protocols/service/crypto/protobuf/opcodes.proto
+index 094d3a02..ef64d044 100644
+--- a/protocols/service/crypto/protobuf/opcodes.proto
++++ b/protocols/service/crypto/protobuf/opcodes.proto
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+ syntax = "proto3";
+@@ -18,4 +18,6 @@ enum Opcode {
+   ASYMMETRIC_DECRYPT  = 0x010a;
+   ASYMMETRIC_ENCRYPT  = 0x010b;
+   GENERATE_RANDOM     = 0x010c;
++  SIGN_MESSAGE        = 0x0110;
++  VERIFY_MESSAGE      = 0x0111;
+ }
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0037-Add-defence-against-uninitialised-multi-part-transac.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0037-Add-defence-against-uninitialised-multi-part-transac.patch
new file mode 100644
index 0000000..a56e0f8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0037-Add-defence-against-uninitialised-multi-part-transac.patch
@@ -0,0 +1,123 @@
+From 9a83c32964ee2b1ecb7b36b4c08466202efd3bf2 Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Fri, 11 Feb 2022 14:19:26 +0000
+Subject: [PATCH] Add defence against uninitialised multi-part transaction
+
+Adds checks for the condition where there is an attempt to
+setup a multi-part transaction without first initialising
+transaction state.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: I754479260fed0490d8f32b41a077d26028dc9903
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ components/service/crypto/client/psa/psa_cipher.c | 14 +++++++++++++-
+ components/service/crypto/client/psa/psa_hash.c   |  8 +++++++-
+ components/service/crypto/client/psa/psa_mac.c    | 10 ++++++++--
+ 3 files changed, 28 insertions(+), 4 deletions(-)
+
+diff --git a/components/service/crypto/client/psa/psa_cipher.c b/components/service/crypto/client/psa/psa_cipher.c
+index 70836ea6..3ab8ea21 100644
+--- a/components/service/crypto/client/psa/psa_cipher.c
++++ b/components/service/crypto/client/psa/psa_cipher.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -13,6 +13,12 @@ psa_status_t psa_cipher_encrypt_setup(psa_cipher_operation_t *operation,
+ 	psa_key_id_t key,
+ 	psa_algorithm_t alg)
+ {
++	if (psa_crypto_client_instance.init_status != PSA_SUCCESS)
++		return psa_crypto_client_instance.init_status;
++
++	if (operation->handle)
++		return PSA_ERROR_BAD_STATE;
++
+ 	return crypto_caller_cipher_encrypt_setup(&psa_crypto_client_instance.base,
+ 		&operation->handle,
+ 		key, alg);
+@@ -22,6 +28,12 @@ psa_status_t psa_cipher_decrypt_setup(psa_cipher_operation_t *operation,
+ 	psa_key_id_t key,
+ 	psa_algorithm_t alg)
+ {
++	if (psa_crypto_client_instance.init_status != PSA_SUCCESS)
++		return psa_crypto_client_instance.init_status;
++
++	if (operation->handle)
++		return PSA_ERROR_BAD_STATE;
++
+ 	return crypto_caller_cipher_decrypt_setup(&psa_crypto_client_instance.base,
+ 		&operation->handle,
+ 		key, alg);
+diff --git a/components/service/crypto/client/psa/psa_hash.c b/components/service/crypto/client/psa/psa_hash.c
+index 7005c390..83278de6 100644
+--- a/components/service/crypto/client/psa/psa_hash.c
++++ b/components/service/crypto/client/psa/psa_hash.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -14,6 +14,9 @@ psa_status_t psa_hash_setup(psa_hash_operation_t *operation,
+ 	if (psa_crypto_client_instance.init_status != PSA_SUCCESS)
+ 		return psa_crypto_client_instance.init_status;
+ 
++	if (operation->handle)
++		return PSA_ERROR_BAD_STATE;
++
+ 	return crypto_caller_hash_setup(&psa_crypto_client_instance.base,
+ 		&operation->handle, alg);
+ }
+@@ -55,6 +58,9 @@ psa_status_t psa_hash_verify(psa_hash_operation_t *operation,
+ psa_status_t psa_hash_clone(const psa_hash_operation_t *source_operation,
+ 	psa_hash_operation_t *target_operation)
+ {
++	if (target_operation->handle)
++		return PSA_ERROR_BAD_STATE;
++
+ 	return crypto_caller_hash_clone(&psa_crypto_client_instance.base,
+ 		source_operation->handle,
+ 		&target_operation->handle);
+diff --git a/components/service/crypto/client/psa/psa_mac.c b/components/service/crypto/client/psa/psa_mac.c
+index 5efa1c4d..5c5eb32a 100644
+--- a/components/service/crypto/client/psa/psa_mac.c
++++ b/components/service/crypto/client/psa/psa_mac.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -16,6 +16,9 @@ psa_status_t psa_mac_sign_setup(psa_mac_operation_t *operation,
+ 	if (psa_crypto_client_instance.init_status != PSA_SUCCESS)
+ 		return psa_crypto_client_instance.init_status;
+ 
++	if (operation->handle)
++		return PSA_ERROR_BAD_STATE;
++
+ 	return crypto_caller_mac_sign_setup(&psa_crypto_client_instance.base,
+ 		&operation->handle,
+ 		key, alg);
+@@ -28,7 +31,10 @@ psa_status_t psa_mac_verify_setup(psa_mac_operation_t *operation,
+ 	if (psa_crypto_client_instance.init_status != PSA_SUCCESS)
+ 		return psa_crypto_client_instance.init_status;
+ 
+-	return crypto_caller_mac_sign_setup(&psa_crypto_client_instance.base,
++	if (operation->handle)
++		return PSA_ERROR_BAD_STATE;
++
++	return crypto_caller_mac_verify_setup(&psa_crypto_client_instance.base,
+ 		&operation->handle,
+ 		key, alg);
+ }
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0038-Integrate-AEAD-operation-support.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0038-Integrate-AEAD-operation-support.patch
new file mode 100644
index 0000000..2ad1efb
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0038-Integrate-AEAD-operation-support.patch
@@ -0,0 +1,521 @@
+From 00b4f777b377c69f948f5a9d68cbfc8fa8c38a86 Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Fri, 11 Feb 2022 14:24:53 +0000
+Subject: [PATCH] Integrate AEAD operation support
+
+Resolves issues and integrates AEAD support into the crypto service
+provider and clients.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: I5fbe78a2dd825f592e26fd665f60c18b576f9de9
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ .../caller/packed-c/crypto_caller_aead.h      |  70 +++---
+ .../client/caller/stub/crypto_caller_aead.h   |  12 +-
+ .../service/crypto/client/psa/psa_aead.c      | 221 +++++++++++++++---
+ .../factory/full/crypto_provider_factory.c    |  16 +-
+ .../component-test/component-test.cmake       |   4 +-
+ deployments/crypto/opteesp/CMakeLists.txt     |   4 +-
+ deployments/libts/linux-pc/CMakeLists.txt     |   4 +-
+ deployments/se-proxy/opteesp/CMakeLists.txt   |   4 +-
+ 8 files changed, 263 insertions(+), 72 deletions(-)
+
+diff --git a/components/service/crypto/client/caller/packed-c/crypto_caller_aead.h b/components/service/crypto/client/caller/packed-c/crypto_caller_aead.h
+index 3d9947d5..c4ffb20c 100644
+--- a/components/service/crypto/client/caller/packed-c/crypto_caller_aead.h
++++ b/components/service/crypto/client/caller/packed-c/crypto_caller_aead.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -20,38 +20,6 @@
+ extern "C" {
+ #endif
+ 
+-static inline psa_status_t crypto_caller_aead_encrypt(struct service_client *context,
+-	psa_key_id_t key,
+-	psa_algorithm_t alg,
+-	const uint8_t *nonce,
+-	size_t nonce_length,
+-	const uint8_t *additional_data,
+-	size_t additional_data_length,
+-	const uint8_t *plaintext,
+-	size_t plaintext_length,
+-	uint8_t *aeadtext,
+-	size_t aeadtext_size,
+-	size_t *aeadtext_length)
+-{
+-	return PSA_ERROR_NOT_SUPPORTED;
+-}
+-
+-static inline psa_status_t crypto_caller_aead_decrypt(struct service_client *context,
+-	psa_key_id_t key,
+-	psa_algorithm_t alg,
+-	const uint8_t *nonce,
+-	size_t nonce_length,
+-	const uint8_t *additional_data,
+-	size_t additional_data_length,
+-	const uint8_t *aeadtext,
+-	size_t aeadtext_length,
+-	uint8_t *plaintext,
+-	size_t plaintext_size,
+-	size_t *plaintext_length)
+-{
+-	return PSA_ERROR_NOT_SUPPORTED;
+-}
+-
+ static inline psa_status_t common_aead_setup(struct service_client *context,
+ 	uint32_t *op_handle,
+ 	psa_key_id_t key,
+@@ -247,7 +215,7 @@ static inline psa_status_t crypto_caller_aead_set_lengths(struct service_client
+ {
+ 	psa_status_t psa_status = PSA_ERROR_GENERIC_ERROR;
+ 	struct ts_crypto_aead_set_lengths_in req_msg;
+-	size_t req_fixed_len = sizeof(struct ts_crypto_aead_abort_in);
++	size_t req_fixed_len = sizeof(struct ts_crypto_aead_set_lengths_in);
+ 	size_t req_len = req_fixed_len;
+ 
+ 	req_msg.op_handle = op_handle;
+@@ -611,6 +579,40 @@ static inline psa_status_t crypto_caller_aead_abort(struct service_client *conte
+ 	return psa_status;
+ }
+ 
++/**
++ * The maximum data length that may be carried in an update operation will be
++ * constrained by the maximum call payload capacity imposed by the end-to-end
++ * RPC call path. These functions return the maximum update size when serialization
++ * overheads are considered. This allows large paylaods to be processed in
++ * maximum size chunks.
++ */
++static inline size_t crypto_caller_aead_max_update_ad_size(const struct service_client *context)
++{
++	/* Returns the maximum number of bytes of additional data that may be
++	 * carried as a parameter of the aead_update_ad operation
++	 * using the packed-c encoding.
++	 */
++	size_t payload_space = context->service_info.max_payload;
++	size_t overhead = sizeof(struct ts_crypto_aead_update_ad_in) + TLV_HDR_LEN;
++
++	return (payload_space > overhead) ? payload_space - overhead : 0;
++}
++
++static inline size_t crypto_caller_aead_max_update_size(const struct service_client *context)
++{
++	/* Returns the maximum number of bytes that may be
++	 * carried as a parameter of the aead_update operation
++	 * using the packed-c encoding.
++	 */
++	size_t payload_space = context->service_info.max_payload;
++	size_t overhead = sizeof(struct ts_crypto_aead_update_in) + TLV_HDR_LEN;
++
++	/* Allow for output to be a whole number of blocks */
++	overhead += PSA_BLOCK_CIPHER_BLOCK_MAX_SIZE;
++
++	return (payload_space > overhead) ? payload_space - overhead : 0;
++}
++
+ #ifdef __cplusplus
+ }
+ #endif
+diff --git a/components/service/crypto/client/caller/stub/crypto_caller_aead.h b/components/service/crypto/client/caller/stub/crypto_caller_aead.h
+index 18aa8cec..455e7ac1 100644
+--- a/components/service/crypto/client/caller/stub/crypto_caller_aead.h
++++ b/components/service/crypto/client/caller/stub/crypto_caller_aead.h
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -135,6 +135,16 @@ static inline psa_status_t crypto_caller_aead_abort(struct service_client *conte
+ 	return PSA_ERROR_NOT_SUPPORTED;
+ }
+ 
++static inline size_t crypto_caller_aead_max_update_ad_size(const struct service_client *context)
++{
++	return 0;
++}
++
++static inline size_t crypto_caller_aead_max_update_size(const struct service_client *context)
++{
++	return 0;
++}
++
+ #ifdef __cplusplus
+ }
+ #endif
+diff --git a/components/service/crypto/client/psa/psa_aead.c b/components/service/crypto/client/psa/psa_aead.c
+index 22fd3da1..e4579e63 100644
+--- a/components/service/crypto/client/psa/psa_aead.c
++++ b/components/service/crypto/client/psa/psa_aead.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -8,37 +8,6 @@
+ #include "psa_crypto_client.h"
+ #include "crypto_caller_selector.h"
+ 
+-
+-psa_status_t psa_aead_encrypt(psa_key_id_t key,
+-	psa_algorithm_t alg,
+-	const uint8_t *nonce,
+-	size_t nonce_length,
+-	const uint8_t *additional_data,
+-	size_t additional_data_length,
+-	const uint8_t *plaintext,
+-	size_t plaintext_length,
+-	uint8_t *aeadtext,
+-	size_t aeadtext_size,
+-	size_t *aeadtext_length)
+-{
+-	return PSA_ERROR_NOT_SUPPORTED;
+-}
+-
+-psa_status_t psa_aead_decrypt(psa_key_id_t key,
+-	psa_algorithm_t alg,
+-	const uint8_t *nonce,
+-	size_t nonce_length,
+-	const uint8_t *additional_data,
+-	size_t additional_data_length,
+-	const uint8_t *aeadtext,
+-	size_t aeadtext_length,
+-	uint8_t *plaintext,
+-	size_t plaintext_size,
+-	size_t *plaintext_length)
+-{
+-	return PSA_ERROR_NOT_SUPPORTED;
+-}
+-
+ psa_status_t psa_aead_encrypt_setup(psa_aead_operation_t *operation,
+ 	psa_key_id_t key,
+ 	psa_algorithm_t alg)
+@@ -143,3 +112,191 @@ psa_status_t psa_aead_abort(psa_aead_operation_t *operation)
+ 	return crypto_caller_aead_abort(&psa_crypto_client_instance.base,
+ 		operation->handle);
+ }
++
++static psa_status_t multi_aead_update_ad(psa_aead_operation_t *operation,
++	const uint8_t *input,
++	size_t input_length)
++{
++	psa_status_t psa_status = PSA_SUCCESS;
++	size_t max_update_size =
++		crypto_caller_aead_max_update_ad_size(&psa_crypto_client_instance.base);
++	size_t bytes_input = 0;
++
++	if (!max_update_size) {
++
++		/* Don't know the max update size so assume that the entire
++		 * input and output can be handled in a single update.  If
++		 * this isn't true, the first aead update operation will fail
++		 * safely.
++		 */
++		max_update_size = input_length;
++	}
++
++	while (bytes_input < input_length) {
++
++		size_t bytes_remaining = input_length - bytes_input;
++		size_t update_len = (bytes_remaining < max_update_size) ?
++			bytes_remaining :
++			max_update_size;
++
++		psa_status = psa_aead_update_ad(operation,
++			&input[bytes_input], update_len);
++
++		if (psa_status != PSA_SUCCESS) break;
++
++		bytes_input += update_len;
++	}
++
++	return psa_status;
++}
++
++static psa_status_t multi_aead_update(psa_aead_operation_t *operation,
++	const uint8_t *input,
++	size_t input_length,
++	uint8_t *output,
++	size_t output_size,
++	size_t *output_length)
++{
++	psa_status_t psa_status = PSA_SUCCESS;
++	size_t max_update_size =
++		crypto_caller_aead_max_update_size(&psa_crypto_client_instance.base);
++	size_t bytes_input = 0;
++	size_t bytes_output = 0;
++
++	*output_length = 0;
++
++	if (!max_update_size) {
++
++		/* Don't know the max update size so assume that the entire
++		 * input and output can be handled in a single update.  If
++		 * this isn't true, the first aead update operation will fail
++		 * safely.
++		 */
++		max_update_size = input_length;
++	}
++
++	while ((bytes_input < input_length) && (bytes_output < output_size)) {
++
++		size_t update_output_len = 0;
++		size_t bytes_remaining = input_length - bytes_input;
++		size_t update_len = (bytes_remaining < max_update_size) ?
++			bytes_remaining :
++			max_update_size;
++
++		psa_status = psa_aead_update(operation,
++			&input[bytes_input], update_len,
++			&output[bytes_output], output_size - bytes_output, &update_output_len);
++
++		if (psa_status != PSA_SUCCESS) break;
++
++		bytes_input += update_len;
++		bytes_output += update_output_len;
++	}
++
++	if (psa_status == PSA_SUCCESS) {
++
++		*output_length = bytes_output;
++	}
++
++	return psa_status;
++}
++
++psa_status_t psa_aead_encrypt(psa_key_id_t key,
++	psa_algorithm_t alg,
++	const uint8_t *nonce,
++	size_t nonce_length,
++	const uint8_t *additional_data,
++	size_t additional_data_length,
++	const uint8_t *plaintext,
++	size_t plaintext_length,
++	uint8_t *aeadtext,
++	size_t aeadtext_size,
++	size_t *aeadtext_length)
++{
++	psa_aead_operation_t operation = psa_aead_operation_init();
++	size_t bytes_output = 0;
++	*aeadtext_length = 0;
++
++	psa_status_t psa_status = psa_aead_encrypt_setup(&operation, key, alg);
++	if (psa_status != PSA_SUCCESS) return psa_status;
++
++	if ((psa_status = psa_aead_set_lengths(&operation, additional_data_length, plaintext_length),
++			psa_status == PSA_SUCCESS) &&
++		(psa_status = psa_aead_set_nonce(&operation, nonce, nonce_length),
++			psa_status == PSA_SUCCESS) &&
++		(psa_status = multi_aead_update_ad(&operation, additional_data, additional_data_length),
++			psa_status == PSA_SUCCESS) &&
++		(psa_status = multi_aead_update(&operation, plaintext, plaintext_length,
++			aeadtext, aeadtext_size, &bytes_output),
++			psa_status == PSA_SUCCESS))
++	{
++		size_t remaining_aead_len = 0;
++		size_t tag_len = 0;
++
++		psa_status = psa_aead_finish(&operation,
++			NULL, 0, &remaining_aead_len,
++			&aeadtext[bytes_output], aeadtext_size - bytes_output, &tag_len);
++
++		if (psa_status == PSA_SUCCESS) {
++
++			*aeadtext_length = bytes_output + remaining_aead_len + tag_len;
++		}
++	}
++	else {
++
++		psa_aead_abort(&operation);
++	}
++
++	return psa_status;
++}
++
++psa_status_t psa_aead_decrypt(psa_key_id_t key,
++	psa_algorithm_t alg,
++	const uint8_t *nonce,
++	size_t nonce_length,
++	const uint8_t *additional_data,
++	size_t additional_data_length,
++	const uint8_t *aeadtext,
++	size_t aeadtext_length,
++	uint8_t *plaintext,
++	size_t plaintext_size,
++	size_t *plaintext_length)
++{
++	psa_aead_operation_t operation = psa_aead_operation_init();
++	size_t bytes_output = 0;
++	*plaintext_length = 0;
++
++	psa_status_t psa_status = psa_aead_decrypt_setup(&operation, key, alg);
++	if (psa_status != PSA_SUCCESS) return psa_status;
++
++	size_t tag_len = PSA_ALG_AEAD_GET_TAG_LENGTH(alg);
++	size_t ciphertext_len = (aeadtext_length > tag_len) ? aeadtext_length - tag_len : 0;
++
++	if ((psa_status = psa_aead_set_lengths(&operation, additional_data_length, ciphertext_len),
++			psa_status == PSA_SUCCESS) &&
++		(psa_status = psa_aead_set_nonce(&operation, nonce, nonce_length),
++			psa_status == PSA_SUCCESS) &&
++		(psa_status = multi_aead_update_ad(&operation, additional_data, additional_data_length),
++			psa_status == PSA_SUCCESS) &&
++		(psa_status = multi_aead_update(&operation, aeadtext, ciphertext_len,
++			plaintext, plaintext_size, &bytes_output),
++			psa_status == PSA_SUCCESS))
++	{
++		size_t remaining_plaintext_len = 0;
++
++		psa_status = psa_aead_verify(&operation,
++			NULL, 0, &remaining_plaintext_len,
++			&aeadtext[bytes_output], aeadtext_length - bytes_output);
++
++		if (psa_status == PSA_SUCCESS) {
++
++			*plaintext_length = bytes_output + remaining_plaintext_len;
++		}
++	}
++	else {
++
++		psa_aead_abort(&operation);
++	}
++
++	return psa_status;
++}
+diff --git a/components/service/crypto/factory/full/crypto_provider_factory.c b/components/service/crypto/factory/full/crypto_provider_factory.c
+index 2d926eb6..ee2b4473 100644
+--- a/components/service/crypto/factory/full/crypto_provider_factory.c
++++ b/components/service/crypto/factory/full/crypto_provider_factory.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  *
+@@ -17,6 +17,8 @@
+ #include <service/crypto/provider/extension/key_derivation/serializer/packed-c/packedc_key_derivation_provider_serializer.h>
+ #include <service/crypto/provider/extension/mac/mac_provider.h>
+ #include <service/crypto/provider/extension/mac/serializer/packed-c/packedc_mac_provider_serializer.h>
++#include <service/crypto/provider/extension/aead/aead_provider.h>
++#include <service/crypto/provider/extension/aead/serializer/packed-c/packedc_aead_provider_serializer.h>
+ #include <service/discovery/provider/discovery_provider.h>
+ #include <service/discovery/provider/serializer/packed-c/packedc_discovery_provider_serializer.h>
+ 
+@@ -34,6 +36,7 @@ static struct full_crypto_provider
+ 	struct cipher_provider cipher_provider;
+ 	struct key_derivation_provider key_derivation_provider;
+ 	struct mac_provider mac_provider;
++	struct aead_provider aead_provider;
+ 
+ } instance;
+ 
+@@ -98,6 +101,17 @@ struct crypto_provider *crypto_provider_factory_create(void)
+ 	crypto_provider_extend(&instance.crypto_provider,
+ 		&instance.mac_provider.base_provider);
+ 
++	/**
++	 * Extend with aead operations
++	 */
++	aead_provider_init(&instance.aead_provider);
++
++	aead_provider_register_serializer(&instance.aead_provider,
++		TS_RPC_ENCODING_PACKED_C, packedc_aead_provider_serializer_instance());
++
++	crypto_provider_extend(&instance.crypto_provider,
++		&instance.aead_provider.base_provider);
++
+ 	return &instance.crypto_provider;
+ }
+ 
+diff --git a/deployments/component-test/component-test.cmake b/deployments/component-test/component-test.cmake
+index a0233c34..c3b015ab 100644
+--- a/deployments/component-test/component-test.cmake
++++ b/deployments/component-test/component-test.cmake
+@@ -1,5 +1,5 @@
+ #-------------------------------------------------------------------------------
+-# Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++# Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+ #
+ # SPDX-License-Identifier: BSD-3-Clause
+ #
+@@ -85,6 +85,8 @@ add_components(
+ 		"components/service/crypto/provider/extension/key_derivation/serializer/packed-c"
+ 		"components/service/crypto/provider/extension/mac"
+ 		"components/service/crypto/provider/extension/mac/serializer/packed-c"
++		"components/service/crypto/provider/extension/aead"
++		"components/service/crypto/provider/extension/aead/serializer/packed-c"
+ 		"components/service/crypto/provider/test"
+ 		"components/service/crypto/backend/mbedcrypto"
+ 		"components/service/crypto/factory/full"
+diff --git a/deployments/crypto/opteesp/CMakeLists.txt b/deployments/crypto/opteesp/CMakeLists.txt
+index 8ada74e9..eb5d0847 100644
+--- a/deployments/crypto/opteesp/CMakeLists.txt
++++ b/deployments/crypto/opteesp/CMakeLists.txt
+@@ -1,5 +1,5 @@
+ #-------------------------------------------------------------------------------
+-# Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++# Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+ #
+ # SPDX-License-Identifier: BSD-3-Clause
+ #
+@@ -62,6 +62,8 @@ add_components(TARGET "crypto-sp"
+ 		"components/service/crypto/provider/extension/key_derivation/serializer/packed-c"
+ 		"components/service/crypto/provider/extension/mac"
+ 		"components/service/crypto/provider/extension/mac/serializer/packed-c"
++		"components/service/crypto/provider/extension/aead"
++		"components/service/crypto/provider/extension/aead/serializer/packed-c"
+ 		"components/service/crypto/factory/full"
+ 		"components/service/crypto/backend/mbedcrypto"
+ 		"components/service/crypto/backend/mbedcrypto/trng_adapter/platform"
+diff --git a/deployments/libts/linux-pc/CMakeLists.txt b/deployments/libts/linux-pc/CMakeLists.txt
+index fc98407c..97eaaa73 100644
+--- a/deployments/libts/linux-pc/CMakeLists.txt
++++ b/deployments/libts/linux-pc/CMakeLists.txt
+@@ -1,5 +1,5 @@
+ #-------------------------------------------------------------------------------
+-# Copyright (c) 2020-2021, Arm Limited and Contributors. All rights reserved.
++# Copyright (c) 2020-2022, Arm Limited and Contributors. All rights reserved.
+ #
+ # SPDX-License-Identifier: BSD-3-Clause
+ #
+@@ -71,6 +71,8 @@ add_components(
+ 		"components/service/crypto/provider/extension/key_derivation/serializer/packed-c"
+ 		"components/service/crypto/provider/extension/mac"
+ 		"components/service/crypto/provider/extension/mac/serializer/packed-c"
++		"components/service/crypto/provider/extension/aead"
++		"components/service/crypto/provider/extension/aead/serializer/packed-c"
+ 		"components/service/crypto/factory/full"
+ 		"components/service/crypto/backend/mbedcrypto"
+ 		"components/service/crypto/backend/mbedcrypto/trng_adapter/linux"
+diff --git a/deployments/se-proxy/opteesp/CMakeLists.txt b/deployments/se-proxy/opteesp/CMakeLists.txt
+index 953bb716..24a8ca65 100644
+--- a/deployments/se-proxy/opteesp/CMakeLists.txt
++++ b/deployments/se-proxy/opteesp/CMakeLists.txt
+@@ -1,5 +1,5 @@
+ #-------------------------------------------------------------------------------
+-# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++# Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+ #
+ # SPDX-License-Identifier: BSD-3-Clause
+ #
+@@ -70,6 +70,8 @@ add_components(TARGET "se-proxy"
+ 		"components/service/crypto/provider/extension/key_derivation/serializer/packed-c"
+ 		"components/service/crypto/provider/extension/mac"
+ 		"components/service/crypto/provider/extension/mac/serializer/packed-c"
++		"components/service/crypto/provider/extension/aead"
++		"components/service/crypto/provider/extension/aead/serializer/packed-c"
+ 		"components/service/crypto/factory/full"
+ 		"components/service/secure_storage/include"
+ 		"components/service/secure_storage/frontend/secure_storage_provider"
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0039-Add-IV-generation-to-one-shot-cipher-operation.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0039-Add-IV-generation-to-one-shot-cipher-operation.patch
new file mode 100644
index 0000000..0c93a26
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0039-Add-IV-generation-to-one-shot-cipher-operation.patch
@@ -0,0 +1,96 @@
+From 43388a8e071980d9146f935f486a859d0a04322b Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Tue, 15 Feb 2022 15:46:58 +0000
+Subject: [PATCH] Add IV generation to one-shot cipher operation
+
+The functions psa_cipher_encrypt and psa_cipher_decrypt are
+one-shot operations that can take an arbitrary sized input.
+These operations are implemented as client-side functions
+that use multi-part cipher operations to allow large inputs
+to be handled. The existing implementations were missing the
+generation and setting of the IV at the start of the data.
+This was leading to PSA Arch test failures (248 & 249). This
+commit adds the missing IV handling and resolves the test
+failures.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: I4afb555ee7062ebb387e5bb27fb1e082288ad8c7
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ .../service/crypto/client/psa/psa_cipher.c    | 40 +++++++++++++++----
+ 1 file changed, 33 insertions(+), 7 deletions(-)
+
+diff --git a/components/service/crypto/client/psa/psa_cipher.c b/components/service/crypto/client/psa/psa_cipher.c
+index 3ab8ea21..111af829 100644
+--- a/components/service/crypto/client/psa/psa_cipher.c
++++ b/components/service/crypto/client/psa/psa_cipher.c
+@@ -8,7 +8,6 @@
+ #include "psa_crypto_client.h"
+ #include "crypto_caller_selector.h"
+ 
+-
+ psa_status_t psa_cipher_encrypt_setup(psa_cipher_operation_t *operation,
+ 	psa_key_id_t key,
+ 	psa_algorithm_t alg)
+@@ -171,9 +170,16 @@ psa_status_t psa_cipher_encrypt(psa_key_id_t key,
+ 
+ 	if (psa_status == PSA_SUCCESS) {
+ 
++		size_t ciphertext_len = 0;
++		size_t iv_len = 0;
++
++		psa_cipher_generate_iv(&operation, output, output_size, &iv_len);
++
+ 		psa_status = multi_cipher_update(&operation,
+ 			input, input_length,
+-			output, output_size, output_length);
++			&output[iv_len], output_size - iv_len, &ciphertext_len);
++
++		*output_length = iv_len + ciphertext_len;
+ 	}
+ 
+ 	return psa_status;
+@@ -187,14 +193,34 @@ psa_status_t psa_cipher_decrypt(psa_key_id_t key,
+ 	size_t output_size,
+ 	size_t *output_length)
+ {
+-	psa_cipher_operation_t operation = psa_cipher_operation_init();
+-	psa_status_t psa_status = psa_cipher_decrypt_setup(&operation, key, alg);
++	psa_key_attributes_t attributes = PSA_KEY_ATTRIBUTES_INIT;
++	psa_status_t psa_status = psa_get_key_attributes(key, &attributes);
+ 
+ 	if (psa_status == PSA_SUCCESS) {
+ 
+-		psa_status = multi_cipher_update(&operation,
+-			input, input_length,
+-			output, output_size, output_length);
++		psa_cipher_operation_t operation = psa_cipher_operation_init();
++		psa_status = psa_cipher_decrypt_setup(&operation, key, alg);
++
++		if (psa_status == PSA_SUCCESS) {
++
++			size_t iv_len = PSA_CIPHER_IV_LENGTH(psa_get_key_type(&attributes), alg);
++
++			if (input_length >= iv_len) {
++
++				psa_cipher_set_iv(&operation, input, iv_len);
++
++				psa_status = multi_cipher_update(&operation,
++					&input[iv_len], input_length - iv_len,
++					output, output_size, output_length);
++			}
++			else {
++
++				psa_cipher_abort(&operation);
++				psa_status = PSA_ERROR_INVALID_ARGUMENT;
++			}
++		}
++
++		psa_reset_key_attributes(&attributes);
+ 	}
+ 
+ 	return psa_status;
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0040-Fix-multi-part-termination-on-error.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0040-Fix-multi-part-termination-on-error.patch
new file mode 100644
index 0000000..bdafcea
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0040-Fix-multi-part-termination-on-error.patch
@@ -0,0 +1,241 @@
+From 07277e2ab4b54e5844c28f0cb33e64a91aa5f492 Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Wed, 16 Feb 2022 10:37:04 +0000
+Subject: [PATCH] Fix multi-part termination on error
+
+For multi-part operations, the PSA Crypto API specifies that if
+the final operation does not return PSA_SUCCESS, the abort
+operaion must be called by a client to clean-up the operation.
+This change modifies behaviour in-line with the API definition.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: Ia3d3ec004164647a7ab5988cac45c39c22e76e9a
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ components/service/crypto/client/psa/psa_aead.c       |  8 ++++++++
+ components/service/crypto/client/psa/psa_cipher.c     |  4 ++++
+ components/service/crypto/client/psa/psa_hash.c       | 10 ++++++++++
+ components/service/crypto/client/psa/psa_mac.c        | 10 ++++++++++
+ .../crypto/provider/extension/aead/aead_provider.c    | 10 +++++-----
+ .../provider/extension/cipher/cipher_provider.c       |  6 +++---
+ .../crypto/provider/extension/hash/hash_provider.c    |  6 +++---
+ .../crypto/provider/extension/mac/mac_provider.c      | 11 +++++++----
+ 8 files changed, 50 insertions(+), 15 deletions(-)
+
+diff --git a/components/service/crypto/client/psa/psa_aead.c b/components/service/crypto/client/psa/psa_aead.c
+index e4579e63..559eb6a3 100644
+--- a/components/service/crypto/client/psa/psa_aead.c
++++ b/components/service/crypto/client/psa/psa_aead.c
+@@ -241,6 +241,10 @@ psa_status_t psa_aead_encrypt(psa_key_id_t key,
+ 
+ 			*aeadtext_length = bytes_output + remaining_aead_len + tag_len;
+ 		}
++		else {
++
++			psa_aead_abort(&operation);
++		}
+ 	}
+ 	else {
+ 
+@@ -292,6 +296,10 @@ psa_status_t psa_aead_decrypt(psa_key_id_t key,
+ 
+ 			*plaintext_length = bytes_output + remaining_plaintext_len;
+ 		}
++		else {
++
++			psa_aead_abort(&operation);
++		}
+ 	}
+ 	else {
+ 
+diff --git a/components/service/crypto/client/psa/psa_cipher.c b/components/service/crypto/client/psa/psa_cipher.c
+index 111af829..4e4264b6 100644
+--- a/components/service/crypto/client/psa/psa_cipher.c
++++ b/components/service/crypto/client/psa/psa_cipher.c
+@@ -146,6 +146,10 @@ static psa_status_t multi_cipher_update(psa_cipher_operation_t *operation,
+ 
+ 				*output_length = bytes_output + finish_output_len;
+ 			}
++			else {
++
++				psa_cipher_abort(operation);
++			}
+ 		}
+ 		else {
+ 
+diff --git a/components/service/crypto/client/psa/psa_hash.c b/components/service/crypto/client/psa/psa_hash.c
+index 83278de6..e5dd0030 100644
+--- a/components/service/crypto/client/psa/psa_hash.c
++++ b/components/service/crypto/client/psa/psa_hash.c
+@@ -137,6 +137,11 @@ psa_status_t psa_hash_compare(psa_algorithm_t alg,
+ 	if (psa_status == PSA_SUCCESS) {
+ 
+ 		psa_status = psa_hash_verify(&operation, hash, hash_length);
++
++		if (psa_status != PSA_SUCCESS) {
++
++			psa_hash_abort(&operation);
++		}
+ 	}
+ 
+ 	return psa_status;
+@@ -155,6 +160,11 @@ psa_status_t psa_hash_compute(psa_algorithm_t alg,
+ 	if (psa_status == PSA_SUCCESS) {
+ 
+ 		psa_status = psa_hash_finish(&operation, hash, hash_size, hash_length);
++
++		if (psa_status != PSA_SUCCESS) {
++
++			psa_hash_abort(&operation);
++		}
+ 	}
+ 
+ 	return psa_status;
+diff --git a/components/service/crypto/client/psa/psa_mac.c b/components/service/crypto/client/psa/psa_mac.c
+index 5c5eb32a..a3db8644 100644
+--- a/components/service/crypto/client/psa/psa_mac.c
++++ b/components/service/crypto/client/psa/psa_mac.c
+@@ -129,6 +129,11 @@ psa_status_t psa_mac_verify(psa_key_id_t key,
+ 	if (psa_status == PSA_SUCCESS) {
+ 
+ 		psa_status = psa_mac_verify_finish(&operation, mac, mac_length);
++
++		if (psa_status != PSA_SUCCESS) {
++
++			psa_mac_abort(&operation);
++		}
+ 	}
+ 
+ 	return psa_status;
+@@ -153,6 +158,11 @@ psa_status_t psa_mac_compute(psa_key_id_t key,
+ 	if (psa_status == PSA_SUCCESS) {
+ 
+ 		psa_status = psa_mac_sign_finish(&operation, mac, mac_size, mac_length);
++
++		if (psa_status != PSA_SUCCESS) {
++
++			psa_mac_abort(&operation);
++		}
+ 	}
+ 
+ 	return psa_status;
+diff --git a/components/service/crypto/provider/extension/aead/aead_provider.c b/components/service/crypto/provider/extension/aead/aead_provider.c
+index f4e81a03..14a25436 100644
+--- a/components/service/crypto/provider/extension/aead/aead_provider.c
++++ b/components/service/crypto/provider/extension/aead/aead_provider.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -369,9 +369,9 @@ static rpc_status_t aead_finish_handler(void *context, struct call_req *req)
+ 				rpc_status = serializer->serialize_aead_finish_resp(resp_buf,
+ 					ciphertext, ciphertext_len,
+ 					tag, tag_len);
+-			}
+ 
+-			crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++				crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++			}
+ 		}
+ 
+ 		call_req_set_opstatus(req, psa_status);
+@@ -418,9 +418,9 @@ static rpc_status_t aead_verify_handler(void *context, struct call_req *req)
+ 				struct call_param_buf *resp_buf = call_req_get_resp_buf(req);
+ 				rpc_status = serializer->serialize_aead_verify_resp(resp_buf,
+ 					plaintext, plaintext_len);
+-			}
+ 
+-			crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++				crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++			}
+ 		}
+ 
+ 		call_req_set_opstatus(req, psa_status);
+diff --git a/components/service/crypto/provider/extension/cipher/cipher_provider.c b/components/service/crypto/provider/extension/cipher/cipher_provider.c
+index 8e7a86de..a5dd0371 100644
+--- a/components/service/crypto/provider/extension/cipher/cipher_provider.c
++++ b/components/service/crypto/provider/extension/cipher/cipher_provider.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -283,9 +283,9 @@ static rpc_status_t cipher_finish_handler(void *context, struct call_req* req)
+ 
+ 				struct call_param_buf *resp_buf = call_req_get_resp_buf(req);
+ 				rpc_status = serializer->serialize_cipher_finish_resp(resp_buf, output, output_len);
+-			}
+ 
+-			crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++				crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++			}
+ 		}
+ 
+ 		call_req_set_opstatus(req, psa_status);
+diff --git a/components/service/crypto/provider/extension/hash/hash_provider.c b/components/service/crypto/provider/extension/hash/hash_provider.c
+index 2c560513..fd39d440 100644
+--- a/components/service/crypto/provider/extension/hash/hash_provider.c
++++ b/components/service/crypto/provider/extension/hash/hash_provider.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -179,9 +179,9 @@ static rpc_status_t hash_finish_handler(void *context, struct call_req* req)
+ 
+ 				struct call_param_buf *resp_buf = call_req_get_resp_buf(req);
+ 				rpc_status = serializer->serialize_hash_finish_resp(resp_buf, hash, hash_len);
+-			}
+ 
+-			crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++				crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++			}
+ 		}
+ 
+ 		call_req_set_opstatus(req, psa_status);
+diff --git a/components/service/crypto/provider/extension/mac/mac_provider.c b/components/service/crypto/provider/extension/mac/mac_provider.c
+index 96fe4cf3..eef55586 100644
+--- a/components/service/crypto/provider/extension/mac/mac_provider.c
++++ b/components/service/crypto/provider/extension/mac/mac_provider.c
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+  *
+  * SPDX-License-Identifier: BSD-3-Clause
+  */
+@@ -181,9 +181,9 @@ static rpc_status_t mac_sign_finish_handler(void *context, struct call_req* req)
+ 
+ 				struct call_param_buf *resp_buf = call_req_get_resp_buf(req);
+ 				rpc_status = serializer->serialize_mac_sign_finish_resp(resp_buf, mac, mac_len);
+-			}
+ 
+-			crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++				crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++			}
+ 		}
+ 
+ 		call_req_set_opstatus(req, psa_status);
+@@ -220,7 +220,10 @@ static rpc_status_t mac_verify_finish_handler(void *context, struct call_req* re
+ 
+ 			psa_status = psa_mac_verify_finish(&crypto_context->op.mac, mac, mac_len);
+ 
+-			crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++			if (psa_status == PSA_SUCCESS) {
++
++				crypto_context_pool_free(&this_instance->context_pool, crypto_context);
++			}
+ 		}
+ 
+ 		call_req_set_opstatus(req, psa_status);
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0041-Abort-AEAD-operation-if-client-provided-buffer-is-to.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0041-Abort-AEAD-operation-if-client-provided-buffer-is-to.patch
new file mode 100644
index 0000000..6a11552
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0041-Abort-AEAD-operation-if-client-provided-buffer-is-to.patch
@@ -0,0 +1,49 @@
+From 92987ec20beedb44d08d429947958c1c068d815c Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Wed, 16 Feb 2022 11:36:09 +0000
+Subject: [PATCH] Abort AEAD operation if client provided buffer is too small
+
+To enable PSA Arch test c258 to pass, handling is added in the
+PSA API client adaptor for AEAD (psa_aead.c) to abort an AEAD
+operation if an update operation is performed but the client
+provided buffer for the output is too small.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: Ib4b26ebc0a83a8928e1b643fba4becd935f6deb0
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ components/service/crypto/client/psa/psa_aead.c | 14 +++++++++++++-
+ 1 file changed, 13 insertions(+), 1 deletion(-)
+
+diff --git a/components/service/crypto/client/psa/psa_aead.c b/components/service/crypto/client/psa/psa_aead.c
+index 559eb6a3..c820d222 100644
+--- a/components/service/crypto/client/psa/psa_aead.c
++++ b/components/service/crypto/client/psa/psa_aead.c
+@@ -74,10 +74,22 @@ psa_status_t psa_aead_update(psa_aead_operation_t *operation,
+ 	size_t output_size,
+ 	size_t *output_length)
+ {
+-	return crypto_caller_aead_update(&psa_crypto_client_instance.base,
++	psa_status_t status = crypto_caller_aead_update(&psa_crypto_client_instance.base,
+ 		operation->handle,
+ 		input, input_length,
+ 		output, output_size, output_length);
++
++	/*
++	 * If too small a buffer has been provided for the output, the operation
++	 * state will have been updated but the result can't be put anywhere. This
++	 * is an unrecoveral condition so abort the operation.
++	 */
++	if (status == PSA_ERROR_BUFFER_TOO_SMALL) {
++
++		psa_aead_abort(operation);
++	}
++
++	return status;
+ }
+ 
+ psa_status_t psa_aead_finish(psa_aead_operation_t *operation,
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0042-Peg-to-updated-t_cose-version-fc3a4b2c.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0042-Peg-to-updated-t_cose-version-fc3a4b2c.patch
new file mode 100644
index 0000000..fedb79c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0042-Peg-to-updated-t_cose-version-fc3a4b2c.patch
@@ -0,0 +1,94 @@
+From 75c0689513e7da7fb26bf23c1da4e1aa49783d46 Mon Sep 17 00:00:00 2001
+From: Julian Hall <julian.hall@arm.com>
+Date: Tue, 11 Jan 2022 09:43:52 +0000
+Subject: [PATCH] Peg to updated t_cose version fc3a4b2c
+
+The current version of TS fails to build due to a regression introduced
+by a new t_cose version in the upstream project.
+The issue is caused by the t_cose external component incorrectly using
+tip of master as the upstream version id, which makes strict dependency
+control impossible. Change the upstream version id to an SHA, to enable
+controlling compatibility issues introduced by future upstream updates.
+
+At the same time update the dependency to the current latest version.
+The upstream project is now compatile with mbedtls 3.0.0 API changes
+so the previously required compatibility patch has been removed.
+
+Signed-off-by: Julian Hall <julian.hall@arm.com>
+Change-Id: I9491a5210904cc369846da2af45b0f7e5913bed8
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ .../0002-add-tls3_0_0-compatibility.patch     | 31 -------------------
+ external/t_cose/t_cose.cmake                  |  5 ++-
+ 2 files changed, 2 insertions(+), 34 deletions(-)
+ delete mode 100644 external/t_cose/0002-add-tls3_0_0-compatibility.patch
+
+diff --git a/external/t_cose/0002-add-tls3_0_0-compatibility.patch b/external/t_cose/0002-add-tls3_0_0-compatibility.patch
+deleted file mode 100644
+index 20a7d131..00000000
+--- a/external/t_cose/0002-add-tls3_0_0-compatibility.patch
++++ /dev/null
+@@ -1,31 +0,0 @@
+-diff --git a/crypto_adapters/t_cose_psa_crypto.c b/crypto_adapters/t_cose_psa_crypto.c
+-index 49c5b60..3aa7b58 100644
+---- a/crypto_adapters/t_cose_psa_crypto.c
+-+++ b/crypto_adapters/t_cose_psa_crypto.c
+-@@ -99,7 +99,7 @@ static enum t_cose_err_t psa_status_to_t_cose_error_signing(psa_status_t err)
+-            err == PSA_ERROR_INVALID_SIGNATURE   ? T_COSE_ERR_SIG_VERIFY :
+-            err == PSA_ERROR_NOT_SUPPORTED       ? T_COSE_ERR_UNSUPPORTED_SIGNING_ALG:
+-            err == PSA_ERROR_INSUFFICIENT_MEMORY ? T_COSE_ERR_INSUFFICIENT_MEMORY :
+--           err == PSA_ERROR_TAMPERING_DETECTED  ? T_COSE_ERR_TAMPERING_DETECTED :
+-+           err == PSA_ERROR_CORRUPTION_DETECTED ? T_COSE_ERR_TAMPERING_DETECTED :
+-                                                   T_COSE_ERR_SIG_FAIL;
+- }
+- 
+-@@ -152,7 +152,7 @@ t_cose_crypto_pub_key_verify(int32_t               cose_algorithm_id,
+-      * Crypto ceases providing backwards compatibility then this code
+-      * has to be changed to use psa_verify_hash().
+-      */
+--    psa_result = psa_asymmetric_verify(verification_key_psa,
+-+    psa_result = psa_verify_hash(verification_key_psa,
+-                                        psa_alg_id,
+-                                        hash_to_verify.ptr,
+-                                        hash_to_verify.len,
+-@@ -212,7 +212,7 @@ t_cose_crypto_pub_key_sign(int32_t                cose_algorithm_id,
+-      * providing backwards compatibility then this code has to be
+-      * changed to use psa_sign_hash().
+-      */
+--    psa_result = psa_asymmetric_sign(signing_key_psa,
+-+    psa_result = psa_sign_hash(signing_key_psa,
+-                                      psa_alg_id,
+-                                      hash_to_sign.ptr,
+-                                      hash_to_sign.len,
+diff --git a/external/t_cose/t_cose.cmake b/external/t_cose/t_cose.cmake
+index 660824bd..9321466f 100644
+--- a/external/t_cose/t_cose.cmake
++++ b/external/t_cose/t_cose.cmake
+@@ -1,5 +1,5 @@
+ #-------------------------------------------------------------------------------
+-# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
++# Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+ #
+ # SPDX-License-Identifier: BSD-3-Clause
+ #
+@@ -16,7 +16,7 @@ endif()
+ 
+ # External component details
+ set(T_COSE_URL "https://github.com/laurencelundblade/t_cose.git" CACHE STRING "t_cose repository URL")
+-set(T_COSE_REFSPEC "master" CACHE STRING "t_cose git refspec")
++set(T_COSE_REFSPEC "fc3a4b2c7196ff582e8242de8bd4a1bc4eec577f" CACHE STRING "t_cose git refspec")
+ set(T_COSE_INSTALL_PATH "${CMAKE_CURRENT_BINARY_DIR}/t_cose_install" CACHE PATH "t_cose installation directory")
+ set(T_COSE_PACKAGE_PATH "${T_COSE_INSTALL_PATH}/libt_cose/cmake" CACHE PATH "t_cose CMake package directory")
+ 
+@@ -37,7 +37,6 @@ FetchContent_Declare(
+ 
+ 	PATCH_COMMAND git stash
+ 		COMMAND git am ${CMAKE_CURRENT_LIST_DIR}/0001-add-install-definition.patch
+-		COMMAND git apply ${CMAKE_CURRENT_LIST_DIR}/0002-add-tls3_0_0-compatibility.patch
+ 		COMMAND git reset HEAD~1
+ 
+ )
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0043-pass-sysroot_yocto.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0043-pass-sysroot_yocto.patch
new file mode 100644
index 0000000..64ac094
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0043-pass-sysroot_yocto.patch
@@ -0,0 +1,110 @@
+From 24436d459ddde697c89ff947c821cec9c5e0906e Mon Sep 17 00:00:00 2001
+From: Vishnu Banavath <vishnu.banavath@arm.com>
+Date: Wed, 16 Feb 2022 15:55:55 +0000
+Subject: [PATCH] pass sysroot_yocto
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ deployments/libts/libts-import.cmake         |  3 +++
+ external/MbedTLS/MbedTLS.cmake               |  1 +
+ external/psa_arch_tests/psa_arch_tests.cmake | 25 +++++++++++++-------
+ 3 files changed, 20 insertions(+), 9 deletions(-)
+
+diff --git a/deployments/libts/libts-import.cmake b/deployments/libts/libts-import.cmake
+index 792ba86c..b900ce3f 100644
+--- a/deployments/libts/libts-import.cmake
++++ b/deployments/libts/libts-import.cmake
+@@ -27,9 +27,12 @@ set(LIBTS_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/_deps/libts-build" CACHE PATH
+ 
+ file(MAKE_DIRECTORY ${LIBTS_BINARY_DIR})
+ 
++set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} --sysroot=${SYSROOT_YOCTO}")
++
+ #Configure the library
+ execute_process(COMMAND
+ 	${CMAKE_COMMAND}
++                -DCMAKE_SYSROOT=${SYSROOT_YOCTO}
+ 		-DCMAKE_INSTALL_PREFIX=${LIBTS_INSTALL_PATH}
+ 		-GUnix\ Makefiles
+ 		${LIBTS_SOURCE_DIR}
+diff --git a/external/MbedTLS/MbedTLS.cmake b/external/MbedTLS/MbedTLS.cmake
+index 3350d8a0..33467cf0 100644
+--- a/external/MbedTLS/MbedTLS.cmake
++++ b/external/MbedTLS/MbedTLS.cmake
+@@ -103,6 +103,7 @@ if (NOT MBEDCRYPTO_LIB_FILE)
+ 	execute_process(COMMAND
+ 		${CMAKE_COMMAND} -E env CROSS_COMPILE=${CROSS_COMPILE}
+ 			${CMAKE_COMMAND}
++                                -DCMAKE_SYSROOT=${SYSROOT_YOCTO}
+ 				-DENABLE_PROGRAMS=OFF
+ 				-DENABLE_TESTING=OFF
+ 				-DUNSAFE_BUILD=ON
+diff --git a/external/psa_arch_tests/psa_arch_tests.cmake b/external/psa_arch_tests/psa_arch_tests.cmake
+index f6d2fb9f..42f73a37 100644
+--- a/external/psa_arch_tests/psa_arch_tests.cmake
++++ b/external/psa_arch_tests/psa_arch_tests.cmake
+@@ -5,30 +5,33 @@
+ #
+ #-------------------------------------------------------------------------------
+ 
+-# Temporarily using modified tests used for tf-m verification
++# Determine the number of processes to run while running parallel builds.
++# Pass -DPROCESSOR_COUNT=<n> to cmake to override.
++if(NOT DEFINED PROCESSOR_COUNT)
++	include(ProcessorCount)
++	ProcessorCount(PROCESSOR_COUNT)
++	set(PROCESSOR_COUNT ${PROCESSOR_COUNT} CACHE STRING "Number of cores to use for parallel builds.")
++endif()
++
+ set(PSA_ARCH_TESTS_URL "https://github.com/bensze01/psa-arch-tests.git" CACHE STRING "psa-arch-tests repository URL")
+ set(PSA_ARCH_TESTS_REFSPEC "fix-multipart-aead" CACHE STRING "psa-arch-tests git refspec")
+-
+-#set(PSA_ARCH_TESTS_URL "https://github.com/ARM-software/psa-arch-tests.git" CACHE STRING "psa-arch-tests repository URL")
+-#set(PSA_ARCH_TESTS_REFSPEC "2a1852252a9b9af655cbe02d5d3c930952d0d798" CACHE STRING "psa-arch-tests v22.01_API1.4_ADAC_BETA")
+ set(PSA_ARCH_TESTS_INSTALL_PATH "${CMAKE_CURRENT_BINARY_DIR}/psa-arch-tests_install" CACHE PATH "psa-arch-tests installation directory")
+ set(PSA_ARCH_TESTS_PACKAGE_PATH "${PSA_ARCH_TESTS_INSTALL_PATH}/libpsa-arch-tests/cmake" CACHE PATH "psa-arch-tests CMake package directory")
+-set(PSA_ARCH_TESTS_SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/_deps/psa_arch_tests-src" CACHE PATH "psa-arch-tests source.")
++
++include(FetchContent)
+ 
+ # Checking git
+ find_program(GIT_COMMAND "git")
+ if (NOT GIT_COMMAND)
+ 	message(FATAL_ERROR "Please install git")
+ endif()
+-
++if ("${PSA_ARCH_TESTS_PATH}" STREQUAL "DOWNLOAD")
+ # Fetching psa-arch-tests
+ FetchContent_Declare(
+ 	psa-arch-tests
+ 	GIT_REPOSITORY ${PSA_ARCH_TESTS_URL}
+ 	GIT_TAG ${PSA_ARCH_TESTS_REFSPEC}
+ 	GIT_SHALLOW TRUE
+-	PATCH_COMMAND git stash
+-		COMMAND git apply ${CMAKE_CURRENT_LIST_DIR}/modify_attest_config.patch
+ )
+ 
+ # FetchContent_GetProperties exports psa-arch-tests_SOURCE_DIR and psa-arch-tests_BINARY_DIR variables
+@@ -37,7 +40,10 @@ if(NOT psa-arch-tests_POPULATED)
+ 	message(STATUS "Fetching psa-arch-tests")
+ 	FetchContent_Populate(psa-arch-tests)
+ endif()
+-
++else()
++    set(psa-arch-tests_SOURCE_DIR "${TS_ROOT}/../psa-arch-tests")
++    set(psa-arch-tests_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}")
++endif()
+ # Ensure list of include paths is separated correctly
+ string(REPLACE ";" "\\;" PSA_ARCH_TESTS_EXTERNAL_INCLUDE_PATHS "${PSA_ARCH_TESTS_EXTERNAL_INCLUDE_PATHS}")
+ 
+@@ -47,6 +53,7 @@ string(REPLACE ";" " " PSA_ARCH_TEST_EXTERNAL_DEFS "${PSA_ARCH_TEST_EXTERNAL_DEF
+ # Configure the psa-arch-test library
+ execute_process(COMMAND
+ 	${CMAKE_COMMAND}
++			-DCMAKE_SYSROOT=${SYSROOT_YOCTO}
+ 			-DTOOLCHAIN=INHERIT
+ 			-DCMAKE_TOOLCHAIN_FILE=${TS_EXTERNAL_LIB_TOOLCHAIN_FILE}
+ 			-DPSA_INCLUDE_PATHS=${PSA_ARCH_TESTS_EXTERNAL_INCLUDE_PATHS}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0044-Fix-Crypto-interface-structure-aligned-with-tf-m-cha.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0044-Fix-Crypto-interface-structure-aligned-with-tf-m-cha.patch
new file mode 100644
index 0000000..68d0a97
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0044-Fix-Crypto-interface-structure-aligned-with-tf-m-cha.patch
@@ -0,0 +1,29 @@
+From 0a0007d594db2fceed413cd73e7f08dd8d8ddd57 Mon Sep 17 00:00:00 2001
+From: Satish Kumar <satish.kumar01@arm.com>
+Date: Sun, 13 Feb 2022 09:01:10 +0000
+Subject: [PATCH] Fix: Crypto interface structure aligned with tf-m change.
+
+NO NEED TO RAISE PR: The PR for this FIX  is raied by Emek.
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ components/service/crypto/backend/psa_ipc/crypto_ipc_backend.h | 3 ++-
+ 1 file changed, 2 insertions(+), 1 deletion(-)
+
+diff --git a/components/service/crypto/backend/psa_ipc/crypto_ipc_backend.h b/components/service/crypto/backend/psa_ipc/crypto_ipc_backend.h
+index c13c20e8..ec25eaf8 100644
+--- a/components/service/crypto/backend/psa_ipc/crypto_ipc_backend.h
++++ b/components/service/crypto/backend/psa_ipc/crypto_ipc_backend.h
+@@ -38,7 +38,8 @@ struct psa_ipc_crypto_pack_iovec {
+ 				      *   multipart operation
+ 				      */
+ 	uint32_t capacity;             /*!< Key derivation capacity */
+-
++	uint32_t ad_length;            /*!< Additional Data length for multipart AEAD */
++	uint32_t plaintext_length;     /*!< Plaintext length for multipart AEAD */
+ 	struct psa_ipc_crypto_aead_pack_input aead_in; /*!< FixMe: Temporarily used for
+ 							    *   AEAD until the API is
+ 							    *   restructured
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0045-Integrate-remaining-psa-ipc-client-APIs.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0045-Integrate-remaining-psa-ipc-client-APIs.patch
new file mode 100644
index 0000000..a08ab32
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0045-Integrate-remaining-psa-ipc-client-APIs.patch
@@ -0,0 +1,494 @@
+From b8060d9e15b1b910cf9b466a3f43088c71d7a38f Mon Sep 17 00:00:00 2001
+From: Satish Kumar <satish.kumar01@arm.com>
+Date: Sun, 13 Feb 2022 09:49:51 +0000
+Subject: [PATCH] Integrate remaining psa-ipc client APIs.
+
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ .../caller/psa_ipc/crypto_caller_aead.h       | 297 +++++++++++++++++-
+ .../caller/psa_ipc/crypto_caller_sign_hash.h  |  35 +++
+ .../psa_ipc/crypto_caller_verify_hash.h       |  33 +-
+ 3 files changed, 352 insertions(+), 13 deletions(-)
+
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_aead.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_aead.h
+index 78517fe3..f6aadd8b 100644
+--- a/components/service/crypto/client/caller/psa_ipc/crypto_caller_aead.h
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_aead.h
+@@ -152,7 +152,27 @@ static inline psa_status_t crypto_caller_aead_encrypt_setup(
+ 					    psa_key_id_t key,
+ 					    psa_algorithm_t alg)
+ {
+-	return PSA_ERROR_NOT_SUPPORTED;
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++	    .sfn_id = TFM_CRYPTO_AEAD_ENCRYPT_SETUP_SID,
++	    .key_id = key,
++	    .alg = alg,
++	    .op_handle = (*op_handle),
++	};
++
++	struct psa_invec in_vec[] = {
++	    {.base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec)}
++	};
++	struct psa_outvec out_vec[] = {
++	    {.base = psa_ptr_to_u32(op_handle), .len = sizeof(uint32_t)}
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++	                   IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
+ }
+ 
+ static inline psa_status_t crypto_caller_aead_decrypt_setup(
+@@ -161,7 +181,26 @@ static inline psa_status_t crypto_caller_aead_decrypt_setup(
+ 					    psa_key_id_t key,
+ 					    psa_algorithm_t alg)
+ {
+-	return PSA_ERROR_NOT_SUPPORTED;
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++	    .sfn_id = TFM_CRYPTO_AEAD_DECRYPT_SETUP_SID,
++	    .key_id = key,
++	    .alg = alg,
++	    .op_handle = (*op_handle),
++	};
++
++	struct psa_invec in_vec[] = {
++	    {.base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec)}
++	};
++	struct psa_outvec out_vec[] = {
++	    {.base = psa_ptr_to_u32(op_handle), .len = sizeof(uint32_t)}
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++	                   IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++	return status;
+ }
+ 
+ static inline psa_status_t crypto_caller_aead_generate_nonce(
+@@ -171,7 +210,27 @@ static inline psa_status_t crypto_caller_aead_generate_nonce(
+ 					     size_t nonce_size,
+ 					     size_t *nonce_length)
+ {
+-	return PSA_ERROR_NOT_SUPPORTED;
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++	    .sfn_id = TFM_CRYPTO_AEAD_GENERATE_NONCE_SID,
++	    .op_handle = op_handle,
++	};
++
++	struct psa_invec in_vec[] = {
++	    {.base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec)},
++	};
++	struct psa_outvec out_vec[] = {
++	    {.base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t)},
++	    {.base = psa_ptr_to_u32(nonce), .len = nonce_size}
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++	                   IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*nonce_length = out_vec[1].len;
++	return status;
+ }
+ 
+ static inline psa_status_t crypto_caller_aead_set_nonce(
+@@ -180,7 +239,25 @@ static inline psa_status_t crypto_caller_aead_set_nonce(
+ 						const uint8_t *nonce,
+ 						size_t nonce_length)
+ {
+-	return PSA_ERROR_NOT_SUPPORTED;
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++	    .sfn_id = TFM_CRYPTO_AEAD_SET_NONCE_SID,
++	    .op_handle = op_handle,
++	};
++
++	struct psa_invec in_vec[] = {
++	    {.base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec)},
++	    {.base = psa_ptr_to_u32(nonce), .len = nonce_length}
++	};
++	struct psa_outvec out_vec[] = {
++	    {.base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t)}
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++	                   IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++	return status;
+ }
+ 
+ static inline psa_status_t crypto_caller_aead_set_lengths(
+@@ -189,7 +266,27 @@ static inline psa_status_t crypto_caller_aead_set_lengths(
+ 					  size_t ad_length,
+ 					  size_t plaintext_length)
+ {
+-	return PSA_ERROR_NOT_SUPPORTED;
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++	    .sfn_id = TFM_CRYPTO_AEAD_SET_LENGTHS_SID,
++	    .ad_length = ad_length,
++	    .plaintext_length = plaintext_length,
++	    .op_handle = op_handle,
++	};
++
++	struct psa_invec in_vec[] = {
++	    {.base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec)},
++	};
++	struct psa_outvec out_vec[] = {
++	    {.base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t)}
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++	                   IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	return status;
+ }
+ 
+ static inline psa_status_t crypto_caller_aead_update_ad(
+@@ -198,7 +295,35 @@ static inline psa_status_t crypto_caller_aead_update_ad(
+ 						const uint8_t *input,
+ 						size_t input_length)
+ {
+-	return PSA_ERROR_NOT_SUPPORTED;
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++	    .sfn_id = TFM_CRYPTO_AEAD_UPDATE_AD_SID,
++	    .op_handle = op_handle,
++	};
++
++	/* Sanitize the optional input */
++	if ((input == NULL) && (input_length != 0)) {
++	    return PSA_ERROR_INVALID_ARGUMENT;
++	}
++
++	struct psa_invec in_vec[] = {
++	    {.base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec)},
++	    {.base = psa_ptr_const_to_u32(input), .len = input_length}
++	};
++	struct psa_outvec out_vec[] = {
++	    {.base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t)}
++	};
++
++	size_t in_len = IOVEC_LEN(in_vec);
++
++	if (input == NULL) {
++	    in_len--;
++	}
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++	                   in_len, out_vec, IOVEC_LEN(out_vec));
++	return status;
+ }
+ 
+ static inline psa_status_t crypto_caller_aead_update(
+@@ -210,7 +335,38 @@ static inline psa_status_t crypto_caller_aead_update(
+ 					     size_t output_size,
+ 					     size_t *output_length)
+ {
+-	return PSA_ERROR_NOT_SUPPORTED;
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++	    .sfn_id = TFM_CRYPTO_AEAD_UPDATE_SID,
++	    .op_handle = op_handle,
++	};
++
++	/* Sanitize the optional input */
++	if ((input == NULL) && (input_length != 0)) {
++	    return PSA_ERROR_INVALID_ARGUMENT;
++	}
++
++	struct psa_invec in_vec[] = {
++	    {.base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec)},
++	    {.base = psa_ptr_const_to_u32(input), .len = input_length}
++	};
++	struct psa_outvec out_vec[] = {
++	    {.base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t)},
++	    {.base = psa_ptr_const_to_u32(output), .len = output_size},
++	};
++
++	size_t in_len = IOVEC_LEN(in_vec);
++
++	if (input == NULL) {
++	    in_len--;
++	}
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++	                   in_len, out_vec, IOVEC_LEN(out_vec));
++
++	*output_length = out_vec[1].len;
++	return status;
+ }
+ 
+ static inline psa_status_t crypto_caller_aead_finish(
+@@ -223,7 +379,48 @@ static inline psa_status_t crypto_caller_aead_finish(
+ 					     size_t tag_size,
+ 					     size_t *tag_length)
+ {
+-	return PSA_ERROR_NOT_SUPPORTED;
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++	    .sfn_id = TFM_CRYPTO_AEAD_FINISH_SID,
++	    .op_handle = op_handle,
++	};
++
++	/* Sanitize the optional output */
++	if ((aeadtext == NULL) && (aeadtext_size != 0)) {
++	    return PSA_ERROR_INVALID_ARGUMENT;
++	}
++
++	struct psa_invec in_vec[] = {
++	    {.base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec)},
++	};
++	struct psa_outvec out_vec[] = {
++	    {.base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t)},
++	    {.base = psa_ptr_const_to_u32(tag), .len = tag_size},
++	    {.base = psa_ptr_const_to_u32(aeadtext), .len = aeadtext_size}
++	};
++
++	size_t out_len = IOVEC_LEN(out_vec);
++
++	if (aeadtext == NULL || aeadtext_size == 0) {
++	    out_len--;
++	}
++	if ((out_len == 3) && (aeadtext_length == NULL)) {
++	    return PSA_ERROR_INVALID_ARGUMENT;
++	}
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++	                   IOVEC_LEN(in_vec), out_vec, out_len);
++
++	*tag_length = out_vec[1].len;
++
++	if (out_len == 3) {
++	    *aeadtext_length = out_vec[2].len;
++	} else {
++	    *aeadtext_length = 0;
++	}
++	return status;
+ }
+ 
+ static inline psa_status_t crypto_caller_aead_verify(
+@@ -235,14 +432,94 @@ static inline psa_status_t crypto_caller_aead_verify(
+ 					     const uint8_t *tag,
+ 					     size_t tag_length)
+ {
+-	return PSA_ERROR_NOT_SUPPORTED;
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++	    .sfn_id = TFM_CRYPTO_AEAD_VERIFY_SID,
++	    .op_handle = op_handle,
++	};
++
++	/* Sanitize the optional output */
++	if ((plaintext == NULL) && (plaintext_size != 0)) {
++	    return PSA_ERROR_INVALID_ARGUMENT;
++	}
++
++	struct psa_invec in_vec[] = {
++	    {.base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec)},
++	    {.base = psa_ptr_const_to_u32(tag), .len = tag_length}
++	};
++	struct psa_outvec out_vec[] = {
++	    {.base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t)},
++	    {.base = psa_ptr_const_to_u32(plaintext), .len = plaintext_size},
++	};
++
++	size_t out_len = IOVEC_LEN(out_vec);
++
++	if (plaintext == NULL || plaintext_size == 0) {
++	    out_len--;
++	}
++	if ((out_len == 2) && (plaintext_length == NULL)) {
++	    return PSA_ERROR_INVALID_ARGUMENT;
++	}
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++	                   IOVEC_LEN(in_vec), out_vec, out_len);
++
++	if (out_len == 2) {
++	    *plaintext_length = out_vec[1].len;
++	} else {
++	    *plaintext_length = 0;
++	}
++	return status;
+ }
+ 
+ static inline psa_status_t crypto_caller_aead_abort(
+ 					    struct service_client *context,
+ 					    uint32_t op_handle)
+ {
+-	return PSA_ERROR_NOT_SUPPORTED;
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++	    .sfn_id = TFM_CRYPTO_AEAD_ABORT_SID,
++	    .op_handle = op_handle,
++	};
++
++	struct psa_invec in_vec[] = {
++	    {.base = psa_ptr_to_u32(&iov), .len = sizeof(struct psa_ipc_crypto_pack_iovec)},
++	};
++	struct psa_outvec out_vec[] = {
++	    {.base = psa_ptr_to_u32(&op_handle), .len = sizeof(uint32_t)},
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++	                   IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++	return status;
++}
++
++static inline size_t crypto_caller_aead_max_update_size(const struct service_client *context)
++{
++       /* Returns the maximum number of bytes that may be
++        * carried as a parameter of the mac_update operation
++        *  using the packed-c encoding.
++        */
++       size_t payload_space = context->service_info.max_payload;
++       size_t overhead = iov_size;
++
++       return (payload_space > overhead) ? payload_space - overhead : 0;
++}
++
++static inline size_t crypto_caller_aead_max_update_ad_size(const struct service_client *context)
++{
++	/* Returns the maximum number of bytes that may be
++	 * carried as a parameter of the mac_update operation
++	 *  using the packed-c encoding.
++	 */
++	size_t payload_space = context->service_info.max_payload;
++	size_t overhead = iov_size;
++
++	return (payload_space > overhead) ? payload_space - overhead : 0;
+ }
+ 
+ #ifdef __cplusplus
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_sign_hash.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_sign_hash.h
+index 71d88ced..e4a2b167 100644
+--- a/components/service/crypto/client/caller/psa_ipc/crypto_caller_sign_hash.h
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_sign_hash.h
+@@ -57,6 +57,41 @@ static inline psa_status_t crypto_caller_sign_hash(struct service_client *contex
+ 	return status;
+ }
+ 
++static inline psa_status_t crypto_caller_sign_message(struct service_client *context,
++						   psa_key_id_t id,
++						   psa_algorithm_t alg,
++						   const uint8_t *hash,
++						   size_t hash_length,
++						   uint8_t *signature,
++						   size_t signature_size,
++						   size_t *signature_length)
++{
++	struct service_client *ipc = context;
++	struct rpc_caller *caller = ipc->caller;
++	psa_status_t status;
++	struct psa_ipc_crypto_pack_iovec iov = {
++		.sfn_id = TFM_CRYPTO_SIGN_MESSAGE_SID,
++		.key_id = id,
++		.alg = alg,
++	};
++	struct psa_invec in_vec[] = {
++		{ .base = psa_ptr_to_u32(&iov), .len = iov_size },
++		{ .base = psa_ptr_const_to_u32(hash), .len = hash_length },
++	};
++	struct psa_outvec out_vec[] = {
++		{ .base = psa_ptr_to_u32(signature), .len = signature_size },
++	};
++
++	status = psa_call(caller, TFM_CRYPTO_HANDLE, PSA_IPC_CALL, in_vec,
++			  IOVEC_LEN(in_vec), out_vec, IOVEC_LEN(out_vec));
++
++	*signature_length = out_vec[0].len;
++
++	return status;
++}
++
++
++
+ #ifdef __cplusplus
+ }
+ #endif
+diff --git a/components/service/crypto/client/caller/psa_ipc/crypto_caller_verify_hash.h b/components/service/crypto/client/caller/psa_ipc/crypto_caller_verify_hash.h
+index e16f6e54..cc9279ee 100644
+--- a/components/service/crypto/client/caller/psa_ipc/crypto_caller_verify_hash.h
++++ b/components/service/crypto/client/caller/psa_ipc/crypto_caller_verify_hash.h
+@@ -24,19 +24,20 @@
+ extern "C" {
+ #endif
+ 
+-static inline psa_status_t crypto_caller_verify_hash(struct service_client *context,
++static inline psa_status_t crypto_caller_common(struct service_client *context,
+ 						     psa_key_id_t id,
+ 						     psa_algorithm_t alg,
+ 						     const uint8_t *hash,
+ 						     size_t hash_length,
+ 						     const uint8_t *signature,
+-						     size_t signature_length)
++						     size_t signature_length,
++						     uint32_t sfn_id)
+ {
+ 	struct service_client *ipc = context;
+ 	struct rpc_caller *caller = ipc->caller;
+ 	psa_status_t status;
+ 	struct psa_ipc_crypto_pack_iovec iov = {
+-		.sfn_id = TFM_CRYPTO_VERIFY_HASH_SID,
++		.sfn_id = sfn_id,
+ 		.key_id = id,
+ 		.alg = alg,
+ 	};
+@@ -52,6 +53,32 @@ static inline psa_status_t crypto_caller_verify_hash(struct service_client *cont
+ 	return status;
+ }
+ 
++static inline psa_status_t crypto_caller_verify_hash(struct service_client *context,
++						     psa_key_id_t id,
++						     psa_algorithm_t alg,
++						     const uint8_t *hash,
++						     size_t hash_length,
++						     const uint8_t *signature,
++						     size_t signature_length)
++{
++
++	return crypto_caller_common(context,id,alg,hash,hash_length,
++			signature,signature_length, TFM_CRYPTO_VERIFY_HASH_SID);
++}
++
++static inline psa_status_t crypto_caller_verify_message(struct service_client *context,
++						     psa_key_id_t id,
++						     psa_algorithm_t alg,
++						     const uint8_t *hash,
++						     size_t hash_length,
++						     const uint8_t *signature,
++						     size_t signature_length)
++{
++
++	return crypto_caller_common(context,id,alg,hash,hash_length,
++			signature,signature_length, TFM_CRYPTO_VERIFY_MESSAGE_SID);
++}
++
+ #ifdef __cplusplus
+ }
+ #endif
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0046-Fix-update-psa_set_key_usage_flags-definition-to-the.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0046-Fix-update-psa_set_key_usage_flags-definition-to-the.patch
new file mode 100644
index 0000000..4bd846d
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0046-Fix-update-psa_set_key_usage_flags-definition-to-the.patch
@@ -0,0 +1,40 @@
+From a037ef21c0334117ad0741776a4b7b6e1a428d19 Mon Sep 17 00:00:00 2001
+From: Satish Kumar <satish.kumar01@arm.com>
+Date: Mon, 14 Feb 2022 17:52:00 +0000
+Subject: [PATCH] Fix : update psa_set_key_usage_flags definition to the latest
+ from the tf-m
+
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ components/service/crypto/include/psa/crypto_struct.h | 10 ++++++++++
+ 1 file changed, 10 insertions(+)
+
+diff --git a/components/service/crypto/include/psa/crypto_struct.h b/components/service/crypto/include/psa/crypto_struct.h
+index 1bc55e37..b4a7ed4b 100644
+--- a/components/service/crypto/include/psa/crypto_struct.h
++++ b/components/service/crypto/include/psa/crypto_struct.h
+@@ -155,9 +155,19 @@ static inline psa_key_lifetime_t psa_get_key_lifetime(
+     return( attributes->lifetime );
+ }
+ 
++static inline void psa_extend_key_usage_flags( psa_key_usage_t *usage_flags )
++{
++    if( *usage_flags & PSA_KEY_USAGE_SIGN_HASH )
++        *usage_flags |= PSA_KEY_USAGE_SIGN_MESSAGE;
++
++    if( *usage_flags & PSA_KEY_USAGE_VERIFY_HASH )
++        *usage_flags |= PSA_KEY_USAGE_VERIFY_MESSAGE;
++}
++
+ static inline void psa_set_key_usage_flags(psa_key_attributes_t *attributes,
+                                            psa_key_usage_t usage_flags)
+ {
++    psa_extend_key_usage_flags( &usage_flags );
+     attributes->usage = usage_flags;
+ }
+ 
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0047-Fixes-in-AEAD-for-psa-arch-test-54-and-58.patch b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0047-Fixes-in-AEAD-for-psa-arch-test-54-and-58.patch
new file mode 100644
index 0000000..4ad4be0
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions/corstone1000/0047-Fixes-in-AEAD-for-psa-arch-test-54-and-58.patch
@@ -0,0 +1,120 @@
+From 55463b12cca39d2c6a3fd18bbd3d28ae95dff8cf Mon Sep 17 00:00:00 2001
+From: Satish Kumar <satish.kumar01@arm.com>
+Date: Mon, 14 Feb 2022 08:22:25 +0000
+Subject: [PATCH] Fixes in AEAD for psa-arch test 54 and 58.
+
+Signed-off-by: Satish Kumar <satish.kumar01@arm.com>
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Emekcan Aras <Emekcan.Aras@arm.com>
+
+
+---
+ .../crypto/client/caller/packed-c/crypto_caller_aead.h    | 1 +
+ components/service/crypto/include/psa/crypto_sizes.h      | 2 +-
+ .../crypto/provider/extension/aead/aead_provider.c        | 8 ++++++--
+ .../extension/aead/serializer/aead_provider_serializer.h  | 1 +
+ .../packed-c/packedc_aead_provider_serializer.c           | 2 ++
+ protocols/service/crypto/packed-c/aead.h                  | 1 +
+ 6 files changed, 12 insertions(+), 3 deletions(-)
+
+diff --git a/components/service/crypto/client/caller/packed-c/crypto_caller_aead.h b/components/service/crypto/client/caller/packed-c/crypto_caller_aead.h
+index c4ffb20c..a91f66c1 100644
+--- a/components/service/crypto/client/caller/packed-c/crypto_caller_aead.h
++++ b/components/service/crypto/client/caller/packed-c/crypto_caller_aead.h
+@@ -309,6 +309,7 @@ static inline psa_status_t crypto_caller_aead_update(struct service_client *cont
+ 	size_t req_len = req_fixed_len;
+ 
+ 	*output_length = 0;
++        req_msg.output_size = output_size;
+ 	req_msg.op_handle = op_handle;
+ 
+ 	/* Mandatory input data parameter */
+diff --git a/components/service/crypto/include/psa/crypto_sizes.h b/components/service/crypto/include/psa/crypto_sizes.h
+index 4d7bf6e9..e3c4df29 100644
+--- a/components/service/crypto/include/psa/crypto_sizes.h
++++ b/components/service/crypto/include/psa/crypto_sizes.h
+@@ -351,7 +351,7 @@
+  *       just the largest size that may be generated by
+  *       #psa_aead_generate_nonce().
+  */
+-#define PSA_AEAD_NONCE_MAX_SIZE 12
++#define PSA_AEAD_NONCE_MAX_SIZE 16
+ 
+ /** A sufficient output buffer size for psa_aead_update().
+  *
+diff --git a/components/service/crypto/provider/extension/aead/aead_provider.c b/components/service/crypto/provider/extension/aead/aead_provider.c
+index 14a25436..6b144db8 100644
+--- a/components/service/crypto/provider/extension/aead/aead_provider.c
++++ b/components/service/crypto/provider/extension/aead/aead_provider.c
+@@ -283,10 +283,11 @@ static rpc_status_t aead_update_handler(void *context, struct call_req *req)
+ 	uint32_t op_handle;
+ 	const uint8_t *input;
+ 	size_t input_len;
++        uint32_t recv_output_size;
+ 
+ 	if (serializer)
+ 		rpc_status = serializer->deserialize_aead_update_req(req_buf, &op_handle,
+-			&input, &input_len);
++			&recv_output_size, &input, &input_len);
+ 
+ 	if (rpc_status == TS_RPC_CALL_ACCEPTED) {
+ 
+@@ -300,9 +301,12 @@ static rpc_status_t aead_update_handler(void *context, struct call_req *req)
+ 		if (crypto_context) {
+ 
+ 			size_t output_len = 0;
+-			size_t output_size = PSA_AEAD_UPDATE_OUTPUT_MAX_SIZE(input_len);
++			size_t output_size = PSA_AEAD_UPDATE_OUTPUT_MAX_SIZE(24);
+ 			uint8_t *output = malloc(output_size);
+ 
++                        if (recv_output_size < output_size) {
++                            output_size = recv_output_size;
++                        }
+ 			if (output) {
+ 
+ 				psa_status = psa_aead_update(&crypto_context->op.aead,
+diff --git a/components/service/crypto/provider/extension/aead/serializer/aead_provider_serializer.h b/components/service/crypto/provider/extension/aead/serializer/aead_provider_serializer.h
+index bb1a2a97..0156aaba 100644
+--- a/components/service/crypto/provider/extension/aead/serializer/aead_provider_serializer.h
++++ b/components/service/crypto/provider/extension/aead/serializer/aead_provider_serializer.h
+@@ -51,6 +51,7 @@ struct aead_provider_serializer {
+ 	/* Operation: aead_update */
+ 	rpc_status_t (*deserialize_aead_update_req)(const struct call_param_buf *req_buf,
+ 		uint32_t *op_handle,
++                uint32_t *output_size,
+ 		const uint8_t **input, size_t *input_len);
+ 
+ 	rpc_status_t (*serialize_aead_update_resp)(struct call_param_buf *resp_buf,
+diff --git a/components/service/crypto/provider/extension/aead/serializer/packed-c/packedc_aead_provider_serializer.c b/components/service/crypto/provider/extension/aead/serializer/packed-c/packedc_aead_provider_serializer.c
+index 6f00b3e3..45c739ab 100644
+--- a/components/service/crypto/provider/extension/aead/serializer/packed-c/packedc_aead_provider_serializer.c
++++ b/components/service/crypto/provider/extension/aead/serializer/packed-c/packedc_aead_provider_serializer.c
+@@ -192,6 +192,7 @@ static rpc_status_t deserialize_aead_update_ad_req(const struct call_param_buf *
+ /* Operation: aead_update */
+ static rpc_status_t deserialize_aead_update_req(const struct call_param_buf *req_buf,
+ 	uint32_t *op_handle,
++        uint32_t *output_size,
+ 	const uint8_t **input, size_t *input_len)
+ {
+ 	rpc_status_t rpc_status = TS_RPC_ERROR_INVALID_REQ_BODY;
+@@ -208,6 +209,7 @@ static rpc_status_t deserialize_aead_update_req(const struct call_param_buf *req
+ 		memcpy(&recv_msg, req_buf->data, expected_fixed_len);
+ 
+ 		*op_handle = recv_msg.op_handle;
++                *output_size = recv_msg.output_size;
+ 
+ 		tlv_const_iterator_begin(&req_iter,
+ 			(uint8_t*)req_buf->data + expected_fixed_len,
+diff --git a/protocols/service/crypto/packed-c/aead.h b/protocols/service/crypto/packed-c/aead.h
+index 0be266b5..435fd3b5 100644
+--- a/protocols/service/crypto/packed-c/aead.h
++++ b/protocols/service/crypto/packed-c/aead.h
+@@ -98,6 +98,7 @@ enum
+ struct __attribute__ ((__packed__)) ts_crypto_aead_update_in
+ {
+   uint32_t op_handle;
++  uint32_t output_size;
+ };
+ 
+ /* Variable length input parameter tags */
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions_%.bbappend b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions_%.bbappend
new file mode 100644
index 0000000..8a37a281
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/secure-partitions_%.bbappend
@@ -0,0 +1,4 @@
+MACHINE_TS_REQUIRE ?= ""
+MACHINE_TS_REQUIRE:corstone1000 = "ts-corstone1000.inc"
+
+require ${MACHINE_TS_REQUIRE}
diff --git a/meta-arm/meta-arm-bsp/recipes-security/trusted-services/ts-corstone1000.inc b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/ts-corstone1000.inc
new file mode 100644
index 0000000..88c46a7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/recipes-security/trusted-services/ts-corstone1000.inc
@@ -0,0 +1,126 @@
+FILESEXTRAPATHS:prepend := "${THISDIR}/secure-partitions/corstone1000:"
+
+COMPATIBLE_MACHINE = "corstone1000"
+
+LIC_FILES_CHKSUM += "file://../mbedtls/LICENSE;md5=3b83ef96387f14655fc854ddc3c6bd57 \
+                     file://../nanopb/LICENSE.txt;md5=9db4b73a55a3994384112efcdb37c01f \
+                     file://../openamp/LICENSE.md;md5=a8d8cf662ef6bf9936a1e1413585ecbf \
+		     file://../libmetal/LICENSE.md;md5=fe0b8a4beea8f0813b606d15a3df3d3c \
+		     file://../psa-arch-tests/LICENSE.md;md5=2a944942e1496af1886903d274dedb13 \
+		     "
+
+SRC_URI:append = " \
+                  ${SRC_URI_MBEDTLS} ${SRC_URI_NANOPB} ${SRC_URI_OPENAMP} ${SRC_URI_LIBMETAL} ${SRC_URI_ARCH-TESTS}\
+                  file://0001-tools-cmake-common-applying-lowercase-project-conven.patch \
+                  file://0002-fix-EARLY_TA_PATHS-env-variable.patch \
+                  file://0003-se-proxy-dts-add-se-proxy-as-child-node.patch \
+                  file://0004-Update-mm-comm-buffer-region-in-dts-file.patch \
+                  file://0005-Configure-NV-storage-macro.patch \
+                  file://0006-Use-device-region.patch \
+                  file://0007-Add-openamp-to-SE-proxy-deployment.patch \
+                  file://0008-Implement-mhu-driver-and-the-OpenAmp-conversion-laye.patch \
+                  file://0009-Add-openamp-rpc-caller.patch \
+                  file://0010-add-psa-client-definitions-for-ff-m.patch \
+                  file://0011-Add-common-service-component-to-ipc-support.patch \
+                  file://0012-Add-secure-storage-ipc-backend.patch \
+                  file://0013-Use-secure-storage-ipc-and-openamp-for-se_proxy.patch \
+                  file://0014-Add-uefi-variable-append-write-support.patch \
+                  file://0015-Add-UEFI-variable-support-for-QueryVariableInfo.patch \
+                  file://0016-Add-uefi-test-deployment.patch \
+                  file://0017-Fix-interface-ID-parameter-setting-in-sp-ffarpc_call.patch \
+                  file://0018-Support-FFARPC-call-requests-with-no-shared-buffer.patch \
+                  file://0019-Run-psa-arch-test.patch \
+                  file://0020-Use-address-instead-of-pointers.patch \
+                  file://0021-Add-psa-ipc-attestation-to-se-proxy.patch \
+                  file://0022-Setup-its-backend-as-openamp-rpc-using-secure-storag.patch \
+                  file://0023-add-psa-ipc-crypto-backend.patch \
+                  file://0024-Increase-SMM-gateway-UEFI-variable-macro-value.patch \
+                  file://0025-Add-stub-capsule-update-service-components.patch \
+                  file://0026-Add-logs-to-functions-in-SMM-gateway-SP.patch \
+                  file://0027-Configure-storage-size.patch \
+                  file://0028-Revert-Add-uefi-variable-append-write-support.patch \
+                  file://0029-Change-UID-of-variable-index-in-SMM.patch \
+                  file://0030-Add-missing-features-to-setVariable.patch \
+                  file://0031-Add-invalid-parameter-check-in-getNextVariableName.patch \
+                  file://0032-smm_gateway-add-checks-for-null-attributes.patch \
+		  file://0033-Enhance-mbedtls-fetch-process.patch \
+		  file://0034-Fix-format-specifier-in-logging_caller.patch \
+		  file://0035-Update-refspecs-for-mbedtls-and-psa-arch-tests-for-v.patch \
+		  file://0036-Separate-sign-verify-message-and-hash-operations.patch \
+		  file://0037-Add-defence-against-uninitialised-multi-part-transac.patch \
+		  file://0038-Integrate-AEAD-operation-support.patch \
+		  file://0039-Add-IV-generation-to-one-shot-cipher-operation.patch \
+		  file://0040-Fix-multi-part-termination-on-error.patch \
+		  file://0041-Abort-AEAD-operation-if-client-provided-buffer-is-to.patch \
+		  file://0042-Peg-to-updated-t_cose-version-fc3a4b2c.patch \
+		  file://0043-pass-sysroot_yocto.patch \
+		  file://0044-Fix-Crypto-interface-structure-aligned-with-tf-m-cha.patch \
+		  file://0045-Integrate-remaining-psa-ipc-client-APIs.patch \
+		  file://0046-Fix-update-psa_set_key_usage_flags-definition-to-the.patch \
+		  file://0047-Fixes-in-AEAD-for-psa-arch-test-54-and-58.patch \
+		  file://0003-corstone1000-port-crypto-config.patch;patchdir=../psa-arch-tests \
+                  "
+
+SRC_URI_MBEDTLS = "git://github.com/ARMmbed/mbedtls.git;protocol=https;branch=development;name=mbedtls;destsuffix=git/mbedtls"
+SRCREV_mbedtls = "d65aeb37349ad1a50e0f6c9b694d4b5290d60e49"
+
+SRC_URI_NANOPB = "git://github.com/nanopb/nanopb.git;name=nanopb;protocol=https;branch=master;destsuffix=git/nanopb"
+SRCREV_nanopb = "df0e92f474f9cca704fe2b31483f0b4d1b1715a4"
+
+SRC_URI_OPENAMP = "git://github.com/OpenAMP/open-amp.git;name=openamp;protocol=https;branch=main;destsuffix=git/openamp"
+SRCREV_openamp = "347397decaa43372fc4d00f965640ebde042966d"
+
+SRC_URI_LIBMETAL = "git://github.com/OpenAMP/libmetal.git;name=libmetal;protocol=https;branch=main;destsuffix=git/libmetal"
+SRCREV_libmetal = "f252f0e007fbfb8b3a52b1d5901250ddac96baad"
+
+SRC_URI_ARCH-TESTS = "git://github.com/bensze01/psa-arch-tests.git;name=psa-arch-tests;protocol=https;nobranch=1;destsuffix=git/psa-arch-tests"
+SRCREV_psa-arch-tests = "5d1a87f9c0a82e1632a3145687b4c8d7cbbeed2d"
+
+TS_ENVIRONMENT_LINUX = "arm-linux"
+TS_PLATFORM = "arm/corstone1000"
+TS_ENVIRONMENT = "opteesp"
+SP_PACKAGING_METHOD = "embedded"
+
+# Secure Enclave proxy secure partition
+TS_DEPLOYMENTS += "'deployments/se-proxy/${TS_ENVIRONMENT}'"
+
+# smm-gateway secure partition
+TS_DEPLOYMENTS += "'deployments/smm-gateway/${TS_ENVIRONMENT}'"
+
+PSA_API_TESTS = "deployments/psa-api-test/protected_storage/${TS_ENVIRONMENT_LINUX}"
+PSA_API_TESTS += "deployments/psa-api-test/internal_trusted_storage/${TS_ENVIRONMENT_LINUX}"
+PSA_API_TESTS += "deployments/psa-api-test/initial_attestation/${TS_ENVIRONMENT_LINUX}"
+PSA_API_TESTS += "deployments/psa-api-test/crypto/${TS_ENVIRONMENT_LINUX}"
+
+# ffa-debugfs-mod provides arm_ffa_user.h needed by psa-arch-tests source-code
+DEPENDS += "ffa-debugfs-mod"
+
+do_configure:append() {
+    for PSA_API_TEST in ${PSA_API_TESTS}; do
+        cmake \
+	    -DSYSROOT_YOCTO=${RECIPE_SYSROOT} \
+	    -S ${S}/$PSA_API_TEST -B "${B}/$PSA_API_TEST"
+    done
+}
+
+do_compile:append() {
+    for PSA_API_TEST in ${PSA_API_TESTS}; do
+        cmake --build "${B}/$PSA_API_TEST"
+    done
+}
+
+do_install:append() {
+    for PSA_API_TEST in ${PSA_API_TESTS}; do
+        install -d -m 0755 ${D}${libdir}/${PSA_API_TEST}
+        install -d -m 0755 ${D}${includedir}/${PSA_API_TEST}
+        psafile_fullpath=`ls ${B}/${PSA_API_TEST}/psa-*`
+        psafile_filename="`basename -s .bin ${psafile_fullpath}`"
+        install -D -p -m 0755 ${psafile_fullpath} ${D}/${bindir}/${psafile_filename}
+   done
+   cp -rf ${B}/${PSA_API_TEST}/libts_install/arm-linux/lib/*.so* ${D}/${libdir}
+}
+
+FILES:${PN}-dev = "${includedir}/deployments/psa-api-test/"
+INSANE_SKIP:${PN}-psa-api-tests += "rpaths dev-so buildpaths"
+PACKAGES += "${PN}-psa-api-tests"
+FILES:${PN}-psa-api-tests = "${libdir} ${bindir}"
diff --git a/meta-arm/meta-arm-bsp/wic/core-image-minimal.corstone500.wks b/meta-arm/meta-arm-bsp/wic/core-image-minimal.corstone500.wks
new file mode 100644
index 0000000..0ab359c
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/wic/core-image-minimal.corstone500.wks
@@ -0,0 +1,12 @@
+# WIC partitioning for corstone500
+# Layout and maximum sizes (to be defined):
+#
+
+# Rawcopy of the FIP binary
+part --source rawcopy --sourceparams="file=fip.bin" --no-table --align 1 --fixed-size 1
+
+# Rawcopy of the kernel binary
+part --source rawcopy --sourceparams="file=zImage" --no-table --fixed-size 12
+
+# Rawcopy of the rootfs
+part --source rawcopy --sourceparams="file=${IMGDEPLOYDIR}/core-image-minimal-corstone500.squashfs" --no-table
diff --git a/meta-arm/meta-arm-bsp/wic/corstone1000-image.corstone1000.wks b/meta-arm/meta-arm-bsp/wic/corstone1000-image.corstone1000.wks
new file mode 100644
index 0000000..c58d7d6
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/wic/corstone1000-image.corstone1000.wks
@@ -0,0 +1,15 @@
+# WIC partitioning for corstone1000
+# Layout and maximum sizes (to be defined):
+#
+
+part --source rawcopy --sourceparams="file=bl2_signed.bin" --align 1 --no-table --fixed-size 100k
+part --source rawcopy --sourceparams="file=bl2_signed.bin" --align 1 --no-table --fixed-size 100k
+
+part --source rawcopy --sourceparams="file=tfm_s_signed.bin" --align 1 --no-table --fixed-size 376k
+part --source rawcopy --sourceparams="file=tfm_s_signed.bin" --align 1 --no-table --fixed-size 376k
+
+# Rawcopy of the FIP binary
+part --source rawcopy --sourceparams="file=signed_fip-corstone1000.bin" --align 1 --no-table --fixed-size 2
+
+# Rawcopy of kernel with initramfs
+part --source rawcopy --sourceparams="file=Image-initramfs-${MACHINE}.bin" --no-table --fixed-size 12
diff --git a/meta-arm/meta-arm-bsp/wic/fvp-base.wks b/meta-arm/meta-arm-bsp/wic/fvp-base.wks
new file mode 100644
index 0000000..8399d04
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/wic/fvp-base.wks
@@ -0,0 +1,3 @@
+# For fvp-base* machines we just need to populate the rootfs partition
+
+part / --source rootfs --ondisk sda --fstype=ext4 --label root --align 1024 --extra-space 100
diff --git a/meta-arm/meta-arm-bsp/wic/n1sdp-efidisk.wks b/meta-arm/meta-arm-bsp/wic/n1sdp-efidisk.wks
new file mode 100644
index 0000000..b131dd8
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/wic/n1sdp-efidisk.wks
@@ -0,0 +1,9 @@
+# short-description: Create an EFI disk image
+# long-description: Creates a partitioned EFI disk image that the user
+# can directly dd to boot media. Uses a custom grub.cfg file to configure the boot.
+
+part /boot --source bootimg-efi --sourceparams="loader=grub-efi" --ondisk sda --label msdos --active --align 1024
+
+part / --source rootfs --ondisk sda --fstype=ext4 --label root --align 1024 --uuid=6a60524d-061d-454a-bfd1-38989910eccd
+
+bootloader --ptable gpt --configfile="n1sdp-grub.cfg" --timeout=5
diff --git a/meta-arm/meta-arm-bsp/wic/n1sdp-grub.cfg b/meta-arm/meta-arm-bsp/wic/n1sdp-grub.cfg
new file mode 100644
index 0000000..7323ec7
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/wic/n1sdp-grub.cfg
@@ -0,0 +1,18 @@
+set debug="loader,mm"
+set term="vt100"
+set default="0"
+set timeout="5"
+
+menuentry 'Arm reference image boot on N1SDP (ACPI)' {
+        linux /Image earlycon=pl011,0x2A400000 console=ttyAMA0,115200 root=PARTUUID=6a60524d-061d-454a-bfd1-38989910eccd rootwait rootfstype=ext4 acpi=force
+}
+
+menuentry 'Arm reference image boot on Single-Chip N1SDP (Device Tree)' {
+        devicetree /n1sdp-single-chip.dtb
+        linux /Image earlycon=pl011,0x2A400000 console=ttyAMA0,115200 root=PARTUUID=6a60524d-061d-454a-bfd1-38989910eccd rootwait rootfstype=ext4
+}
+
+menuentry 'Arm reference image boot on Multi-Chip N1SDP (Device Tree)' {
+        devicetree /n1sdp-multi-chip.dtb
+        linux /Image earlycon=pl011,0x2A400000 console=ttyAMA0,115200 root=PARTUUID=6a60524d-061d-454a-bfd1-38989910eccd rootwait rootfstype=ext4
+}
diff --git a/meta-arm/meta-arm-bsp/wic/sgi575-efidisk.wks b/meta-arm/meta-arm-bsp/wic/sgi575-efidisk.wks
new file mode 100644
index 0000000..d4c79d3
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/wic/sgi575-efidisk.wks
@@ -0,0 +1,11 @@
+# short-description: Create an EFI disk image
+# long-description: Creates a partitioned EFI disk image that the user
+# can directly dd to boot media. Uses a custom grub.cfg file to configure the boot.
+
+part /boot --source bootimg-efi --sourceparams="loader=grub-efi" --ondisk sda --label msdos --active --align 1024
+
+part / --source rootfs --ondisk sda --fstype=ext4 --label root --align 1024 --uuid=9c53a91b-e182-4ff1-aeac-6ee2c432ae94
+
+part swap --ondisk sda --size 44 --label swap1 --fstype=swap
+
+bootloader --ptable gpt --configfile="sgi575-grub.cfg" --timeout=5
diff --git a/meta-arm/meta-arm-bsp/wic/sgi575-grub.cfg b/meta-arm/meta-arm-bsp/wic/sgi575-grub.cfg
new file mode 100644
index 0000000..a176389
--- /dev/null
+++ b/meta-arm/meta-arm-bsp/wic/sgi575-grub.cfg
@@ -0,0 +1,9 @@
+set debug="loader,mm"
+set term="vt100"
+set default="0"
+set timeout="1"
+
+menuentry 'Arm reference image boot on sgi575' {
+        linux /Image acpi=force console=ttyAMA0,115200 ip=dhcp root=PARTUUID=9c53a91b-e182-4ff1-aeac-6ee2c432ae94 rootwait verbose debug
+}
+
diff --git a/meta-arm/meta-arm-toolchain/README.md b/meta-arm/meta-arm-toolchain/README.md
new file mode 100644
index 0000000..9ee05ed
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/README.md
@@ -0,0 +1,114 @@
+meta-arm-toolchain Yocto Layer
+==============================
+
+This layer contains recipes for GNU Arm toolchains which could either be built
+from source or pre-built toolchain binaries.
+
+Information regarding contributing, reporting bugs, etc can be found in the
+top-level meta-arm readme file.
+
+Source Arm toolchain for Linux development
+------------------------------------------
+
+Recipes for GNU Arm toolchain built from source are provided under
+``recipes-devtools/gcc/``. In order to use Arm toolchain instead of OE core
+toolchain, one just needs to override ``GCCVERSION`` in corresponding distro
+conf file.
+
+-  Eg. to use GNU Arm toolchain version ``9.2``
+   GCCVERSION = "arm-9.2"
+
+Pre-built Arm toolchain for Linux development
+---------------------------------------------
+
+Recipes for pre-built GNU Arm toolchain for Linux development are provided under
+``recipes-devtools/external-arm-toolchain/``.
+
+external-arm-toolchain.bb
+~~~~~~~~~~~~~~~~~~~~~~~~~
+
+This recipe provides support for pre-built GNU toolchains targeting processors
+from the Arm Cortex-A family and implementing the Arm A-profile architecture.
+
+Usage
+^^^^^
+
+In order to use any of pre-built Arm toolchain versions (8.2, 8.3, 9.2 and so
+on), a user needs to download and untar tool-set on host machine at a particular
+installation path eg: ``/opt/toolchain/``. Then user needs to specify following
+in ``conf/local.conf`` in order to replace OE toolchain with pre-built GNU-A
+toolchain:
+
+TCMODE = "external-arm"
+EXTERNAL_TOOLCHAIN = "<path-to-the-toolchain>"
+
+-  Eg. for AArch64 (eg. qemuarm64 machine in poky distro)
+   EXTERNAL_TOOLCHAIN = "\
+     <installation-path>/gcc-arm-9.2-2019.12-x86_64-aarch64-none-linux-gnu \
+   "
+
+-  Eg. for AArch32 (eg. qemuarm machine in poky distro)
+   EXTERNAL_TOOLCHAIN = "\
+     <installation-path>/gcc-arm-9.2-2019.12-x86_64-arm-none-linux-gnueabihf \
+   "
+
+Supported distros and machines
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Since this pre-built GNU-A tool-set simply replaces OE toolchain, so it is
+meant to be distro and machine agnostic as long as one is cross-compiling for
+Arm A-profile architecture.
+
+Tested distro and machines (for zeus stable release):
+1.  Distro: poky; machines: qemuarm and qemuarm64 (build and boot tested)
+2.  Distro: RPB; machines: dragonboard-410c (build and boot tested)
+3.  Distro: world; machines: qemuarm and qemuarm64. Build tested for following
+    layers:
+    - poky/meta
+    - poky/meta-poky
+    - poky/meta-yocto-bsp
+    - meta-openembedded/meta-oe
+    - meta-openembedded/meta-python
+    - meta-openembedded/meta-networking
+
+SDK support
+^^^^^^^^^^^
+
+Pre-built toochain provides support to build OE SDK which has been tested using
+following commands:
+
+$ bitbake core-image-base -c populate_sdk
+$ bitbake core-image-base -c testsdk
+
+Note: Currently generated SDK only uses glibc provided by pre-built toolchain.
+      The cross compiler, binutils, gdb/gdbserver etc. are built from source.
+      This is something we would like to improve in future in order to package
+      most of the components from pre-built toolchain instead.
+
+Pre-built Arm toolchain for bare-metal development
+--------------------------------------------------
+
+Recipes for pre-built GNU Arm toolchain for bare-metal development are provided
+under ``recipes-devtools/external-arm-toolchain/``.
+
+gcc-arm-none-eabi_<version>.bb
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+This recipe provides support for pre-built GNU Arm Embedded toolchain for
+bare-metal software development on devices based on 32-bit Arm Cortex-A,
+Cortex-R and Cortex-M processors.
+
+Supported version: 9-2019-q4-major
+
+gcc-aarch64-none-elf_<version>.bb
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+This recipe provides support for pre-built GNU Arm toolchain for bare-metal
+software development on devices based on 64-bit Arm Cortex-A processors.
+
+Supported version: 9.2-2019.12
+
+Layer maintainer(s)
+-------------------
+* Sumit Garg <sumit.garg@linaro.org>
+* Denys Dmytriyenko <denis@denix.org>
diff --git a/meta-arm/meta-arm-toolchain/conf/distro/include/external-arm-toolchain-versions.inc b/meta-arm/meta-arm-toolchain/conf/distro/include/external-arm-toolchain-versions.inc
new file mode 100644
index 0000000..244de26
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/conf/distro/include/external-arm-toolchain-versions.inc
@@ -0,0 +1,132 @@
+def eat_run(d, cmd, *args):
+    import bb.process
+    import subprocess
+
+    topdir = d.getVar('TOPDIR', True)
+    toolchain_path = d.getVar('EXTERNAL_TOOLCHAIN', True)
+    if not toolchain_path:
+        return 'UNKNOWN', 'UNKNOWN'
+
+    target_prefix = d.getVar('TARGET_PREFIX', True)
+    path = os.path.join(toolchain_path, 'bin', target_prefix + cmd)
+    args = [path] + list(args)
+
+    return bb.process.run(args, cwd=topdir, stderr=subprocess.PIPE)
+
+def eat_get_version(d):
+    try:
+        stdout, stderr = eat_run(d, 'gcc', '-v')
+    except bb.process.CmdError as exc:
+        bb.error('Failed to obtain external Arm toolchain version: %s' % exc)
+        return 'UNKNOWN'
+    else:
+        last_line = stderr.splitlines()[-1]
+        return last_line
+
+# Extract the YYYY.MM version
+def eat_get_main_version(d):
+    version = eat_get_version(d)
+    bb.debug(2, 'Trying for parse version info from: %s' % version)
+    if version != 'UNKNOWN':
+        if version.split()[4] == '(GNU':
+            # gcc version 9.2.1 20191025 (GNU Toolchain for the A-profile Architecture 9.2-2019.12 (arm-9.10))
+            # gcc version 8.2.1 20180802 (GNU Toolchain for the A-profile Architecture 8.2-2018.11 (arm-rel-8.26))
+            return version.split()[10].split('-')[1]
+        elif version.split()[3] == '(GNU':
+            # gcc version 8.3.0 (GNU Toolchain for the A-profile Architecture 8.3-2019.03 (arm-rel-8.36))
+            return version.split()[9].split('-')[1]
+        else:
+            bb.error('Failed to parse external Arm toolchain version from: %s' % version)
+    else:
+        return version
+
+# Extract the x.y.z version from 'gcc version 4.9.1'
+def eat_get_gcc_version(d):
+    version = eat_get_version(d)
+    if version != 'UNKNOWN':
+        return version.split()[2]
+    else:
+        return version
+
+def eat_get_libc_version(d):
+    import os,bb
+    import subprocess
+
+    syspath = bb.data.expand('${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}', d)
+    if not syspath:
+        return 'UNKNOWN'
+
+    topdir = d.getVar('TOPDIR', True)
+    lddpath = syspath + '/libc/usr/bin/ldd'
+
+    if os.path.exists(lddpath):
+        cmd = '/bin/sh ' + lddpath + ' --version'
+        try:
+            stdout, stderr = bb.process.run(cmd, cwd=topdir, stderr=subprocess.PIPE)
+        except bb.process.CmdError as exc:
+            bb.error('Failed to obtain external Arm libc version: %s' % exc)
+            return 'UNKNOWN'
+        else:
+            first_line = stdout.splitlines()[0]
+            return first_line.split()[2]
+
+    return 'UNKNOWN'
+
+def eat_get_kernel_version(d):
+    import os,bb
+    syspath = bb.data.expand('${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}', d)
+    if not syspath:
+        return 'UNKNOWN'
+
+    vf = syspath + '/libc/usr/include/linux/version.h'
+
+    try:
+        f = open(vf, 'r')
+    except (OSError, IOError):
+        return 'UNKNOWN'
+
+    l = f.readlines();
+    f.close();
+    for s in l:
+        if s.find('LINUX_VERSION_CODE') > 0:
+            ver = int(s.split()[2])
+            maj = ver / 65536
+            ver = ver % 65536
+            min = ver / 256
+            ver = ver % 256
+            return str(maj)+'.'+str(min)+'.'+str(ver)
+    return 'UNKNOWN'
+
+def eat_get_gdb_version(d):
+    try:
+        stdout, stderr = eat_run(d, 'gdb', '-v')
+    except bb.process.CmdError:
+        return 'UNKNOWN'
+    else:
+        first_line = stdout.splitlines()[0]
+        return first_line.split()[-1]
+
+def eat_get_bfd_version(d):
+    try:
+        stdout, stderr = eat_run(d, 'as', '--version')
+    except bb.process.CmdError:
+        return 'UNKNOWN'
+    else:
+        first_line = stdout.splitlines()[0]
+        return first_line.split()[-1]
+
+python external_arm_toolchain_version_handler () {
+    if not isinstance(e, bb.event.ConfigParsed):
+        return
+    d = e.data
+    ld = d.createCopy()
+    ld.finalize()
+
+    d.setVar('EAT_VER_MAIN', eat_get_main_version(ld))
+    d.setVar('EAT_VER_GCC', eat_get_gcc_version(ld))
+    d.setVar('EAT_VER_LIBC', eat_get_libc_version(ld))
+    d.setVar('EAT_VER_KERNEL', eat_get_kernel_version(ld))
+    d.setVar('EAT_VER_GDB', eat_get_gdb_version(ld))
+    d.setVar('EAT_VER_BFD', eat_get_bfd_version(ld))
+}
+addhandler external_arm_toolchain_version_handler
diff --git a/meta-arm/meta-arm-toolchain/conf/distro/include/tcmode-external-arm.inc b/meta-arm/meta-arm-toolchain/conf/distro/include/tcmode-external-arm.inc
new file mode 100644
index 0000000..178fb71
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/conf/distro/include/tcmode-external-arm.inc
@@ -0,0 +1,121 @@
+#
+# Configuration to use an external Arm binary toolchain
+#
+
+EXTERNAL_TOOLCHAIN ?= "/usr/local/arm-binary-toolchain/${TARGET_ARCH}"
+
+TOOLCHAIN_PATH_ADD = "${EXTERNAL_TOOLCHAIN}/bin:"
+PATH =. "${TOOLCHAIN_PATH_ADD}"
+
+EAT_TARGET_SYS:arm ?= "${@ 'arm-none-linux-gnueabihf' if os.path.exists('${EXTERNAL_TOOLCHAIN}/bin/arm-none-linux-gnueabihf-gcc') else 'arm-linux-gnueabihf'}"
+EAT_TARGET_SYS:aarch64 ?= "${@ 'aarch64-none-linux-gnu' if os.path.exists('${EXTERNAL_TOOLCHAIN}/bin/aarch64-none-linux-gnu-gcc') else 'aarch64-linux-gnu'}"
+EAT_TARGET_SYS = "${TARGET_SYS}"
+TARGET_PREFIX = "${EAT_TARGET_SYS}-"
+
+EAT_LIBDIR:arm = "lib"
+EAT_LIBDIR:aarch64 = "lib64"
+
+GCCMULTILIB:forcevariable = "--disable-multilib"
+IMAGE_LINGUAS:forcevariable = ""
+
+# Blacklist toolchain recipes as a belt-and-suspenders way to use the external toolchain
+SKIP_RECIPE[glibc] = "Using external toolchain"
+SKIP_RECIPE[libgcc] = "Using external toolchain"
+SKIP_RECIPE[gcc-cross] = "Using external toolchain"
+SKIP_RECIPE[gcc-cross-aarch64] = "Using external toolchain"
+SKIP_RECIPE[gcc-cross-arm] = "Using external toolchain"
+SKIP_RECIPE[gcc-runtime] = "Using external toolchain"
+SKIP_RECIPE[gcc-sanitizers] = "Using external toolchain"
+
+PREFERRED_PROVIDER_linux-libc-headers = "external-arm-toolchain"
+PREFERRED_PROVIDER_linux-libc-headers-dev = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/${TARGET_PREFIX}gcc = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/${TARGET_PREFIX}gcc-initial = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/${TARGET_PREFIX}g++ = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/${TARGET_PREFIX}binutils = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/${TARGET_PREFIX}libc-for-gcc = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/${TARGET_PREFIX}compilerlibs = "external-arm-toolchain"
+PREFERRED_PROVIDER_glibc = "external-arm-toolchain"
+PREFERRED_PROVIDER_libgcc = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/libc = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/libc-locale = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/libintl = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/libiconv = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/crypt = "external-arm-toolchain"
+PREFERRED_PROVIDER_glibc-thread-db = "external-arm-toolchain"
+PREFERRED_PROVIDER_glibc-mtrace = "external-arm-toolchain"
+PREFERRED_PROVIDER_libc-mtrace = "external-arm-toolchain"
+PREFERRED_PROVIDER_virtual/linux-libc-headers = "external-arm-toolchain"
+
+PREFERRED_PROVIDER_gcc-cross-canadian-${TRANSLATED_TARGET_ARCH} ?= "external-arm-sdk-toolchain-${TRANSLATED_TARGET_ARCH}"
+PREFERRED_PROVIDER_binutils-cross-canadian-${TRANSLATED_TARGET_ARCH} ?= "external-arm-sdk-toolchain-${TRANSLATED_TARGET_ARCH}"
+PREFERRED_PROVIDER_gdb-cross-canadian-${TRANSLATED_TARGET_ARCH} ?= "external-arm-sdk-toolchain-${TRANSLATED_TARGET_ARCH}"
+
+TOOLCHAIN_OPTIONS = " --sysroot=${STAGING_DIR_HOST}"
+
+DISTRO_FEATURES_LIBC = "ipv4 ipv6 libc-backtrace libc-big-macros libc-bsd libc-cxx-tests libc-catgets libc-crypt \
+			libc-crypt-ufc libc-db-aliases libc-envz libc-fcvt libc-fmtmsg libc-fstab libc-ftraverse \
+			libc-getlogin libc-idn libc-inet-anl libc-libm libc-libm-big \
+			libc-locales libc-locale-code libc-charsets \
+			libc-memusage libc-nis libc-nsswitch libc-rcmd libc-rtld-debug libc-spawn libc-streams libc-sunrpc \
+			libc-utmp libc-utmpx libc-wordexp libc-posix-clang-wchar libc-posix-regexp libc-posix-regexp-glibc \
+			libc-posix-wchar-io"
+
+ENABLE_BINARY_LOCALE_GENERATION = "0"
+GLIBC_INTERNAL_USE_BINARY_LOCALE = "precompiled"
+LIBCOVERRIDE = ":libc-glibc"
+LIBC_DEPENDENCIES:remove = "glibc-gconv-cp1252 glibc-gconv-ibm850 glibc-gconv-iso8859-1 glibc-gconv-iso8859-15 glibc-localedata-i18n"
+
+ERROR_QA[type] ?= "list"
+python toolchain_metadata_setup () {
+    import subprocess
+    if not isinstance(e, bb.event.ConfigParsed):
+        return
+
+    d = e.data
+    l = d.createCopy()
+    l.finalize()
+    oe_import(l)
+
+    external_toolchain = l.getVar('EXTERNAL_TOOLCHAIN', True)
+    if not external_toolchain or external_toolchain == 'UNDEFINED':
+        bb.fatal("Error: EXTERNAL_TOOLCHAIN must be set to the path to your arm toolchain")
+
+    if not os.path.exists(external_toolchain):
+        bb.fatal("Error: EXTERNAL_TOOLCHAIN path '%s' does not exist" % external_toolchain)
+
+    # The external toolchain may not have been built with the oe-core preferred
+    # gnu hash setting, so ensure that the corresponding sanity check is not an error.
+    error_qa = oe.data.typed_value('ERROR_QA', l)
+    if 'ldflags' in error_qa:
+        error_qa.remove('ldflags')
+        d.setVar('ERROR_QA', ' '.join(error_qa))
+}
+addhandler toolchain_metadata_setup
+
+def populate_toolchain_links(d):
+    import errno
+    import os
+    from glob import glob
+
+    d = d.createCopy()
+    d.finalize()
+
+    pattern = bb.data.expand('${EXTERNAL_TOOLCHAIN}/bin/${TARGET_PREFIX}*', d)
+    files = glob(pattern)
+    if not files:
+        bb.fatal("Unable to populate toolchain binary symlinks")
+
+    bindir = d.getVar('STAGING_BINDIR_TOOLCHAIN', True)
+    bb.mkdirhier(bindir)
+    for f in files:
+        base = os.path.basename(f)
+        newpath = os.path.join(bindir, base)
+        try:
+            os.symlink(f, newpath)
+        except OSError as exc:
+            if exc.errno == errno.EEXIST:
+                break
+            bb.fatal("Unable to populate toolchain binary symlink for %s: %s" % (newpath, exc))
+
+require conf/distro/include/external-arm-toolchain-versions.inc
diff --git a/meta-arm/meta-arm-toolchain/conf/layer.conf b/meta-arm/meta-arm-toolchain/conf/layer.conf
new file mode 100644
index 0000000..3cb8b06
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/conf/layer.conf
@@ -0,0 +1,12 @@
+BBPATH .= ":${LAYERDIR}"
+BBFILES += "\
+            ${LAYERDIR}/recipes-*/*/*.bb \
+            ${LAYERDIR}/recipes-*/*/*.bbappend \
+           "
+
+BBFILE_COLLECTIONS += "arm-toolchain"
+BBFILE_PATTERN_arm-toolchain := "^${LAYERDIR}/"
+BBFILE_PRIORITY_arm-toolchain = "5"
+
+LAYERDEPENDS_arm-toolchain = "core"
+LAYERSERIES_COMPAT_arm-toolchain = "kirkstone"
diff --git a/meta-arm/meta-arm-toolchain/recipes-bsp/grub/grub%.bbappend b/meta-arm/meta-arm-toolchain/recipes-bsp/grub/grub%.bbappend
new file mode 100644
index 0000000..37383d4
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-bsp/grub/grub%.bbappend
@@ -0,0 +1 @@
+require ${@oe.utils.conditional('TCMODE', 'external-arm', 'grub-external-arm.inc', '', d)}
diff --git a/meta-arm/meta-arm-toolchain/recipes-bsp/grub/grub-external-arm.inc b/meta-arm/meta-arm-toolchain/recipes-bsp/grub/grub-external-arm.inc
new file mode 100644
index 0000000..dc260be
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-bsp/grub/grub-external-arm.inc
@@ -0,0 +1,5 @@
+# grub uses --target triplet to find binutils binaries such as objcopy
+# Since external-arm toolchain uses aarch64-none-linux-gnu triplet,
+# OE-defined TARGET_SYS differs from EAT_TARGET_SYS used by external-arm
+# toolchain, grub needs passing the correct --target to configure script
+CONFIGUREOPTS:append:class-target = " --target=${EAT_TARGET_SYS}"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/androidclang_r416183b.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/androidclang_r416183b.bb
new file mode 100644
index 0000000..963fd60
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/androidclang_r416183b.bb
@@ -0,0 +1,52 @@
+# SPDX-License-Identifier: MIT
+#
+# Copyright (c) 2021 Arm Limited
+#
+
+SUMMARY = "Android Clang compiler"
+DESCRIPTION = "Android Clang compiler, version r416183b. This is based on Clang 12.0.5 \
+Intended usage is to build kernel images that match the output of the Android (hermetic) \
+build system"
+
+LICENSE = "MIT"
+
+LIC_FILES_CHKSUM = "file://MODULE_LICENSE_MIT;md5=d41d8cd98f00b204e9800998ecf8427e"
+
+ANDROID_CLANG_VERSION = "clang-r416183b"
+ANDROID_CLANG_HASH = "bd96dfe349c962681f0e5388af874c771ef96670"
+
+COMPATIBLE_HOST = "x86_64.*-linux"
+
+SRC_URI = "https://android.googlesource.com/platform/prebuilts/clang/host/linux-x86/+archive/${ANDROID_CLANG_HASH}/${ANDROID_CLANG_VERSION}.tar.gz;subdir=${ANDROID_CLANG_VERSION}"
+
+# We need to set the checksum to "ignore" because the tarball is dynamically generated and has a new checksum every time
+# (the contents are the same, but the time stamp differs)
+BB_STRICT_CHECKSUM = "ignore"
+
+S = "${WORKDIR}/${ANDROID_CLANG_VERSION}"
+
+FILES:${PN} = "${libexecdir} ${bindir}"
+
+do_install() {
+    install -d ${D}${libexecdir}/${ANDROID_CLANG_VERSION}/
+
+    cp --no-preserve=ownership -r ${S}/. ${D}${libexecdir}/${ANDROID_CLANG_VERSION}/
+    # Strip bad RPATHs in the embedded python3
+    chrpath -d ${D}${libexecdir}/${ANDROID_CLANG_VERSION}/python3/lib/python*/lib-dynload/*.so
+
+    install -d ${D}${bindir}
+    # Symlink all executables into bindir
+    for f in ${D}${libexecdir}/${ANDROID_CLANG_VERSION}/bin/*; do
+        ln -rs $f ${D}${bindir}/$(basename $f)
+    done
+}
+
+INHIBIT_DEFAULT_DEPS = "1"
+
+INSANE_SKIP:${PN} = "already-stripped libdir staticdev file-rdeps arch dev-so"
+
+INHIBIT_SYSROOT_STRIP = "1"
+INHIBIT_PACKAGE_STRIP = "1"
+INHIBIT_PACKAGE_DEBUG_SPLIT = "1"
+
+BBCLASSEXTEND = "native nativesdk"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/arm-binary-toolchain.inc b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/arm-binary-toolchain.inc
new file mode 100644
index 0000000..528b006
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/arm-binary-toolchain.inc
@@ -0,0 +1,26 @@
+INHIBIT_DEFAULT_DEPS = "1"
+
+FILES:${PN} = "${libexecdir} ${bindir}"
+
+BINNAME = "${@d.getVar("BPN").strip("gcc-")}"
+
+do_install() {
+    install -d ${D}${bindir} ${D}${libexecdir}/${BPN}/
+    cp -r ${S}/. ${D}${libexecdir}/${BPN}
+
+    # Symlink all executables into bindir
+    for f in ${D}${libexecdir}/${BPN}/bin/*; do
+        ln -rs $f ${D}${bindir}/$(basename $f)
+    done
+}
+
+INSANE_SKIP:${PN} = "already-stripped libdir staticdev file-rdeps arch dev-so"
+
+INHIBIT_SYSROOT_STRIP = "1"
+INHIBIT_PACKAGE_STRIP = "1"
+INHIBIT_PACKAGE_DEBUG_SPLIT = "1"
+
+# Need to mark these as private until do_package's soname-finder only looks in $libdir
+PRIVATE_LIBS = "libgcc_s.so.1 libstdc++.so.6"
+
+BBCLASSEXTEND = "native nativesdk"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/external-arm-sdk-toolchain.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/external-arm-sdk-toolchain.bb
new file mode 100644
index 0000000..138245d
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/external-arm-sdk-toolchain.bb
@@ -0,0 +1,143 @@
+inherit cross-canadian
+
+require license.inc
+
+PN = "external-arm-sdk-toolchain-${TARGET_ARCH}"
+BPN = "external-arm-sdk-toolchain"
+PV = "${EAT_VER_MAIN}"
+
+INHIBIT_PACKAGE_DEBUG_SPLIT = "1"
+INHIBIT_PACKAGE_STRIP = "1"
+INHIBIT_SYSROOT_STRIP = "1"
+INHIBIT_DEFAULT_DEPS = "1"
+EXCLUDE_FROM_SHLIBS = "1"
+
+LIC_FILES_CHKSUM = "file://${COMMON_LICENSE_DIR}/MIT;md5=0835ade698e0bcf8506ecda2f7b4f302"
+
+# Skip packaging QA checks for prebuilt binaries
+INSANE_SKIP:gcc-cross-canadian-${TRANSLATED_TARGET_ARCH} = "dev-so staticdev file-rdeps libdir"
+INSANE_SKIP:gdb-cross-canadian-${TRANSLATED_TARGET_ARCH} = "dev-so file-rdeps"
+INSANE_SKIP:binutils-cross-canadian-${TRANSLATED_TARGET_ARCH} = "dev-so file-rdeps"
+
+# Skip file dependencies in RPM for prebuilt binaries
+SKIP_FILEDEPS = "1"
+
+PROVIDES = "\
+	gcc-cross-canadian-${TRANSLATED_TARGET_ARCH} \
+	gdb-cross-canadian-${TRANSLATED_TARGET_ARCH} \
+	binutils-cross-canadian-${TRANSLATED_TARGET_ARCH} \
+"
+
+PACKAGES = "\
+	gcc-cross-canadian-${TRANSLATED_TARGET_ARCH} \
+	gdb-cross-canadian-${TRANSLATED_TARGET_ARCH} \
+	binutils-cross-canadian-${TRANSLATED_TARGET_ARCH} \
+"
+
+# Adjust defaults in line with external toolchain
+bindir = "${exec_prefix}/bin"
+libdir = "${exec_prefix}/lib"
+libexecdir = "${exec_prefix}/libexec"
+datadir = "${exec_prefix}/share"
+gcclibdir = "${libdir}/gcc"
+
+FILES:gcc-cross-canadian-${TRANSLATED_TARGET_ARCH} = "\
+	${prefix}/${EAT_TARGET_SYS}/lib/libstdc++.* \
+	${prefix}/${EAT_TARGET_SYS}/lib/libgcc_s.* \
+	${prefix}/${EAT_TARGET_SYS}/lib/libsupc++.* \
+	${prefix}/${EAT_TARGET_SYS}/include \
+	${gcclibdir}/${EAT_TARGET_SYS}/${EAT_VER_GCC}/* \
+	${bindir}/${TARGET_PREFIX}gcov \
+	${bindir}/${TARGET_PREFIX}gcc* \
+	${bindir}/${TARGET_PREFIX}g++ \
+	${bindir}/${TARGET_PREFIX}cpp \
+	${libexecdir}/* \
+"
+
+FILES:gdb-cross-canadian-${TRANSLATED_TARGET_ARCH} = "\
+	${bindir}/${TARGET_PREFIX}gdb* \
+	${datadir}/gdb/* \
+"
+
+FILES:binutils-cross-canadian-${TRANSLATED_TARGET_ARCH} = "\
+	${prefix}/${EAT_TARGET_SYS}/bin/ld* \
+	${prefix}/${EAT_TARGET_SYS}/bin/objcopy \
+	${prefix}/${EAT_TARGET_SYS}/bin/strip \
+	${prefix}/${EAT_TARGET_SYS}/bin/nm \
+	${prefix}/${EAT_TARGET_SYS}/bin/ranlib \
+	${prefix}/${EAT_TARGET_SYS}/bin/as \
+	${prefix}/${EAT_TARGET_SYS}/bin/ar \
+	${prefix}/${EAT_TARGET_SYS}/bin/objdump \
+	${prefix}/${EAT_TARGET_SYS}/lib/ldscripts/* \
+	${bindir}/${TARGET_PREFIX}ld* \
+	${bindir}/${TARGET_PREFIX}addr2line \
+	${bindir}/${TARGET_PREFIX}objcopy \
+	${bindir}/${TARGET_PREFIX}readelf \
+	${bindir}/${TARGET_PREFIX}strip \
+	${bindir}/${TARGET_PREFIX}nm \
+	${bindir}/${TARGET_PREFIX}ranlib \
+	${bindir}/${TARGET_PREFIX}gprof \
+	${bindir}/${TARGET_PREFIX}as \
+	${bindir}/${TARGET_PREFIX}c++filt \
+	${bindir}/${TARGET_PREFIX}ar \
+	${bindir}/${TARGET_PREFIX}strings \
+	${bindir}/${TARGET_PREFIX}objdump \
+	${bindir}/${TARGET_PREFIX}size \
+"
+
+DESCRIPTION:gcc-cross-canadian-${TRANSLATED_TARGET_ARCH} = "The GNU cc and gcc C compilers"
+DESCRIPTION:gdb-cross-canadian-${TRANSLATED_TARGET_ARCH} = "gdb - GNU debugger"
+DESCRIPTION:binutils-cross-canadian-${TRANSLATED_TARGET_ARCH} = "A GNU collection of binary utilities"
+
+LICENSE:gcc-cross-canadian-${TRANSLATED_TARGET_ARCH} = "${EAT_GCC_LICENSE}"
+LICENSE:gdb-cross-canadian-${TRANSLATED_TARGET_ARCH} = "${EAT_GDB_LICENSE}"
+LICENSE:binutils-cross-canadian-${TRANSLATED_TARGET_ARCH} = "${EAT_BFD_LICENSE}"
+
+PKGV:gcc-cross-canadian-${TRANSLATED_TARGET_ARCH} = "${EAT_VER_GCC}"
+PKGV:gdb-cross-canadian-${TRANSLATED_TARGET_ARCH} = "${EAT_VER_GDB}"
+PKGV:binutils-cross-canadian-${TRANSLATED_TARGET_ARCH} = "${EAT_VER_BFD}"
+
+do_install() {
+	install -d ${D}${prefix}/${EAT_TARGET_SYS}/bin
+	install -d ${D}${prefix}/${EAT_TARGET_SYS}/lib
+	install -d ${D}${prefix}/${EAT_TARGET_SYS}/include
+	install -d ${D}${bindir}
+	install -d ${D}${libdir}
+	install -d ${D}${prefix}/${EAT_TARGET_SYS}/lib/ldscripts
+	install -d ${D}${libexecdir}
+	install -d ${D}${datadir}/gdb
+	install -d ${D}${gcclibdir}/${EAT_TARGET_SYS}/${EAT_VER_GCC}/include
+
+	CP_ARGS="-Prf --preserve=mode,timestamps --no-preserve=ownership"
+
+	# gcc
+	for i in libstdc++.* libgcc_s.* libsupc++.*; do
+		cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/${EAT_LIBDIR}/$i ${D}${prefix}/${EAT_TARGET_SYS}/lib
+	done
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/include/* ${D}${prefix}/${EAT_TARGET_SYS}/include
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/lib/gcc/${EAT_TARGET_SYS}/${EAT_VER_GCC}/* ${D}${gcclibdir}/${EAT_TARGET_SYS}/${EAT_VER_GCC}
+	for i in gcov gcc* g++ cpp; do
+		cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/bin/${TARGET_PREFIX}$i ${D}${bindir}
+	done
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/libexec/* ${D}${libexecdir}
+
+	# gdb
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/bin/${TARGET_PREFIX}gdb* ${D}${bindir}
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/share/gdb/* ${D}${datadir}/gdb/
+
+	# binutils
+	for i in ld* objcopy strip nm ranlib as ar objdump; do
+		cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/bin/$i ${D}${prefix}/${EAT_TARGET_SYS}/bin
+	done
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/lib/ldscripts/* ${D}${prefix}/${EAT_TARGET_SYS}/lib/ldscripts
+	for i in ld* addr2line objcopy readelf strip nm ranlib gprof as c++filt ar strings objdump size; do
+		cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/bin/${TARGET_PREFIX}$i ${D}${bindir}
+	done
+}
+
+python () {
+    if not d.getVar("EAT_VER_MAIN", False):
+        raise bb.parse.SkipPackage("External ARM toolchain not configured (EAT_VER_MAIN not set).")
+    if d.getVar('TCLIBC', True) != "glibc":
+        raise bb.parse.SkipPackage("incompatible with %s" % d.getVar('TCLIBC', True))
+}
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/external-arm-toolchain.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/external-arm-toolchain.bb
new file mode 100644
index 0000000..dcc8ebf
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/external-arm-toolchain.bb
@@ -0,0 +1,673 @@
+require recipes-core/glibc/glibc-package.inc
+
+require license.inc
+
+INHIBIT_DEFAULT_DEPS = "1"
+
+LIC_FILES_CHKSUM = "\
+	file://${COMMON_LICENSE_DIR}/MIT;md5=0835ade698e0bcf8506ecda2f7b4f302 \
+	file://${COREBASE}/meta/COPYING.MIT;md5=3da9cfbcb788c80a0384361b4de20420 \
+"
+
+PROVIDES += "\
+	linux-libc-headers \
+	virtual/${TARGET_PREFIX}gcc \
+	virtual/${TARGET_PREFIX}g++ \
+	virtual/${TARGET_PREFIX}gcc-initial \
+	virtual/${TARGET_PREFIX}binutils \
+	virtual/${TARGET_PREFIX}libc-for-gcc \
+	virtual/${TARGET_PREFIX}compilerlibs \
+	virtual/libc \
+	virtual/libintl \
+	virtual/libiconv \
+	virtual/crypt \
+	glibc-mtrace \
+	glibc-thread-db \
+	glibc \
+	libc-mtrace \
+	gcc-runtime \
+	libgcc \
+	libg2c \
+	libg2c-dev \
+	libssp \
+	libssp-dev \
+	libssp-staticdev \
+	libgfortran \
+	libgfortran-dev \
+	libgfortran-staticdev \
+	libmudflap \
+	libmudflap-dev \
+	libgomp \
+	libgomp-dev \
+	libgomp-staticdev \
+	libitm \
+	libitm-dev \
+	libitm-staticdev \
+	libquadmath \
+	libquadmath-dev \
+	libquadmath-staticdev \
+	virtual/linux-libc-headers \
+	libgcov-staticdev \
+	virtual/libc-locale \
+"
+
+PV = "${EAT_VER_MAIN}"
+
+BINV = "${EAT_VER_GCC}"
+
+SRC_URI = "file://SUPPORTED"
+
+do_install() {
+	# Add stubs for files OE-core expects
+	install -d ${S}/nscd/
+	touch  ${S}/nscd/nscd.init
+	touch  ${S}/nscd/nscd.conf
+	touch  ${S}/nscd/nscd.service
+	touch  ${S}/../makedbs.sh
+
+	install -d ${D}${base_libdir}
+	install -d ${D}${base_sbindir}
+	install -d ${D}${bindir}
+	install -d ${D}${sbindir}
+	install -d ${D}${libdir}
+	install -d ${D}${libexecdir}
+	install -d ${D}${datadir}
+	install -d ${D}${includedir}
+	install -d ${D}/include
+	install -d ${D}${libdir}/${TARGET_SYS}/${EAT_VER_GCC}
+	install -d ${D}${libdir}/gcc/${TARGET_SYS}/${EAT_VER_GCC}
+
+	CP_ARGS="-Prf --preserve=mode,timestamps --no-preserve=ownership"
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/${EAT_LIBDIR}/*  ${D}${base_libdir}
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/libc/${EAT_LIBDIR}/*  ${D}${base_libdir}
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/libc/usr/${EAT_LIBDIR}/*  ${D}${libdir}
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/libc/usr/share/*  ${D}${datadir}
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/libc/usr/include/*  ${D}${includedir}
+
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/include/* ${D}${includedir}
+	if [ -d ${D}${includedir}/c++/${EAT_VER_GCC}/${EAT_TARGET_SYS} ]; then
+		mv ${D}${includedir}/c++/${EAT_VER_GCC}/${EAT_TARGET_SYS} ${D}${includedir}/c++/${EAT_VER_GCC}/${TARGET_SYS}
+	fi
+	ln -sf ../usr/include/c++ ${D}/include/c++
+
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/libc/usr/bin/* ${D}${bindir}
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/libc/usr/sbin/* ${D}${sbindir}
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/libc/sbin/* ${D}${base_sbindir}
+	rm -rf ${D}${bindir}/gdbserver
+	sed -i -e 's#/arm/tools/gnu/bash/4.2/rhe6-x86_64##' ${D}${bindir}/tzselect
+	sed -i -e 's#/arm/tools/gnu/bash/4.2/rhe6-x86_64##' ${D}${bindir}/ldd
+	sed -i -e 's#/usr/bin/bash#/bin/sh#' ${D}${bindir}/tzselect
+	sed -i -e 's#/usr/bin/bash#/bin/sh#' ${D}${bindir}/ldd
+	sed -i -e 's#/bin/bash#/bin/sh#' ${D}${bindir}/tzselect
+	sed -i -e 's#/bin/bash#/bin/sh#' ${D}${bindir}/ldd
+
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/lib/gcc/${EAT_TARGET_SYS}/${EAT_VER_GCC}/crt*.o ${D}${libdir}/${TARGET_SYS}/${EAT_VER_GCC}/
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/lib/gcc/${EAT_TARGET_SYS}/${EAT_VER_GCC}/libgcc* ${D}${libdir}/${TARGET_SYS}/${EAT_VER_GCC}/
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/lib/gcc/${EAT_TARGET_SYS}/${EAT_VER_GCC}/libgcov* ${D}${libdir}/gcc/${TARGET_SYS}/${EAT_VER_GCC}/
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/lib/gcc/${EAT_TARGET_SYS}/${EAT_VER_GCC}/include ${D}${libdir}/gcc/${TARGET_SYS}/${EAT_VER_GCC}/
+	cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/lib/gcc/${EAT_TARGET_SYS}/${EAT_VER_GCC}/finclude ${D}${libdir}/gcc/${TARGET_SYS}/${EAT_VER_GCC}/
+
+	# fix up the copied symlinks (they are still pointing to the multiarch directory)
+	linker_name="${@bb.utils.contains("TUNE_FEATURES", "aarch64", "ld-linux-aarch64.so.1", bb.utils.contains("TUNE_FEATURES", "callconvention-hard", "ld-linux-armhf.so.3", "ld-linux.so.3",d), d)}"
+	if [ -f ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/libc/${EAT_LIBDIR}/ld-${EAT_VER_LIBC}.so ]; then
+		ln -sf ld-${EAT_VER_LIBC}.so ${D}${base_libdir}/${linker_name}
+	else
+		cp ${CP_ARGS} ${EXTERNAL_TOOLCHAIN}/${EAT_TARGET_SYS}/libc/lib/${linker_name} ${D}${base_libdir}/
+	fi
+	ln -sf ../../lib/librt.so.1 ${D}${libdir}/librt.so
+	ln -sf ../../lib/libcrypt.so.1 ${D}${libdir}/libcrypt.so
+	ln -sf ../../lib/libresolv.so.2 ${D}${libdir}/libresolv.so
+	ln -sf ../../lib/libnss_hesiod.so.2 ${D}${libdir}/libnss_hesiod.so
+	ln -sf ../../lib/libutil.so.1 ${D}${libdir}/libutil.so
+	ln -sf ../../lib/libBrokenLocale.so.1 ${D}${libdir}/libBrokenLocale.so
+	ln -sf ../../lib/libpthread.so.0 ${D}${libdir}/libpthread.so
+	ln -sf ../../lib/libthread_db.so.1 ${D}${libdir}/libthread_db.so
+	ln -sf ../../lib/libanl.so.1 ${D}${libdir}/libanl.so
+	ln -sf ../../lib/libdl.so.2 ${D}${libdir}/libdl.so
+	ln -sf ../../lib/libnss_db.so.2 ${D}${libdir}/libnss_db.so
+	ln -sf ../../lib/libnss_dns.so.2 ${D}${libdir}/libnss_dns.so
+	ln -sf ../../lib/libnss_files.so.2 ${D}${libdir}/libnss_files.so
+	ln -sf ../../lib/libnss_compat.so.2 ${D}${libdir}/libnss_compat.so
+	ln -sf ../../lib/libm.so.6 ${D}${libdir}/libm.so
+	ln -sf ../../lib/libc_malloc_debug.so.0 ${D}${libdir}/libc_malloc_debug.so
+
+	# remove potential .so duplicates from base_libdir
+	# for all symlinks created above in libdir
+	rm -f ${D}${base_libdir}/librt.so
+	rm -f ${D}${base_libdir}/libcrypt.so
+	rm -f ${D}${base_libdir}/libresolv.so
+	rm -f ${D}${base_libdir}/libnss_hesiod.so
+	rm -f ${D}${base_libdir}/libutil.so
+	rm -f ${D}${base_libdir}/libBrokenLocale.so
+	rm -f ${D}${base_libdir}/libpthread.so
+	rm -f ${D}${base_libdir}/libthread_db.so
+	rm -f ${D}${base_libdir}/libanl.so
+	rm -f ${D}${base_libdir}/libdl.so
+	rm -f ${D}${base_libdir}/libnss_db.so
+	rm -f ${D}${base_libdir}/libnss_dns.so
+	rm -f ${D}${base_libdir}/libnss_files.so
+	rm -f ${D}${base_libdir}/libnss_compat.so
+	rm -f ${D}${base_libdir}/libm.so
+
+	# Move these completely to ${libdir} and delete duplicates in ${base_libdir}
+	for lib in asan hwasan atomic gfortran gomp itm lsan sanitizer stdc++ tsan ubsan; do
+		if [ -e ${D}${base_libdir}/lib${lib}.spec ] ; then
+			mv ${D}${base_libdir}/lib${lib}.spec ${D}${libdir}
+		fi
+		if [ -e ${D}${base_libdir}/lib${lib}.a ] ; then
+			mv ${D}${base_libdir}/lib${lib}.a ${D}${libdir}
+		fi
+		rm -f ${D}${base_libdir}/lib${lib}*
+	done
+
+	# Clean up duplicate libs that are both in base_libdir and libdir
+	rm -f ${D}${libdir}/libgcc*
+
+	# Besides ld-${EAT_VER_LIBC}.so, other libs can have duplicates like lib*-${EAT_VER_LIBC}.so
+	# Only remove them if both are regular files and are identical
+	for i in ${D}${base_libdir}/lib*-${EAT_VER_LIBC}.so; do
+		if [ ! -e $i ] ; then
+			continue
+		fi
+
+		f=$(echo $i | sed 's/-${EAT_VER_LIBC}//')
+		l=$(ls $f.*)
+		if [ $(readlink -f $i ) = $l ]; then
+			echo "$i is a symlink of $l, keep it"
+		elif [ $(readlink -f $l ) = $i ]; then
+			echo "$l is a symlink of $i, keep it"
+		else
+			cmp -s $i $l
+			if [ $? -eq 0 ]; then
+				echo "$i is a duplicate of $l, remove it"
+				rm $i
+			else
+				echo "$i and $l are different files, keep them both"
+			fi
+		fi
+	done
+
+	if [ -d ${D}${base_libdir}/arm-linux-gnueabi ]; then
+	   rm -rf ${D}${base_libdir}/arm-linux-gnueabi
+	fi
+
+	if [ -d ${D}${base_libdir}/ldscripts ]; then
+	   rm -rf ${D}${base_libdir}/ldscripts
+	fi
+
+	# Provided by libnsl2
+	rm -rf ${D}${includedir}/rpcsvc/yppasswd.*
+	# Provided by quota
+	rm -rf ${D}${includedir}/rpcsvc/rquota.*
+
+	if [ -f ${D}${libdir}/libc.so ];then
+		sed -i -e "s# /${EAT_LIBDIR}/${EAT_TARGET_SYS}# ../../${EAT_LIBDIR}#g" -e "s# /usr/${EAT_LIBDIR}/# /usr/lib/#g" -e "s# /usr/${EAT_LIBDIR}/${EAT_TARGET_SYS}# .#g" -e "s# /${EAT_LIBDIR}/ld-linux# ../../${EAT_LIBDIR}/ld-linux#g" ${D}${libdir}/libc.so
+		sed -i -e "s# /${EAT_LIBDIR}/libc.so.6# /lib/libc.so.6#g" ${D}${libdir}/libc.so
+		sed -i -e "s# /lib# ../../lib#g" -e "s# /usr/lib# .#g" ${D}${libdir}/libc.so
+	fi
+
+	if [ -f ${D}${base_libdir}/libc.so ];then
+		sed -i -e "s# /${EAT_LIBDIR}/${EAT_TARGET_SYS}# ../../lib#g" -e "s# /usr/${EAT_LIBDIR}/${EAT_TARGET_SYS}# .#g" -e "s# /${EAT_LIBDIR}/# /lib/#g" ${D}${base_libdir}/libc.so
+		if [ -f ${D}${base_libdir}/libc.so.6 ]; then
+			sed -i -e "s# /usr/${EAT_LIBDIR}/libc.so.6# /lib/libc.so.6#g" -e "s# /${EAT_LIBDIR}/libc.so.6# /lib/libc.so.6#g" ${D}${base_libdir}/libc.so.6
+		fi
+	fi
+
+	# Remove if empty
+	rmdir ${D}${bindir} || true
+	rmdir ${D}${sbindir} || true
+	rmdir ${D}${base_sbindir} || true
+	rmdir ${D}${libexecdir} || true
+
+	# Remove unused /usr/share/info/dir
+	rm -f ${D}${infodir}/dir
+}
+
+# External toolchain doesn't provide multilib support so make corresponding
+# install API as an empty API to avoid an unnecessary errors.
+oe_multilib_header () {
+	return
+}
+
+PACKAGES_DYNAMIC = "^locale-base-.* \
+                    ^glibc-gconv-.* ^glibc-charmap-.* ^glibc-localedata-.* ^glibc-binary-localedata-.* \
+                    ^${MLPREFIX}glibc-gconv$"
+
+# PACKAGES is split up according to the 'source' recipes/includes in OE-core
+# Stylistic differences are kept to make copy/pasting easier.
+
+# From gcc-runtime.inc
+
+PACKAGES += "\
+    gcc-runtime-dbg \
+    libstdc++ \
+    libstdc++-precompile-dev \
+    libstdc++-dev \
+    libstdc++-staticdev \
+    libg2c \
+    libg2c-dev \
+    libssp \
+    libssp-dev \
+    libssp-staticdev \
+    libmudflap \
+    libmudflap-dev \
+    libmudflap-staticdev \
+    libquadmath \
+    libquadmath-dev \
+    libquadmath-staticdev \
+    libgomp \
+    libgomp-dev \
+    libgomp-staticdev \
+    libatomic \
+    libatomic-dev \
+    libatomic-staticdev \
+    libitm \
+    libitm-dev \
+    libitm-staticdev \
+"
+
+# From gcc-sanitizers.inc
+
+PACKAGES += "gcc-sanitizers gcc-sanitizers-dbg"
+PACKAGES += "libasan libubsan liblsan libtsan"
+PACKAGES += "libasan-dev libubsan-dev liblsan-dev libtsan-dev"
+PACKAGES += "libasan-staticdev libubsan-staticdev liblsan-staticdev libtsan-staticdev"
+
+# From libgfortran.inc:
+
+PACKAGES += "\
+    libgfortran-dbg \
+    libgfortran \
+    libgfortran-dev \
+    libgfortran-staticdev \
+"
+
+# libgcc.inc uses ${PN}, so replace that
+
+PACKAGES += "\
+    libgcc \
+    libgcc-dev \
+    libgcc-dbg \
+"
+
+# ... and the leftovers
+
+PACKAGES =+ "\
+        ${PN}-mtrace \
+	libgcov-staticdev \
+	linux-libc-headers \
+	linux-libc-headers-dev \
+"
+
+# Re-order PACKAGES list in order to shift ${PN}-dev towards the end as
+# it is meant to pick up remaining dev libraries and headers that aren't
+# picked up by other packages. And since some static libraries needs to
+# be packaged in ${PN}-dev, so we need to keep ${PN}-staticdev later in
+# order.
+
+PACKAGES := "${@oe.utils.str_filter_out('${PN}-dev', '${PACKAGES}', d)}"
+PACKAGES := "${@oe.utils.str_filter_out('${PN}-staticdev', '${PACKAGES}', d)}"
+PACKAGES += "\
+	${PN}-dev \
+	${PN}-staticdev \
+"
+
+INSANE_SKIP:${PN}-dbg = "staticdev"
+INSANE_SKIP:${PN}-utils += "ldflags"
+INSANE_SKIP:libstdc++ += "ldflags"
+INSANE_SKIP:libgfortran += "ldflags"
+INSANE_SKIP:libgcc += "ldflags dev-deps"
+INSANE_SKIP:libgcc-dev += "staticdev"
+INSANE_SKIP:libgfortran += "ldflags dev-deps"
+INSANE_SKIP:libstdc++ += "ldflags dev-deps"
+INSANE_SKIP:libatomic += "ldflags"
+INSANE_SKIP:libasan += "ldflags"
+INSANE_SKIP:libubsan += "ldflags"
+INSANE_SKIP:libssp += "ldflags"
+INSANE_SKIP:libgomp += "ldflags"
+INSANE_SKIP:libitm += "ldflags"
+INSANE_SKIP:gdbserver += "ldflags"
+
+# OE-core has literally listed 'glibc' in LIBC_DEPENDENCIES :/
+RPROVIDES:${PN} = "glibc rtld(GNU_HASH)"
+# Add runtime provides for the other libc* packages as well
+RPROVIDES:${PN}-dev = "glibc-dev"
+RPROVIDES:${PN}-doc = "glibc-doc"
+RPROVIDES:${PN}-dbg = "glibc-dbg"
+RPROVIDES:${PN}-pic = "glibc-pic"
+RPROVIDES:${PN}-utils = "glibc-utils"
+RPROVIDES:${PN}-mtrace = "glibc-mtrace libc-mtrace"
+
+PKG:${PN} = "glibc"
+PKG:${PN}-dev = "glibc-dev"
+PKG:${PN}-doc = "glibc-doc"
+PKG:${PN}-dbg = "glibc-dbg"
+PKG:${PN}-pic = "glibc-pic"
+PKG:${PN}-utils = "glibc-utils"
+PKG:${PN}-mtrace = "glibc-mtrace"
+PKG:${PN}-gconv = "glibc-gconv"
+PKG:${PN}-extra-nss = "glibc-extra-nss"
+PKG:${PN}-thread-db = "glibc-thread-db"
+PKG:${PN}-pcprofile = "glibc-pcprofile"
+PKG:${PN}-staticdev = "glibc-staticdev"
+
+PKGV = "${EAT_VER_LIBC}"
+PKGV:${PN} = "${EAT_VER_LIBC}"
+PKGV:${PN}-dev = "${EAT_VER_LIBC}"
+PKGV:${PN}-doc = "${EAT_VER_LIBC}"
+PKGV:${PN}-dbg = "${EAT_VER_LIBC}"
+PKGV:${PN}-pic = "${EAT_VER_LIBC}"
+PKGV:${PN}-utils = "${EAT_VER_LIBC}"
+PKGV:${PN}-mtrace = "${EAT_VER_LIBC}"
+PKGV:${PN}-gconv = "${EAT_VER_LIBC}"
+PKGV:${PN}-extra-nss = "${EAT_VER_LIBC}"
+PKGV:${PN}-thread-db = "${EAT_VER_LIBC}"
+PKGV:${PN}-pcprofile = "${EAT_VER_LIBC}"
+PKGV:${PN}-staticdev = "${EAT_VER_LIBC}"
+PKGV:catchsegv = "${EAT_VER_LIBC}"
+PKGV:glibc-extra-nss = "${EAT_VER_LIBC}"
+PKGV:glibc-thread-db = "${EAT_VER_LIBC}"
+
+PKGV:libmemusage = "${EAT_VER_LIBC}"
+PKGV:libsegfault = "${EAT_VER_LIBC}"
+PKGV:libsotruss = "${EAT_VER_LIBC}"
+PKGV:sln = "${EAT_VER_LIBC}"
+PKGV:nscd = "${EAT_VER_LIBC}"
+PKGV:ldd = "${EAT_VER_LIBC}"
+
+PKGV:libasan-dev = "${EAT_VER_GCC}"
+PKGV:libasan = "${EAT_VER_GCC}"
+PKGV:libasan-staticdev = "${EAT_VER_GCC}"
+PKGV:libatomic-dev = "${EAT_VER_GCC}"
+PKGV:libatomic = "${EAT_VER_GCC}"
+PKGV:libatomic-staticdev = "${EAT_VER_GCC}"
+PKGV:libg2c-dev = "${EAT_VER_GCC}"
+PKGV:libg2c = "${EAT_VER_GCC}"
+PKGV:libgcc-dev = "${EAT_VER_GCC}"
+PKGV:libgcc = "${EAT_VER_GCC}"
+PKGV:libgfortran-dbg = "${EAT_VER_GCC}"
+PKGV:libgfortran-dev = "${EAT_VER_GCC}"
+PKGV:libgfortran = "${EAT_VER_GCC}"
+PKGV:libgfortran-staticdev = "${EAT_VER_GCC}"
+PKGV:libgomp-dev = "${EAT_VER_GCC}"
+PKGV:libgomp = "${EAT_VER_GCC}"
+PKGV:libgomp-staticdev = "${EAT_VER_GCC}"
+PKGV:libitm-dev = "${EAT_VER_GCC}"
+PKGV:libitm = "${EAT_VER_GCC}"
+PKGV:libitm-staticdev = "${EAT_VER_GCC}"
+PKGV:liblsan-dev = "${EAT_VER_GCC}"
+PKGV:liblsan = "${EAT_VER_GCC}"
+PKGV:liblsan-staticdev = "${EAT_VER_GCC}"
+PKGV:libmudflap-dev = "${EAT_VER_GCC}"
+PKGV:libmudflap = "${EAT_VER_GCC}"
+PKGV:libmudflap-staticdev = "${EAT_VER_GCC}"
+PKGV:libquadmath-dev = "${EAT_VER_GCC}"
+PKGV:libquadmath = "${EAT_VER_GCC}"
+PKGV:libquadmath-staticdev = "${EAT_VER_GCC}"
+PKGV:libssp-dev = "${EAT_VER_GCC}"
+PKGV:libssp = "${EAT_VER_GCC}"
+PKGV:libssp-staticdev = "${EAT_VER_GCC}"
+PKGV:libstdc++-dbg = "${EAT_VER_GCC}"
+PKGV:libstdc++-dev = "${EAT_VER_GCC}"
+PKGV:libstdc++ = "${EAT_VER_GCC}"
+PKGV:libstdc++-precompile-dev = "${EAT_VER_GCC}"
+PKGV:libstdc++-staticdev = "${EAT_VER_GCC}"
+PKGV:libtsan-dev = "${EAT_VER_GCC}"
+PKGV:libtsan = "${EAT_VER_GCC}"
+PKGV:libtsan-staticdev = "${EAT_VER_GCC}"
+PKGV:libubsan-dev = "${EAT_VER_GCC}"
+PKGV:libubsan = "${EAT_VER_GCC}"
+PKGV:libubsan-staticdev = "${EAT_VER_GCC}"
+
+PKGV:linux-libc-headers-dev = "${EAT_VER_KERNEL}"
+PKGV:linux-libc-headers = "${EAT_VER_KERNEL}"
+
+PKGV:gdbserver = "${EAT_VER_GDBSERVER}"
+
+ALLOW_EMPTY:${PN}-mtrace = "1"
+FILES:${PN}-mtrace = "${bindir}/mtrace"
+
+FILES:libgcov-staticdev = "${libdir}/gcc/${TARGET_SYS}/${BINV}/libgcov.a"
+
+FILES:libsegfault = "${base_libdir}/libSegFault*"
+
+FILES:catchsegv = "${bindir}/catchsegv"
+RDEPENDS:catchsegv = "libsegfault"
+
+# From libgfortran.inc:
+
+FILES:libgfortran = "${libdir}/libgfortran.so.*"
+FILES:libgfortran-dev = "\
+    ${libdir}/libgfortran*.so \
+    ${libdir}/libgfortran.spec \
+    ${libdir}/libgfortran.la \
+    ${libdir}/gcc/${TARGET_SYS}/${BINV}/libgfortranbegin.* \
+    ${libdir}/gcc/${TARGET_SYS}/${BINV}/libcaf_single* \
+    ${libdir}/gcc/${TARGET_SYS}/${BINV}/finclude/ \
+"
+FILES:libgfortran-staticdev = "${libdir}/libgfortran.a"
+
+
+# From gcc-sanitizers.inc:
+
+FILES:libasan += "${libdir}/libasan.so.* ${libdir}/libhwasan.so.*"
+FILES:libasan-dev += "\
+    ${libdir}/libasan_preinit.o \
+    ${libdir}/libasan.so \
+    ${libdir}/libhwasan.so \
+    ${libdir}/libasan.la \
+"
+FILES:libasan-staticdev += "${libdir}/libasan.a \
+    ${libdir}/libhwasan.a \
+"
+
+FILES:libubsan += "${libdir}/libubsan.so.*"
+FILES:libubsan-dev += "\
+    ${libdir}/libubsan.so \
+    ${libdir}/libubsan.la \
+"
+FILES:libubsan-staticdev += "${libdir}/libubsan.a"
+
+FILES:liblsan += "${libdir}/liblsan.so.*"
+FILES:liblsan-dev += "\
+    ${libdir}/liblsan.so \
+    ${libdir}/liblsan.la \
+    ${libdir}/liblsan_preinit.o \
+"
+FILES:liblsan-staticdev += "${libdir}/liblsan.a"
+
+FILES:libtsan += "${libdir}/libtsan.so.*"
+FILES:libtsan-dev += "\
+    ${libdir}/libtsan.so \
+    ${libdir}/libtsan.la \
+    ${libdir}/libtsan_*.o \
+"
+FILES:libtsan-staticdev += "${libdir}/libtsan.a"
+
+FILES:gcc-sanitizers = "${libdir}/*.spec ${libdir}/gcc/${TARGET_SYS}/${BINV}/include/sanitizer/*.h"
+
+# From libgcc.inc:
+
+FILES:libgcc = "${base_libdir}/libgcc_s.so.1"
+
+FILES:libgcc-dev = "\
+    ${base_libdir}/libgcc*.so \
+    ${@oe.utils.conditional('BASETARGET_SYS', '${TARGET_SYS}', '', '${libdir}/${BASETARGET_SYS}', d)} \
+    ${libdir}/${TARGET_SYS}/${BINV}* \
+    ${libdir}/${TARGET_ARCH}${TARGET_VENDOR}* \
+    ${libdir}/gcc/${TARGET_SYS}/${BINV}/include \
+"
+
+FILES:linux-libc-headers = ""
+FILES:linux-libc-headers-dev = "\
+	${includedir}/asm* \
+	${includedir}/linux \
+	${includedir}/mtd \
+	${includedir}/rdma \
+	${includedir}/scsi \
+	${includedir}/sound \
+	${includedir}/video \
+"
+FILES:${PN} += "\
+	${libdir}/bin \
+	${libdir}/locale \
+	${libdir}/gconv/gconv-modules \
+	${datadir}/zoneinfo \
+	${base_libdir}/libcrypt*.so.* \
+	${base_libdir}/libcrypt-*.so \
+	${base_libdir}/libc.so.* \
+	${base_libdir}/libc-*.so \
+	${base_libdir}/libm.so.* \
+	${base_libdir}/libmemusage.so \
+	${base_libdir}/libm-*.so \
+	${base_libdir}/ld*.so.* \
+	${base_libdir}/ld-*.so \
+	${base_libdir}/libpthread*.so.* \
+	${base_libdir}/libpthread*.so \
+	${base_libdir}/libpthread-*.so \
+	${base_libdir}/libresolv*.so.* \
+	${base_libdir}/libresolv-*.so \
+	${base_libdir}/librt*.so.* \
+	${base_libdir}/librt-*.so \
+	${base_libdir}/libutil*.so.* \
+	${base_libdir}/libutil-*.so \
+	${base_libdir}/libnss_files*.so.* \
+	${base_libdir}/libnss_files-*.so \
+	${base_libdir}/libnss_compat*.so.* \
+	${base_libdir}/libnss_compat-*.so \
+	${base_libdir}/libnss_dns*.so.* \
+	${base_libdir}/libnss_dns-*.so \
+	${base_libdir}/libnss_nis*.so.* \
+	${base_libdir}/libnss_nisplus-*.so \
+	${base_libdir}/libnss_nisplus*.so.* \
+	${base_libdir}/libnss_nis-*.so \
+	${base_libdir}/libnss_hesiod*.so.* \
+	${base_libdir}/libnss_hesiod-*.so \
+	${base_libdir}/libdl*.so.* \
+	${base_libdir}/libdl-*.so \
+	${base_libdir}/libanl*.so.* \
+	${base_libdir}/libanl-*.so \
+	${base_libdir}/libBrokenLocale*.so.* \
+	${base_libdir}/libBrokenLocale-*.so \
+	${base_libdir}/libthread_db*.so.* \
+	${base_libdir}/libthread_db-*.so \
+	${base_libdir}/libmemusage.so \
+	${base_libdir}/libSegFault.so \
+	${base_libdir}/libpcprofile.so \
+    "
+
+FILES:${PN}-dbg += "${base_libdir}/debug"
+
+# From gcc-runtime.inc
+
+# include python debugging scripts
+FILES:gcc-runtime-dbg += "\
+    ${libdir}/libstdc++.so.*-gdb.py \
+    ${datadir}/gcc-${BINV}/python/libstdcxx \
+"
+
+FILES:libg2c = "${target_libdir}/libg2c.so.*"
+SUMMARY:libg2c = "Companion runtime library for g77"
+FILES:libg2c-dev = "\
+    ${libdir}/libg2c.so \
+    ${libdir}/libg2c.a \
+    ${libdir}/libfrtbegin.a \
+"
+SUMMARY:libg2c-dev = "Companion runtime library for g77 - development files"
+
+FILES:libstdc++ = "${libdir}/libstdc++.so.*"
+SUMMARY:libstdc++ = "GNU standard C++ library"
+FILES:libstdc++-dev = "\
+    /include/c++ \
+    ${includedir}/c++/ \
+    ${libdir}/libstdc++.so \
+    ${libdir}/libstdc++*.la \
+    ${libdir}/libsupc++.la \
+"
+SUMMARY:libstdc++-dev = "GNU standard C++ library - development files"
+
+FILES:libstdc++-staticdev = "\
+    ${libdir}/libstdc++*.a \
+    ${libdir}/libsupc++.a \
+"
+SUMMARY:libstdc++-staticdev = "GNU standard C++ library - static development files"
+
+FILES:libstdc++-precompile-dev = "${includedir}/c++/${TARGET_SYS}/bits/*.gch"
+SUMMARY:libstdc++-precompile-dev = "GNU standard C++ library - precompiled header files"
+
+FILES:libssp = "${libdir}/libssp.so.*"
+SUMMARY:libssp = "GNU stack smashing protection library"
+FILES:libssp-dev = "\
+    ${libdir}/libssp*.so \
+    ${libdir}/libssp*_nonshared.a \
+    ${libdir}/libssp*.la \
+    ${libdir}/gcc/${TARGET_SYS}/${BINV}/include/ssp \
+"
+SUMMARY:libssp-dev = "GNU stack smashing protection library - development files"
+FILES:libssp-staticdev = "${libdir}/libssp*.a"
+SUMMARY:libssp-staticdev = "GNU stack smashing protection library - static development files"
+
+FILES:libquadmath = "${libdir}/libquadmath*.so.*"
+SUMMARY:libquadmath = "GNU quad-precision math library"
+FILES:libquadmath-dev = "\
+    ${libdir}/gcc/${TARGET_SYS}/${BINV}/include/quadmath* \
+    ${libdir}/libquadmath*.so \
+    ${libdir}/libquadmath.la \
+"
+SUMMARY:libquadmath-dev = "GNU quad-precision math library - development files"
+FILES:libquadmath-staticdev = "${libdir}/libquadmath.a"
+SUMMARY:libquadmath-staticdev = "GNU quad-precision math library - static development files"
+
+# NOTE: mudflap has been removed as of gcc 4.9 and has been superseded by the address sanitiser
+FILES:libmudflap = "${libdir}/libmudflap*.so.*"
+SUMMARY:libmudflap = "Pointer debugging library for gcc"
+FILES:libmudflap-dev = "\
+    ${libdir}/libmudflap*.so \
+    ${libdir}/libmudflap.la \
+"
+SUMMARY:libmudflap-dev = "Pointer debugging library for gcc - development files"
+FILES:libmudflap-staticdev = "${libdir}/libmudflap.a"
+SUMMARY:libmudflap-staticdev = "Pointer debugging library for gcc - static development files"
+
+FILES:libgomp = "${libdir}/libgomp*${SOLIBS}"
+SUMMARY:libgomp = "GNU OpenMP parallel programming library"
+FILES:libgomp-dev = "\
+    ${libdir}/libgomp*${SOLIBSDEV} \
+    ${libdir}/libgomp*.la \
+    ${libdir}/libgomp.spec \
+    ${libdir}/gcc/${TARGET_SYS}/${BINV}/include/omp.h \
+    ${libdir}/gcc/${TARGET_SYS}/${BINV}/include/openacc.h \
+"
+SUMMARY:libgomp-dev = "GNU OpenMP parallel programming library - development files"
+FILES:libgomp-staticdev = "${libdir}/libgomp*.a"
+SUMMARY:libgomp-staticdev = "GNU OpenMP parallel programming library - static development files"
+
+FILES:libatomic = "${libdir}/libatomic.so.*"
+SUMMARY:libatomic = "GNU C++11 atomics support library"
+FILES:libatomic-dev = "\
+    ${libdir}/libatomic.so \
+    ${libdir}/libatomic.la \
+"
+SUMMARY:libatomic-dev = "GNU C++11 atomics support library - development files"
+FILES:libatomic-staticdev = "${libdir}/libatomic.a"
+SUMMARY:libatomic-staticdev = "GNU C++11 atomics support library - static development files"
+
+FILES:libitm = "${libdir}/libitm.so.*"
+SUMMARY:libitm = "GNU transactional memory support library"
+FILES:libitm-dev = "\
+    ${libdir}/libitm.so \
+    ${libdir}/libitm.la \
+    ${libdir}/libitm.spec \
+"
+SUMMARY:libitm-dev = "GNU transactional memory support library - development files"
+FILES:libitm-staticdev = "${libdir}/libitm.a"
+SUMMARY:libitm-staticdev = "GNU transactional memory support library - static development files"
+
+EAT_VER_MAIN ??= ""
+
+python () {
+    if not d.getVar("EAT_VER_MAIN", False):
+        raise bb.parse.SkipPackage("External ARM toolchain not configured (EAT_VER_MAIN not set).")
+    if d.getVar('TCLIBC', True) != "glibc":
+        raise bb.parse.SkipPackage("incompatible with %s" % d.getVar('TCLIBC', True))
+}
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/files/SUPPORTED b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/files/SUPPORTED
new file mode 100644
index 0000000..9615075
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/files/SUPPORTED
@@ -0,0 +1 @@
+POSIX
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/gcc-aarch64-none-elf_11.2-2022.02.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/gcc-aarch64-none-elf_11.2-2022.02.bb
new file mode 100644
index 0000000..2a5a4ef
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/gcc-aarch64-none-elf_11.2-2022.02.bb
@@ -0,0 +1,23 @@
+# Copyright (C) 2020 Texas Instruments Inc.
+# Released under the MIT license (see COPYING.MIT for the terms)
+
+require arm-binary-toolchain.inc
+
+COMPATIBLE_HOST = "(x86_64|aarch64).*-linux"
+
+SUMMARY = "Arm GNU Toolchain - AArch64 bare-metal target (aarch64-none-elf)"
+LICENSE = "GPL-3.0-with-GCC-exception & GPL-3.0-only"
+
+LIC_FILES_CHKSUM:aarch64 = "file://share/doc/gcc/Copying.html;md5=be4f8b5ff7319cd54f6c52db5d6f36b0"
+LIC_FILES_CHKSUM:x86-64 = "file://share/doc/gcc/Copying.html;md5=1f07179249795891179bb3798bac7887"
+
+PROVIDES = "virtual/aarch64-none-elf-gcc"
+
+SRC_URI = "https://developer.arm.com/-/media/Files/downloads/gnu/${PV}/binrel/gcc-arm-${PV}-${HOST_ARCH}-${BINNAME}.tar.xz;name=gcc-${HOST_ARCH}"
+SRC_URI[gcc-aarch64.sha256sum] = "3b15725545a0211a17b63e72d4f10241f7ffbe7ce94cb9612590ceacde16992c"
+SRC_URI[gcc-x86_64.sha256sum] = "b0a015a9e8cbb44ed2fe5ad755a7a7ae254d54f93df3bf47378485b0ba8b828b"
+
+S = "${WORKDIR}/gcc-arm-${PV}-${HOST_ARCH}-${BINNAME}"
+
+UPSTREAM_CHECK_URI = "https://developer.arm.com/tools-and-software/open-source-software/developer-tools/gnu-toolchain/gnu-a/downloads"
+UPSTREAM_CHECK_REGEX = "gcc-arm-(?P<pver>.+)-${HOST_ARCH}-${BINNAME}\.tar\.\w+"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/gcc-arm-none-eabi_11.2-2022.02.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/gcc-arm-none-eabi_11.2-2022.02.bb
new file mode 100644
index 0000000..26f0ee8
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/gcc-arm-none-eabi_11.2-2022.02.bb
@@ -0,0 +1,23 @@
+# Copyright (C) 2019 Garmin Ltd. or its subsidiaries
+# Released under the MIT license (see COPYING.MIT for the terms)
+
+require arm-binary-toolchain.inc
+
+COMPATIBLE_HOST = "(x86_64|aarch64).*-linux"
+
+SUMMARY = "Arm GNU Toolchain - AArch32 bare-metal target (arm-none-eabi)"
+LICENSE = "GPL-3.0-with-GCC-exception & GPL-3.0-only"
+
+LIC_FILES_CHKSUM:aarch64 = "file://share/doc/gcc/Copying.html;md5=be4f8b5ff7319cd54f6c52db5d6f36b0"
+LIC_FILES_CHKSUM:x86-64 = "file://share/doc/gcc/Copying.html;md5=1f07179249795891179bb3798bac7887"
+
+PROVIDES = "virtual/arm-none-eabi-gcc"
+
+SRC_URI = "https://developer.arm.com/-/media/Files/downloads/gnu/${PV}/binrel/gcc-arm-${PV}-${HOST_ARCH}-${BINNAME}.tar.xz;name=gcc-${HOST_ARCH}"
+SRC_URI[gcc-aarch64.sha256sum] = "ef1d82e5894e3908cb7ed49c5485b5b95deefa32872f79c2b5f6f5447cabf55f"
+SRC_URI[gcc-x86_64.sha256sum] = "8c5acd5ae567c0100245b0556941c237369f210bceb196edfe5a2e7532c60326"
+
+S = "${WORKDIR}/gcc-arm-${PV}-${HOST_ARCH}-${BINNAME}"
+
+UPSTREAM_CHECK_URI = "https://developer.arm.com/tools-and-software/open-source-software/developer-tools/gnu-toolchain/downloads"
+UPSTREAM_CHECK_REGEX = "${BPN}-(?P<pver>.+)-${HOST_ARCH}-linux\.tar\.\w+"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/license.inc b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/license.inc
new file mode 100644
index 0000000..4260dc7
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/external-arm-toolchain/license.inc
@@ -0,0 +1,79 @@
+LICENSE = "GPL-3.0-with-GCC-exception & GPL-3.0-only & GPL-2.0-only & LGPL-3.0-only & LGPL-2.1-only & LGPL-2.0-only"
+
+EAT_BFD_LICENSE = "GPL-3.0-only"
+EAT_GCC_LICENSE = "GPL-3.0-with-GCC-exception & GPL-3.0-only"
+EAT_GDB_LICENSE = "GPL-3.0-only"
+EAT_LIBC_LICENSE = "GPL-2.0-only & LGPL-2.1-only"
+EAT_RLE_LICENSE = "GPL-3.0-with-GCC-exception"
+
+LICENSE:${PN} = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-dev = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-doc = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-dbg = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-pic = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-utils = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-mtrace = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-gconv = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-extra-nss = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-thread-db = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-pcprofile = "${EAT_LIBC_LICENSE}"
+LICENSE:${PN}-staticdev = "${EAT_LIBC_LICENSE}"
+LICENSE:catchsegv = "${EAT_LIBC_LICENSE}"
+LICENSE:glibc-extra-nss = "${EAT_LIBC_LICENSE}"
+LICENSE:glibc-thread-db = "${EAT_LIBC_LICENSE}"
+
+LICENSE:libmemusage = "${EAT_LIBC_LICENSE}"
+LICENSE:libsegfault = "${EAT_LIBC_LICENSE}"
+LICENSE:libsotruss = "${EAT_LIBC_LICENSE}"
+LICENSE:sln = "${EAT_LIBC_LICENSE}"
+LICENSE:nscd = "${EAT_LIBC_LICENSE}"
+LICENSE:ldd = "${EAT_LIBC_LICENSE}"
+
+LICENSE:libasan-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libasan = "${EAT_GCC_LICENSE}"
+LICENSE:libasan-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:libatomic-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libatomic = "${EAT_GCC_LICENSE}"
+LICENSE:libatomic-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:libg2c-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libg2c = "${EAT_GCC_LICENSE}"
+LICENSE:libgcc-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libgcc = "${EAT_GCC_LICENSE}"
+LICENSE:libgfortran-dbg = "${EAT_GCC_LICENSE}"
+LICENSE:libgfortran-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libgfortran = "${EAT_GCC_LICENSE}"
+LICENSE:libgfortran-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:libgomp-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libgomp = "${EAT_GCC_LICENSE}"
+LICENSE:libgomp-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:libitm-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libitm = "${EAT_GCC_LICENSE}"
+LICENSE:libitm-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:liblsan-dev = "${EAT_GCC_LICENSE}"
+LICENSE:liblsan = "${EAT_GCC_LICENSE}"
+LICENSE:liblsan-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:libmudflap-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libmudflap = "${EAT_GCC_LICENSE}"
+LICENSE:libmudflap-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:libquadmath-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libquadmath = "${EAT_GCC_LICENSE}"
+LICENSE:libquadmath-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:libssp-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libssp = "${EAT_GCC_LICENSE}"
+LICENSE:libssp-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:libstdc++-dbg = "${EAT_GCC_LICENSE}"
+LICENSE:libstdc++-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libstdc++ = "${EAT_GCC_LICENSE}"
+LICENSE:libstdc++-precompile-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libstdc++-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:libtsan-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libtsan = "${EAT_GCC_LICENSE}"
+LICENSE:libtsan-staticdev = "${EAT_GCC_LICENSE}"
+LICENSE:libubsan-dev = "${EAT_GCC_LICENSE}"
+LICENSE:libubsan = "${EAT_GCC_LICENSE}"
+LICENSE:libubsan-staticdev = "${EAT_GCC_LICENSE}"
+
+LICENSE:linux-libc-headers-dev = "GPL-2.0-only"
+LICENSE:linux-libc-headers = "GPL-2.0-only"
+
+LICENSE:gdbserver = "GPL-2.0-only & GPL-3.0-only & LGPL-2.0-only & LGPL-3.0-only"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2.inc b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2.inc
new file mode 100644
index 0000000..04c6751
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2.inc
@@ -0,0 +1,121 @@
+require recipes-devtools/gcc/gcc-common.inc
+
+# Third digit in PV should be incremented after a minor release
+ARM_GCC_VERSION = "11.2"
+PV = "arm-${ARM_GCC_VERSION}"
+CVE_VERSION = "11.2"
+
+# BINV should be incremented to a revision after a minor gcc release
+
+BINV = "11.2.1"
+
+MMYY = "22.02"
+RELEASE = "20${MMYY}"
+PR = "r${RELEASE}"
+
+FILESEXTRAPATHS =. "${FILE_DIRNAME}/gcc-arm-${ARM_GCC_VERSION}:${FILE_DIRNAME}/gcc-arm-${ARM_GCC_VERSION}/backport:"
+
+DEPENDS =+ "mpfr gmp libmpc zlib flex-native"
+NATIVEDEPS = "mpfr-native gmp-native libmpc-native zlib-native flex-native"
+
+LICENSE = "GPL-3.0-with-GCC-exception & GPL-3.0-only"
+
+LIC_FILES_CHKSUM = "\
+    file://COPYING;md5=59530bdf33659b29e73d4adb9f9f6552 \
+    file://COPYING3;md5=d32239bcb673463ab874e80d47fae504 \
+    file://COPYING3.LIB;md5=6a6a8e020838b23406c81b19c1d46df6 \
+    file://COPYING.LIB;md5=2d5025d4aa3495befef8f17206a5b0a1 \
+    file://COPYING.RUNTIME;md5=fe60d87048567d4fe8c8a0ed2448bcc8 \
+"
+
+BASEURI ?= "https://developer.arm.com/-/media/Files/downloads/gnu/${ARM_GCC_VERSION}-${RELEASE}/srcrel/gcc-arm-src-snapshot-${ARM_GCC_VERSION}-${RELEASE}.tar.xz"
+SRC_URI = "\
+           ${BASEURI} \
+           file://0001-gcc-4.3.1-ARCH_FLAGS_FOR_TARGET.patch \
+           file://0002-gcc-poison-system-directories.patch \
+           file://0004-64-bit-multilib-hack.patch \
+           file://0007-Use-the-defaults.h-in-B-instead-of-S-and-t-oe-in-B.patch \
+           file://0009-cpp-honor-sysroot.patch \
+           file://0011-Define-GLIBC_DYNAMIC_LINKER-and-UCLIBC_DYNAMIC_LINKE.patch \
+           file://0012-gcc-Fix-argument-list-too-long-error.patch \
+           file://0014-libtool.patch \
+           file://0015-gcc-armv4-pass-fix-v4bx-to-linker-to-support-EABI.patch \
+           file://0016-Use-the-multilib-config-files-from-B-instead-of-usin.patch \
+           file://0017-Avoid-using-libdir-from-.la-which-usually-points-to-.patch \
+           file://0018-export-CPP.patch \
+           file://0019-Ensure-target-gcc-headers-can-be-included.patch \
+           file://0020-Don-t-search-host-directory-during-relink-if-inst_pr.patch \
+           file://0023-libcc1-fix-libcc1-s-install-path-and-rpath.patch \
+           file://0024-handle-sysroot-support-for-nativesdk-gcc.patch \
+           file://0025-Search-target-sysroot-gcc-version-specific-dirs-with.patch \
+           file://0027-nios2-Define-MUSL_DYNAMIC_LINKER.patch \
+           file://0028-Add-ssp_nonshared-to-link-commandline-for-musl-targe.patch \
+           file://0029-Link-libgcc-using-LDFLAGS-not-just-SHLIB_LDFLAGS.patch \
+           file://0030-sync-gcc-stddef.h-with-musl.patch \
+           file://0033-Re-introduce-spe-commandline-options.patch \
+           file://0034-libgcc_s-Use-alias-for-__cpu_indicator_init-instead-.patch \
+           file://0035-gentypes-genmodes-Do-not-use-__LINE__-for-maintainin.patch \
+           file://0036-mingw32-Enable-operation_not_supported.patch \
+           file://0037-libatomic-Do-not-enforce-march-on-aarch64.patch \
+           file://0041-apply-debug-prefix-maps-before-checksumming-DIEs.patch \
+           file://0006-If-CXXFLAGS-contains-something-unsupported-by-the-bu.patch \
+           file://0001-Fix-install-path-of-linux64.h.patch \
+           file://0001-CVE-2021-42574.patch \
+           file://0002-CVE-2021-42574.patch \
+           file://0003-CVE-2021-42574.patch \
+           file://0004-CVE-2021-42574.patch \
+           file://0001-CVE-2021-46195.patch \
+"
+SRC_URI[sha256sum] = "417662cd4e79e74b9fbac67bf1edac1c03d11ea58f10144487b7e25e58ff4931"
+
+S = "${TMPDIR}/work-shared/gcc-${PV}-${PR}/gcc-arm-src-snapshot-${ARM_GCC_VERSION}-${RELEASE}"
+
+# For dev release snapshotting
+#S = "${TMPDIR}/work-shared/gcc-${PV}-${PR}/gcc-${RELEASE}"
+#B = "${WORKDIR}/gcc-${PV}/build.${HOST_SYS}.${TARGET_SYS}"
+
+# Language Overrides
+FORTRAN = ""
+JAVA = ""
+
+SSP ?= "--disable-libssp"
+SSP:mingw32 = "--enable-libssp"
+
+EXTRA_OECONF_BASE = "\
+    ${SSP} \
+    --enable-libitm \
+    --enable-lto \
+    --disable-bootstrap \
+    --with-system-zlib \
+    ${@'--with-linker-hash-style=${LINKER_HASH_STYLE}' if '${LINKER_HASH_STYLE}' else ''} \
+    --enable-linker-build-id \
+    --with-ppl=no \
+    --with-cloog=no \
+    --enable-checking=release \
+    --enable-cheaders=c_global \
+    --without-isl \
+"
+
+EXTRA_OECONF_INITIAL = "\
+    --disable-libgomp \
+    --disable-libitm \
+    --disable-libquadmath \
+    --with-system-zlib \
+    --disable-lto \
+    --disable-plugin \
+    --enable-linker-build-id \
+    --enable-decimal-float=no \
+    --without-isl \
+    --disable-libssp \
+"
+
+EXTRA_OECONF:append_aarchilp32 = " --with-abi=ilp32"
+
+EXTRA_OECONF_PATHS = "\
+    --with-gxx-include-dir=/not/exist{target_includedir}/c++/${BINV} \
+    --with-sysroot=/not/exist \
+    --with-build-sysroot=${STAGING_DIR_TARGET} \
+"
+
+# Is a binutils 2.26 issue, not gcc
+CVE_CHECK_IGNORE += "CVE-2021-37322"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-CVE-2021-42574.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-CVE-2021-42574.patch
new file mode 100644
index 0000000..4d680cc
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-CVE-2021-42574.patch
@@ -0,0 +1,2282 @@
+From bd5e882cf6e0def3dd1bc106075d59a303fe0d1e Mon Sep 17 00:00:00 2001
+From: David Malcolm <dmalcolm@redhat.com>
+Date: Mon, 18 Oct 2021 18:55:31 -0400
+Subject: [PATCH] diagnostics: escape non-ASCII source bytes for certain
+ diagnostics
+MIME-Version: 1.0
+Content-Type: text/plain; charset=utf8
+Content-Transfer-Encoding: 8bit
+
+This patch adds support to GCC's diagnostic subsystem for escaping certain
+bytes and Unicode characters when quoting source code.
+
+Specifically, this patch adds a new flag rich_location::m_escape_on_output
+which is a hint from a diagnostic that non-ASCII bytes in the pertinent
+lines of the user's source code should be escaped when printed.
+
+The patch sets this for the following diagnostics:
+- when complaining about stray bytes in the program (when these
+are non-printable)
+- when complaining about "null character(s) ignored");
+- for -Wnormalized= (and generate source ranges for such warnings)
+
+The escaping is controlled by a new option:
+  -fdiagnostics-escape-format=[unicode|bytes]
+
+For example, consider a diagnostic involing a source line containing the
+string "before" followed by the Unicode character U+03C0 ("GREEK SMALL
+LETTER PI", with UTF-8 encoding 0xCF 0x80) followed by the byte 0xBF
+(a stray UTF-8 trailing byte), followed by the string "after", where the
+diagnostic highlights the U+03C0 character.
+
+By default, this line will be printed verbatim to the user when
+reporting a diagnostic at it, as:
+
+ beforeÏXafter
+       ^
+
+(using X for the stray byte to avoid putting invalid UTF-8 in this
+commit message)
+
+If the diagnostic sets the "escape" flag, it will be printed as:
+
+ before<U+03C0><BF>after
+       ^~~~~~~~
+
+with -fdiagnostics-escape-format=unicode (the default), or as:
+
+  before<CF><80><BF>after
+        ^~~~~~~~
+
+if the user supplies -fdiagnostics-escape-format=bytes.
+
+This only affects how the source is printed; it does not affect
+how column numbers that are printed (as per -fdiagnostics-column-unit=
+and -fdiagnostics-column-origin=).
+
+gcc/c-family/ChangeLog:
+	* c-lex.c (c_lex_with_flags): When complaining about non-printable
+	CPP_OTHER tokens, set the "escape on output" flag.
+
+gcc/ChangeLog:
+	* common.opt (fdiagnostics-escape-format=): New.
+	(diagnostics_escape_format): New enum.
+	(DIAGNOSTICS_ESCAPE_FORMAT_UNICODE): New enum value.
+	(DIAGNOSTICS_ESCAPE_FORMAT_BYTES): Likewise.
+	* diagnostic-format-json.cc (json_end_diagnostic): Add
+	"escape-source" attribute.
+	* diagnostic-show-locus.c
+	(exploc_with_display_col::exploc_with_display_col): Replace
+	"tabstop" param with a cpp_char_column_policy and add an "aspect"
+	param.  Use these to compute m_display_col accordingly.
+	(struct char_display_policy): New struct.
+	(layout::m_policy): New field.
+	(layout::m_escape_on_output): New field.
+	(def_policy): New function.
+	(make_range): Update for changes to exploc_with_display_col ctor.
+	(default_print_decoded_ch): New.
+	(width_per_escaped_byte): New.
+	(escape_as_bytes_width): New.
+	(escape_as_bytes_print): New.
+	(escape_as_unicode_width): New.
+	(escape_as_unicode_print): New.
+	(make_policy): New.
+	(layout::layout): Initialize new fields.  Update m_exploc ctor
+	call for above change to ctor.
+	(layout::maybe_add_location_range): Update for changes to
+	exploc_with_display_col ctor.
+	(layout::calculate_x_offset_display): Update for change to
+	cpp_display_width.
+	(layout::print_source_line): Pass policy
+	to cpp_display_width_computation. Capture cpp_decoded_char when
+	calling process_next_codepoint.  Move printing of source code to
+	m_policy.m_print_cb.
+	(line_label::line_label): Pass in policy rather than context.
+	(layout::print_any_labels): Update for change to line_label ctor.
+	(get_affected_range): Pass in policy rather than context, updating
+	calls to location_compute_display_column accordingly.
+	(get_printed_columns): Likewise, also for cpp_display_width.
+	(correction::correction): Pass in policy rather than tabstop.
+	(correction::compute_display_cols): Pass m_policy rather than
+	m_tabstop to cpp_display_width.
+	(correction::m_tabstop): Replace with...
+	(correction::m_policy): ...this.
+	(line_corrections::line_corrections): Pass in policy rather than
+	context.
+	(line_corrections::m_context): Replace with...
+	(line_corrections::m_policy): ...this.
+	(line_corrections::add_hint): Update to use m_policy rather than
+	m_context.
+	(line_corrections::add_hint): Likewise.
+	(layout::print_trailing_fixits): Likewise.
+	(selftest::test_display_widths): New.
+	(selftest::test_layout_x_offset_display_utf8): Update to use
+	policy rather than tabstop.
+	(selftest::test_one_liner_labels_utf8): Add test of escaping
+	source lines.
+	(selftest::test_diagnostic_show_locus_one_liner_utf8): Update to
+	use policy rather than tabstop.
+	(selftest::test_overlapped_fixit_printing): Likewise.
+	(selftest::test_overlapped_fixit_printing_utf8): Likewise.
+	(selftest::test_overlapped_fixit_printing_2): Likewise.
+	(selftest::test_tab_expansion): Likewise.
+	(selftest::test_escaping_bytes_1): New.
+	(selftest::test_escaping_bytes_2): New.
+	(selftest::diagnostic_show_locus_c_tests): Call the new tests.
+	* diagnostic.c (diagnostic_initialize): Initialize
+	context->escape_format.
+	(convert_column_unit): Update to use default character width policy.
+	(selftest::test_diagnostic_get_location_text): Likewise.
+	* diagnostic.h (enum diagnostics_escape_format): New enum.
+	(diagnostic_context::escape_format): New field.
+	* doc/invoke.texi (-fdiagnostics-escape-format=): New option.
+	(-fdiagnostics-format=): Add "escape-source" attribute to examples
+	of JSON output, and document it.
+	* input.c (location_compute_display_column): Pass in "policy"
+	rather than "tabstop", passing to
+	cpp_byte_column_to_display_column.
+	(selftest::test_cpp_utf8): Update to use cpp_char_column_policy.
+	* input.h (class cpp_char_column_policy): New forward decl.
+	(location_compute_display_column): Pass in "policy" rather than
+	"tabstop".
+	* opts.c (common_handle_option): Handle
+	OPT_fdiagnostics_escape_format_.
+	* selftest.c (temp_source_file::temp_source_file): New ctor
+	overload taking a size_t.
+	* selftest.h (temp_source_file::temp_source_file): Likewise.
+
+gcc/testsuite/ChangeLog:
+	* c-c++-common/diagnostic-format-json-1.c: Add regexp to consume
+	"escape-source" attribute.
+	* c-c++-common/diagnostic-format-json-2.c: Likewise.
+	* c-c++-common/diagnostic-format-json-3.c: Likewise.
+	* c-c++-common/diagnostic-format-json-4.c: Likewise, twice.
+	* c-c++-common/diagnostic-format-json-5.c: Likewise.
+	* gcc.dg/cpp/warn-normalized-4-bytes.c: New test.
+	* gcc.dg/cpp/warn-normalized-4-unicode.c: New test.
+	* gcc.dg/encoding-issues-bytes.c: New test.
+	* gcc.dg/encoding-issues-unicode.c: New test.
+	* gfortran.dg/diagnostic-format-json-1.F90: Add regexp to consume
+	"escape-source" attribute.
+	* gfortran.dg/diagnostic-format-json-2.F90: Likewise.
+	* gfortran.dg/diagnostic-format-json-3.F90: Likewise.
+
+libcpp/ChangeLog:
+	* charset.c (convert_escape): Use encoding_rich_location when
+	complaining about nonprintable unknown escape sequences.
+	(cpp_display_width_computation::::cpp_display_width_computation):
+	Pass in policy rather than tabstop.
+	(cpp_display_width_computation::process_next_codepoint): Add "out"
+	param and populate *out if non-NULL.
+	(cpp_display_width_computation::advance_display_cols): Pass NULL
+	to process_next_codepoint.
+	(cpp_byte_column_to_display_column): Pass in policy rather than
+	tabstop.  Pass NULL to process_next_codepoint.
+	(cpp_display_column_to_byte_column): Pass in policy rather than
+	tabstop.
+	* errors.c (cpp_diagnostic_get_current_location): New function,
+	splitting out the logic from...
+	(cpp_diagnostic): ...here.
+	(cpp_warning_at): New function.
+	(cpp_pedwarning_at): New function.
+	* include/cpplib.h (cpp_warning_at): New decl for rich_location.
+	(cpp_pedwarning_at): Likewise.
+	(struct cpp_decoded_char): New.
+	(struct cpp_char_column_policy): New.
+	(cpp_display_width_computation::cpp_display_width_computation):
+	Replace "tabstop" param with "policy".
+	(cpp_display_width_computation::process_next_codepoint): Add "out"
+	param.
+	(cpp_display_width_computation::m_tabstop): Replace with...
+	(cpp_display_width_computation::m_policy): ...this.
+	(cpp_byte_column_to_display_column): Replace "tabstop" param with
+	"policy".
+	(cpp_display_width): Likewise.
+	(cpp_display_column_to_byte_column): Likewise.
+	* include/line-map.h (rich_location::escape_on_output_p): New.
+	(rich_location::set_escape_on_output): New.
+	(rich_location::m_escape_on_output): New.
+	* internal.h (cpp_diagnostic_get_current_location): New decl.
+	(class encoding_rich_location): New.
+	* lex.c (skip_whitespace): Use encoding_rich_location when
+	complaining about null characters.
+	(warn_about_normalization): Generate a source range when
+	complaining about improperly normalized tokens, rather than just a
+	point, and use encoding_rich_location so that the source code
+	is escaped on printing.
+	* line-map.c (rich_location::rich_location): Initialize
+	m_escape_on_output.
+
+Signed-off-by: David Malcolm <dmalcolm@redhat.com>
+
+CVE: CVE-2021-42574
+Upstream-Status: Backport [https://gcc.gnu.org/git/gitweb.cgi?p=gcc.git;h=bd5e882cf6e0def3dd1bc106075d59a303fe0d1e]
+Signed-off-by: Pgowda <pgowda.cve@gmail.com>
+
+---
+ gcc/c-family/c-lex.c                          |   6 +-
+ gcc/common.opt                                |  13 +
+ gcc/diagnostic-format-json.cc                 |   3 +
+ gcc/diagnostic-show-locus.c                   | 580 +++++++++++++++---
+ gcc/diagnostic.c                              |  10 +-
+ gcc/diagnostic.h                              |  18 +
+ gcc/doc/invoke.texi                           |  43 +-
+ gcc/input.c                                   |  62 +-
+ gcc/input.h                                   |   7 +-
+ gcc/opts.c                                    |   4 +
+ gcc/selftest.c                                |  15 +
+ gcc/selftest.h                                |   2 +
+ .../c-c++-common/diagnostic-format-json-1.c   |   1 +
+ .../c-c++-common/diagnostic-format-json-2.c   |   1 +
+ .../c-c++-common/diagnostic-format-json-3.c   |   1 +
+ .../c-c++-common/diagnostic-format-json-4.c   |   2 +
+ .../c-c++-common/diagnostic-format-json-5.c   |   1 +
+ .../gcc.dg/cpp/warn-normalized-4-bytes.c      |  21 +
+ .../gcc.dg/cpp/warn-normalized-4-unicode.c    |  19 +
+ gcc/testsuite/gcc.dg/encoding-issues-bytes.c  | Bin 0 -> 595 bytes
+ .../gcc.dg/encoding-issues-unicode.c          | Bin 0 -> 613 bytes
+ .../gfortran.dg/diagnostic-format-json-1.F90  |   1 +
+ .../gfortran.dg/diagnostic-format-json-2.F90  |   1 +
+ .../gfortran.dg/diagnostic-format-json-3.F90  |   1 +
+ libcpp/charset.c                              |  63 +-
+ libcpp/errors.c                               |  82 ++-
+ libcpp/include/cpplib.h                       |  76 ++-
+ libcpp/include/line-map.h                     |  13 +
+ libcpp/internal.h                             |  23 +
+ libcpp/lex.c                                  |  38 +-
+ libcpp/line-map.c                             |   3 +-
+ 31 files changed, 942 insertions(+), 168 deletions(-)
+ create mode 100644 gcc/testsuite/gcc.dg/cpp/warn-normalized-4-bytes.c
+ create mode 100644 gcc/testsuite/gcc.dg/cpp/warn-normalized-4-unicode.c
+ create mode 100644 gcc/testsuite/gcc.dg/encoding-issues-bytes.c
+ create mode 100644 gcc/testsuite/gcc.dg/encoding-issues-unicode.c
+
+diff --git a/gcc/c-family/c-lex.c b/gcc/c-family/c-lex.c
+--- a/gcc/c-family/c-lex.c	2021-07-27 23:55:06.980283060 -0700
++++ b/gcc/c-family/c-lex.c	2021-12-14 01:16:01.541943272 -0800
+@@ -603,7 +603,11 @@ c_lex_with_flags (tree *value, location_
+ 	else if (ISGRAPH (c))
+ 	  error_at (*loc, "stray %qc in program", (int) c);
+ 	else
+-	  error_at (*loc, "stray %<\\%o%> in program", (int) c);
++	  {
++	    rich_location rich_loc (line_table, *loc);
++	    rich_loc.set_escape_on_output (true);
++	    error_at (&rich_loc, "stray %<\\%o%> in program", (int) c);
++	  }
+       }
+       goto retry;
+ 
+diff --git a/gcc/common.opt b/gcc/common.opt
+--- a/gcc/common.opt	2021-12-13 22:08:44.939137107 -0800
++++ b/gcc/common.opt	2021-12-14 01:16:01.541943272 -0800
+@@ -1348,6 +1348,10 @@ fdiagnostics-format=
+ Common Joined RejectNegative Enum(diagnostics_output_format)
+ -fdiagnostics-format=[text|json]	Select output format.
+ 
++fdiagnostics-escape-format=
++Common Joined RejectNegative Enum(diagnostics_escape_format)
++-fdiagnostics-escape-format=[unicode|bytes]	Select how to escape non-printable-ASCII bytes in the source for diagnostics that suggest it.
++
+ ; Required for these enum values.
+ SourceInclude
+ diagnostic.h
+@@ -1362,6 +1366,15 @@ EnumValue
+ Enum(diagnostics_column_unit) String(byte) Value(DIAGNOSTICS_COLUMN_UNIT_BYTE)
+ 
+ Enum
++Name(diagnostics_escape_format) Type(int)
++
++EnumValue
++Enum(diagnostics_escape_format) String(unicode) Value(DIAGNOSTICS_ESCAPE_FORMAT_UNICODE)
++
++EnumValue
++Enum(diagnostics_escape_format) String(bytes) Value(DIAGNOSTICS_ESCAPE_FORMAT_BYTES)
++
++Enum
+ Name(diagnostics_output_format) Type(int)
+ 
+ EnumValue
+diff --git a/gcc/diagnostic.c b/gcc/diagnostic.c
+--- a/gcc/diagnostic.c	2021-07-27 23:55:07.232286576 -0700
++++ b/gcc/diagnostic.c	2021-12-14 01:16:01.545943202 -0800
+@@ -230,6 +230,7 @@ diagnostic_initialize (diagnostic_contex
+   context->column_unit = DIAGNOSTICS_COLUMN_UNIT_DISPLAY;
+   context->column_origin = 1;
+   context->tabstop = 8;
++  context->escape_format = DIAGNOSTICS_ESCAPE_FORMAT_UNICODE;
+   context->edit_context_ptr = NULL;
+   context->diagnostic_group_nesting_depth = 0;
+   context->diagnostic_group_emission_count = 0;
+@@ -382,7 +383,10 @@ convert_column_unit (enum diagnostics_co
+       gcc_unreachable ();
+ 
+     case DIAGNOSTICS_COLUMN_UNIT_DISPLAY:
+-      return location_compute_display_column (s, tabstop);
++      {
++	cpp_char_column_policy policy (tabstop, cpp_wcwidth);
++	return location_compute_display_column (s, policy);
++      }
+ 
+     case DIAGNOSTICS_COLUMN_UNIT_BYTE:
+       return s.column;
+@@ -2275,8 +2279,8 @@ test_diagnostic_get_location_text ()
+     const char *const content = "smile \xf0\x9f\x98\x82\n";
+     const int line_bytes = strlen (content) - 1;
+     const int def_tabstop = 8;
+-    const int display_width = cpp_display_width (content, line_bytes,
+-						 def_tabstop);
++    const cpp_char_column_policy policy (def_tabstop, cpp_wcwidth);
++    const int display_width = cpp_display_width (content, line_bytes, policy);
+     ASSERT_EQ (line_bytes - 2, display_width);
+     temp_source_file tmp (SELFTEST_LOCATION, ".c", content);
+     const char *const fname = tmp.get_filename ();
+diff --git a/gcc/diagnostic-format-json.cc b/gcc/diagnostic-format-json.cc
+--- a/gcc/diagnostic-format-json.cc	2021-07-27 23:55:07.232286576 -0700
++++ b/gcc/diagnostic-format-json.cc	2021-12-14 01:16:01.541943272 -0800
+@@ -264,6 +264,9 @@ json_end_diagnostic (diagnostic_context
+       json::value *path_value = context->make_json_for_path (context, path);
+       diag_obj->set ("path", path_value);
+     }
++
++  diag_obj->set ("escape-source",
++		 new json::literal (richloc->escape_on_output_p ()));
+ }
+ 
+ /* No-op implementation of "begin_group_cb" for JSON output.  */
+diff --git a/gcc/diagnostic.h b/gcc/diagnostic.h
+--- a/gcc/diagnostic.h	2021-07-27 23:55:07.236286632 -0700
++++ b/gcc/diagnostic.h	2021-12-14 01:16:01.545943202 -0800
+@@ -38,6 +38,20 @@ enum diagnostics_column_unit
+   DIAGNOSTICS_COLUMN_UNIT_BYTE
+ };
+ 
++/* An enum for controlling how to print non-ASCII characters/bytes when
++   a diagnostic suggests escaping the source code on output.  */
++
++enum diagnostics_escape_format
++{
++  /* Escape non-ASCII Unicode characters in the form <U+XXXX> and
++     non-UTF-8 bytes in the form <XX>.  */
++  DIAGNOSTICS_ESCAPE_FORMAT_UNICODE,
++
++  /* Escape non-ASCII bytes in the form <XX> (thus showing the underlying
++     encoding of non-ASCII Unicode characters).  */
++  DIAGNOSTICS_ESCAPE_FORMAT_BYTES
++};
++
+ /* Enum for overriding the standard output format.  */
+ 
+ enum diagnostics_output_format
+@@ -320,6 +334,10 @@ struct diagnostic_context
+   /* The size of the tabstop for tab expansion.  */
+   int tabstop;
+ 
++  /* How should non-ASCII/non-printable bytes be escaped when
++     a diagnostic suggests escaping the source code on output.  */
++  enum diagnostics_escape_format escape_format;
++
+   /* If non-NULL, an edit_context to which fix-it hints should be
+      applied, for generating patches.  */
+   edit_context *edit_context_ptr;
+diff --git a/gcc/diagnostic-show-locus.c b/gcc/diagnostic-show-locus.c
+--- a/gcc/diagnostic-show-locus.c	2021-07-27 23:55:07.232286576 -0700
++++ b/gcc/diagnostic-show-locus.c	2021-12-14 01:16:01.545943202 -0800
+@@ -175,10 +175,26 @@ enum column_unit {
+ class exploc_with_display_col : public expanded_location
+ {
+  public:
+-  exploc_with_display_col (const expanded_location &exploc, int tabstop)
+-    : expanded_location (exploc),
+-      m_display_col (location_compute_display_column (exploc, tabstop))
+-  {}
++  exploc_with_display_col (const expanded_location &exploc,
++			   const cpp_char_column_policy &policy,
++			   enum location_aspect aspect)
++  : expanded_location (exploc),
++    m_display_col (location_compute_display_column (exploc, policy))
++  {
++    if (exploc.column > 0)
++      {
++	/* m_display_col is now the final column of the byte.
++	   If escaping has happened, we may want the first column instead.  */
++	if (aspect != LOCATION_ASPECT_FINISH)
++	  {
++	    expanded_location prev_exploc (exploc);
++	    prev_exploc.column--;
++	    int prev_display_col
++	      = (location_compute_display_column (prev_exploc, policy));
++	    m_display_col = prev_display_col + 1;
++	  }
++      }
++  }
+ 
+   int m_display_col;
+ };
+@@ -313,6 +329,31 @@ test_line_span ()
+ 
+ #endif /* #if CHECKING_P */
+ 
++/* A bundle of information containing how to print unicode
++   characters and bytes when quoting source code.
++
++   Provides a unified place to support escaping some subset
++   of characters to some format.
++
++   Extends char_column_policy; printing is split out to avoid
++   libcpp having to know about pretty_printer.  */
++
++struct char_display_policy : public cpp_char_column_policy
++{
++ public:
++  char_display_policy (int tabstop,
++		       int (*width_cb) (cppchar_t c),
++		       void (*print_cb) (pretty_printer *pp,
++					 const cpp_decoded_char &cp))
++  : cpp_char_column_policy (tabstop, width_cb),
++    m_print_cb (print_cb)
++  {
++  }
++
++  void (*m_print_cb) (pretty_printer *pp,
++		      const cpp_decoded_char &cp);
++};
++
+ /* A class to control the overall layout when printing a diagnostic.
+ 
+    The layout is determined within the constructor.
+@@ -345,6 +386,8 @@ class layout
+ 
+   void print_line (linenum_type row);
+ 
++  void on_bad_codepoint (const char *ptr, cppchar_t ch, size_t ch_sz);
++
+  private:
+   bool will_show_line_p (linenum_type row) const;
+   void print_leading_fixits (linenum_type row);
+@@ -386,6 +429,7 @@ class layout
+  private:
+   diagnostic_context *m_context;
+   pretty_printer *m_pp;
++  char_display_policy m_policy;
+   location_t m_primary_loc;
+   exploc_with_display_col m_exploc;
+   colorizer m_colorizer;
+@@ -398,6 +442,7 @@ class layout
+   auto_vec <line_span> m_line_spans;
+   int m_linenum_width;
+   int m_x_offset_display;
++  bool m_escape_on_output;
+ };
+ 
+ /* Implementation of "class colorizer".  */
+@@ -646,6 +691,11 @@ layout_range::intersects_line_p (linenum
+ /* Default for when we don't care what the tab expansion is set to.  */
+ static const int def_tabstop = 8;
+ 
++static cpp_char_column_policy def_policy ()
++{
++  return cpp_char_column_policy (8, cpp_wcwidth);
++}
++
+ /* Create some expanded locations for testing layout_range.  The filename
+    member of the explocs is set to the empty string.  This member will only be
+    inspected by the calls to location_compute_display_column() made from the
+@@ -662,10 +712,13 @@ make_range (int start_line, int start_co
+     = {"", start_line, start_col, NULL, false};
+   const expanded_location finish_exploc
+     = {"", end_line, end_col, NULL, false};
+-  return layout_range (exploc_with_display_col (start_exploc, def_tabstop),
+-		       exploc_with_display_col (finish_exploc, def_tabstop),
++  return layout_range (exploc_with_display_col (start_exploc, def_policy (),
++						LOCATION_ASPECT_START),
++		       exploc_with_display_col (finish_exploc, def_policy (),
++						LOCATION_ASPECT_FINISH),
+ 		       SHOW_RANGE_WITHOUT_CARET,
+-		       exploc_with_display_col (start_exploc, def_tabstop),
++		       exploc_with_display_col (start_exploc, def_policy (),
++						LOCATION_ASPECT_CARET),
+ 		       0, NULL);
+ }
+ 
+@@ -959,6 +1012,164 @@ fixit_cmp (const void *p_a, const void *
+   return hint_a->get_start_loc () - hint_b->get_start_loc ();
+ }
+ 
++/* Callbacks for use when not escaping the source.  */
++
++/* The default callback for char_column_policy::m_width_cb is cpp_wcwidth.  */
++
++/* Callback for char_display_policy::m_print_cb for printing source chars
++   when not escaping the source.  */
++
++static void
++default_print_decoded_ch (pretty_printer *pp,
++			  const cpp_decoded_char &decoded_ch)
++{
++  for (const char *ptr = decoded_ch.m_start_byte;
++       ptr != decoded_ch.m_next_byte; ptr++)
++    {
++      if (*ptr == '\0' || *ptr == '\r')
++	{
++	  pp_space (pp);
++	  continue;
++	}
++
++      pp_character (pp, *ptr);
++    }
++}
++
++/* Callbacks for use with DIAGNOSTICS_ESCAPE_FORMAT_BYTES.  */
++
++static const int width_per_escaped_byte = 4;
++
++/* Callback for char_column_policy::m_width_cb for determining the
++   display width when escaping with DIAGNOSTICS_ESCAPE_FORMAT_BYTES.  */
++
++static int
++escape_as_bytes_width (cppchar_t ch)
++{
++  if (ch < 0x80 && ISPRINT (ch))
++    return cpp_wcwidth (ch);
++  else
++    {
++      if (ch <=   0x7F) return 1 * width_per_escaped_byte;
++      if (ch <=  0x7FF) return 2 * width_per_escaped_byte;
++      if (ch <= 0xFFFF) return 3 * width_per_escaped_byte;
++      return 4 * width_per_escaped_byte;
++    }
++}
++
++/* Callback for char_display_policy::m_print_cb for printing source chars
++   when escaping with DIAGNOSTICS_ESCAPE_FORMAT_BYTES.  */
++
++static void
++escape_as_bytes_print (pretty_printer *pp,
++		       const cpp_decoded_char &decoded_ch)
++{
++  if (!decoded_ch.m_valid_ch)
++    {
++      for (const char *iter = decoded_ch.m_start_byte;
++	   iter != decoded_ch.m_next_byte; ++iter)
++	{
++	  char buf[16];
++	  sprintf (buf, "<%02x>", (unsigned char)*iter);
++	  pp_string (pp, buf);
++	}
++      return;
++    }
++
++  cppchar_t ch = decoded_ch.m_ch;
++  if (ch < 0x80 && ISPRINT (ch))
++    pp_character (pp, ch);
++  else
++    {
++      for (const char *iter = decoded_ch.m_start_byte;
++	   iter < decoded_ch.m_next_byte; ++iter)
++	{
++	  char buf[16];
++	  sprintf (buf, "<%02x>", (unsigned char)*iter);
++	  pp_string (pp, buf);
++	}
++    }
++}
++
++/* Callbacks for use with DIAGNOSTICS_ESCAPE_FORMAT_UNICODE.  */
++
++/* Callback for char_column_policy::m_width_cb for determining the
++   display width when escaping with DIAGNOSTICS_ESCAPE_FORMAT_UNICODE.  */
++
++static int
++escape_as_unicode_width (cppchar_t ch)
++{
++  if (ch < 0x80 && ISPRINT (ch))
++    return cpp_wcwidth (ch);
++  else
++    {
++      // Width of "<U+%04x>"
++      if (ch > 0xfffff)
++	return 10;
++      else if (ch > 0xffff)
++	return 9;
++      else
++	return 8;
++    }
++}
++
++/* Callback for char_display_policy::m_print_cb for printing source chars
++   when escaping with DIAGNOSTICS_ESCAPE_FORMAT_UNICODE.  */
++
++static void
++escape_as_unicode_print (pretty_printer *pp,
++			 const cpp_decoded_char &decoded_ch)
++{
++  if (!decoded_ch.m_valid_ch)
++    {
++      escape_as_bytes_print (pp, decoded_ch);
++      return;
++    }
++
++  cppchar_t ch = decoded_ch.m_ch;
++  if (ch < 0x80 && ISPRINT (ch))
++    pp_character (pp, ch);
++  else
++    {
++      char buf[16];
++      sprintf (buf, "<U+%04X>", ch);
++      pp_string (pp, buf);
++    }
++}
++
++/* Populate a char_display_policy based on DC and RICHLOC.  */
++
++static char_display_policy
++make_policy (const diagnostic_context &dc,
++	     const rich_location &richloc)
++{
++  /* The default is to not escape non-ASCII bytes.  */
++  char_display_policy result
++    (dc.tabstop, cpp_wcwidth, default_print_decoded_ch);
++
++  /* If the diagnostic suggests escaping non-ASCII bytes, then
++     use policy from user-supplied options.  */
++  if (richloc.escape_on_output_p ())
++    {
++      result.m_undecoded_byte_width = width_per_escaped_byte;
++      switch (dc.escape_format)
++	{
++	default:
++	  gcc_unreachable ();
++	case DIAGNOSTICS_ESCAPE_FORMAT_UNICODE:
++	  result.m_width_cb = escape_as_unicode_width;
++	  result.m_print_cb = escape_as_unicode_print;
++	  break;
++	case DIAGNOSTICS_ESCAPE_FORMAT_BYTES:
++	  result.m_width_cb = escape_as_bytes_width;
++	  result.m_print_cb = escape_as_bytes_print;
++	  break;
++	}
++    }
++
++  return result;
++}
++
+ /* Implementation of class layout.  */
+ 
+ /* Constructor for class layout.
+@@ -975,8 +1186,10 @@ layout::layout (diagnostic_context * con
+ 		diagnostic_t diagnostic_kind)
+ : m_context (context),
+   m_pp (context->printer),
++  m_policy (make_policy (*context, *richloc)),
+   m_primary_loc (richloc->get_range (0)->m_loc),
+-  m_exploc (richloc->get_expanded_location (0), context->tabstop),
++  m_exploc (richloc->get_expanded_location (0), m_policy,
++	    LOCATION_ASPECT_CARET),
+   m_colorizer (context, diagnostic_kind),
+   m_colorize_source_p (context->colorize_source_p),
+   m_show_labels_p (context->show_labels_p),
+@@ -986,7 +1199,8 @@ layout::layout (diagnostic_context * con
+   m_fixit_hints (richloc->get_num_fixit_hints ()),
+   m_line_spans (1 + richloc->get_num_locations ()),
+   m_linenum_width (0),
+-  m_x_offset_display (0)
++  m_x_offset_display (0),
++  m_escape_on_output (richloc->escape_on_output_p ())
+ {
+   for (unsigned int idx = 0; idx < richloc->get_num_locations (); idx++)
+     {
+@@ -1072,10 +1286,13 @@ layout::maybe_add_location_range (const
+ 
+   /* Everything is now known to be in the correct source file,
+      but it may require further sanitization.  */
+-  layout_range ri (exploc_with_display_col (start, m_context->tabstop),
+-		   exploc_with_display_col (finish, m_context->tabstop),
++  layout_range ri (exploc_with_display_col (start, m_policy,
++					    LOCATION_ASPECT_START),
++		   exploc_with_display_col (finish, m_policy,
++					    LOCATION_ASPECT_FINISH),
+ 		   loc_range->m_range_display_kind,
+-		   exploc_with_display_col (caret, m_context->tabstop),
++		   exploc_with_display_col (caret, m_policy,
++					    LOCATION_ASPECT_CARET),
+ 		   original_idx, loc_range->m_label);
+ 
+   /* If we have a range that finishes before it starts (perhaps
+@@ -1409,7 +1626,7 @@ layout::calculate_x_offset_display ()
+     = get_line_bytes_without_trailing_whitespace (line.get_buffer (),
+ 						  line.length ());
+   int eol_display_column
+-    = cpp_display_width (line.get_buffer (), line_bytes, m_context->tabstop);
++    = cpp_display_width (line.get_buffer (), line_bytes, m_policy);
+   if (caret_display_column > eol_display_column
+       || !caret_display_column)
+     {
+@@ -1488,7 +1705,7 @@ layout::print_source_line (linenum_type
+   /* This object helps to keep track of which display column we are at, which is
+      necessary for computing the line bounds in display units, for doing
+      tab expansion, and for implementing m_x_offset_display.  */
+-  cpp_display_width_computation dw (line, line_bytes, m_context->tabstop);
++  cpp_display_width_computation dw (line, line_bytes, m_policy);
+ 
+   /* Skip the first m_x_offset_display display columns.  In case the leading
+      portion that will be skipped ends with a character with wcwidth > 1, then
+@@ -1536,7 +1753,8 @@ layout::print_source_line (linenum_type
+ 	 tabs and replacing some control bytes with spaces as necessary.  */
+       const char *c = dw.next_byte ();
+       const int start_disp_col = dw.display_cols_processed () + 1;
+-      const int this_display_width = dw.process_next_codepoint ();
++      cpp_decoded_char cp;
++      const int this_display_width = dw.process_next_codepoint (&cp);
+       if (*c == '\t')
+ 	{
+ 	  /* The returned display width is the number of spaces into which the
+@@ -1545,15 +1763,6 @@ layout::print_source_line (linenum_type
+ 	    pp_space (m_pp);
+ 	  continue;
+ 	}
+-      if (*c == '\0' || *c == '\r')
+-	{
+-	  /* cpp_wcwidth() promises to return 1 for all control bytes, and we
+-	     want to output these as a single space too, so this case is
+-	     actually the same as the '\t' case.  */
+-	  gcc_assert (this_display_width == 1);
+-	  pp_space (m_pp);
+-	  continue;
+-	}
+ 
+       /* We have a (possibly multibyte) character to output; update the line
+ 	 bounds if it is not whitespace.  */
+@@ -1565,7 +1774,8 @@ layout::print_source_line (linenum_type
+ 	}
+ 
+       /* Output the character.  */
+-      while (c != dw.next_byte ()) pp_character (m_pp, *c++);
++      m_policy.m_print_cb (m_pp, cp);
++      c = dw.next_byte ();
+     }
+   print_newline ();
+   return lbounds;
+@@ -1664,14 +1874,14 @@ layout::print_annotation_line (linenum_t
+ class line_label
+ {
+ public:
+-  line_label (diagnostic_context *context, int state_idx, int column,
++  line_label (const cpp_char_column_policy &policy,
++	      int state_idx, int column,
+ 	      label_text text)
+   : m_state_idx (state_idx), m_column (column),
+     m_text (text), m_label_line (0), m_has_vbar (true)
+   {
+     const int bytes = strlen (text.m_buffer);
+-    m_display_width
+-      = cpp_display_width (text.m_buffer, bytes, context->tabstop);
++    m_display_width = cpp_display_width (text.m_buffer, bytes, policy);
+   }
+ 
+   /* Sorting is primarily by column, then by state index.  */
+@@ -1731,7 +1941,7 @@ layout::print_any_labels (linenum_type r
+ 	if (text.m_buffer == NULL)
+ 	  continue;
+ 
+-	labels.safe_push (line_label (m_context, i, disp_col, text));
++	labels.safe_push (line_label (m_policy, i, disp_col, text));
+       }
+   }
+ 
+@@ -2011,7 +2221,7 @@ public:
+ 
+ /* Get the range of bytes or display columns that HINT would affect.  */
+ static column_range
+-get_affected_range (diagnostic_context *context,
++get_affected_range (const cpp_char_column_policy &policy,
+ 		    const fixit_hint *hint, enum column_unit col_unit)
+ {
+   expanded_location exploc_start = expand_location (hint->get_start_loc ());
+@@ -2022,13 +2232,11 @@ get_affected_range (diagnostic_context *
+   int finish_column;
+   if (col_unit == CU_DISPLAY_COLS)
+     {
+-      start_column
+-	= location_compute_display_column (exploc_start, context->tabstop);
++      start_column = location_compute_display_column (exploc_start, policy);
+       if (hint->insertion_p ())
+ 	finish_column = start_column - 1;
+       else
+-	finish_column
+-	  = location_compute_display_column (exploc_finish, context->tabstop);
++	finish_column = location_compute_display_column (exploc_finish, policy);
+     }
+   else
+     {
+@@ -2041,12 +2249,13 @@ get_affected_range (diagnostic_context *
+ /* Get the range of display columns that would be printed for HINT.  */
+ 
+ static column_range
+-get_printed_columns (diagnostic_context *context, const fixit_hint *hint)
++get_printed_columns (const cpp_char_column_policy &policy,
++		     const fixit_hint *hint)
+ {
+   expanded_location exploc = expand_location (hint->get_start_loc ());
+-  int start_column = location_compute_display_column (exploc, context->tabstop);
++  int start_column = location_compute_display_column (exploc, policy);
+   int hint_width = cpp_display_width (hint->get_string (), hint->get_length (),
+-				      context->tabstop);
++				      policy);
+   int final_hint_column = start_column + hint_width - 1;
+   if (hint->insertion_p ())
+     {
+@@ -2056,8 +2265,7 @@ get_printed_columns (diagnostic_context
+     {
+       exploc = expand_location (hint->get_next_loc ());
+       --exploc.column;
+-      int finish_column
+-	= location_compute_display_column (exploc, context->tabstop);
++      int finish_column = location_compute_display_column (exploc, policy);
+       return column_range (start_column,
+ 			   MAX (finish_column, final_hint_column));
+     }
+@@ -2075,13 +2283,13 @@ public:
+ 	      column_range affected_columns,
+ 	      column_range printed_columns,
+ 	      const char *new_text, size_t new_text_len,
+-	      int tabstop)
++	      const cpp_char_column_policy &policy)
+   : m_affected_bytes (affected_bytes),
+     m_affected_columns (affected_columns),
+     m_printed_columns (printed_columns),
+     m_text (xstrdup (new_text)),
+     m_byte_length (new_text_len),
+-    m_tabstop (tabstop),
++    m_policy (policy),
+     m_alloc_sz (new_text_len + 1)
+   {
+     compute_display_cols ();
+@@ -2099,7 +2307,7 @@ public:
+ 
+   void compute_display_cols ()
+   {
+-    m_display_cols = cpp_display_width (m_text, m_byte_length, m_tabstop);
++    m_display_cols = cpp_display_width (m_text, m_byte_length, m_policy);
+   }
+ 
+   void overwrite (int dst_offset, const char_span &src_span)
+@@ -2127,7 +2335,7 @@ public:
+   char *m_text;
+   size_t m_byte_length; /* Not including null-terminator.  */
+   int m_display_cols;
+-  int m_tabstop;
++  const cpp_char_column_policy &m_policy;
+   size_t m_alloc_sz;
+ };
+ 
+@@ -2163,15 +2371,16 @@ correction::ensure_terminated ()
+ class line_corrections
+ {
+ public:
+-  line_corrections (diagnostic_context *context, const char *filename,
++  line_corrections (const char_display_policy &policy,
++		    const char *filename,
+ 		    linenum_type row)
+-    : m_context (context), m_filename (filename), m_row (row)
++  : m_policy (policy), m_filename (filename), m_row (row)
+   {}
+   ~line_corrections ();
+ 
+   void add_hint (const fixit_hint *hint);
+ 
+-  diagnostic_context *m_context;
++  const char_display_policy &m_policy;
+   const char *m_filename;
+   linenum_type m_row;
+   auto_vec <correction *> m_corrections;
+@@ -2217,10 +2426,10 @@ source_line::source_line (const char *fi
+ void
+ line_corrections::add_hint (const fixit_hint *hint)
+ {
+-  column_range affected_bytes = get_affected_range (m_context, hint, CU_BYTES);
+-  column_range affected_columns = get_affected_range (m_context, hint,
++  column_range affected_bytes = get_affected_range (m_policy, hint, CU_BYTES);
++  column_range affected_columns = get_affected_range (m_policy, hint,
+ 						      CU_DISPLAY_COLS);
+-  column_range printed_columns = get_printed_columns (m_context, hint);
++  column_range printed_columns = get_printed_columns (m_policy, hint);
+ 
+   /* Potentially consolidate.  */
+   if (!m_corrections.is_empty ())
+@@ -2289,7 +2498,7 @@ line_corrections::add_hint (const fixit_
+ 					   printed_columns,
+ 					   hint->get_string (),
+ 					   hint->get_length (),
+-					   m_context->tabstop));
++					   m_policy));
+ }
+ 
+ /* If there are any fixit hints on source line ROW, print them.
+@@ -2303,7 +2512,7 @@ layout::print_trailing_fixits (linenum_t
+ {
+   /* Build a list of correction instances for the line,
+      potentially consolidating hints (for the sake of readability).  */
+-  line_corrections corrections (m_context, m_exploc.file, row);
++  line_corrections corrections (m_policy, m_exploc.file, row);
+   for (unsigned int i = 0; i < m_fixit_hints.length (); i++)
+     {
+       const fixit_hint *hint = m_fixit_hints[i];
+@@ -2646,6 +2855,59 @@ namespace selftest {
+ 
+ /* Selftests for diagnostic_show_locus.  */
+ 
++/* Verify that cpp_display_width correctly handles escaping.  */
++
++static void
++test_display_widths ()
++{
++  gcc_rich_location richloc (UNKNOWN_LOCATION);
++
++  /* U+03C0 "GREEK SMALL LETTER PI".  */
++  const char *pi = "\xCF\x80";
++  /* U+1F642 "SLIGHTLY SMILING FACE".  */
++  const char *emoji = "\xF0\x9F\x99\x82";
++  /* Stray trailing byte of a UTF-8 character.  */
++  const char *stray = "\xBF";
++  /* U+10FFFF.  */
++  const char *max_codepoint = "\xF4\x8F\xBF\xBF";
++
++  /* No escaping.  */
++  {
++    test_diagnostic_context dc;
++    char_display_policy policy (make_policy (dc, richloc));
++    ASSERT_EQ (cpp_display_width (pi, strlen (pi), policy), 1);
++    ASSERT_EQ (cpp_display_width (emoji, strlen (emoji), policy), 2);
++    ASSERT_EQ (cpp_display_width (stray, strlen (stray), policy), 1);
++    /* Don't check width of U+10FFFF; it's in a private use plane.  */
++  }
++
++  richloc.set_escape_on_output (true);
++
++  {
++    test_diagnostic_context dc;
++    dc.escape_format = DIAGNOSTICS_ESCAPE_FORMAT_UNICODE;
++    char_display_policy policy (make_policy (dc, richloc));
++    ASSERT_EQ (cpp_display_width (pi, strlen (pi), policy), 8);
++    ASSERT_EQ (cpp_display_width (emoji, strlen (emoji), policy), 9);
++    ASSERT_EQ (cpp_display_width (stray, strlen (stray), policy), 4);
++    ASSERT_EQ (cpp_display_width (max_codepoint, strlen (max_codepoint),
++				  policy),
++	       strlen ("<U+10FFFF>"));
++  }
++
++  {
++    test_diagnostic_context dc;
++    dc.escape_format = DIAGNOSTICS_ESCAPE_FORMAT_BYTES;
++    char_display_policy policy (make_policy (dc, richloc));
++    ASSERT_EQ (cpp_display_width (pi, strlen (pi), policy), 8);
++    ASSERT_EQ (cpp_display_width (emoji, strlen (emoji), policy), 16);
++    ASSERT_EQ (cpp_display_width (stray, strlen (stray), policy), 4);
++    ASSERT_EQ (cpp_display_width (max_codepoint, strlen (max_codepoint),
++				  policy),
++	       16);
++  }
++}
++
+ /* For precise tests of the layout, make clear where the source line will
+    start.  test_left_margin sets the total byte count from the left side of the
+    screen to the start of source lines, after the line number and the separator,
+@@ -2715,10 +2977,10 @@ test_layout_x_offset_display_utf8 (const
+   char_span lspan = location_get_source_line (tmp.get_filename (), 1);
+   ASSERT_EQ (line_display_cols,
+ 	     cpp_display_width (lspan.get_buffer (), lspan.length (),
+-				def_tabstop));
++				def_policy ()));
+   ASSERT_EQ (line_display_cols,
+ 	     location_compute_display_column (expand_location (line_end),
+-					      def_tabstop));
++					      def_policy ()));
+   ASSERT_EQ (0, memcmp (lspan.get_buffer () + (emoji_col - 1),
+ 			"\xf0\x9f\x98\x82\xf0\x9f\x98\x82", 8));
+ 
+@@ -2866,12 +3128,13 @@ test_layout_x_offset_display_tab (const
+   ASSERT_EQ ('\t', *(lspan.get_buffer () + (tab_col - 1)));
+   for (int tabstop = 1; tabstop != num_tabstops; ++tabstop)
+     {
++      cpp_char_column_policy policy (tabstop, cpp_wcwidth);
+       ASSERT_EQ (line_bytes + extra_width[tabstop],
+ 		 cpp_display_width (lspan.get_buffer (), lspan.length (),
+-				    tabstop));
++				    policy));
+       ASSERT_EQ (line_bytes + extra_width[tabstop],
+ 		 location_compute_display_column (expand_location (line_end),
+-						  tabstop));
++						  policy));
+     }
+ 
+   /* Check that the tab is expanded to the expected number of spaces.  */
+@@ -4003,6 +4266,43 @@ test_one_liner_labels_utf8 ()
+ 			   " bb\xf0\x9f\x98\x82\xf0\x9f\x98\x82\n",
+ 		  pp_formatted_text (dc.printer));
+   }
++
++  /* Example of escaping the source lines.  */
++  {
++    text_range_label label0 ("label 0\xf0\x9f\x98\x82");
++    text_range_label label1 ("label 1\xcf\x80");
++    text_range_label label2 ("label 2\xcf\x80");
++    gcc_rich_location richloc (foo, &label0);
++    richloc.add_range (bar, SHOW_RANGE_WITHOUT_CARET, &label1);
++    richloc.add_range (field, SHOW_RANGE_WITHOUT_CARET, &label2);
++    richloc.set_escape_on_output (true);
++
++    {
++      test_diagnostic_context dc;
++      dc.escape_format = DIAGNOSTICS_ESCAPE_FORMAT_UNICODE;
++      diagnostic_show_locus (&dc, &richloc, DK_ERROR);
++      ASSERT_STREQ (" <U+1F602>_foo = <U+03C0>_bar.<U+1F602>_field<U+03C0>;\n"
++		    " ^~~~~~~~~~~~~   ~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~~~~~\n"
++		    " |               |            |\n"
++		    " |               |            label 2\xcf\x80\n"
++		    " |               label 1\xcf\x80\n"
++		    " label 0\xf0\x9f\x98\x82\n",
++		    pp_formatted_text (dc.printer));
++    }
++    {
++      test_diagnostic_context dc;
++      dc.escape_format = DIAGNOSTICS_ESCAPE_FORMAT_BYTES;
++      diagnostic_show_locus (&dc, &richloc, DK_ERROR);
++      ASSERT_STREQ
++	(" <f0><9f><98><82>_foo = <cf><80>_bar.<f0><9f><98><82>_field<cf><80>;\n"
++	 " ^~~~~~~~~~~~~~~~~~~~   ~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n"
++	 " |                      |            |\n"
++	 " |                      |            label 2\xcf\x80\n"
++	 " |                      label 1\xcf\x80\n"
++	 " label 0\xf0\x9f\x98\x82\n",
++	 pp_formatted_text (dc.printer));
++    }
++  }
+ }
+ 
+ /* Make sure that colorization codes don't interrupt a multibyte
+@@ -4057,9 +4357,9 @@ test_diagnostic_show_locus_one_liner_utf
+ 
+   char_span lspan = location_get_source_line (tmp.get_filename (), 1);
+   ASSERT_EQ (25, cpp_display_width (lspan.get_buffer (), lspan.length (),
+-				    def_tabstop));
++				    def_policy ()));
+   ASSERT_EQ (25, location_compute_display_column (expand_location (line_end),
+-						  def_tabstop));
++						  def_policy ()));
+ 
+   test_one_liner_simple_caret_utf8 ();
+   test_one_liner_caret_and_range_utf8 ();
+@@ -4445,30 +4745,31 @@ test_overlapped_fixit_printing (const li
+ 		  pp_formatted_text (dc.printer));
+ 
+     /* Unit-test the line_corrections machinery.  */
++    char_display_policy policy (make_policy (dc, richloc));
+     ASSERT_EQ (3, richloc.get_num_fixit_hints ());
+     const fixit_hint *hint_0 = richloc.get_fixit_hint (0);
+     ASSERT_EQ (column_range (12, 12),
+-	       get_affected_range (&dc, hint_0, CU_BYTES));
++	       get_affected_range (policy, hint_0, CU_BYTES));
+     ASSERT_EQ (column_range (12, 12),
+-	       get_affected_range (&dc, hint_0, CU_DISPLAY_COLS));
+-    ASSERT_EQ (column_range (12, 22), get_printed_columns (&dc, hint_0));
++	       get_affected_range (policy, hint_0, CU_DISPLAY_COLS));
++    ASSERT_EQ (column_range (12, 22), get_printed_columns (policy, hint_0));
+     const fixit_hint *hint_1 = richloc.get_fixit_hint (1);
+     ASSERT_EQ (column_range (18, 18),
+-	       get_affected_range (&dc, hint_1, CU_BYTES));
++	       get_affected_range (policy, hint_1, CU_BYTES));
+     ASSERT_EQ (column_range (18, 18),
+-	       get_affected_range (&dc, hint_1, CU_DISPLAY_COLS));
+-    ASSERT_EQ (column_range (18, 20), get_printed_columns (&dc, hint_1));
++	       get_affected_range (policy, hint_1, CU_DISPLAY_COLS));
++    ASSERT_EQ (column_range (18, 20), get_printed_columns (policy, hint_1));
+     const fixit_hint *hint_2 = richloc.get_fixit_hint (2);
+     ASSERT_EQ (column_range (29, 28),
+-	       get_affected_range (&dc, hint_2, CU_BYTES));
++	       get_affected_range (policy, hint_2, CU_BYTES));
+     ASSERT_EQ (column_range (29, 28),
+-	       get_affected_range (&dc, hint_2, CU_DISPLAY_COLS));
+-    ASSERT_EQ (column_range (29, 29), get_printed_columns (&dc, hint_2));
++	       get_affected_range (policy, hint_2, CU_DISPLAY_COLS));
++    ASSERT_EQ (column_range (29, 29), get_printed_columns (policy, hint_2));
+ 
+     /* Add each hint in turn to a line_corrections instance,
+        and verify that they are consolidated into one correction instance
+        as expected.  */
+-    line_corrections lc (&dc, tmp.get_filename (), 1);
++    line_corrections lc (policy, tmp.get_filename (), 1);
+ 
+     /* The first replace hint by itself.  */
+     lc.add_hint (hint_0);
+@@ -4660,30 +4961,31 @@ test_overlapped_fixit_printing_utf8 (con
+ 		  pp_formatted_text (dc.printer));
+ 
+     /* Unit-test the line_corrections machinery.  */
++    char_display_policy policy (make_policy (dc, richloc));
+     ASSERT_EQ (3, richloc.get_num_fixit_hints ());
+     const fixit_hint *hint_0 = richloc.get_fixit_hint (0);
+     ASSERT_EQ (column_range (14, 14),
+-	       get_affected_range (&dc, hint_0, CU_BYTES));
++	       get_affected_range (policy, hint_0, CU_BYTES));
+     ASSERT_EQ (column_range (12, 12),
+-	       get_affected_range (&dc, hint_0, CU_DISPLAY_COLS));
+-    ASSERT_EQ (column_range (12, 22), get_printed_columns (&dc, hint_0));
++	       get_affected_range (policy, hint_0, CU_DISPLAY_COLS));
++    ASSERT_EQ (column_range (12, 22), get_printed_columns (policy, hint_0));
+     const fixit_hint *hint_1 = richloc.get_fixit_hint (1);
+     ASSERT_EQ (column_range (22, 22),
+-	       get_affected_range (&dc, hint_1, CU_BYTES));
++	       get_affected_range (policy, hint_1, CU_BYTES));
+     ASSERT_EQ (column_range (18, 18),
+-	       get_affected_range (&dc, hint_1, CU_DISPLAY_COLS));
+-    ASSERT_EQ (column_range (18, 20), get_printed_columns (&dc, hint_1));
++	       get_affected_range (policy, hint_1, CU_DISPLAY_COLS));
++    ASSERT_EQ (column_range (18, 20), get_printed_columns (policy, hint_1));
+     const fixit_hint *hint_2 = richloc.get_fixit_hint (2);
+     ASSERT_EQ (column_range (35, 34),
+-	       get_affected_range (&dc, hint_2, CU_BYTES));
++	       get_affected_range (policy, hint_2, CU_BYTES));
+     ASSERT_EQ (column_range (30, 29),
+-	       get_affected_range (&dc, hint_2, CU_DISPLAY_COLS));
+-    ASSERT_EQ (column_range (30, 30), get_printed_columns (&dc, hint_2));
++	       get_affected_range (policy, hint_2, CU_DISPLAY_COLS));
++    ASSERT_EQ (column_range (30, 30), get_printed_columns (policy, hint_2));
+ 
+     /* Add each hint in turn to a line_corrections instance,
+        and verify that they are consolidated into one correction instance
+        as expected.  */
+-    line_corrections lc (&dc, tmp.get_filename (), 1);
++    line_corrections lc (policy, tmp.get_filename (), 1);
+ 
+     /* The first replace hint by itself.  */
+     lc.add_hint (hint_0);
+@@ -4877,15 +5179,16 @@ test_overlapped_fixit_printing_2 (const
+     richloc.add_fixit_insert_before (col_21, "}");
+ 
+     /* These fixits should be accepted; they can't be consolidated.  */
++    char_display_policy policy (make_policy (dc, richloc));
+     ASSERT_EQ (2, richloc.get_num_fixit_hints ());
+     const fixit_hint *hint_0 = richloc.get_fixit_hint (0);
+     ASSERT_EQ (column_range (23, 22),
+-	       get_affected_range (&dc, hint_0, CU_BYTES));
+-    ASSERT_EQ (column_range (23, 23), get_printed_columns (&dc, hint_0));
++	       get_affected_range (policy, hint_0, CU_BYTES));
++    ASSERT_EQ (column_range (23, 23), get_printed_columns (policy, hint_0));
+     const fixit_hint *hint_1 = richloc.get_fixit_hint (1);
+     ASSERT_EQ (column_range (21, 20),
+-	       get_affected_range (&dc, hint_1, CU_BYTES));
+-    ASSERT_EQ (column_range (21, 21), get_printed_columns (&dc, hint_1));
++	       get_affected_range (policy, hint_1, CU_BYTES));
++    ASSERT_EQ (column_range (21, 21), get_printed_columns (policy, hint_1));
+ 
+     /* Verify that they're printed correctly.  */
+     diagnostic_show_locus (&dc, &richloc, DK_ERROR);
+@@ -5152,10 +5455,11 @@ test_tab_expansion (const line_table_cas
+      ....................123 45678901234 56789012345  columns  */
+ 
+   const int tabstop = 8;
++  cpp_char_column_policy policy (tabstop, cpp_wcwidth);
+   const int first_non_ws_byte_col = 7;
+   const int right_quote_byte_col = 15;
+   const int last_byte_col = 25;
+-  ASSERT_EQ (35, cpp_display_width (content, last_byte_col, tabstop));
++  ASSERT_EQ (35, cpp_display_width (content, last_byte_col, policy));
+ 
+   temp_source_file tmp (SELFTEST_LOCATION, ".c", content);
+   line_table_test ltt (case_);
+@@ -5198,6 +5502,114 @@ test_tab_expansion (const line_table_cas
+   }
+ }
+ 
++/* Verify that the escaping machinery can cope with a variety of different
++   invalid bytes.  */
++
++static void
++test_escaping_bytes_1 (const line_table_case &case_)
++{
++  const char content[] = "before\0\1\2\3\r\x80\xff""after\n";
++  const size_t sz = sizeof (content);
++  temp_source_file tmp (SELFTEST_LOCATION, ".c", content, sz);
++  line_table_test ltt (case_);
++  const line_map_ordinary *ord_map = linemap_check_ordinary
++    (linemap_add (line_table, LC_ENTER, false, tmp.get_filename (), 0));
++  linemap_line_start (line_table, 1, 100);
++
++  location_t finish
++    = linemap_position_for_line_and_column (line_table, ord_map, 1,
++					    strlen (content));
++
++  if (finish > LINE_MAP_MAX_LOCATION_WITH_COLS)
++    return;
++
++  /* Locations of the NUL and \r bytes.  */
++  location_t nul_loc
++    = linemap_position_for_line_and_column (line_table, ord_map, 1, 7);
++  location_t r_loc
++    = linemap_position_for_line_and_column (line_table, ord_map, 1, 11);
++  gcc_rich_location richloc (nul_loc);
++  richloc.add_range (r_loc);
++
++  {
++    test_diagnostic_context dc;
++    diagnostic_show_locus (&dc, &richloc, DK_ERROR);
++    ASSERT_STREQ (" before \1\2\3 \x80\xff""after\n"
++		  "       ^   ~\n",
++		  pp_formatted_text (dc.printer));
++  }
++  richloc.set_escape_on_output (true);
++  {
++    test_diagnostic_context dc;
++    dc.escape_format = DIAGNOSTICS_ESCAPE_FORMAT_UNICODE;
++    diagnostic_show_locus (&dc, &richloc, DK_ERROR);
++    ASSERT_STREQ
++      (" before<U+0000><U+0001><U+0002><U+0003><U+000D><80><ff>after\n"
++       "       ^~~~~~~~                        ~~~~~~~~\n",
++       pp_formatted_text (dc.printer));
++  }
++  {
++    test_diagnostic_context dc;
++    dc.escape_format = DIAGNOSTICS_ESCAPE_FORMAT_BYTES;
++    diagnostic_show_locus (&dc, &richloc, DK_ERROR);
++    ASSERT_STREQ (" before<00><01><02><03><0d><80><ff>after\n"
++		  "       ^~~~            ~~~~\n",
++		  pp_formatted_text (dc.printer));
++  }
++}
++
++/* As above, but verify that we handle the initial byte of a line
++   correctly.  */
++
++static void
++test_escaping_bytes_2 (const line_table_case &case_)
++{
++  const char content[]  = "\0after\n";
++  const size_t sz = sizeof (content);
++  temp_source_file tmp (SELFTEST_LOCATION, ".c", content, sz);
++  line_table_test ltt (case_);
++  const line_map_ordinary *ord_map = linemap_check_ordinary
++    (linemap_add (line_table, LC_ENTER, false, tmp.get_filename (), 0));
++  linemap_line_start (line_table, 1, 100);
++
++  location_t finish
++    = linemap_position_for_line_and_column (line_table, ord_map, 1,
++					    strlen (content));
++
++  if (finish > LINE_MAP_MAX_LOCATION_WITH_COLS)
++    return;
++
++  /* Location of the NUL byte.  */
++  location_t nul_loc
++    = linemap_position_for_line_and_column (line_table, ord_map, 1, 1);
++  gcc_rich_location richloc (nul_loc);
++
++  {
++    test_diagnostic_context dc;
++    diagnostic_show_locus (&dc, &richloc, DK_ERROR);
++    ASSERT_STREQ ("  after\n"
++		  " ^\n",
++		  pp_formatted_text (dc.printer));
++  }
++  richloc.set_escape_on_output (true);
++  {
++    test_diagnostic_context dc;
++    dc.escape_format = DIAGNOSTICS_ESCAPE_FORMAT_UNICODE;
++    diagnostic_show_locus (&dc, &richloc, DK_ERROR);
++    ASSERT_STREQ (" <U+0000>after\n"
++		  " ^~~~~~~~\n",
++		  pp_formatted_text (dc.printer));
++  }
++  {
++    test_diagnostic_context dc;
++    dc.escape_format = DIAGNOSTICS_ESCAPE_FORMAT_BYTES;
++    diagnostic_show_locus (&dc, &richloc, DK_ERROR);
++    ASSERT_STREQ (" <00>after\n"
++		  " ^~~~\n",
++		  pp_formatted_text (dc.printer));
++  }
++}
++
+ /* Verify that line numbers are correctly printed for the case of
+    a multiline range in which the width of the line numbers changes
+    (e.g. from "9" to "10").  */
+@@ -5254,6 +5666,8 @@ diagnostic_show_locus_c_tests ()
+   test_layout_range_for_single_line ();
+   test_layout_range_for_multiple_lines ();
+ 
++  test_display_widths ();
++
+   for_each_line_table_case (test_layout_x_offset_display_utf8);
+   for_each_line_table_case (test_layout_x_offset_display_tab);
+ 
+@@ -5274,6 +5688,8 @@ diagnostic_show_locus_c_tests ()
+   for_each_line_table_case (test_fixit_replace_containing_newline);
+   for_each_line_table_case (test_fixit_deletion_affecting_newline);
+   for_each_line_table_case (test_tab_expansion);
++  for_each_line_table_case (test_escaping_bytes_1);
++  for_each_line_table_case (test_escaping_bytes_2);
+ 
+   test_line_numbers_multiline_range ();
+ }
+diff --git a/gcc/doc/invoke.texi b/gcc/doc/invoke.texi
+--- a/gcc/doc/invoke.texi	2021-12-13 23:23:05.764437151 -0800
++++ b/gcc/doc/invoke.texi	2021-12-14 01:16:01.553943061 -0800
+@@ -312,7 +312,8 @@ Objective-C and Objective-C++ Dialects}.
+ -fdiagnostics-show-path-depths @gol
+ -fno-show-column @gol
+ -fdiagnostics-column-unit=@r{[}display@r{|}byte@r{]} @gol
+--fdiagnostics-column-origin=@var{origin}}
++-fdiagnostics-column-origin=@var{origin} @gol
++-fdiagnostics-escape-format=@r{[}unicode@r{|}bytes@r{]}}
+ 
+ @item Warning Options
+ @xref{Warning Options,,Options to Request or Suppress Warnings}.
+@@ -5083,6 +5084,38 @@ first column.  The default value of 1 co
+ behavior and to the GNU style guide.  Some utilities may perform better with an
+ origin of 0; any non-negative value may be specified.
+ 
++@item -fdiagnostics-escape-format=@var{FORMAT}
++@opindex fdiagnostics-escape-format
++When GCC prints pertinent source lines for a diagnostic it normally attempts
++to print the source bytes directly.  However, some diagnostics relate to encoding
++issues in the source file, such as malformed UTF-8, or issues with Unicode
++normalization.  These diagnostics are flagged so that GCC will escape bytes
++that are not printable ASCII when printing their pertinent source lines.
++
++This option controls how such bytes should be escaped.
++
++The default @var{FORMAT}, @samp{unicode} displays Unicode characters that
++are not printable ASCII in the form @samp{<U+XXXX>}, and bytes that do not
++correspond to a Unicode character validly-encoded in UTF-8-encoded will be
++displayed as hexadecimal in the form @samp{<XX>}.
++
++For example, a source line containing the string @samp{before} followed by the
++Unicode character U+03C0 (``GREEK SMALL LETTER PI'', with UTF-8 encoding
++0xCF 0x80) followed by the byte 0xBF (a stray UTF-8 trailing byte), followed by
++the string @samp{after} will be printed for such a diagnostic as:
++
++@smallexample
++ before<U+03C0><BF>after
++@end smallexample
++
++Setting @var{FORMAT} to @samp{bytes} will display all non-printable-ASCII bytes
++in the form @samp{<XX>}, thus showing the underlying encoding of non-ASCII
++Unicode characters.  For the example above, the following will be printed:
++
++@smallexample
++ before<CF><80><BF>after
++@end smallexample
++
+ @item -fdiagnostics-format=@var{FORMAT}
+ @opindex fdiagnostics-format
+ Select a different format for printing diagnostics.
+@@ -5150,9 +5183,11 @@ might be printed in JSON form (after for
+                         @}
+                     @}
+                 ],
++                "escape-source": false,
+                 "message": "...this statement, but the latter is @dots{}"
+             @}
+         ]
++	"escape-source": false,
+ 	"column-origin": 1,
+     @},
+     @dots{}
+@@ -5239,6 +5274,7 @@ of the expression, which have labels.  I
+                 "label": "T @{aka struct t@}"
+             @}
+         ],
++        "escape-source": false,
+         "message": "invalid operands to binary + @dots{}"
+     @}
+ @end smallexample
+@@ -5292,6 +5328,7 @@ might be printed in JSON form as:
+                 @}
+             @}
+         ],
++        "escape-source": false,
+         "message": "\u2018struct s\u2019 has no member named @dots{}"
+     @}
+ @end smallexample
+@@ -5349,6 +5386,10 @@ For example, the intraprocedural example
+     ]
+ @end smallexample
+ 
++Diagnostics have a boolean attribute @code{escape-source}, hinting whether
++non-ASCII bytes should be escaped when printing the pertinent lines of
++source code (@code{true} for diagnostics involving source encoding issues).
++
+ @end table
+ 
+ @node Warning Options
+diff --git a/gcc/input.c b/gcc/input.c
+--- a/gcc/input.c	2021-07-27 23:55:07.328287915 -0700
++++ b/gcc/input.c	2021-12-14 01:16:01.553943061 -0800
+@@ -913,7 +913,8 @@ make_location (location_t caret, source_
+    source line in order to calculate the display width.  If that cannot be done
+    for any reason, then returns the byte column as a fallback.  */
+ int
+-location_compute_display_column (expanded_location exploc, int tabstop)
++location_compute_display_column (expanded_location exploc,
++				 const cpp_char_column_policy &policy)
+ {
+   if (!(exploc.file && *exploc.file && exploc.line && exploc.column))
+     return exploc.column;
+@@ -921,7 +922,7 @@ location_compute_display_column (expande
+   /* If line is NULL, this function returns exploc.column which is the
+      desired fallback.  */
+   return cpp_byte_column_to_display_column (line.get_buffer (), line.length (),
+-					    exploc.column, tabstop);
++					    exploc.column, policy);
+ }
+ 
+ /* Dump statistics to stderr about the memory usage of the line_table
+@@ -3611,43 +3612,50 @@ test_line_offset_overflow ()
+ void test_cpp_utf8 ()
+ {
+   const int def_tabstop = 8;
++  cpp_char_column_policy policy (def_tabstop, cpp_wcwidth);
++
+   /* Verify that wcwidth of invalid UTF-8 or control bytes is 1.  */
+   {
+-    int w_bad = cpp_display_width ("\xf0!\x9f!\x98!\x82!", 8, def_tabstop);
++    int w_bad = cpp_display_width ("\xf0!\x9f!\x98!\x82!", 8, policy);
+     ASSERT_EQ (8, w_bad);
+-    int w_ctrl = cpp_display_width ("\r\n\v\0\1", 5, def_tabstop);
++    int w_ctrl = cpp_display_width ("\r\n\v\0\1", 5, policy);
+     ASSERT_EQ (5, w_ctrl);
+   }
+ 
+   /* Verify that wcwidth of valid UTF-8 is as expected.  */
+   {
+-    const int w_pi = cpp_display_width ("\xcf\x80", 2, def_tabstop);
++    const int w_pi = cpp_display_width ("\xcf\x80", 2, policy);
+     ASSERT_EQ (1, w_pi);
+-    const int w_emoji = cpp_display_width ("\xf0\x9f\x98\x82", 4, def_tabstop);
++    const int w_emoji = cpp_display_width ("\xf0\x9f\x98\x82", 4, policy);
+     ASSERT_EQ (2, w_emoji);
+     const int w_umlaut_precomposed = cpp_display_width ("\xc3\xbf", 2,
+-							def_tabstop);
++							policy);
+     ASSERT_EQ (1, w_umlaut_precomposed);
+     const int w_umlaut_combining = cpp_display_width ("y\xcc\x88", 3,
+-						      def_tabstop);
++						      policy);
+     ASSERT_EQ (1, w_umlaut_combining);
+-    const int w_han = cpp_display_width ("\xe4\xb8\xba", 3, def_tabstop);
++    const int w_han = cpp_display_width ("\xe4\xb8\xba", 3, policy);
+     ASSERT_EQ (2, w_han);
+-    const int w_ascii = cpp_display_width ("GCC", 3, def_tabstop);
++    const int w_ascii = cpp_display_width ("GCC", 3, policy);
+     ASSERT_EQ (3, w_ascii);
+     const int w_mixed = cpp_display_width ("\xcf\x80 = 3.14 \xf0\x9f\x98\x82"
+ 					   "\x9f! \xe4\xb8\xba y\xcc\x88",
+-					   24, def_tabstop);
++					   24, policy);
+     ASSERT_EQ (18, w_mixed);
+   }
+ 
+   /* Verify that display width properly expands tabs.  */
+   {
+     const char *tstr = "\tabc\td";
+-    ASSERT_EQ (6, cpp_display_width (tstr, 6, 1));
+-    ASSERT_EQ (10, cpp_display_width (tstr, 6, 3));
+-    ASSERT_EQ (17, cpp_display_width (tstr, 6, 8));
+-    ASSERT_EQ (1, cpp_display_column_to_byte_column (tstr, 6, 7, 8));
++    ASSERT_EQ (6, cpp_display_width (tstr, 6,
++				     cpp_char_column_policy (1, cpp_wcwidth)));
++    ASSERT_EQ (10, cpp_display_width (tstr, 6,
++				      cpp_char_column_policy (3, cpp_wcwidth)));
++    ASSERT_EQ (17, cpp_display_width (tstr, 6,
++				      cpp_char_column_policy (8, cpp_wcwidth)));
++    ASSERT_EQ (1,
++	       cpp_display_column_to_byte_column
++		 (tstr, 6, 7, cpp_char_column_policy (8, cpp_wcwidth)));
+   }
+ 
+   /* Verify that cpp_byte_column_to_display_column can go past the end,
+@@ -3660,13 +3668,13 @@ void test_cpp_utf8 ()
+       /* 111122223456
+ 	 Byte columns.  */
+ 
+-    ASSERT_EQ (5, cpp_display_width (str, 6, def_tabstop));
++    ASSERT_EQ (5, cpp_display_width (str, 6, policy));
+     ASSERT_EQ (105,
+-	       cpp_byte_column_to_display_column (str, 6, 106, def_tabstop));
++	       cpp_byte_column_to_display_column (str, 6, 106, policy));
+     ASSERT_EQ (10000,
+-	       cpp_byte_column_to_display_column (NULL, 0, 10000, def_tabstop));
++	       cpp_byte_column_to_display_column (NULL, 0, 10000, policy));
+     ASSERT_EQ (0,
+-	       cpp_byte_column_to_display_column (NULL, 10000, 0, def_tabstop));
++	       cpp_byte_column_to_display_column (NULL, 10000, 0, policy));
+   }
+ 
+   /* Verify that cpp_display_column_to_byte_column can go past the end,
+@@ -3680,25 +3688,25 @@ void test_cpp_utf8 ()
+       /* 000000000000000000000000000000000111111
+ 	 111122223333444456666777788889999012345
+ 	 Byte columns.  */
+-    ASSERT_EQ (4, cpp_display_column_to_byte_column (str, 15, 2, def_tabstop));
++    ASSERT_EQ (4, cpp_display_column_to_byte_column (str, 15, 2, policy));
+     ASSERT_EQ (15,
+-	       cpp_display_column_to_byte_column (str, 15, 11, def_tabstop));
++	       cpp_display_column_to_byte_column (str, 15, 11, policy));
+     ASSERT_EQ (115,
+-	       cpp_display_column_to_byte_column (str, 15, 111, def_tabstop));
++	       cpp_display_column_to_byte_column (str, 15, 111, policy));
+     ASSERT_EQ (10000,
+-	       cpp_display_column_to_byte_column (NULL, 0, 10000, def_tabstop));
++	       cpp_display_column_to_byte_column (NULL, 0, 10000, policy));
+     ASSERT_EQ (0,
+-	       cpp_display_column_to_byte_column (NULL, 10000, 0, def_tabstop));
++	       cpp_display_column_to_byte_column (NULL, 10000, 0, policy));
+ 
+     /* Verify that we do not interrupt a UTF-8 sequence.  */
+-    ASSERT_EQ (4, cpp_display_column_to_byte_column (str, 15, 1, def_tabstop));
++    ASSERT_EQ (4, cpp_display_column_to_byte_column (str, 15, 1, policy));
+ 
+     for (int byte_col = 1; byte_col <= 15; ++byte_col)
+       {
+ 	const int disp_col
+-	  = cpp_byte_column_to_display_column (str, 15, byte_col, def_tabstop);
++	  = cpp_byte_column_to_display_column (str, 15, byte_col, policy);
+ 	const int byte_col2
+-	  = cpp_display_column_to_byte_column (str, 15, disp_col, def_tabstop);
++	  = cpp_display_column_to_byte_column (str, 15, disp_col, policy);
+ 
+ 	/* If we ask for the display column in the middle of a UTF-8
+ 	   sequence, it will return the length of the partial sequence,
+diff --git a/gcc/input.h b/gcc/input.h
+--- a/gcc/input.h	2021-07-27 23:55:07.328287915 -0700
++++ b/gcc/input.h	2021-12-14 01:16:01.553943061 -0800
+@@ -39,8 +39,11 @@ STATIC_ASSERT (BUILTINS_LOCATION < RESER
+ extern bool is_location_from_builtin_token (location_t);
+ extern expanded_location expand_location (location_t);
+ 
+-extern int location_compute_display_column (expanded_location exploc,
+-					    int tabstop);
++class cpp_char_column_policy;
++
++extern int
++location_compute_display_column (expanded_location exploc,
++				 const cpp_char_column_policy &policy);
+ 
+ /* A class capturing the bounds of a buffer, to allow for run-time
+    bounds-checking in a checked build.  */
+diff --git a/gcc/opts.c b/gcc/opts.c
+--- a/gcc/opts.c	2021-07-27 23:55:07.364288417 -0700
++++ b/gcc/opts.c	2021-12-14 01:16:01.553943061 -0800
+@@ -2573,6 +2573,10 @@ common_handle_option (struct gcc_options
+       dc->column_origin = value;
+       break;
+ 
++    case OPT_fdiagnostics_escape_format_:
++      dc->escape_format = (enum diagnostics_escape_format)value;
++      break;
++
+     case OPT_fdiagnostics_show_cwe:
+       dc->show_cwe = value;
+       break;
+diff --git a/gcc/selftest.c b/gcc/selftest.c
+--- a/gcc/selftest.c	2021-07-27 23:55:07.500290315 -0700
++++ b/gcc/selftest.c	2021-12-14 01:16:01.557942991 -0800
+@@ -193,6 +193,21 @@ temp_source_file::temp_source_file (cons
+   fclose (out);
+ }
+ 
++/* As above, but with a size, to allow for NUL bytes in CONTENT.  */
++
++temp_source_file::temp_source_file (const location &loc,
++				    const char *suffix,
++				    const char *content,
++				    size_t sz)
++: named_temp_file (suffix)
++{
++  FILE *out = fopen (get_filename (), "w");
++  if (!out)
++    fail_formatted (loc, "unable to open tempfile: %s", get_filename ());
++  fwrite (content, sz, 1, out);
++  fclose (out);
++}
++
+ /* Avoid introducing locale-specific differences in the results
+    by hardcoding open_quote and close_quote.  */
+ 
+diff --git a/gcc/selftest.h b/gcc/selftest.h
+--- a/gcc/selftest.h	2021-07-27 23:55:07.500290315 -0700
++++ b/gcc/selftest.h	2021-12-14 01:16:01.557942991 -0800
+@@ -112,6 +112,8 @@ class temp_source_file : public named_te
+  public:
+   temp_source_file (const location &loc, const char *suffix,
+ 		    const char *content);
++  temp_source_file (const location &loc, const char *suffix,
++		    const char *content, size_t sz);
+ };
+ 
+ /* RAII-style class for avoiding introducing locale-specific differences
+diff --git a/gcc/testsuite/c-c++-common/diagnostic-format-json-1.c b/gcc/testsuite/c-c++-common/diagnostic-format-json-1.c
+--- a/gcc/testsuite/c-c++-common/diagnostic-format-json-1.c	2021-07-27 23:55:07.596291654 -0700
++++ b/gcc/testsuite/c-c++-common/diagnostic-format-json-1.c	2021-12-14 01:16:01.557942991 -0800
+@@ -9,6 +9,7 @@
+ 
+ /* { dg-regexp "\"kind\": \"error\"" } */
+ /* { dg-regexp "\"column-origin\": 1" } */
++/* { dg-regexp "\"escape-source\": false" } */
+ /* { dg-regexp "\"message\": \"#error message\"" } */
+ 
+ /* { dg-regexp "\"caret\": \{" } */
+diff --git a/gcc/testsuite/c-c++-common/diagnostic-format-json-2.c b/gcc/testsuite/c-c++-common/diagnostic-format-json-2.c
+--- a/gcc/testsuite/c-c++-common/diagnostic-format-json-2.c	2021-07-27 23:55:07.596291654 -0700
++++ b/gcc/testsuite/c-c++-common/diagnostic-format-json-2.c	2021-12-14 01:16:01.557942991 -0800
+@@ -9,6 +9,7 @@
+ 
+ /* { dg-regexp "\"kind\": \"warning\"" } */
+ /* { dg-regexp "\"column-origin\": 1" } */
++/* { dg-regexp "\"escape-source\": false" } */
+ /* { dg-regexp "\"message\": \"#warning message\"" } */
+ /* { dg-regexp "\"option\": \"-Wcpp\"" } */
+ /* { dg-regexp "\"option_url\": \"https:\[^\n\r\"\]*#index-Wcpp\"" } */
+diff --git a/gcc/testsuite/c-c++-common/diagnostic-format-json-3.c b/gcc/testsuite/c-c++-common/diagnostic-format-json-3.c
+--- a/gcc/testsuite/c-c++-common/diagnostic-format-json-3.c	2021-07-27 23:55:07.596291654 -0700
++++ b/gcc/testsuite/c-c++-common/diagnostic-format-json-3.c	2021-12-14 01:16:01.557942991 -0800
+@@ -9,6 +9,7 @@
+ 
+ /* { dg-regexp "\"kind\": \"error\"" } */
+ /* { dg-regexp "\"column-origin\": 1" } */
++/* { dg-regexp "\"escape-source\": false" } */
+ /* { dg-regexp "\"message\": \"#warning message\"" } */
+ /* { dg-regexp "\"option\": \"-Werror=cpp\"" } */
+ /* { dg-regexp "\"option_url\": \"https:\[^\n\r\"\]*#index-Wcpp\"" } */
+diff --git a/gcc/testsuite/c-c++-common/diagnostic-format-json-4.c b/gcc/testsuite/c-c++-common/diagnostic-format-json-4.c
+--- a/gcc/testsuite/c-c++-common/diagnostic-format-json-4.c	2021-07-27 23:55:07.596291654 -0700
++++ b/gcc/testsuite/c-c++-common/diagnostic-format-json-4.c	2021-12-14 01:16:01.557942991 -0800
+@@ -19,6 +19,7 @@ int test (void)
+ 
+ /* { dg-regexp "\"kind\": \"note\"" } */
+ /* { dg-regexp "\"message\": \"...this statement, but the latter is misleadingly indented as if it were guarded by the 'if'\"" } */
++/* { dg-regexp "\"escape-source\": false" } */
+ 
+ /* { dg-regexp "\"caret\": \{" } */
+ /* { dg-regexp "\"file\": \"\[^\n\r\"\]*diagnostic-format-json-4.c\"" } */
+@@ -39,6 +40,7 @@ int test (void)
+ /* { dg-regexp "\"kind\": \"warning\"" } */
+ /* { dg-regexp "\"column-origin\": 1" } */
+ /* { dg-regexp "\"message\": \"this 'if' clause does not guard...\"" } */
++/* { dg-regexp "\"escape-source\": false" } */
+ /* { dg-regexp "\"option\": \"-Wmisleading-indentation\"" } */
+ /* { dg-regexp "\"option_url\": \"https:\[^\n\r\"\]*#index-Wmisleading-indentation\"" } */
+ 
+diff --git a/gcc/testsuite/c-c++-common/diagnostic-format-json-5.c b/gcc/testsuite/c-c++-common/diagnostic-format-json-5.c
+--- a/gcc/testsuite/c-c++-common/diagnostic-format-json-5.c	2021-07-27 23:55:07.596291654 -0700
++++ b/gcc/testsuite/c-c++-common/diagnostic-format-json-5.c	2021-12-14 01:16:01.557942991 -0800
+@@ -14,6 +14,7 @@ int test (struct s *ptr)
+ 
+ /* { dg-regexp "\"kind\": \"error\"" } */
+ /* { dg-regexp "\"column-origin\": 1" } */
++/* { dg-regexp "\"escape-source\": false" } */
+ /* { dg-regexp "\"message\": \".*\"" } */
+ 
+ /* Verify fix-it hints.  */
+diff --git a/gcc/testsuite/gcc.dg/cpp/warn-normalized-4-bytes.c b/gcc/testsuite/gcc.dg/cpp/warn-normalized-4-bytes.c
+--- a/gcc/testsuite/gcc.dg/cpp/warn-normalized-4-bytes.c	1969-12-31 16:00:00.000000000 -0800
++++ b/gcc/testsuite/gcc.dg/cpp/warn-normalized-4-bytes.c	2021-12-14 01:16:01.557942991 -0800
+@@ -0,0 +1,21 @@
++// { dg-do preprocess }
++// { dg-options "-std=gnu99 -Werror=normalized=nfc -fdiagnostics-show-caret -fdiagnostics-escape-format=bytes" }
++/* { dg-message "some warnings being treated as errors" "" {target "*-*-*"} 0 } */
++
++/* à½ = U+0F43 TIBETAN LETTER GHA, which has decomposition "0F42 0FB7" i.e.
++   U+0F42 TIBETAN LETTER GA: à½
++   U+0FB7 TIBETAN SUBJOINED LETTER HA: à¾·
++
++   The UTF-8 encoding of U+0F43 TIBETAN LETTER GHA is: E0 BD 83.  */
++
++foo before_\u0F43_after bar // { dg-error "`before_.U00000f43_after' is not in NFC .-Werror=normalized=." }
++/* { dg-begin-multiline-output "" }
++ foo before_\u0F43_after bar
++     ^~~~~~~~~~~~~~~~~~~
++   { dg-end-multiline-output "" } */
++
++foo before_à½_after bar // { dg-error "`before_.U00000f43_after' is not in NFC .-Werror=normalized=." }
++/* { dg-begin-multiline-output "" }
++ foo before_<e0><bd><83>_after bar
++     ^~~~~~~~~~~~~~~~~~~~~~~~~
++   { dg-end-multiline-output "" } */
+diff --git a/gcc/testsuite/gcc.dg/cpp/warn-normalized-4-unicode.c b/gcc/testsuite/gcc.dg/cpp/warn-normalized-4-unicode.c
+--- a/gcc/testsuite/gcc.dg/cpp/warn-normalized-4-unicode.c	1969-12-31 16:00:00.000000000 -0800
++++ b/gcc/testsuite/gcc.dg/cpp/warn-normalized-4-unicode.c	2021-12-14 01:16:01.557942991 -0800
+@@ -0,0 +1,19 @@
++// { dg-do preprocess }
++// { dg-options "-std=gnu99 -Werror=normalized=nfc -fdiagnostics-show-caret -fdiagnostics-escape-format=unicode" }
++/* { dg-message "some warnings being treated as errors" "" {target "*-*-*"} 0 } */
++
++/* à½ = U+0F43 TIBETAN LETTER GHA, which has decomposition "0F42 0FB7" i.e.
++   U+0F42 TIBETAN LETTER GA: à½
++   U+0FB7 TIBETAN SUBJOINED LETTER HA: à¾·  */
++
++foo before_\u0F43_after bar  // { dg-error "`before_.U00000f43_after' is not in NFC .-Werror=normalized=." }
++/* { dg-begin-multiline-output "" }
++ foo before_\u0F43_after bar
++     ^~~~~~~~~~~~~~~~~~~
++   { dg-end-multiline-output "" } */
++
++foo before_à½_after bar // { dg-error "`before_.U00000f43_after' is not in NFC .-Werror=normalized=." }
++/* { dg-begin-multiline-output "" }
++ foo before_<U+0F43>_after bar
++     ^~~~~~~~~~~~~~~~~~~~~
++   { dg-end-multiline-output "" } */
+diff --git a/gcc/testsuite/gfortran.dg/diagnostic-format-json-1.F90 b/gcc/testsuite/gfortran.dg/diagnostic-format-json-1.F90
+--- a/gcc/testsuite/gfortran.dg/diagnostic-format-json-1.F90	2021-07-27 23:55:08.472303878 -0700
++++ b/gcc/testsuite/gfortran.dg/diagnostic-format-json-1.F90	2021-12-14 01:16:01.557942991 -0800
+@@ -9,6 +9,7 @@
+ 
+ ! { dg-regexp "\"kind\": \"error\"" }
+ ! { dg-regexp "\"column-origin\": 1" }
++! { dg-regexp "\"escape-source\": false" }
+ ! { dg-regexp "\"message\": \"#error message\"" }
+ 
+ ! { dg-regexp "\"caret\": \{" }
+diff --git a/gcc/testsuite/gfortran.dg/diagnostic-format-json-2.F90 b/gcc/testsuite/gfortran.dg/diagnostic-format-json-2.F90
+--- a/gcc/testsuite/gfortran.dg/diagnostic-format-json-2.F90	2021-07-27 23:55:08.472303878 -0700
++++ b/gcc/testsuite/gfortran.dg/diagnostic-format-json-2.F90	2021-12-14 01:16:01.557942991 -0800
+@@ -9,6 +9,7 @@
+ 
+ ! { dg-regexp "\"kind\": \"warning\"" }
+ ! { dg-regexp "\"column-origin\": 1" }
++! { dg-regexp "\"escape-source\": false" }
+ ! { dg-regexp "\"message\": \"#warning message\"" }
+ ! { dg-regexp "\"option\": \"-Wcpp\"" }
+ ! { dg-regexp "\"option_url\": \"\[^\n\r\"\]*#index-Wcpp\"" }
+diff --git a/gcc/testsuite/gfortran.dg/diagnostic-format-json-3.F90 b/gcc/testsuite/gfortran.dg/diagnostic-format-json-3.F90
+--- a/gcc/testsuite/gfortran.dg/diagnostic-format-json-3.F90	2021-07-27 23:55:08.472303878 -0700
++++ b/gcc/testsuite/gfortran.dg/diagnostic-format-json-3.F90	2021-12-14 01:16:01.557942991 -0800
+@@ -9,6 +9,7 @@
+ 
+ ! { dg-regexp "\"kind\": \"error\"" }
+ ! { dg-regexp "\"column-origin\": 1" }
++! { dg-regexp "\"escape-source\": false" }
+ ! { dg-regexp "\"message\": \"#warning message\"" }
+ ! { dg-regexp "\"option\": \"-Werror=cpp\"" }
+ ! { dg-regexp "\"option_url\": \"\[^\n\r\"\]*#index-Wcpp\"" }
+diff --git a/libcpp/charset.c b/libcpp/charset.c
+--- a/libcpp/charset.c	2021-07-27 23:55:08.712307227 -0700
++++ b/libcpp/charset.c	2021-12-14 01:16:01.557942991 -0800
+@@ -1552,12 +1552,14 @@ convert_escape (cpp_reader *pfile, const
+ 		   "unknown escape sequence: '\\%c'", (int) c);
+       else
+ 	{
++	  encoding_rich_location rich_loc (pfile);
++
+ 	  /* diagnostic.c does not support "%03o".  When it does, this
+ 	     code can use %03o directly in the diagnostic again.  */
+ 	  char buf[32];
+ 	  sprintf(buf, "%03o", (int) c);
+-	  cpp_error (pfile, CPP_DL_PEDWARN,
+-		     "unknown escape sequence: '\\%s'", buf);
++	  cpp_error_at (pfile, CPP_DL_PEDWARN, &rich_loc,
++			"unknown escape sequence: '\\%s'", buf);
+ 	}
+     }
+ 
+@@ -2280,14 +2282,16 @@ cpp_string_location_reader::get_next ()
+ }
+ 
+ cpp_display_width_computation::
+-cpp_display_width_computation (const char *data, int data_length, int tabstop) :
++cpp_display_width_computation (const char *data, int data_length,
++			       const cpp_char_column_policy &policy) :
+   m_begin (data),
+   m_next (m_begin),
+   m_bytes_left (data_length),
+-  m_tabstop (tabstop),
++  m_policy (policy),
+   m_display_cols (0)
+ {
+-  gcc_assert (m_tabstop > 0);
++  gcc_assert (policy.m_tabstop > 0);
++  gcc_assert (policy.m_width_cb);
+ }
+ 
+ 
+@@ -2299,19 +2303,28 @@ cpp_display_width_computation (const cha
+    point to a valid UTF-8-encoded sequence, then it will be treated as a single
+    byte with display width 1.  m_cur_display_col is the current display column,
+    relative to which tab stops should be expanded.  Returns the display width of
+-   the codepoint just processed.  */
++   the codepoint just processed.
++   If OUT is non-NULL, it is populated.  */
+ 
+ int
+-cpp_display_width_computation::process_next_codepoint ()
++cpp_display_width_computation::process_next_codepoint (cpp_decoded_char *out)
+ {
+   cppchar_t c;
+   int next_width;
+ 
++  if (out)
++    out->m_start_byte = m_next;
++
+   if (*m_next == '\t')
+     {
+       ++m_next;
+       --m_bytes_left;
+-      next_width = m_tabstop - (m_display_cols % m_tabstop);
++      next_width = m_policy.m_tabstop - (m_display_cols % m_policy.m_tabstop);
++      if (out)
++	{
++	  out->m_ch = '\t';
++	  out->m_valid_ch = true;
++	}
+     }
+   else if (one_utf8_to_cppchar ((const uchar **) &m_next, &m_bytes_left, &c)
+ 	   != 0)
+@@ -2321,14 +2334,24 @@ cpp_display_width_computation::process_n
+ 	 of one.  */
+       ++m_next;
+       --m_bytes_left;
+-      next_width = 1;
++      next_width = m_policy.m_undecoded_byte_width;
++      if (out)
++	out->m_valid_ch = false;
+     }
+   else
+     {
+       /*  one_utf8_to_cppchar() has updated m_next and m_bytes_left for us.  */
+-      next_width = cpp_wcwidth (c);
++      next_width = m_policy.m_width_cb (c);
++      if (out)
++	{
++	  out->m_ch = c;
++	  out->m_valid_ch = true;
++	}
+     }
+ 
++  if (out)
++    out->m_next_byte = m_next;
++
+   m_display_cols += next_width;
+   return next_width;
+ }
+@@ -2344,7 +2367,7 @@ cpp_display_width_computation::advance_d
+   const int start = m_display_cols;
+   const int target = start + n;
+   while (m_display_cols < target && !done ())
+-    process_next_codepoint ();
++    process_next_codepoint (NULL);
+   return m_display_cols - start;
+ }
+ 
+@@ -2352,29 +2375,33 @@ cpp_display_width_computation::advance_d
+     how many display columns are occupied by the first COLUMN bytes.  COLUMN
+     may exceed DATA_LENGTH, in which case the phantom bytes at the end are
+     treated as if they have display width 1.  Tabs are expanded to the next tab
+-    stop, relative to the start of DATA.  */
++    stop, relative to the start of DATA, and non-printable-ASCII characters
++    will be escaped as per POLICY.  */
+ 
+ int
+ cpp_byte_column_to_display_column (const char *data, int data_length,
+-				   int column, int tabstop)
++				   int column,
++				   const cpp_char_column_policy &policy)
+ {
+   const int offset = MAX (0, column - data_length);
+-  cpp_display_width_computation dw (data, column - offset, tabstop);
++  cpp_display_width_computation dw (data, column - offset, policy);
+   while (!dw.done ())
+-    dw.process_next_codepoint ();
++    dw.process_next_codepoint (NULL);
+   return dw.display_cols_processed () + offset;
+ }
+ 
+ /*  For the string of length DATA_LENGTH bytes that begins at DATA, compute
+     the least number of bytes that will result in at least DISPLAY_COL display
+     columns.  The return value may exceed DATA_LENGTH if the entire string does
+-    not occupy enough display columns.  */
++    not occupy enough display columns.  Non-printable-ASCII characters
++    will be escaped as per POLICY.  */
+ 
+ int
+ cpp_display_column_to_byte_column (const char *data, int data_length,
+-				   int display_col, int tabstop)
++				   int display_col,
++				   const cpp_char_column_policy &policy)
+ {
+-  cpp_display_width_computation dw (data, data_length, tabstop);
++  cpp_display_width_computation dw (data, data_length, policy);
+   const int avail_display = dw.advance_display_cols (display_col);
+   return dw.bytes_processed () + MAX (0, display_col - avail_display);
+ }
+diff --git a/libcpp/errors.c b/libcpp/errors.c
+--- a/libcpp/errors.c	2021-07-27 23:55:08.712307227 -0700
++++ b/libcpp/errors.c	2021-12-14 01:16:01.557942991 -0800
+@@ -27,6 +27,31 @@ along with this program; see the file CO
+ #include "cpplib.h"
+ #include "internal.h"
+ 
++/* Get a location_t for the current location in PFILE,
++   generally that of the previously lexed token.  */
++
++location_t
++cpp_diagnostic_get_current_location (cpp_reader *pfile)
++{
++  if (CPP_OPTION (pfile, traditional))
++    {
++      if (pfile->state.in_directive)
++	return pfile->directive_line;
++      else
++	return pfile->line_table->highest_line;
++    }
++  /* We don't want to refer to a token before the beginning of the
++     current run -- that is invalid.  */
++  else if (pfile->cur_token == pfile->cur_run->base)
++    {
++      return 0;
++    }
++  else
++    {
++      return pfile->cur_token[-1].src_loc;
++    }
++}
++
+ /* Print a diagnostic at the given location.  */
+ 
+ ATTRIBUTE_FPTR_PRINTF(5,0)
+@@ -52,25 +77,7 @@ cpp_diagnostic (cpp_reader * pfile, enum
+ 		enum cpp_warning_reason reason,
+ 		const char *msgid, va_list *ap)
+ {
+-  location_t src_loc;
+-
+-  if (CPP_OPTION (pfile, traditional))
+-    {
+-      if (pfile->state.in_directive)
+-	src_loc = pfile->directive_line;
+-      else
+-	src_loc = pfile->line_table->highest_line;
+-    }
+-  /* We don't want to refer to a token before the beginning of the
+-     current run -- that is invalid.  */
+-  else if (pfile->cur_token == pfile->cur_run->base)
+-    {
+-      src_loc = 0;
+-    }
+-  else
+-    {
+-      src_loc = pfile->cur_token[-1].src_loc;
+-    }
++  location_t src_loc = cpp_diagnostic_get_current_location (pfile);
+   rich_location richloc (pfile->line_table, src_loc);
+   return cpp_diagnostic_at (pfile, level, reason, &richloc, msgid, ap);
+ }
+@@ -142,6 +149,43 @@ cpp_warning_syshdr (cpp_reader * pfile,
+ 
+   va_end (ap);
+   return ret;
++}
++
++/* As cpp_warning above, but use RICHLOC as the location of the diagnostic.  */
++
++bool cpp_warning_at (cpp_reader *pfile, enum cpp_warning_reason reason,
++		     rich_location *richloc, const char *msgid, ...)
++{
++  va_list ap;
++  bool ret;
++
++  va_start (ap, msgid);
++
++  ret = cpp_diagnostic_at (pfile, CPP_DL_WARNING, reason, richloc,
++			   msgid, &ap);
++
++  va_end (ap);
++  return ret;
++
++}
++
++/* As cpp_pedwarning above, but use RICHLOC as the location of the
++   diagnostic.  */
++
++bool
++cpp_pedwarning_at (cpp_reader * pfile, enum cpp_warning_reason reason,
++		   rich_location *richloc, const char *msgid, ...)
++{
++  va_list ap;
++  bool ret;
++
++  va_start (ap, msgid);
++
++  ret = cpp_diagnostic_at (pfile, CPP_DL_PEDWARN, reason, richloc,
++			   msgid, &ap);
++
++  va_end (ap);
++  return ret;
+ }
+ 
+ /* Print a diagnostic at a specific location.  */
+diff --git a/libcpp/include/cpplib.h b/libcpp/include/cpplib.h
+--- a/libcpp/include/cpplib.h	2021-12-13 23:23:05.768437079 -0800
++++ b/libcpp/include/cpplib.h	2021-12-14 01:20:16.189507386 -0800
+@@ -1275,6 +1275,14 @@ extern bool cpp_warning_syshdr (cpp_read
+ 				const char *msgid, ...)
+   ATTRIBUTE_PRINTF_3;
+ 
++/* As their counterparts above, but use RICHLOC.  */
++extern bool cpp_warning_at (cpp_reader *, enum cpp_warning_reason,
++			    rich_location *richloc, const char *msgid, ...)
++  ATTRIBUTE_PRINTF_4;
++extern bool cpp_pedwarning_at (cpp_reader *, enum cpp_warning_reason,
++			       rich_location *richloc, const char *msgid, ...)
++  ATTRIBUTE_PRINTF_4;
++
+ /* Output a diagnostic with "MSGID: " preceding the
+    error string of errno.  No location is printed.  */
+ extern bool cpp_errno (cpp_reader *, enum cpp_diagnostic_level,
+@@ -1435,42 +1443,95 @@ extern const char * cpp_get_userdef_suff
+ 
+ /* In charset.c */
+ 
++/* The result of attempting to decode a run of UTF-8 bytes.  */
++
++struct cpp_decoded_char
++{
++  const char *m_start_byte;
++  const char *m_next_byte;
++
++  bool m_valid_ch;
++  cppchar_t m_ch;
++};
++
++/* Information for mapping between code points and display columns.
++
++   This is a tabstop value, along with a callback for getting the
++   widths of characters.  Normally this callback is cpp_wcwidth, but we
++   support other schemes for escaping non-ASCII unicode as a series of
++   ASCII chars when printing the user's source code in diagnostic-show-locus.c
++
++   For example, consider:
++   - the Unicode character U+03C0 "GREEK SMALL LETTER PI" (UTF-8: 0xCF 0x80)
++   - the Unicode character U+1F642 "SLIGHTLY SMILING FACE"
++     (UTF-8: 0xF0 0x9F 0x99 0x82)
++   - the byte 0xBF (a stray trailing byte of a UTF-8 character)
++   Normally U+03C0 would occupy one display column, U+1F642
++   would occupy two display columns, and the stray byte would be
++   printed verbatim as one display column.
++
++   However when escaping them as unicode code points as "<U+03C0>"
++   and "<U+1F642>" they occupy 8 and 9 display columns respectively,
++   and when escaping them as bytes as "<CF><80>" and "<F0><9F><99><82>"
++   they occupy 8 and 16 display columns respectively.  In both cases
++   the stray byte is escaped to <BF> as 4 display columns.  */
++
++struct cpp_char_column_policy
++{
++  cpp_char_column_policy (int tabstop,
++			  int (*width_cb) (cppchar_t c))
++  : m_tabstop (tabstop),
++    m_undecoded_byte_width (1),
++    m_width_cb (width_cb)
++  {}
++
++  int m_tabstop;
++  /* Width in display columns of a stray byte that isn't decodable
++     as UTF-8.  */
++  int m_undecoded_byte_width;
++  int (*m_width_cb) (cppchar_t c);
++};
++
+ /* A class to manage the state while converting a UTF-8 sequence to cppchar_t
+    and computing the display width one character at a time.  */
+ class cpp_display_width_computation {
+  public:
+   cpp_display_width_computation (const char *data, int data_length,
+-				 int tabstop);
++				 const cpp_char_column_policy &policy);
+   const char *next_byte () const { return m_next; }
+   int bytes_processed () const { return m_next - m_begin; }
+   int bytes_left () const { return m_bytes_left; }
+   bool done () const { return !bytes_left (); }
+   int display_cols_processed () const { return m_display_cols; }
+ 
+-  int process_next_codepoint ();
++  int process_next_codepoint (cpp_decoded_char *out);
+   int advance_display_cols (int n);
+ 
+  private:
+   const char *const m_begin;
+   const char *m_next;
+   size_t m_bytes_left;
+-  const int m_tabstop;
++  const cpp_char_column_policy &m_policy;
+   int m_display_cols;
+ };
+ 
+ /* Convenience functions that are simple use cases for class
+    cpp_display_width_computation.  Tab characters will be expanded to spaces
+-   as determined by TABSTOP.  */
++   as determined by POLICY.m_tabstop, and non-printable-ASCII characters
++   will be escaped as per POLICY.  */
++
+ int cpp_byte_column_to_display_column (const char *data, int data_length,
+-				       int column, int tabstop);
++				       int column,
++				       const cpp_char_column_policy &policy);
+ inline int cpp_display_width (const char *data, int data_length,
+-			      int tabstop)
++			      const cpp_char_column_policy &policy)
+ {
+   return cpp_byte_column_to_display_column (data, data_length, data_length,
+-					    tabstop);
++					    policy);
+ }
+ int cpp_display_column_to_byte_column (const char *data, int data_length,
+-				       int display_col, int tabstop);
++				       int display_col,
++				       const cpp_char_column_policy &policy);
+ int cpp_wcwidth (cppchar_t c);
+ 
+ #endif /* ! LIBCPP_CPPLIB_H */
+diff --git a/libcpp/include/line-map.h b/libcpp/include/line-map.h
+--- a/libcpp/include/line-map.h	2021-07-27 23:55:08.716307283 -0700
++++ b/libcpp/include/line-map.h	2021-12-14 01:16:01.557942991 -0800
+@@ -1781,6 +1781,18 @@ class rich_location
+   const diagnostic_path *get_path () const { return m_path; }
+   void set_path (const diagnostic_path *path) { m_path = path; }
+ 
++  /* A flag for hinting that the diagnostic involves character encoding
++     issues, and thus that it will be helpful to the user if we show some
++     representation of how the characters in the pertinent source lines
++     are encoded.
++     The default is false (i.e. do not escape).
++     When set to true, non-ASCII bytes in the pertinent source lines will
++     be escaped in a manner controlled by the user-supplied option
++     -fdiagnostics-escape-format=, so that the user can better understand
++     what's going on with the encoding in their source file.  */
++  bool escape_on_output_p () const { return m_escape_on_output; }
++  void set_escape_on_output (bool flag) { m_escape_on_output = flag; }
++
+ private:
+   bool reject_impossible_fixit (location_t where);
+   void stop_supporting_fixits ();
+@@ -1807,6 +1819,7 @@ protected:
+   bool m_fixits_cannot_be_auto_applied;
+ 
+   const diagnostic_path *m_path;
++  bool m_escape_on_output;
+ };
+ 
+ /* A struct for the result of range_label::get_text: a NUL-terminated buffer
+diff --git a/libcpp/internal.h b/libcpp/internal.h
+--- a/libcpp/internal.h	2021-12-13 23:23:05.768437079 -0800
++++ b/libcpp/internal.h	2021-12-14 01:16:01.557942991 -0800
+@@ -776,6 +776,9 @@ extern void _cpp_do_file_change (cpp_rea
+ extern void _cpp_pop_buffer (cpp_reader *);
+ extern char *_cpp_bracket_include (cpp_reader *);
+ 
++/* In errors.c  */
++extern location_t cpp_diagnostic_get_current_location (cpp_reader *);
++
+ /* In traditional.c.  */
+ extern bool _cpp_scan_out_logical_line (cpp_reader *, cpp_macro *, bool);
+ extern bool _cpp_read_logical_line_trad (cpp_reader *);
+@@ -942,6 +945,26 @@ int linemap_get_expansion_line (class li
+ const char* linemap_get_expansion_filename (class line_maps *,
+ 					    location_t);
+ 
++/* A subclass of rich_location for emitting a diagnostic
++   at the current location of the reader, but flagging
++   it with set_escape_on_output (true).  */
++class encoding_rich_location : public rich_location
++{
++ public:
++  encoding_rich_location (cpp_reader *pfile)
++  : rich_location (pfile->line_table,
++		   cpp_diagnostic_get_current_location (pfile))
++  {
++    set_escape_on_output (true);
++  }
++
++  encoding_rich_location (cpp_reader *pfile, location_t loc)
++  : rich_location (pfile->line_table, loc)
++  {
++    set_escape_on_output (true);
++  }
++};
++
+ #ifdef __cplusplus
+ }
+ #endif
+diff --git a/libcpp/lex.c b/libcpp/lex.c
+--- a/libcpp/lex.c	2021-12-14 01:14:48.435225968 -0800
++++ b/libcpp/lex.c	2021-12-14 01:24:37.220995816 -0800
+@@ -1774,7 +1774,11 @@ skip_whitespace (cpp_reader *pfile, cppc
+   while (is_nvspace (c));
+ 
+   if (saw_NUL)
+-    cpp_error (pfile, CPP_DL_WARNING, "null character(s) ignored");
++    {
++      encoding_rich_location rich_loc (pfile);
++      cpp_error_at (pfile, CPP_DL_WARNING, &rich_loc,
++		    "null character(s) ignored");
++    }
+ 
+   buffer->cur--;
+ }
+@@ -1803,6 +1807,28 @@ warn_about_normalization (cpp_reader *pf
+   if (CPP_OPTION (pfile, warn_normalize) < NORMALIZE_STATE_RESULT (s)
+       && !pfile->state.skipping)
+     {
++      location_t loc = token->src_loc;
++
++      /* If possible, create a location range for the token.  */
++      if (loc >= RESERVED_LOCATION_COUNT
++	  && token->type != CPP_EOF
++	  /* There must be no line notes to process.  */
++	  && (!(pfile->buffer->cur
++		>= pfile->buffer->notes[pfile->buffer->cur_note].pos
++		&& !pfile->overlaid_buffer)))
++	{
++	  source_range tok_range;
++	  tok_range.m_start = loc;
++	  tok_range.m_finish
++	    = linemap_position_for_column (pfile->line_table,
++					   CPP_BUF_COLUMN (pfile->buffer,
++							   pfile->buffer->cur));
++	  loc = COMBINE_LOCATION_DATA (pfile->line_table,
++				       loc, tok_range, NULL);
++	}
++
++      encoding_rich_location rich_loc (pfile, loc);
++
+       /* Make sure that the token is printed using UCNs, even
+ 	 if we'd otherwise happily print UTF-8.  */
+       unsigned char *buf = XNEWVEC (unsigned char, cpp_token_len (token));
+@@ -1810,11 +1836,11 @@ warn_about_normalization (cpp_reader *pf
+ 
+       sz = cpp_spell_token (pfile, token, buf, false) - buf;
+       if (NORMALIZE_STATE_RESULT (s) == normalized_C)
+-	cpp_warning_with_line (pfile, CPP_W_NORMALIZE, token->src_loc, 0,
+-			       "`%.*s' is not in NFKC", (int) sz, buf);
++	cpp_warning_at (pfile, CPP_W_NORMALIZE, &rich_loc,
++			"`%.*s' is not in NFKC", (int) sz, buf);
+       else
+-	cpp_warning_with_line (pfile, CPP_W_NORMALIZE, token->src_loc, 0,
+-			       "`%.*s' is not in NFC", (int) sz, buf);
++	cpp_warning_at (pfile, CPP_W_NORMALIZE, &rich_loc,
++			"`%.*s' is not in NFC", (int) sz, buf);
+       free (buf);
+     }
+ }
+diff --git a/libcpp/line-map.c b/libcpp/line-map.c
+--- a/libcpp/line-map.c	2021-07-27 23:55:08.716307283 -0700
++++ b/libcpp/line-map.c	2021-12-14 01:16:01.561942921 -0800
+@@ -2086,7 +2086,8 @@ rich_location::rich_location (line_maps
+   m_fixit_hints (),
+   m_seen_impossible_fixit (false),
+   m_fixits_cannot_be_auto_applied (false),
+-  m_path (NULL)
++  m_path (NULL),
++  m_escape_on_output (false)
+ {
+   add_range (loc, SHOW_RANGE_WITH_CARET, label);
+ }
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-CVE-2021-46195.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-CVE-2021-46195.patch
new file mode 100644
index 0000000..7b3651c
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-CVE-2021-46195.patch
@@ -0,0 +1,128 @@
+From f10bec5ffa487ad3033ed5f38cfd0fc7d696deab Mon Sep 17 00:00:00 2001
+From: Nick Clifton <nickc@redhat.com>
+Date: Mon, 31 Jan 2022 14:28:42 +0000
+Subject: [PATCH] libiberty: Fix infinite recursion in rust demangler.
+
+libiberty/
+	PR demangler/98886
+	PR demangler/99935
+	* rust-demangle.c (struct rust_demangler): Add a recursion
+	counter.
+	(demangle_path): Increment/decrement the recursion counter upon
+	entry and exit.  Fail if the counter exceeds a fixed limit.
+	(demangle_type): Likewise.
+	(rust_demangle_callback): Initialise the recursion counter,
+	disabling if requested by the option flags.
+
+CVE: CVE-2021-46195
+Upstream-Status: Backport
+[https://gcc.gnu.org/git/gitweb.cgi?p=gcc.git;h=f10bec5ffa487ad3033ed5f38cfd0fc7d696deab]
+Signed-off-by: Pgowda <pgowda.cve@gmail.com>
+---
+ libiberty/rust-demangle.c | 47 ++++++++++++++++++++++++++++++++++-----
+ 1 file changed, 41 insertions(+), 6 deletions(-)
+
+diff --git a/libiberty/rust-demangle.c b/libiberty/rust-demangle.c
+index 18c760491bd..3b24d63892a 100644
+--- a/libiberty/rust-demangle.c
++++ b/libiberty/rust-demangle.c
+@@ -74,6 +74,12 @@ struct rust_demangler
+   /* Rust mangling version, with legacy mangling being -1. */
+   int version;
+ 
++  /* Recursion depth.  */
++  unsigned int recursion;
++  /* Maximum number of times demangle_path may be called recursively.  */
++#define RUST_MAX_RECURSION_COUNT  1024
++#define RUST_NO_RECURSION_LIMIT   ((unsigned int) -1)
++
+   uint64_t bound_lifetime_depth;
+ };
+ 
+@@ -671,6 +677,15 @@ demangle_path (struct rust_demangler *rdm, int in_value)
+   if (rdm->errored)
+     return;
+ 
++  if (rdm->recursion != RUST_NO_RECURSION_LIMIT)
++    {
++      ++ rdm->recursion;
++      if (rdm->recursion > RUST_MAX_RECURSION_COUNT)
++	/* FIXME: There ought to be a way to report
++	   that the recursion limit has been reached.  */
++	goto fail_return;
++    }
++
+   switch (tag = next (rdm))
+     {
+     case 'C':
+@@ -688,10 +703,7 @@ demangle_path (struct rust_demangler *rdm, int in_value)
+     case 'N':
+       ns = next (rdm);
+       if (!ISLOWER (ns) && !ISUPPER (ns))
+-        {
+-          rdm->errored = 1;
+-          return;
+-        }
++	goto fail_return;
+ 
+       demangle_path (rdm, in_value);
+ 
+@@ -776,9 +788,15 @@ demangle_path (struct rust_demangler *rdm, int in_value)
+         }
+       break;
+     default:
+-      rdm->errored = 1;
+-      return;
++      goto fail_return;
+     }
++  goto pass_return;
++
++ fail_return:
++  rdm->errored = 1;
++ pass_return:
++  if (rdm->recursion != RUST_NO_RECURSION_LIMIT)
++    -- rdm->recursion;
+ }
+ 
+ static void
+@@ -870,6 +888,19 @@ demangle_type (struct rust_demangler *rdm)
+       return;
+     }
+ 
++   if (rdm->recursion != RUST_NO_RECURSION_LIMIT)
++    {
++      ++ rdm->recursion;
++      if (rdm->recursion > RUST_MAX_RECURSION_COUNT)
++	/* FIXME: There ought to be a way to report
++	   that the recursion limit has been reached.  */
++	{
++	  rdm->errored = 1;
++	  -- rdm->recursion;
++	  return;
++	}
++    }
++
+   switch (tag)
+     {
+     case 'R':
+@@ -1030,6 +1061,9 @@ demangle_type (struct rust_demangler *rdm)
+       rdm->next--;
+       demangle_path (rdm, 0);
+     }
++
++  if (rdm->recursion != RUST_NO_RECURSION_LIMIT)
++    -- rdm->recursion;
+ }
+ 
+ /* A trait in a trait object may have some "existential projections"
+@@ -1320,6 +1354,7 @@ rust_demangle_callback (const char *mangled, int options,
+   rdm.skipping_printing = 0;
+   rdm.verbose = (options & DMGL_VERBOSE) != 0;
+   rdm.version = 0;
++  rdm.recursion = (options & DMGL_NO_RECURSE_LIMIT) ? RUST_NO_RECURSION_LIMIT : 0;
+   rdm.bound_lifetime_depth = 0;
+ 
+   /* Rust symbols always start with _R (v0) or _ZN (legacy). */
+-- 
+2.27.0
+
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-Fix-install-path-of-linux64.h.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-Fix-install-path-of-linux64.h.patch
new file mode 100644
index 0000000..5bf895d
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-Fix-install-path-of-linux64.h.patch
@@ -0,0 +1,33 @@
+From 58211c7ceb0510b2a11a7f1da3c7fa968c658749 Mon Sep 17 00:00:00 2001
+From: Andrei Gherzan <andrei.gherzan@huawei.com>
+Date: Wed, 22 Dec 2021 12:49:25 +0100
+Subject: [PATCH] Fix install path of linux64.h
+
+We add linux64.h to tm includes[1] as a relative path to B. This patch
+adapts the install path of linux64.h to match the include in tm.h.
+
+[1] 0016-Use-the-multilib-config-files-from-B-instead-of-usin.patch
+
+Signed-off-by: Andrei Gherzan <andrei.gherzan@huawei.com>
+
+Upstream-Status: Inappropriate [configuration]
+---
+ gcc/Makefile.in | 2 ++
+ 1 file changed, 2 insertions(+)
+
+diff --git a/gcc/Makefile.in b/gcc/Makefile.in
+index 9b17d120a..d175ec4e3 100644
+--- a/gcc/Makefile.in
++++ b/gcc/Makefile.in
+@@ -3693,6 +3693,8 @@ install-plugin: installdirs lang.install-plugin s-header-vars install-gengtype
+ 	  "$(srcdir)"/config/* | "$(srcdir)"/common/config/* \
+ 	  | "$(srcdir)"/c-family/* | "$(srcdir)"/*.def ) \
+ 	    base=`echo "$$path" | sed -e "s|$$srcdirstrip/||"`;; \
++	  */linux64.h ) \
++	    base=`dirname $$path`;;\
+ 	  *) base=`basename $$path` ;; \
+ 	  esac; \
+ 	  dest=$(plugin_includedir)/$$base; \
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-gcc-4.3.1-ARCH_FLAGS_FOR_TARGET.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-gcc-4.3.1-ARCH_FLAGS_FOR_TARGET.patch
new file mode 100644
index 0000000..0a108ee
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0001-gcc-4.3.1-ARCH_FLAGS_FOR_TARGET.patch
@@ -0,0 +1,39 @@
+From 368eaf1846733a9920c7cf5bc547a377e7167785 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Fri, 29 Mar 2013 08:37:11 +0400
+Subject: [PATCH] gcc-4.3.1: ARCH_FLAGS_FOR_TARGET
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+
+Upstream-Status: Inappropriate [embedded specific]
+---
+ configure    | 2 +-
+ configure.ac | 2 +-
+ 2 files changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/configure b/configure
+index 504f6410274..bcebad264ec 100755
+--- a/configure
++++ b/configure
+@@ -9728,7 +9728,7 @@ fi
+ # for target_alias and gcc doesn't manage it consistently.
+ target_configargs="--cache-file=./config.cache ${target_configargs}"
+ 
+-FLAGS_FOR_TARGET=
++FLAGS_FOR_TARGET="$ARCH_FLAGS_FOR_TARGET"
+ case " $target_configdirs " in
+  *" newlib "*)
+   case " $target_configargs " in
+diff --git a/configure.ac b/configure.ac
+index 088e735c5db..1289fe08760 100644
+--- a/configure.ac
++++ b/configure.ac
+@@ -3240,7 +3240,7 @@ fi
+ # for target_alias and gcc doesn't manage it consistently.
+ target_configargs="--cache-file=./config.cache ${target_configargs}"
+ 
+-FLAGS_FOR_TARGET=
++FLAGS_FOR_TARGET="$ARCH_FLAGS_FOR_TARGET"
+ case " $target_configdirs " in
+  *" newlib "*)
+   case " $target_configargs " in
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0002-CVE-2021-42574.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0002-CVE-2021-42574.patch
new file mode 100644
index 0000000..9bad81d
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0002-CVE-2021-42574.patch
@@ -0,0 +1,1765 @@
+From 51c500269bf53749b107807d84271385fad35628 Mon Sep 17 00:00:00 2001
+From: Marek Polacek <polacek@redhat.com>
+Date: Wed, 6 Oct 2021 14:33:59 -0400
+Subject: [PATCH] libcpp: Implement -Wbidi-chars for CVE-2021-42574 [PR103026]
+
+From a link below:
+"An issue was discovered in the Bidirectional Algorithm in the Unicode
+Specification through 14.0. It permits the visual reordering of
+characters via control sequences, which can be used to craft source code
+that renders different logic than the logical ordering of tokens
+ingested by compilers and interpreters. Adversaries can leverage this to
+encode source code for compilers accepting Unicode such that targeted
+vulnerabilities are introduced invisibly to human reviewers."
+
+More info:
+https://nvd.nist.gov/vuln/detail/CVE-2021-42574
+https://trojansource.codes/
+
+This is not a compiler bug.  However, to mitigate the problem, this patch
+implements -Wbidi-chars=[none|unpaired|any] to warn about possibly
+misleading Unicode bidirectional control characters the preprocessor may
+encounter.
+
+The default is =unpaired, which warns about improperly terminated
+bidirectional control characters; e.g. a LRE without its corresponding PDF.
+The level =any warns about any use of bidirectional control characters.
+
+This patch handles both UCNs and UTF-8 characters.  UCNs designating
+bidi characters in identifiers are accepted since r204886.  Then r217144
+enabled -fextended-identifiers by default.  Extended characters in C/C++
+identifiers have been accepted since r275979.  However, this patch still
+warns about mixing UTF-8 and UCN bidi characters; there seems to be no
+good reason to allow mixing them.
+
+We warn in different contexts: comments (both C and C++-style), string
+literals, character constants, and identifiers.  Expectedly, UCNs are ignored
+in comments and raw string literals.  The bidirectional control characters
+can nest so this patch handles that as well.
+
+I have not included nor tested this at all with Fortran (which also has
+string literals and line comments).
+
+Dave M. posted patches improving diagnostic involving Unicode characters.
+This patch does not make use of this new infrastructure yet.
+
+	PR preprocessor/103026
+
+gcc/c-family/ChangeLog:
+
+	* c.opt (Wbidi-chars, Wbidi-chars=): New option.
+
+gcc/ChangeLog:
+
+	* doc/invoke.texi: Document -Wbidi-chars.
+
+libcpp/ChangeLog:
+
+	* include/cpplib.h (enum cpp_bidirectional_level): New.
+	(struct cpp_options): Add cpp_warn_bidirectional.
+	(enum cpp_warning_reason): Add CPP_W_BIDIRECTIONAL.
+	* internal.h (struct cpp_reader): Add warn_bidi_p member
+	function.
+	* init.c (cpp_create_reader): Set cpp_warn_bidirectional.
+	* lex.c (bidi): New namespace.
+	(get_bidi_utf8): New function.
+	(get_bidi_ucn): Likewise.
+	(maybe_warn_bidi_on_close): Likewise.
+	(maybe_warn_bidi_on_char): Likewise.
+	(_cpp_skip_block_comment): Implement warning about bidirectional
+	control characters.
+	(skip_line_comment): Likewise.
+	(forms_identifier_p): Likewise.
+	(lex_identifier): Likewise.
+	(lex_string): Likewise.
+	(lex_raw_string): Likewise.
+
+gcc/testsuite/ChangeLog:
+
+	* c-c++-common/Wbidi-chars-1.c: New test.
+	* c-c++-common/Wbidi-chars-2.c: New test.
+	* c-c++-common/Wbidi-chars-3.c: New test.
+	* c-c++-common/Wbidi-chars-4.c: New test.
+	* c-c++-common/Wbidi-chars-5.c: New test.
+	* c-c++-common/Wbidi-chars-6.c: New test.
+	* c-c++-common/Wbidi-chars-7.c: New test.
+	* c-c++-common/Wbidi-chars-8.c: New test.
+	* c-c++-common/Wbidi-chars-9.c: New test.
+	* c-c++-common/Wbidi-chars-10.c: New test.
+	* c-c++-common/Wbidi-chars-11.c: New test.
+	* c-c++-common/Wbidi-chars-12.c: New test.
+	* c-c++-common/Wbidi-chars-13.c: New test.
+	* c-c++-common/Wbidi-chars-14.c: New test.
+	* c-c++-common/Wbidi-chars-15.c: New test.
+	* c-c++-common/Wbidi-chars-16.c: New test.
+	* c-c++-common/Wbidi-chars-17.c: New test.
+
+CVE: CVE-2021-42574
+Upstream-Status: Backport [https://gcc.gnu.org/git/gitweb.cgi?p=gcc.git;h=51c500269bf53749b107807d84271385fad35628]
+Signed-off-by: Pgowda <pgowda.cve@gmail.com>
+
+---
+ gcc/c-family/c.opt                          |  24 ++
+ gcc/doc/invoke.texi                         |  21 +-
+ gcc/testsuite/c-c++-common/Wbidi-chars-1.c  |  12 +
+ gcc/testsuite/c-c++-common/Wbidi-chars-10.c |  27 ++
+ gcc/testsuite/c-c++-common/Wbidi-chars-11.c |  13 +
+ gcc/testsuite/c-c++-common/Wbidi-chars-12.c |  19 +
+ gcc/testsuite/c-c++-common/Wbidi-chars-13.c |  17 +
+ gcc/testsuite/c-c++-common/Wbidi-chars-14.c |  38 ++
+ gcc/testsuite/c-c++-common/Wbidi-chars-15.c |  59 +++
+ gcc/testsuite/c-c++-common/Wbidi-chars-16.c |  26 ++
+ gcc/testsuite/c-c++-common/Wbidi-chars-17.c |  30 ++
+ gcc/testsuite/c-c++-common/Wbidi-chars-2.c  |   9 +
+ gcc/testsuite/c-c++-common/Wbidi-chars-3.c  |  11 +
+ gcc/testsuite/c-c++-common/Wbidi-chars-4.c  | 188 +++++++++
+ gcc/testsuite/c-c++-common/Wbidi-chars-5.c  | 188 +++++++++
+ gcc/testsuite/c-c++-common/Wbidi-chars-6.c  | 155 ++++++++
+ gcc/testsuite/c-c++-common/Wbidi-chars-7.c  |   9 +
+ gcc/testsuite/c-c++-common/Wbidi-chars-8.c  |  13 +
+ gcc/testsuite/c-c++-common/Wbidi-chars-9.c  |  29 ++
+ libcpp/include/cpplib.h                     |  18 +-
+ libcpp/init.c                               |   1 +
+ libcpp/internal.h                           |   7 +
+ libcpp/lex.c                                | 408 +++++++++++++++++++-
+ 23 files changed, 1315 insertions(+), 7 deletions(-)
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-1.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-10.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-11.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-12.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-13.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-14.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-15.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-16.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-17.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-2.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-3.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-4.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-5.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-6.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-7.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-8.c
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-9.c
+
+diff --git a/gcc/c-family/c.opt b/gcc/c-family/c.opt
+index 8a4cd634f77..3976fc368db 100644
+--- a/gcc/c-family/c.opt
++++ b/gcc/c-family/c.opt
+@@ -370,6 +370,30 @@ Wbad-function-cast
+ C ObjC Var(warn_bad_function_cast) Warning
+ Warn about casting functions to incompatible types.
+ 
++Wbidi-chars
++C ObjC C++ ObjC++ Warning Alias(Wbidi-chars=,any,none)
++;
++
++Wbidi-chars=
++C ObjC C++ ObjC++ RejectNegative Joined Warning CPP(cpp_warn_bidirectional) CppReason(CPP_W_BIDIRECTIONAL) Var(warn_bidirectional) Init(bidirectional_unpaired) Enum(cpp_bidirectional_level)
++-Wbidi-chars=[none|unpaired|any] Warn about UTF-8 bidirectional control characters.
++
++; Required for these enum values.
++SourceInclude
++cpplib.h
++
++Enum
++Name(cpp_bidirectional_level) Type(int) UnknownError(argument %qs to %<-Wbidi-chars%> not recognized)
++
++EnumValue
++Enum(cpp_bidirectional_level) String(none) Value(bidirectional_none)
++
++EnumValue
++Enum(cpp_bidirectional_level) String(unpaired) Value(bidirectional_unpaired)
++
++EnumValue
++Enum(cpp_bidirectional_level) String(any) Value(bidirectional_any)
++
+ Wbool-compare
+ C ObjC C++ ObjC++ Var(warn_bool_compare) Warning LangEnabledBy(C ObjC C++ ObjC++,Wall)
+ Warn about boolean expression compared with an integer value different from true/false.
+diff --git a/gcc/doc/invoke.texi b/gcc/doc/invoke.texi
+index 6070288856c..a22758d18ee 100644
+--- a/gcc/doc/invoke.texi
++++ b/gcc/doc/invoke.texi
+@@ -326,7 +326,9 @@ Objective-C and Objective-C++ Dialects}.
+ -Warith-conversion @gol
+ -Warray-bounds  -Warray-bounds=@var{n} @gol
+ -Wno-attributes  -Wattribute-alias=@var{n} -Wno-attribute-alias @gol
+--Wno-attribute-warning  -Wbool-compare  -Wbool-operation @gol
++-Wno-attribute-warning  @gol
++-Wbidi-chars=@r{[}none@r{|}unpaired@r{|}any@r{]} @gol
++-Wbool-compare  -Wbool-operation @gol
+ -Wno-builtin-declaration-mismatch @gol
+ -Wno-builtin-macro-redefined  -Wc90-c99-compat  -Wc99-c11-compat @gol
+ -Wc11-c2x-compat @gol
+@@ -7559,6 +7561,23 @@ Attributes considered include @code{allo
+ This is the default.  You can disable these warnings with either
+ @option{-Wno-attribute-alias} or @option{-Wattribute-alias=0}.
+ 
++@item -Wbidi-chars=@r{[}none@r{|}unpaired@r{|}any@r{]}
++@opindex Wbidi-chars=
++@opindex Wbidi-chars
++@opindex Wno-bidi-chars
++Warn about possibly misleading UTF-8 bidirectional control characters in
++comments, string literals, character constants, and identifiers.  Such
++characters can change left-to-right writing direction into right-to-left
++(and vice versa), which can cause confusion between the logical order and
++visual order.  This may be dangerous; for instance, it may seem that a piece
++of code is not commented out, whereas it in fact is.
++
++There are three levels of warning supported by GCC@.  The default is
++@option{-Wbidi-chars=unpaired}, which warns about improperly terminated
++bidi contexts.  @option{-Wbidi-chars=none} turns the warning off.
++@option{-Wbidi-chars=any} warns about any use of bidirectional control
++characters.
++
+ @item -Wbool-compare
+ @opindex Wno-bool-compare
+ @opindex Wbool-compare
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-10.c b/gcc/testsuite/c-c++-common/Wbidi-chars-10.c
+new file mode 100644
+index 00000000000..34f5ac19271
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-10.c
+@@ -0,0 +1,27 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=unpaired" } */
++/* More nesting testing.  */
++
++/* RLEâ« LRIâ¦ PDFâ¬ PDIâ©*/
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int LRE_\u202a_PDF_\u202c;
++int LRE_\u202a_PDF_\u202c_LRE_\u202a_PDF_\u202c;
++int LRE_\u202a_LRI_\u2066_PDF_\u202c_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int RLE_\u202b_RLI_\u2067_PDF_\u202c_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int RLE_\u202b_RLI_\u2067_PDI_\u2069_PDF_\u202c;
++int FSI_\u2068_LRO_\u202d_PDI_\u2069_PDF_\u202c;
++int FSI_\u2068;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int FSI_\u2068_PDI_\u2069;
++int FSI_\u2068_FSI_\u2068_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069;
++int RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDF_\u202c;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int RLI_\u2067_RLI_\u2067_RLI_\u2067_RLI_\u2067_FSI_\u2068_PDI_\u2069_PDI_\u2069_PDI_\u2069_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-11.c b/gcc/testsuite/c-c++-common/Wbidi-chars-11.c
+new file mode 100644
+index 00000000000..270ce2368a9
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-11.c
+@@ -0,0 +1,13 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=unpaired" } */
++/* Test that we warn when mixing UCN and UTF-8.  */
++
++int LRE_âª_PDF_\u202c;
++/* { dg-warning "mismatch" "" { target *-*-* } .-1 } */
++int LRE_\u202a_PDF_â¬_;
++/* { dg-warning "mismatch" "" { target *-*-* } .-1 } */
++const char *s1 = "LRE_âª_PDF_\u202c";
++/* { dg-warning "mismatch" "" { target *-*-* } .-1 } */
++const char *s2 = "LRE_\u202a_PDF_â¬";
++/* { dg-warning "mismatch" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-12.c b/gcc/testsuite/c-c++-common/Wbidi-chars-12.c
+new file mode 100644
+index 00000000000..b07eec1da91
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-12.c
+@@ -0,0 +1,19 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile { target { c || c++11 } } } */
++/* { dg-options "-Wbidi-chars=any" } */
++/* Test raw strings.  */
++
++const char *s1 = R"(a b c LREâª 1 2 3 PDFâ¬ x y z)";
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++const char *s2 = R"(a b c RLEâ« 1 2 3 PDFâ¬ x y z)";
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++const char *s3 = R"(a b c LROâ­ 1 2 3 PDFâ¬ x y z)";
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++const char *s4 = R"(a b c RLOâ® 1 2 3 PDFâ¬ x y z)";
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++const char *s7 = R"(a b c FSIâ¨ 1 2 3 PDIâ© x y) z";
++/* { dg-warning "U\\+2068" "" { target *-*-* } .-1 } */
++const char *s8 = R"(a b c PDIâ© x y )z";
++/* { dg-warning "U\\+2069" "" { target *-*-* } .-1 } */
++const char *s9 = R"(a b c PDFâ¬ x y z)";
++/* { dg-warning "U\\+202C" "" { target *-*-* } .-1 } */
+diff -uprN '-x*.orig' '-x*.rej' del/gcc-11.2.0/gcc/testsuite/c-c++-common/Wbidi-chars-13.c gcc-11.2.0/gcc/testsuite/c-c++-common/Wbidi-chars-13.c
+--- del/gcc-11.2.0/gcc/testsuite/c-c++-common/Wbidi-chars-13.c	1969-12-31 16:00:00.000000000 -0800
++++ gcc-11.2.0/gcc/testsuite/c-c++-common/Wbidi-chars-13.c	2021-12-13 23:11:22.328439287 -0800
+@@ -0,0 +1,17 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile { target { c || c++11 } } } */
++/* { dg-options "-Wbidi-chars=unpaired" } */
++/* Test raw strings.  */
++
++const char *s1 = R"(a b c LREâª 1 2 3)";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++const char *s2 = R"(a b c RLEâ« 1 2 3)";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++const char *s3 = R"(a b c LROâ­ 1 2 3)";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++const char *s4 = R"(a b c FSIâ¨ 1 2 3)";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++const char *s5 = R"(a b c LRIâ¦ 1 2 3)";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++const char *s6 = R"(a b c RLIâ§ 1 2 3)";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-14.c b/gcc/testsuite/c-c++-common/Wbidi-chars-14.c
+new file mode 100644
+index 00000000000..ba5f75d9553
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-14.c
+@@ -0,0 +1,38 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=unpaired" } */
++/* Test PDI handling, which also pops any subsequent LREs, RLEs, LROs,
++   or RLOs.  */
++
++/* LRI_â¦_LRI_â¦_RLE_â«_RLE_â«_RLE_â«_PDI_â©*/
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++// LRI_â¦_RLE_â«_RLE_â«_RLE_â«_PDI_â©
++// LRI_â¦_RLO_â®_RLE_â«_RLE_â«_PDI_â©
++// LRI_â¦_RLO_â®_RLE_â«_PDI_â©
++// FSI_â¨_RLO_â®_PDI_â©
++// FSI_â¨_FSI_â¨_RLO_â®_PDI_â©
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++
++int LRI_\u2066_LRI_\u2066_LRE_\u202a_LRE_\u202a_LRE_\u202a_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int LRI_\u2066_LRI_\u2066_LRE_\u202a_LRE_\u202a_LRE_\u202a_PDI_\u2069_PDI_\u2069;
++int LRI_\u2066_LRI_\u2066_LRI_\u2066_LRE_\u202a_LRE_\u202a_LRE_\u202a_PDI_\u2069_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int PDI_\u2069;
++int LRI_\u2066_PDI_\u2069;
++int RLI_\u2067_PDI_\u2069;
++int LRE_\u202a_LRI_\u2066_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int LRI_\u2066_LRE_\u202a_PDF_\u202c_PDI_\u2069;
++int LRI_\u2066_LRE_\u202a_LRE_\u202a_PDF_\u202c_PDI_\u2069;
++int RLI_\u2067_LRI_\u2066_LRE_\u202a_LRE_\u202a_PDF_\u202c_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int FSI_\u2068_LRI_\u2066_LRE_\u202a_LRE_\u202a_PDF_\u202c_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int RLO_\u202e_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int RLI_\u2067_PDI_\u2069_RLI_\u2067;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int FSI_\u2068_PDF_\u202c_PDI_\u2069;
++int FSI_\u2068_FSI_\u2068_PDF_\u202c_PDI_\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-15.c b/gcc/testsuite/c-c++-common/Wbidi-chars-15.c
+new file mode 100644
+index 00000000000..a0ce8ff5e2c
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-15.c
+@@ -0,0 +1,59 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=unpaired" } */
++/* Test unpaired bidi control chars in multiline comments.  */
++
++/*
++ * LREâª end
++ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++/*
++ * RLEâ« end
++ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++/*
++ * LROâ­ end
++ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++/*
++ * RLOâ® end
++ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++/*
++ * LRIâ¦ end
++ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++/*
++ * RLIâ§ end
++ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++/*
++ * FSIâ¨ end
++ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++/* LREâª
++   PDFâ¬ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++/* FSIâ¨
++   PDIâ© */
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++
++/* LRE<âª>
++ *
++ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-3 } */
++
++/*
++ * LRE<âª>
++ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++
++/*
++ *
++ * LRE<âª> */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++
++/* RLI<â§> */ /* PDI<â©> */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* LRE<âª> */ /* PDF<â¬> */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-16.c b/gcc/testsuite/c-c++-common/Wbidi-chars-16.c
+new file mode 100644
+index 00000000000..baa0159861c
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-16.c
+@@ -0,0 +1,26 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=any" } */
++/* Test LTR/RTL chars.  */
++
++/* LTR<â> */
++/* { dg-warning "U\\+200E" "" { target *-*-* } .-1 } */
++// LTR<â>
++/* { dg-warning "U\\+200E" "" { target *-*-* } .-1 } */
++/* RTL<â> */
++/* { dg-warning "U\\+200F" "" { target *-*-* } .-1 } */
++// RTL<â>
++/* { dg-warning "U\\+200F" "" { target *-*-* } .-1 } */
++
++const char *s1 = "LTR<â>";
++/* { dg-warning "U\\+200E" "" { target *-*-* } .-1 } */
++const char *s2 = "LTR\u200e";
++/* { dg-warning "U\\+200E" "" { target *-*-* } .-1 } */
++const char *s3 = "LTR\u200E";
++/* { dg-warning "U\\+200E" "" { target *-*-* } .-1 } */
++const char *s4 = "RTL<â>";
++/* { dg-warning "U\\+200F" "" { target *-*-* } .-1 } */
++const char *s5 = "RTL\u200f";
++/* { dg-warning "U\\+200F" "" { target *-*-* } .-1 } */
++const char *s6 = "RTL\u200F";
++/* { dg-warning "U\\+200F" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-17.c b/gcc/testsuite/c-c++-common/Wbidi-chars-17.c
+new file mode 100644
+index 00000000000..07cb4321f96
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-17.c
+@@ -0,0 +1,30 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=unpaired" } */
++/* Test LTR/RTL chars.  */
++
++/* LTR<â> */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++// LTR<â>
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++/* RTL<â> */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++// RTL<â>
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int ltr_\u200e;
++/* { dg-error "universal character " "" { target *-*-* } .-1 } */
++int rtl_\u200f;
++/* { dg-error "universal character " "" { target *-*-* } .-1 } */
++
++const char *s1 = "LTR<â>";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++const char *s2 = "LTR\u200e";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++const char *s3 = "LTR\u200E";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++const char *s4 = "RTL<â>";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++const char *s5 = "RTL\u200f";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++const char *s6 = "RTL\u200F";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-1.c b/gcc/testsuite/c-c++-common/Wbidi-chars-1.c
+new file mode 100644
+index 00000000000..2340374f276
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-1.c
+@@ -0,0 +1,12 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++
++int main() {
++    int isAdmin = 0;
++    /*â® } â¦if (isAdmin)â© â¦ begin admins only */
++/* { dg-warning "bidirectional" "" { target *-*-* } .-1 } */
++        __builtin_printf("You are an admin.\n");
++    /* end admins only â® { â¦*/
++/* { dg-warning "bidirectional" "" { target *-*-* } .-1 } */
++    return 0;
++}
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-2.c b/gcc/testsuite/c-c++-common/Wbidi-chars-2.c
+new file mode 100644
+index 00000000000..2340374f276
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-2.c
+@@ -0,0 +1,9 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++
++int main() {
++    /* Say hello; newlineâ§/*/ return 0 ;
++/* { dg-warning "bidirectional" "" { target *-*-* } .-1 } */
++    __builtin_printf("Hello world.\n");
++    return 0;
++}
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-3.c b/gcc/testsuite/c-c++-common/Wbidi-chars-3.c
+new file mode 100644
+index 00000000000..9dc7edb6e64
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-3.c
+@@ -0,0 +1,11 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++
++int main() {
++    const char* access_level = "user";
++    if (__builtin_strcmp(access_level, "userâ® â¦// Check if adminâ© â¦")) {
++/* { dg-warning "bidirectional" "" { target *-*-* } .-1 } */
++        __builtin_printf("You are an admin.\n");
++    }
++    return 0;
++}
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-4.c b/gcc/testsuite/c-c++-common/Wbidi-chars-4.c
+new file mode 100644
+index 00000000000..639e5c62e88
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-4.c
+@@ -0,0 +1,188 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=any -Wno-multichar -Wno-overflow" } */
++/* Test all bidi chars in various contexts (identifiers, comments,
++   string literals, character constants), both UCN and UTF-8.  The bidi
++   chars here are properly terminated, except for the character constants.  */
++
++/* a b c LREâª 1 2 3 PDFâ¬ x y z */
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++/* a b c RLEâ« 1 2 3 PDFâ¬ x y z */
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++/* a b c LROâ­ 1 2 3 PDFâ¬ x y z */
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++/* a b c RLOâ® 1 2 3 PDFâ¬ x y z */
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++/* a b c LRIâ¦ 1 2 3 PDIâ© x y z */
++/* { dg-warning "U\\+2066" "" { target *-*-* } .-1 } */
++/* a b c RLIâ§ 1 2 3 PDIâ© x y */
++/* { dg-warning "U\\+2067" "" { target *-*-* } .-1 } */
++/* a b c FSIâ¨ 1 2 3 PDIâ© x y z */
++/* { dg-warning "U\\+2068" "" { target *-*-* } .-1 } */
++
++/* Same but C++ comments instead.  */
++// a b c LREâª 1 2 3 PDFâ¬ x y z
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++// a b c RLEâ« 1 2 3 PDFâ¬ x y z
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++// a b c LROâ­ 1 2 3 PDFâ¬ x y z
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++// a b c RLOâ® 1 2 3 PDFâ¬ x y z
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++// a b c LRIâ¦ 1 2 3 PDIâ© x y z
++/* { dg-warning "U\\+2066" "" { target *-*-* } .-1 } */
++// a b c RLIâ§ 1 2 3 PDIâ© x y
++/* { dg-warning "U\\+2067" "" { target *-*-* } .-1 } */
++// a b c FSIâ¨ 1 2 3 PDIâ© x y z
++/* { dg-warning "U\\+2068" "" { target *-*-* } .-1 } */
++
++/* Here we're closing an unopened context, warn when =any.  */
++/* a b c PDIâ© x y z */
++/* { dg-warning "U\\+2069" "" { target *-*-* } .-1 } */
++/* a b c PDFâ¬ x y z */
++/* { dg-warning "U\\+202C" "" { target *-*-* } .-1 } */
++// a b c PDIâ© x y z
++/* { dg-warning "U\\+2069" "" { target *-*-* } .-1 } */
++// a b c PDFâ¬ x y z
++/* { dg-warning "U\\+202C" "" { target *-*-* } .-1 } */
++
++/* Multiline comments.  */
++/* a b c PDIâ© x y z
++   */
++/* { dg-warning "U\\+2069" "" { target *-*-* } .-2 } */
++/* a b c PDFâ¬ x y z
++   */
++/* { dg-warning "U\\+202C" "" { target *-*-* } .-2 } */
++/* first
++   a b c PDIâ© x y z
++   */
++/* { dg-warning "U\\+2069" "" { target *-*-* } .-2 } */
++/* first
++   a b c PDFâ¬ x y z
++   */
++/* { dg-warning "U\\+202C" "" { target *-*-* } .-2 } */
++/* first
++   a b c PDIâ© x y z */
++/* { dg-warning "U\\+2069" "" { target *-*-* } .-1 } */
++/* first
++   a b c PDFâ¬ x y z */
++/* { dg-warning "U\\+202C" "" { target *-*-* } .-1 } */
++
++void
++g1 ()
++{
++  const char *s1 = "a b c LREâª 1 2 3 PDFâ¬ x y z";
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++  const char *s2 = "a b c RLEâ« 1 2 3 PDFâ¬ x y z";
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++  const char *s3 = "a b c LROâ­ 1 2 3 PDFâ¬ x y z";
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++  const char *s4 = "a b c RLOâ® 1 2 3 PDFâ¬ x y z";
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++  const char *s5 = "a b c LRIâ¦ 1 2 3 PDIâ© x y z";
++/* { dg-warning "U\\+2066" "" { target *-*-* } .-1 } */
++  const char *s6 = "a b c RLIâ§ 1 2 3 PDIâ© x y z";
++/* { dg-warning "U\\+2067" "" { target *-*-* } .-1 } */
++  const char *s7 = "a b c FSIâ¨ 1 2 3 PDIâ© x y z";
++/* { dg-warning "U\\+2068" "" { target *-*-* } .-1 } */
++  const char *s8 = "a b c PDIâ© x y z";
++/* { dg-warning "U\\+2069" "" { target *-*-* } .-1 } */
++  const char *s9 = "a b c PDFâ¬ x y z";
++/* { dg-warning "U\\+202C" "" { target *-*-* } .-1 } */
++
++  const char *s10 = "a b c LRE\u202a 1 2 3 PDF\u202c x y z";
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++  const char *s11 = "a b c LRE\u202A 1 2 3 PDF\u202c x y z";
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++  const char *s12 = "a b c RLE\u202b 1 2 3 PDF\u202c x y z";
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++  const char *s13 = "a b c RLE\u202B 1 2 3 PDF\u202c x y z";
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++  const char *s14 = "a b c LRO\u202d 1 2 3 PDF\u202c x y z";
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++  const char *s15 = "a b c LRO\u202D 1 2 3 PDF\u202c x y z";
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++  const char *s16 = "a b c RLO\u202e 1 2 3 PDF\u202c x y z";
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++  const char *s17 = "a b c RLO\u202E 1 2 3 PDF\u202c x y z";
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++  const char *s18 = "a b c LRI\u2066 1 2 3 PDI\u2069 x y z";
++/* { dg-warning "U\\+2066" "" { target *-*-* } .-1 } */
++  const char *s19 = "a b c RLI\u2067 1 2 3 PDI\u2069 x y z";
++/* { dg-warning "U\\+2067" "" { target *-*-* } .-1 } */
++  const char *s20 = "a b c FSI\u2068 1 2 3 PDI\u2069 x y z";
++/* { dg-warning "U\\+2068" "" { target *-*-* } .-1 } */
++}
++
++void
++g2 ()
++{
++  const char c1 = '\u202a';
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++  const char c2 = '\u202A';
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++  const char c3 = '\u202b';
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++  const char c4 = '\u202B';
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++  const char c5 = '\u202d';
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++  const char c6 = '\u202D';
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++  const char c7 = '\u202e';
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++  const char c8 = '\u202E';
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++  const char c9 = '\u2066';
++/* { dg-warning "U\\+2066" "" { target *-*-* } .-1 } */
++  const char c10 = '\u2067';
++/* { dg-warning "U\\+2067" "" { target *-*-* } .-1 } */
++  const char c11 = '\u2068';
++/* { dg-warning "U\\+2068" "" { target *-*-* } .-1 } */
++}
++
++int aâªbâ¬c;
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++int aâ«bâ¬c;
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++int aâ­bâ¬c;
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++int aâ®bâ¬c;
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++int aâ¦bâ©c;
++/* { dg-warning "U\\+2066" "" { target *-*-* } .-1 } */
++int aâ§bâ©c;
++/* { dg-warning "U\\+2067" "" { target *-*-* } .-1 } */
++int aâ¨bâ©c;
++/* { dg-warning "U\\+2068" "" { target *-*-* } .-1 } */
++int Aâ¬X;
++/* { dg-warning "U\\+202C" "" { target *-*-* } .-1 } */
++int A\u202cY;
++/* { dg-warning "U\\+202C" "" { target *-*-* } .-1 } */
++int A\u202CY2;
++/* { dg-warning "U\\+202C" "" { target *-*-* } .-1 } */
++
++int d\u202ae\u202cf;
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++int d\u202Ae\u202cf2;
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++int d\u202be\u202cf;
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++int d\u202Be\u202cf2;
++/* { dg-warning "U\\+202B" "" { target *-*-* } .-1 } */
++int d\u202de\u202cf;
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++int d\u202De\u202cf2;
++/* { dg-warning "U\\+202D" "" { target *-*-* } .-1 } */
++int d\u202ee\u202cf;
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++int d\u202Ee\u202cf2;
++/* { dg-warning "U\\+202E" "" { target *-*-* } .-1 } */
++int d\u2066e\u2069f;
++/* { dg-warning "U\\+2066" "" { target *-*-* } .-1 } */
++int d\u2067e\u2069f;
++/* { dg-warning "U\\+2067" "" { target *-*-* } .-1 } */
++int d\u2068e\u2069f;
++/* { dg-warning "U\\+2068" "" { target *-*-* } .-1 } */
++int X\u2069;
++/* { dg-warning "U\\+2069" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-5.c b/gcc/testsuite/c-c++-common/Wbidi-chars-5.c
+new file mode 100644
+index 00000000000..68cb053144b
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-5.c
+@@ -0,0 +1,188 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=unpaired -Wno-multichar -Wno-overflow" } */
++/* Test all bidi chars in various contexts (identifiers, comments,
++   string literals, character constants), both UCN and UTF-8.  The bidi
++   chars here are properly terminated, except for the character constants.  */
++
++/* a b c LREâª 1 2 3 PDFâ¬ x y z */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++/* a b c RLEâ« 1 2 3 PDFâ¬ x y z */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++/* a b c LROâ­ 1 2 3 PDFâ¬ x y z */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++/* a b c RLOâ® 1 2 3 PDFâ¬ x y z */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++/* a b c LRIâ¦ 1 2 3 PDIâ© x y z */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++/* a b c RLIâ§ 1 2 3 PDIâ© x y */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++/* a b c FSIâ¨ 1 2 3 PDIâ© x y z */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++
++/* Same but C++ comments instead.  */
++// a b c LREâª 1 2 3 PDFâ¬ x y z
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++// a b c RLEâ« 1 2 3 PDFâ¬ x y z
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++// a b c LROâ­ 1 2 3 PDFâ¬ x y z
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++// a b c RLOâ® 1 2 3 PDFâ¬ x y z
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++// a b c LRIâ¦ 1 2 3 PDIâ© x y z
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++// a b c RLIâ§ 1 2 3 PDIâ© x y
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++// a b c FSIâ¨ 1 2 3 PDIâ© x y z
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++
++/* Here we're closing an unopened context, warn when =any.  */
++/* a b c PDIâ© x y z */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++/* a b c PDFâ¬ x y z */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++// a b c PDIâ© x y z
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++// a b c PDFâ¬ x y z
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++
++/* Multiline comments.  */
++/* a b c PDIâ© x y z
++   */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-2 } */
++/* a b c PDFâ¬ x y z
++   */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-2 } */
++/* first
++   a b c PDIâ© x y z
++   */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-2 } */
++/* first
++   a b c PDFâ¬ x y z
++   */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-2 } */
++/* first
++   a b c PDIâ© x y z */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++/* first
++   a b c PDFâ¬ x y z */
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++
++void
++g1 ()
++{
++  const char *s1 = "a b c LREâª 1 2 3 PDFâ¬ x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s2 = "a b c RLEâ« 1 2 3 PDFâ¬ x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s3 = "a b c LROâ­ 1 2 3 PDFâ¬ x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s4 = "a b c RLOâ® 1 2 3 PDFâ¬ x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s5 = "a b c LRIâ¦ 1 2 3 PDIâ© x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s6 = "a b c RLIâ§ 1 2 3 PDIâ© x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s7 = "a b c FSIâ¨ 1 2 3 PDIâ© x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s8 = "a b c PDIâ© x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s9 = "a b c PDFâ¬ x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++
++  const char *s10 = "a b c LRE\u202a 1 2 3 PDF\u202c x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s11 = "a b c LRE\u202A 1 2 3 PDF\u202c x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s12 = "a b c RLE\u202b 1 2 3 PDF\u202c x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s13 = "a b c RLE\u202B 1 2 3 PDF\u202c x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s14 = "a b c LRO\u202d 1 2 3 PDF\u202c x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s15 = "a b c LRO\u202D 1 2 3 PDF\u202c x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s16 = "a b c RLO\u202e 1 2 3 PDF\u202c x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s17 = "a b c RLO\u202E 1 2 3 PDF\u202c x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s18 = "a b c LRI\u2066 1 2 3 PDI\u2069 x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s19 = "a b c RLI\u2067 1 2 3 PDI\u2069 x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++  const char *s20 = "a b c FSI\u2068 1 2 3 PDI\u2069 x y z";
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++}
++
++void
++g2 ()
++{
++  const char c1 = '\u202a';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char c2 = '\u202A';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char c3 = '\u202b';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char c4 = '\u202B';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char c5 = '\u202d';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char c6 = '\u202D';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char c7 = '\u202e';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char c8 = '\u202E';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char c9 = '\u2066';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char c10 = '\u2067';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char c11 = '\u2068';
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++}
++
++int aâªbâ¬c;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int aâ«bâ¬c;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int aâ­bâ¬c;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int aâ®bâ¬c;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int aâ¦bâ©c;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int aâ§bâ©c;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int aâ¨bâ©c;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int Aâ¬X;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int A\u202cY;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int A\u202CY2;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++
++int d\u202ae\u202cf;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int d\u202Ae\u202cf2;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int d\u202be\u202cf;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int d\u202Be\u202cf2;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int d\u202de\u202cf;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int d\u202De\u202cf2;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int d\u202ee\u202cf;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int d\u202Ee\u202cf2;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int d\u2066e\u2069f;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int d\u2067e\u2069f;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int d\u2068e\u2069f;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
++int X\u2069;
++/* { dg-bogus "unpaired" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-6.c b/gcc/testsuite/c-c++-common/Wbidi-chars-6.c
+new file mode 100644
+index 00000000000..0ce6fff2dee
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-6.c
+@@ -0,0 +1,155 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=unpaired" } */
++/* Test nesting of bidi chars in various contexts.  */
++
++/* Terminated by the wrong char:  */
++/* a b c LREâª 1 2 3 PDIâ© x y z */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* a b c RLEâ« 1 2 3 PDIâ© x y  z*/
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* a b c LROâ­ 1 2 3 PDIâ© x y z */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* a b c RLOâ® 1 2 3 PDIâ© x y z */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* a b c LRIâ¦ 1 2 3 PDFâ¬ x y z */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* a b c RLIâ§ 1 2 3 PDFâ¬ x y z */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* a b c FSIâ¨ 1 2 3 PDFâ¬ x y  z*/
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++
++/* LREâª PDFâ¬ */
++/* LREâª LREâª PDFâ¬ PDFâ¬ */
++/* PDFâ¬ LREâª PDFâ¬ */
++/* LREâª PDFâ¬ LREâª PDFâ¬ */
++/* LREâª LREâª PDFâ¬ */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* PDFâ¬ LREâª */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++
++// a b c LREâª 1 2 3 PDIâ© x y z
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++// a b c RLEâ« 1 2 3 PDIâ© x y  z*/
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++// a b c LROâ­ 1 2 3 PDIâ© x y z 
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++// a b c RLOâ® 1 2 3 PDIâ© x y z 
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++// a b c LRIâ¦ 1 2 3 PDFâ¬ x y z 
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++// a b c RLIâ§ 1 2 3 PDFâ¬ x y z 
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++// a b c FSIâ¨ 1 2 3 PDFâ¬ x y  z
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++
++// LREâª PDFâ¬ 
++// LREâª LREâª PDFâ¬ PDFâ¬
++// PDFâ¬ LREâª PDFâ¬
++// LREâª PDFâ¬ LREâª PDFâ¬
++// LREâª LREâª PDFâ¬
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++// PDFâ¬ LREâª
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++
++void
++g1 ()
++{
++  const char *s1 = "a b c LREâª 1 2 3 PDIâ© x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s2 = "a b c LRE\u202a 1 2 3 PDI\u2069 x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s3 = "a b c RLEâ« 1 2 3 PDIâ© x y ";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s4 = "a b c RLE\u202b 1 2 3 PDI\u2069 x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s5 = "a b c LROâ­ 1 2 3 PDIâ© x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s6 = "a b c LRO\u202d 1 2 3 PDI\u2069 x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s7 = "a b c RLOâ® 1 2 3 PDIâ© x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s8 = "a b c RLO\u202e 1 2 3 PDI\u2069 x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s9 = "a b c LRIâ¦ 1 2 3 PDFâ¬ x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s10 = "a b c LRI\u2066 1 2 3 PDF\u202c x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s11 = "a b c RLIâ§ 1 2 3 PDFâ¬ x y z\
++    ";
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++  const char *s12 = "a b c RLI\u2067 1 2 3 PDF\u202c x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s13 = "a b c FSIâ¨ 1 2 3 PDFâ¬ x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s14 = "a b c FSI\u2068 1 2 3 PDF\u202c x y z";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s15 = "PDFâ¬ LREâª";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s16 = "PDF\u202c LRE\u202a";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s17 = "LREâª PDFâ¬";
++  const char *s18 = "LRE\u202a PDF\u202c";
++  const char *s19 = "LREâª LREâª PDFâ¬ PDFâ¬";
++  const char *s20 = "LRE\u202a LRE\u202a PDF\u202c PDF\u202c";
++  const char *s21 = "PDFâ¬ LREâª PDFâ¬";
++  const char *s22 = "PDF\u202c LRE\u202a PDF\u202c";
++  const char *s23 = "LREâª LREâª PDFâ¬";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s24 = "LRE\u202a LRE\u202a PDF\u202c";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s25 = "PDFâ¬ LREâª";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s26 = "PDF\u202c LRE\u202a";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s27 = "PDFâ¬ LRE\u202a";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++  const char *s28 = "PDF\u202c LREâª";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++}
++
++int aLREâªbPDIâ©;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int A\u202aB\u2069C;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int aRLEâ«bPDIâ©;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int a\u202bB\u2069c;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int aLROâ­bPDIâ©;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int a\u202db\u2069c2;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int aRLOâ®bPDIâ©;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int a\u202eb\u2069;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int aLRIâ¦bPDFâ¬;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int a\u2066b\u202c;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int aRLIâ§bPDFâ¬c
++;
++/* { dg-warning "unpaired" "" { target *-*-* } .-2 } */
++int a\u2067b\u202c;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int aFSIâ¨bPDFâ¬;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int a\u2068b\u202c;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int aFSIâ¨bPD\u202C;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int aFSI\u2068bPDFâ¬_;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int aLREâªbPDFâ¬b; 
++int A\u202aB\u202c;
++int a_LREâª_LREâª_b_PDFâ¬_PDFâ¬;
++int A\u202aA\u202aB\u202cB\u202c;
++int aPDFâ¬bLREadPDFâ¬;
++int a_\u202C_\u202a_\u202c;
++int a_LREâª_b_PDFâ¬_c_LREâª_PDFâ¬;
++int a_\u202a_\u202c_\u202a_\u202c_;
++int a_LREâª_b_PDFâ¬_c_LREâª;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int a_\u202a_\u202c_\u202a_;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-7.c b/gcc/testsuite/c-c++-common/Wbidi-chars-7.c
+new file mode 100644
+index 00000000000..d012d420ec0
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-7.c
+@@ -0,0 +1,9 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=any" } */
++/* Test we ignore UCNs in comments.  */
++
++// a b c \u202a 1 2 3
++// a b c \u202A 1 2 3
++/* a b c \u202a 1 2 3 */
++/* a b c \u202A 1 2 3 */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-8.c b/gcc/testsuite/c-c++-common/Wbidi-chars-8.c
+new file mode 100644
+index 00000000000..4f54c5092ec
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-8.c
+@@ -0,0 +1,13 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=any" } */
++/* Test \u vs \U.  */
++
++int a_\u202A;
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++int a_\u202a_2;
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++int a_\U0000202A_3;
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
++int a_\U0000202a_4;
++/* { dg-warning "U\\+202A" "" { target *-*-* } .-1 } */
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-9.c b/gcc/testsuite/c-c++-common/Wbidi-chars-9.c
+new file mode 100644
+index 00000000000..e2af1b1ca97
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-9.c
+@@ -0,0 +1,29 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=unpaired" } */
++/* Test that we properly separate bidi contexts (comment/identifier/character
++   constant/string literal).  */
++
++/* LRE ->âª<- */ int pdf_\u202c_1;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* RLE ->â«<- */ int pdf_\u202c_2;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* LRO ->â­<- */ int pdf_\u202c_3;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* RLO ->â®<- */ int pdf_\u202c_4;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* LRI ->â¦<-*/ int pdi_\u2069_1;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* RLI ->â§<- */ int pdi_\u2069_12;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* FSI ->â¨<- */ int pdi_\u2069_3;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++
++const char *s1 = "LRE\u202a"; /* PDF ->â¬<- */
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++/* LRE ->âª<- */ const char *s2 = "PDF\u202c";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++const char *s3 = "LRE\u202a"; int pdf_\u202c_5;
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
++int lre_\u202a; const char *s4 = "PDF\u202c";
++/* { dg-warning "unpaired" "" { target *-*-* } .-1 } */
+diff --git a/libcpp/include/cpplib.h b/libcpp/include/cpplib.h
+index 176f8c5bbce..112b9c24751 100644
+--- a/libcpp/include/cpplib.h
++++ b/libcpp/include/cpplib.h
+@@ -318,6 +318,17 @@ enum cpp_main_search
+   CMS_system,  /* Search the system INCLUDE path.  */
+ };
+ 
++/* The possible bidirectional control characters checking levels, from least
++   restrictive to most.  */
++enum cpp_bidirectional_level {
++  /* No checking.  */
++  bidirectional_none,
++  /* Only detect unpaired uses of bidirectional control characters.  */
++  bidirectional_unpaired,
++  /* Detect any use of bidirectional control characters.  */
++  bidirectional_any
++};
++
+ /* This structure is nested inside struct cpp_reader, and
+    carries all the options visible to the command line.  */
+ struct cpp_options
+@@ -531,6 +542,10 @@ struct cpp_options
+   /* True if warn about differences between C++98 and C++11.  */
+   bool cpp_warn_cxx11_compat;
+ 
++  /* Nonzero if bidirectional control characters checking is on.  See enum
++     cpp_bidirectional_level.  */
++  unsigned char cpp_warn_bidirectional;
++
+   /* Dependency generation.  */
+   struct
+   {
+@@ -635,7 +650,8 @@ enum cpp_warning_reason {
+   CPP_W_C90_C99_COMPAT,
+   CPP_W_C11_C2X_COMPAT,
+   CPP_W_CXX11_COMPAT,
+-  CPP_W_EXPANSION_TO_DEFINED
++  CPP_W_EXPANSION_TO_DEFINED,
++  CPP_W_BIDIRECTIONAL
+ };
+ 
+ /* Callback for header lookup for HEADER, which is the name of a
+diff --git a/libcpp/init.c b/libcpp/init.c
+index 5a424e23553..f9a8f5f088f 100644
+--- a/libcpp/init.c
++++ b/libcpp/init.c
+@@ -219,6 +219,7 @@ cpp_create_reader (enum c_lang lang, cpp
+       = ENABLE_CANONICAL_SYSTEM_HEADERS;
+   CPP_OPTION (pfile, ext_numeric_literals) = 1;
+   CPP_OPTION (pfile, warn_date_time) = 0;
++  CPP_OPTION (pfile, cpp_warn_bidirectional) = bidirectional_unpaired;
+ 
+   /* Default CPP arithmetic to something sensible for the host for the
+      benefit of dumb users like fix-header.  */
+diff --git a/libcpp/internal.h b/libcpp/internal.h
+index 8577cab6c83..0ce0246c5a2 100644
+--- a/libcpp/internal.h
++++ b/libcpp/internal.h
+@@ -597,6 +597,13 @@ struct cpp_reader
+   /* Location identifying the main source file -- intended to be line
+      zero of said file.  */
+   location_t main_loc;
++
++  /* Returns true iff we should warn about UTF-8 bidirectional control
++     characters.  */
++  bool warn_bidi_p () const
++  {
++    return CPP_OPTION (this, cpp_warn_bidirectional) != bidirectional_none;
++  }
+ };
+ 
+ /* Character classes.  Based on the more primitive macros in safe-ctype.h.
+diff --git a/libcpp/lex.c b/libcpp/lex.c
+index fa2253d41c3..6a4fbce6030 100644
+--- a/libcpp/lex.c
++++ b/libcpp/lex.c
+@@ -1164,6 +1164,324 @@ _cpp_process_line_notes (cpp_reader *pfi
+     }
+ }
+ 
++namespace bidi {
++  enum class kind {
++    NONE, LRE, RLE, LRO, RLO, LRI, RLI, FSI, PDF, PDI, LTR, RTL
++  };
++
++  /* All the UTF-8 encodings of bidi characters start with E2.  */
++  constexpr uchar utf8_start = 0xe2;
++
++  /* A vector holding currently open bidi contexts.  We use a char for
++     each context, its LSB is 1 if it represents a PDF context, 0 if it
++     represents a PDI context.  The next bit is 1 if this context was open
++     by a bidi character written as a UCN, and 0 when it was UTF-8.  */
++  semi_embedded_vec <unsigned char, 16> vec;
++
++  /* Close the whole comment/identifier/string literal/character constant
++     context.  */
++  void on_close ()
++  {
++    vec.truncate (0);
++  }
++
++  /* Pop the last element in the vector.  */
++  void pop ()
++  {
++    unsigned int len = vec.count ();
++    gcc_checking_assert (len > 0);
++    vec.truncate (len - 1);
++  }
++
++  /* Return the context of the Ith element.  */
++  kind ctx_at (unsigned int i)
++  {
++    return (vec[i] & 1) ? kind::PDF : kind::PDI;
++  }
++
++  /* Return which context is currently opened.  */
++  kind current_ctx ()
++  {
++    unsigned int len = vec.count ();
++    if (len == 0)
++      return kind::NONE;
++    return ctx_at (len - 1);
++  }
++
++  /* Return true if the current context comes from a UCN origin, that is,
++     the bidi char which started this bidi context was written as a UCN.  */
++  bool current_ctx_ucn_p ()
++  {
++    unsigned int len = vec.count ();
++    gcc_checking_assert (len > 0);
++    return (vec[len - 1] >> 1) & 1;
++  }
++
++  /* We've read a bidi char, update the current vector as necessary.  */
++  void on_char (kind k, bool ucn_p)
++  {
++    switch (k)
++      {
++      case kind::LRE:
++      case kind::RLE:
++      case kind::LRO:
++      case kind::RLO:
++	vec.push (ucn_p ? 3u : 1u);
++	break;
++      case kind::LRI:
++      case kind::RLI:
++      case kind::FSI:
++	vec.push (ucn_p ? 2u : 0u);
++	break;
++      /* PDF terminates the scope of the last LRE, RLE, LRO, or RLO
++	 whose scope has not yet been terminated.  */
++      case kind::PDF:
++	if (current_ctx () == kind::PDF)
++	  pop ();
++	break;
++      /* PDI terminates the scope of the last LRI, RLI, or FSI whose
++	 scope has not yet been terminated, as well as the scopes of
++	 any subsequent LREs, RLEs, LROs, or RLOs whose scopes have not
++	 yet been terminated.  */
++      case kind::PDI:
++	for (int i = vec.count () - 1; i >= 0; --i)
++	  if (ctx_at (i) == kind::PDI)
++	    {
++	      vec.truncate (i);
++	      break;
++	    }
++	break;
++      case kind::LTR:
++      case kind::RTL:
++	/* These aren't popped by a PDF/PDI.  */
++	break;
++      [[likely]] case kind::NONE:
++	break;
++      default:
++	abort ();
++      }
++  }
++
++  /* Return a descriptive string for K.  */
++  const char *to_str (kind k)
++  {
++    switch (k)
++      {
++      case kind::LRE:
++	return "U+202A (LEFT-TO-RIGHT EMBEDDING)";
++      case kind::RLE:
++	return "U+202B (RIGHT-TO-LEFT EMBEDDING)";
++      case kind::LRO:
++	return "U+202D (LEFT-TO-RIGHT OVERRIDE)";
++      case kind::RLO:
++	return "U+202E (RIGHT-TO-LEFT OVERRIDE)";
++      case kind::LRI:
++	return "U+2066 (LEFT-TO-RIGHT ISOLATE)";
++      case kind::RLI:
++	return "U+2067 (RIGHT-TO-LEFT ISOLATE)";
++      case kind::FSI:
++	return "U+2068 (FIRST STRONG ISOLATE)";
++      case kind::PDF:
++	return "U+202C (POP DIRECTIONAL FORMATTING)";
++      case kind::PDI:
++	return "U+2069 (POP DIRECTIONAL ISOLATE)";
++      case kind::LTR:
++	return "U+200E (LEFT-TO-RIGHT MARK)";
++      case kind::RTL:
++	return "U+200F (RIGHT-TO-LEFT MARK)";
++      default:
++	abort ();
++      }
++  }
++}
++
++/* Parse a sequence of 3 bytes starting with P and return its bidi code.  */
++
++static bidi::kind
++get_bidi_utf8 (const unsigned char *const p)
++{
++  gcc_checking_assert (p[0] == bidi::utf8_start);
++
++  if (p[1] == 0x80)
++    switch (p[2])
++      {
++      case 0xaa:
++	return bidi::kind::LRE;
++      case 0xab:
++	return bidi::kind::RLE;
++      case 0xac:
++	return bidi::kind::PDF;
++      case 0xad:
++	return bidi::kind::LRO;
++      case 0xae:
++	return bidi::kind::RLO;
++      case 0x8e:
++	return bidi::kind::LTR;
++      case 0x8f:
++	return bidi::kind::RTL;
++      default:
++	break;
++      }
++  else if (p[1] == 0x81)
++    switch (p[2])
++      {
++      case 0xa6:
++	return bidi::kind::LRI;
++      case 0xa7:
++	return bidi::kind::RLI;
++      case 0xa8:
++	return bidi::kind::FSI;
++      case 0xa9:
++	return bidi::kind::PDI;
++      default:
++	break;
++      }
++
++  return bidi::kind::NONE;
++}
++
++/* Parse a UCN where P points just past \u or \U and return its bidi code.  */
++
++static bidi::kind
++get_bidi_ucn (const unsigned char *p, bool is_U)
++{
++  /* 6.4.3 Universal Character Names
++      \u hex-quad
++      \U hex-quad hex-quad
++     where \unnnn means \U0000nnnn.  */
++
++  if (is_U)
++    {
++      if (p[0] != '0' || p[1] != '0' || p[2] != '0' || p[3] != '0')
++	return bidi::kind::NONE;
++      /* Skip 4B so we can treat \u and \U the same below.  */
++      p += 4;
++    }
++
++  /* All code points we are looking for start with 20xx.  */
++  if (p[0] != '2' || p[1] != '0')
++    return bidi::kind::NONE;
++  else if (p[2] == '2')
++    switch (p[3])
++      {
++      case 'a':
++      case 'A':
++	return bidi::kind::LRE;
++      case 'b':
++      case 'B':
++	return bidi::kind::RLE;
++      case 'c':
++      case 'C':
++	return bidi::kind::PDF;
++      case 'd':
++      case 'D':
++	return bidi::kind::LRO;
++      case 'e':
++      case 'E':
++	return bidi::kind::RLO;
++      default:
++	break;
++      }
++  else if (p[2] == '6')
++    switch (p[3])
++      {
++      case '6':
++	return bidi::kind::LRI;
++      case '7':
++	return bidi::kind::RLI;
++      case '8':
++	return bidi::kind::FSI;
++      case '9':
++	return bidi::kind::PDI;
++      default:
++	break;
++      }
++  else if (p[2] == '0')
++    switch (p[3])
++      {
++      case 'e':
++      case 'E':
++	return bidi::kind::LTR;
++      case 'f':
++      case 'F':
++	return bidi::kind::RTL;
++      default:
++	break;
++      }
++
++  return bidi::kind::NONE;
++}
++
++/* We're closing a bidi context, that is, we've encountered a newline,
++   are closing a C-style comment, or are at the end of a string literal,
++   character constant, or identifier.  Warn if this context was not
++   properly terminated by a PDI or PDF.  P points to the last character
++   in this context.  */
++
++static void
++maybe_warn_bidi_on_close (cpp_reader *pfile, const uchar *p)
++{
++  if (CPP_OPTION (pfile, cpp_warn_bidirectional) == bidirectional_unpaired
++      && bidi::vec.count () > 0)
++    {
++      const location_t loc
++	= linemap_position_for_column (pfile->line_table,
++				       CPP_BUF_COLUMN (pfile->buffer, p));
++      cpp_warning_with_line (pfile, CPP_W_BIDIRECTIONAL, loc, 0,
++			     "unpaired UTF-8 bidirectional control character "
++			     "detected");
++    }
++  /* We're done with this context.  */
++  bidi::on_close ();
++}
++
++/* We're at the beginning or in the middle of an identifier/comment/string
++   literal/character constant.  Warn if we've encountered a bidi character.
++   KIND says which bidi character it was; P points to it in the character
++   stream.  UCN_P is true iff this bidi character was written as a UCN.  */
++
++static void
++maybe_warn_bidi_on_char (cpp_reader *pfile, const uchar *p, bidi::kind kind,
++			 bool ucn_p)
++{
++  if (__builtin_expect (kind == bidi::kind::NONE, 1))
++    return;
++
++  const auto warn_bidi = CPP_OPTION (pfile, cpp_warn_bidirectional);
++
++  if (warn_bidi != bidirectional_none)
++    {
++      const location_t loc
++	= linemap_position_for_column (pfile->line_table,
++				       CPP_BUF_COLUMN (pfile->buffer, p));
++      /* It seems excessive to warn about a PDI/PDF that is closing
++	 an opened context because we've already warned about the
++	 opening character.  Except warn when we have a UCN x UTF-8
++	 mismatch.  */
++      if (kind == bidi::current_ctx ())
++	{
++	  if (warn_bidi == bidirectional_unpaired
++	      && bidi::current_ctx_ucn_p () != ucn_p)
++	    cpp_warning_with_line (pfile, CPP_W_BIDIRECTIONAL, loc, 0,
++				   "UTF-8 vs UCN mismatch when closing "
++				   "a context by \"%s\"", bidi::to_str (kind));
++	}
++      else if (warn_bidi == bidirectional_any)
++	{
++	  if (kind == bidi::kind::PDF || kind == bidi::kind::PDI)
++	    cpp_warning_with_line (pfile, CPP_W_BIDIRECTIONAL, loc, 0,
++				   "\"%s\" is closing an unopened context",
++				   bidi::to_str (kind));
++	  else
++	    cpp_warning_with_line (pfile, CPP_W_BIDIRECTIONAL, loc, 0,
++				   "found problematic Unicode character \"%s\"",
++				   bidi::to_str (kind));
++	}
++    }
++  /* We're done with this context.  */
++  bidi::on_char (kind, ucn_p);
++}
++
+ /* Skip a C-style block comment.  We find the end of the comment by
+    seeing if an asterisk is before every '/' we encounter.  Returns
+    nonzero if comment terminated by EOF, zero otherwise.
+@@ -1175,6 +1493,7 @@ _cpp_skip_block_comment (cpp_reader *pfi
+   cpp_buffer *buffer = pfile->buffer;
+   const uchar *cur = buffer->cur;
+   uchar c;
++  const bool warn_bidi_p = pfile->warn_bidi_p ();
+ 
+   cur++;
+   if (*cur == '/')
+@@ -1189,7 +1508,11 @@ _cpp_skip_block_comment (cpp_reader *pfi
+       if (c == '/')
+ 	{
+ 	  if (cur[-2] == '*')
+-	    break;
++	    {
++	      if (warn_bidi_p)
++		maybe_warn_bidi_on_close (pfile, cur);
++	      break;
++	    }
+ 
+ 	  /* Warn about potential nested comments, but not if the '/'
+ 	     comes immediately before the true comment delimiter.
+@@ -1208,6 +1531,8 @@ _cpp_skip_block_comment (cpp_reader *pfi
+ 	{
+ 	  unsigned int cols;
+ 	  buffer->cur = cur - 1;
++	  if (warn_bidi_p)
++	    maybe_warn_bidi_on_close (pfile, cur);
+ 	  _cpp_process_line_notes (pfile, true);
+ 	  if (buffer->next_line >= buffer->rlimit)
+ 	    return true;
+@@ -1218,6 +1543,13 @@ _cpp_skip_block_comment (cpp_reader *pfi
+ 
+ 	  cur = buffer->cur;
+ 	}
++      /* If this is a beginning of a UTF-8 encoding, it might be
++	 a bidirectional control character.  */
++      else if (__builtin_expect (c == bidi::utf8_start, 0) && warn_bidi_p)
++	{
++	  bidi::kind kind = get_bidi_utf8 (cur - 1);
++	  maybe_warn_bidi_on_char (pfile, cur, kind, /*ucn_p=*/false);
++	}
+     }
+ 
+   buffer->cur = cur;
+@@ -1233,9 +1565,31 @@ skip_line_comment (cpp_reader *pfile)
+ {
+   cpp_buffer *buffer = pfile->buffer;
+   location_t orig_line = pfile->line_table->highest_line;
++  const bool warn_bidi_p = pfile->warn_bidi_p ();
+ 
+-  while (*buffer->cur != '\n')
+-    buffer->cur++;
++  if (!warn_bidi_p)
++    while (*buffer->cur != '\n')
++      buffer->cur++;
++  else
++    {
++      while (*buffer->cur != '\n'
++	     && *buffer->cur != bidi::utf8_start)
++	buffer->cur++;
++      if (__builtin_expect (*buffer->cur == bidi::utf8_start, 0))
++	{
++	  while (*buffer->cur != '\n')
++	    {
++	      if (__builtin_expect (*buffer->cur == bidi::utf8_start, 0))
++		{
++		  bidi::kind kind = get_bidi_utf8 (buffer->cur);
++		  maybe_warn_bidi_on_char (pfile, buffer->cur, kind,
++					   /*ucn_p=*/false);
++		}
++	      buffer->cur++;
++	    }
++	  maybe_warn_bidi_on_close (pfile, buffer->cur);
++	}
++    }
+ 
+   _cpp_process_line_notes (pfile, true);
+   return orig_line != pfile->line_table->highest_line;
+@@ -1317,11 +1671,13 @@ static const cppchar_t utf8_signifier =
+ 
+ /* Returns TRUE if the sequence starting at buffer->cur is valid in
+    an identifier.  FIRST is TRUE if this starts an identifier.  */
++
+ static bool
+ forms_identifier_p (cpp_reader *pfile, int first,
+ 		    struct normalize_state *state)
+ {
+   cpp_buffer *buffer = pfile->buffer;
++  const bool warn_bidi_p = pfile->warn_bidi_p ();
+ 
+   if (*buffer->cur == '$')
+     {
+@@ -1344,6 +1700,13 @@ forms_identifier_p (cpp_reader *pfile, i
+       cppchar_t s;
+       if (*buffer->cur >= utf8_signifier)
+ 	{
++	  if (__builtin_expect (*buffer->cur == bidi::utf8_start, 0)
++	      && warn_bidi_p)
++	    {
++	      bidi::kind kind = get_bidi_utf8 (buffer->cur);
++	      maybe_warn_bidi_on_char (pfile, buffer->cur, kind,
++				       /*ucn_p=*/false);
++	    }
+ 	  if (_cpp_valid_utf8 (pfile, &buffer->cur, buffer->rlimit, 1 + !first,
+ 			       state, &s))
+ 	    return true;
+@@ -1352,6 +1715,13 @@ forms_identifier_p (cpp_reader *pfile, i
+ 	       && (buffer->cur[1] == 'u' || buffer->cur[1] == 'U'))
+ 	{
+ 	  buffer->cur += 2;
++	  if (warn_bidi_p)
++	    {
++	      bidi::kind kind = get_bidi_ucn (buffer->cur,
++					      buffer->cur[-1] == 'U');
++	      maybe_warn_bidi_on_char (pfile, buffer->cur, kind,
++				       /*ucn_p=*/true);
++	    }
+ 	  if (_cpp_valid_ucn (pfile, &buffer->cur, buffer->rlimit, 1 + !first,
+ 			      state, &s, NULL, NULL))
+ 	    return true;
+@@ -1460,6 +1830,7 @@ lex_identifier (cpp_reader *pfile, const
+   const uchar *cur;
+   unsigned int len;
+   unsigned int hash = HT_HASHSTEP (0, *base);
++  const bool warn_bidi_p = pfile->warn_bidi_p ();
+ 
+   cur = pfile->buffer->cur;
+   if (! starts_ucn)
+@@ -1483,6 +1854,8 @@ lex_identifier (cpp_reader *pfile, const
+ 	    pfile->buffer->cur++;
+ 	  }
+       } while (forms_identifier_p (pfile, false, nst));
++      if (warn_bidi_p)
++	maybe_warn_bidi_on_close (pfile, pfile->buffer->cur);
+       result = _cpp_interpret_identifier (pfile, base,
+ 					  pfile->buffer->cur - base);
+       *spelling = cpp_lookup (pfile, base, pfile->buffer->cur - base);
+@@ -1719,6 +2092,7 @@ static void
+ lex_raw_string (cpp_reader *pfile, cpp_token *token, const uchar *base)
+ {
+   const uchar *pos = base;
++  const bool warn_bidi_p = pfile->warn_bidi_p ();
+ 
+   /* 'tis a pity this information isn't passed down from the lexer's
+      initial categorization of the token.  */
+@@ -1955,8 +2329,15 @@ lex_raw_string (cpp_reader *pfile, cpp_t
+ 	  pos = base = pfile->buffer->cur;
+ 	  note = &pfile->buffer->notes[pfile->buffer->cur_note];
+ 	}
++      else if (__builtin_expect ((unsigned char) c == bidi::utf8_start, 0)
++	       && warn_bidi_p)
++	maybe_warn_bidi_on_char (pfile, pos - 1, get_bidi_utf8 (pos - 1),
++				 /*ucn_p=*/false);
+     }
+ 
++  if (warn_bidi_p)
++    maybe_warn_bidi_on_close (pfile, pos);
++
+   if (CPP_OPTION (pfile, user_literals))
+     {
+       /* If a string format macro, say from inttypes.h, is placed touching
+@@ -2051,15 +2432,27 @@ lex_string (cpp_reader *pfile, cpp_token
+   else
+     terminator = '>', type = CPP_HEADER_NAME;
+ 
++  const bool warn_bidi_p = pfile->warn_bidi_p ();
+   for (;;)
+     {
+       cppchar_t c = *cur++;
+ 
+       /* In #include-style directives, terminators are not escapable.  */
+       if (c == '\\' && !pfile->state.angled_headers && *cur != '\n')
+-	cur++;
++	{
++	  if ((cur[0] == 'u' || cur[0] == 'U') && warn_bidi_p)
++	    {
++	      bidi::kind kind = get_bidi_ucn (cur + 1, cur[0] == 'U');
++	      maybe_warn_bidi_on_char (pfile, cur, kind, /*ucn_p=*/true);
++	    }
++	  cur++;
++	}
+       else if (c == terminator)
+-	break;
++	{
++	  if (warn_bidi_p)
++	    maybe_warn_bidi_on_close (pfile, cur - 1);
++	  break;
++	}
+       else if (c == '\n')
+ 	{
+ 	  cur--;
+@@ -2076,6 +2469,11 @@ lex_string (cpp_reader *pfile, cpp_token
+ 	}
+       else if (c == '\0')
+ 	saw_NUL = true;
++      else if (__builtin_expect (c == bidi::utf8_start, 0) && warn_bidi_p)
++	{
++	  bidi::kind kind = get_bidi_utf8 (cur - 1);
++	  maybe_warn_bidi_on_char (pfile, cur - 1, kind, /*ucn_p=*/false);
++	}
+     }
+ 
+   if (saw_NUL && !pfile->state.skipping)
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0002-gcc-poison-system-directories.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0002-gcc-poison-system-directories.patch
new file mode 100644
index 0000000..18a9fb8
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0002-gcc-poison-system-directories.patch
@@ -0,0 +1,226 @@
+From 118c6f054711d437167ff125a88c9236bfc8099c Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Mon, 8 Mar 2021 16:04:20 -0800
+Subject: [PATCH] gcc: poison-system-directories
+
+Add /sw/include and /opt/include based on the original
+zecke-no-host-includes.patch patch.  The original patch checked for
+/usr/include, /sw/include and /opt/include and then triggered a failure and
+aborted.
+
+Instead, we add the two missing items to the current scan.  If the user
+wants this to be a failure, they can add "-Werror=poison-system-directories".
+
+Upstream-Status: Pending
+Signed-off-by: Mark Hatle <mark.hatle@windriver.com>
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ gcc/common.opt      |  4 ++++
+ gcc/config.in       | 10 ++++++++++
+ gcc/configure       | 19 +++++++++++++++++++
+ gcc/configure.ac    | 16 ++++++++++++++++
+ gcc/doc/invoke.texi |  9 +++++++++
+ gcc/gcc.c           |  9 +++++++--
+ gcc/incpath.c       | 21 +++++++++++++++++++++
+ 7 files changed, 86 insertions(+), 2 deletions(-)
+
+diff --git a/gcc/common.opt b/gcc/common.opt
+index c75dd36843e..49acbd6ed44 100644
+--- a/gcc/common.opt
++++ b/gcc/common.opt
+@@ -683,6 +683,10 @@ Wreturn-local-addr
+ Common Var(warn_return_local_addr) Init(1) Warning
+ Warn about returning a pointer/reference to a local or temporary variable.
+ 
++Wpoison-system-directories
++Common Var(flag_poison_system_directories) Init(1) Warning
++Warn for -I and -L options using system directories if cross compiling
++
+ Wshadow
+ Common Var(warn_shadow) Warning
+ Warn when one variable shadows another.  Same as -Wshadow=global.
+diff --git a/gcc/config.in b/gcc/config.in
+index 10a13cde586..8848284da41 100644
+--- a/gcc/config.in
++++ b/gcc/config.in
+@@ -218,6 +218,16 @@
+ #endif
+ 
+ 
++/* Define to warn for use of native system header directories */
++#ifndef USED_FOR_TARGET
++#undef ENABLE_POISON_SYSTEM_DIRECTORIES
++#endif
++/* Define to warn for use of native system header directories */
++#ifndef USED_FOR_TARGET
++#undef POISON_BY_DEFAULT
++#endif
++
++
+ /* Define if you want all operations on RTL (the basic data structure of the
+    optimizer and back end) to be checked for dynamic type safety at runtime.
+    This is quite expensive. */
+diff --git a/gcc/configure b/gcc/configure
+index 9bb436ce7bd..3f0734bff11 100755
+--- a/gcc/configure
++++ b/gcc/configure
+@@ -1020,6 +1020,7 @@ enable_maintainer_mode
+ enable_link_mutex
+ enable_link_serialization
+ enable_version_specific_runtime_libs
++enable_poison_system_directories
+ enable_plugin
+ enable_host_shared
+ enable_libquadmath_support
+@@ -1782,6 +1783,8 @@ Optional Features:
+   --enable-version-specific-runtime-libs
+                           specify that runtime libraries should be installed
+                           in a compiler-specific directory
++  --enable-poison-system-directories
++                          warn for use of native system header directories
+   --enable-plugin         enable plugin support
+   --enable-host-shared    build host code as shared libraries
+   --disable-libquadmath-support
+@@ -31325,6 +31328,22 @@ if test "${enable_version_specific_runtime_libs+set}" = set; then :
+ fi
+ 
+ 
++# Check whether --enable-poison-system-directories was given.
++if test "${enable_poison_system_directories+set}" = set; then :
++  enableval=$enable_poison_system_directories;
++else
++  enable_poison_system_directories=no
++fi
++
++if test "x${enable_poison_system_directories}" != "xno"; then
++
++$as_echo "#define ENABLE_POISON_SYSTEM_DIRECTORIES 1" >>confdefs.h
++if test "$enable_poison_system_directories" = "error"; then
++$as_echo "#define POISON_BY_DEFAULT 1" >>confdefs.h
++fi
++
++fi
++
+ # Substitute configuration variables
+ 
+ 
+diff --git a/gcc/configure.ac b/gcc/configure.ac
+index caa611933df..54e21764b3e 100644
+--- a/gcc/configure.ac
++++ b/gcc/configure.ac
+@@ -7123,6 +7123,22 @@ AC_ARG_ENABLE(version-specific-runtime-libs,
+                 [specify that runtime libraries should be
+                  installed in a compiler-specific directory])])
+ 
++AC_ARG_ENABLE([poison-system-directories],
++             AS_HELP_STRING([--enable-poison-system-directories],
++                            [warn for use of native system header directories (no/yes/error)]),,
++             [enable_poison_system_directories=no])
++AC_MSG_NOTICE([poisoned directories $enable_poison_system_directories])
++if test "x${enable_poison_system_directories}" != "xno"; then
++  AC_MSG_NOTICE([poisoned directories enabled])
++  AC_DEFINE([ENABLE_POISON_SYSTEM_DIRECTORIES],
++           [1],
++           [Define to warn for use of native system header directories])
++  if test $enable_poison_system_directories = "error"; then
++   AC_MSG_NOTICE([poisoned directories are fatal])
++   AC_DEFINE([POISON_BY_DEFAULT], [1], [Define to make poison warnings errors])
++  fi
++fi
++
+ # Substitute configuration variables
+ AC_SUBST(subdirs)
+ AC_SUBST(srcdir)
+diff --git a/gcc/doc/invoke.texi b/gcc/doc/invoke.texi
+index 7a368959e5e..6659a903bf0 100644
+--- a/gcc/doc/invoke.texi
++++ b/gcc/doc/invoke.texi
+@@ -369,6 +369,7 @@ Objective-C and Objective-C++ Dialects}.
+ -Wpacked  -Wno-packed-bitfield-compat  -Wpacked-not-aligned  -Wpadded @gol
+ -Wparentheses  -Wno-pedantic-ms-format @gol
+ -Wpointer-arith  -Wno-pointer-compare  -Wno-pointer-to-int-cast @gol
++-Wno-poison-system-directories @gol
+ -Wno-pragmas  -Wno-prio-ctor-dtor  -Wredundant-decls @gol
+ -Wrestrict  -Wno-return-local-addr  -Wreturn-type @gol
+ -Wno-scalar-storage-order  -Wsequence-point @gol
+@@ -7735,6 +7736,14 @@ made up of data only and thus requires no special treatment.  But, for
+ most targets, it is made up of code and thus requires the stack to be
+ made executable in order for the program to work properly.
+ 
++@item -Wno-poison-system-directories
++@opindex Wno-poison-system-directories
++Do not warn for @option{-I} or @option{-L} options using system
++directories such as @file{/usr/include} when cross compiling.  This
++option is intended for use in chroot environments when such
++directories contain the correct headers and libraries for the target
++system rather than the host.
++
+ @item -Wfloat-equal
+ @opindex Wfloat-equal
+ @opindex Wno-float-equal
+diff --git a/gcc/gcc.c b/gcc/gcc.c
+index 7837553958b..19c75b6e20d 100644
+--- a/gcc/gcc.c
++++ b/gcc/gcc.c
+@@ -1152,6 +1152,8 @@ proper position among the other output files.  */
+    "%{fuse-ld=*:-fuse-ld=%*} " LINK_COMPRESS_DEBUG_SPEC \
+    "%X %{o*} %{e*} %{N} %{n} %{r}\
+     %{s} %{t} %{u*} %{z} %{Z} %{!nostdlib:%{!r:%{!nostartfiles:%S}}} \
++    %{Wno-poison-system-directories:--no-poison-system-directories} \
++    %{Werror=poison-system-directories:--error-poison-system-directories} \
+     %{static|no-pie|static-pie:} %@{L*} %(mfwrap) %(link_libgcc) " \
+     VTABLE_VERIFICATION_SPEC " " SANITIZER_EARLY_SPEC " %o "" \
+     %{fopenacc|fopenmp|%:gt(%{ftree-parallelize-loops=*:%*} 1):\
+@@ -1247,8 +1249,11 @@ static const char *cpp_unique_options =
+ static const char *cpp_options =
+ "%(cpp_unique_options) %1 %{m*} %{std*&ansi&trigraphs} %{W*&pedantic*} %{w}\
+  %{f*} %{g*:%{%:debug-level-gt(0):%{g*}\
+- %{!fno-working-directory:-fworking-directory}}} %{O*}\
+- %{undef} %{save-temps*:-fpch-preprocess}";
++ %{!fno-working-directory:-fworking-directory}}} %{O*}"
++#ifdef POISON_BY_DEFAULT
++ " -Werror=poison-system-directories"
++#endif
++ " %{undef} %{save-temps*:-fpch-preprocess}";
+ 
+ /* Pass -d* flags, possibly modifying -dumpdir, -dumpbase et al.
+ 
+diff --git a/gcc/incpath.c b/gcc/incpath.c
+index 446d280321d..fbfc0ce03b8 100644
+--- a/gcc/incpath.c
++++ b/gcc/incpath.c
+@@ -26,6 +26,7 @@
+ #include "intl.h"
+ #include "incpath.h"
+ #include "cppdefault.h"
++#include "diagnostic-core.h"
+ 
+ /* Microsoft Windows does not natively support inodes.
+    VMS has non-numeric inodes.  */
+@@ -395,6 +396,26 @@ merge_include_chains (const char *sysroot, cpp_reader *pfile, int verbose)
+ 	}
+       fprintf (stderr, _("End of search list.\n"));
+     }
++
++#ifdef ENABLE_POISON_SYSTEM_DIRECTORIES
++  if (flag_poison_system_directories)
++    {
++       struct cpp_dir *p;
++
++       for (p = heads[INC_QUOTE]; p; p = p->next)
++         {
++          if ((!strncmp (p->name, "/usr/include", 12))
++              || (!strncmp (p->name, "/usr/local/include", 18))
++              || (!strncmp (p->name, "/usr/X11R6/include", 18))
++              || (!strncmp (p->name, "/sw/include", 11))
++              || (!strncmp (p->name, "/opt/include", 12)))
++            warning (OPT_Wpoison_system_directories,
++                     "include location \"%s\" is unsafe for "
++                     "cross-compilation",
++                     p->name);
++         }
++    }
++#endif
+ }
+ 
+ /* Use given -I paths for #include "..." but not #include <...>, and
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0003-CVE-2021-42574.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0003-CVE-2021-42574.patch
new file mode 100644
index 0000000..2995a6f
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0003-CVE-2021-42574.patch
@@ -0,0 +1,142 @@
+From 1a7f2c0774129750fdf73e9f1b78f0ce983c9ab3 Mon Sep 17 00:00:00 2001
+From: David Malcolm <dmalcolm@redhat.com>
+Date: Tue, 2 Nov 2021 09:54:32 -0400
+Subject: [PATCH] libcpp: escape non-ASCII source bytes in -Wbidi-chars=
+ [PR103026]
+MIME-Version: 1.0
+Content-Type: text/plain; charset=utf8
+Content-Transfer-Encoding: 8bit
+
+This flags rich_locations associated with -Wbidi-chars= so that
+non-ASCII bytes will be escaped when printing the source lines
+(using the diagnostics support I added in
+r12-4825-gbd5e882cf6e0def3dd1bc106075d59a303fe0d1e).
+
+In particular, this ensures that the printed source lines will
+be pure ASCII, and thus the visual ordering of the characters
+will be the same as the logical ordering.
+
+Before:
+
+  Wbidi-chars-1.c: In function âmainâ:
+  Wbidi-chars-1.c:6:43: warning: unpaired UTF-8 bidirectional control character detected [-Wbidi-chars=]
+      6 |     /*â® } â¦if (isAdmin)â© â¦ begin admins only */
+        |                                           ^
+  Wbidi-chars-1.c:9:28: warning: unpaired UTF-8 bidirectional control character detected [-Wbidi-chars=]
+      9 |     /* end admins only â® { â¦*/
+        |                            ^
+
+  Wbidi-chars-11.c:6:15: warning: UTF-8 vs UCN mismatch when closing a context by "U+202C (POP DIRECTIONAL FORMATTING)" [-Wbidi-chars=]
+      6 | int LRE_âª_PDF_\u202c;
+        |               ^
+  Wbidi-chars-11.c:8:19: warning: UTF-8 vs UCN mismatch when closing a context by "U+202C (POP DIRECTIONAL FORMATTING)" [-Wbidi-chars=]
+      8 | int LRE_\u202a_PDF_â¬_;
+        |                   ^
+  Wbidi-chars-11.c:10:28: warning: UTF-8 vs UCN mismatch when closing a context by "U+202C (POP DIRECTIONAL FORMATTING)" [-Wbidi-chars=]
+     10 | const char *s1 = "LRE_âª_PDF_\u202c";
+        |                            ^
+  Wbidi-chars-11.c:12:33: warning: UTF-8 vs UCN mismatch when closing a context by "U+202C (POP DIRECTIONAL FORMATTING)" [-Wbidi-chars=]
+     12 | const char *s2 = "LRE_\u202a_PDF_â¬";
+        |                                 ^
+
+After:
+
+  Wbidi-chars-1.c: In function âmainâ:
+  Wbidi-chars-1.c:6:43: warning: unpaired UTF-8 bidirectional control character detected [-Wbidi-chars=]
+      6 |     /*<U+202E> } <U+2066>if (isAdmin)<U+2069> <U+2066> begin admins only */
+        |                                                                           ^
+  Wbidi-chars-1.c:9:28: warning: unpaired UTF-8 bidirectional control character detected [-Wbidi-chars=]
+      9 |     /* end admins only <U+202E> { <U+2066>*/
+        |                                            ^
+
+  Wbidi-chars-11.c:6:15: warning: UTF-8 vs UCN mismatch when closing a context by "U+202C (POP DIRECTIONAL FORMATTING)" [-Wbidi-chars=]
+      6 | int LRE_<U+202A>_PDF_\u202c;
+        |                       ^
+  Wbidi-chars-11.c:8:19: warning: UTF-8 vs UCN mismatch when closing a context by "U+202C (POP DIRECTIONAL FORMATTING)" [-Wbidi-chars=]
+      8 | int LRE_\u202a_PDF_<U+202C>_;
+        |                   ^
+  Wbidi-chars-11.c:10:28: warning: UTF-8 vs UCN mismatch when closing a context by "U+202C (POP DIRECTIONAL FORMATTING)" [-Wbidi-chars=]
+     10 | const char *s1 = "LRE_<U+202A>_PDF_\u202c";
+        |                                    ^
+  Wbidi-chars-11.c:12:33: warning: UTF-8 vs UCN mismatch when closing a context by "U+202C (POP DIRECTIONAL FORMATTING)" [-Wbidi-chars=]
+     12 | const char *s2 = "LRE_\u202a_PDF_<U+202C>";
+        |                                 ^
+
+libcpp/ChangeLog:
+	PR preprocessor/103026
+	* lex.c (maybe_warn_bidi_on_close): Use a rich_location
+	and call set_escape_on_output (true) on it.
+	(maybe_warn_bidi_on_char): Likewise.
+
+Signed-off-by: David Malcolm <dmalcolm@redhat.com>
+
+CVE: CVE-2021-42574
+Upstream-Status: Backport [https://gcc.gnu.org/git/gitweb.cgi?p=gcc.git;h=1a7f2c0774129750fdf73e9f1b78f0ce983c9ab3]
+Signed-off-by: Pgowda <pgowda.cve@gmail.com>
+
+---
+ libcpp/lex.c | 29 +++++++++++++++++------------
+ 1 file changed, 17 insertions(+), 12 deletions(-)
+
+diff --git a/libcpp/lex.c b/libcpp/lex.c
+index 8188e33b07d..2421d6c0f40 100644
+--- a/libcpp/lex.c
++++ b/libcpp/lex.c
+@@ -1427,9 +1427,11 @@ maybe_warn_bidi_on_close (cpp_reader *pfile, const uchar *p)
+       const location_t loc
+ 	= linemap_position_for_column (pfile->line_table,
+ 				       CPP_BUF_COLUMN (pfile->buffer, p));
+-      cpp_warning_with_line (pfile, CPP_W_BIDIRECTIONAL, loc, 0,
+-			     "unpaired UTF-8 bidirectional control character "
+-			     "detected");
++      rich_location rich_loc (pfile->line_table, loc);
++      rich_loc.set_escape_on_output (true);
++      cpp_warning_at (pfile, CPP_W_BIDIRECTIONAL, &rich_loc,
++		      "unpaired UTF-8 bidirectional control character "
++		      "detected");
+     }
+   /* We're done with this context.  */
+   bidi::on_close ();
+@@ -1454,6 +1456,9 @@ maybe_warn_bidi_on_char (cpp_reader *pfile, const uchar *p, bidi::kind kind,
+       const location_t loc
+ 	= linemap_position_for_column (pfile->line_table,
+ 				       CPP_BUF_COLUMN (pfile->buffer, p));
++      rich_location rich_loc (pfile->line_table, loc);
++      rich_loc.set_escape_on_output (true);
++
+       /* It seems excessive to warn about a PDI/PDF that is closing
+ 	 an opened context because we've already warned about the
+ 	 opening character.  Except warn when we have a UCN x UTF-8
+@@ -1462,20 +1467,20 @@ maybe_warn_bidi_on_char (cpp_reader *pfile, const uchar *p, bidi::kind kind,
+ 	{
+ 	  if (warn_bidi == bidirectional_unpaired
+ 	      && bidi::current_ctx_ucn_p () != ucn_p)
+-	    cpp_warning_with_line (pfile, CPP_W_BIDIRECTIONAL, loc, 0,
+-				   "UTF-8 vs UCN mismatch when closing "
+-				   "a context by \"%s\"", bidi::to_str (kind));
++	    cpp_warning_at (pfile, CPP_W_BIDIRECTIONAL, &rich_loc,
++			    "UTF-8 vs UCN mismatch when closing "
++			    "a context by \"%s\"", bidi::to_str (kind));
+ 	}
+       else if (warn_bidi == bidirectional_any)
+ 	{
+ 	  if (kind == bidi::kind::PDF || kind == bidi::kind::PDI)
+-	    cpp_warning_with_line (pfile, CPP_W_BIDIRECTIONAL, loc, 0,
+-				   "\"%s\" is closing an unopened context",
+-				   bidi::to_str (kind));
++	    cpp_warning_at (pfile, CPP_W_BIDIRECTIONAL, &rich_loc,
++			    "\"%s\" is closing an unopened context",
++			    bidi::to_str (kind));
+ 	  else
+-	    cpp_warning_with_line (pfile, CPP_W_BIDIRECTIONAL, loc, 0,
+-				   "found problematic Unicode character \"%s\"",
+-				   bidi::to_str (kind));
++	    cpp_warning_at (pfile, CPP_W_BIDIRECTIONAL, &rich_loc,
++			    "found problematic Unicode character \"%s\"",
++			    bidi::to_str (kind));
+ 	}
+     }
+   /* We're done with this context.  */
+-- 
+2.27.0
+
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0004-64-bit-multilib-hack.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0004-64-bit-multilib-hack.patch
new file mode 100644
index 0000000..5f14dd2
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0004-64-bit-multilib-hack.patch
@@ -0,0 +1,133 @@
+From 2fa5c93641b75a662839c1b6eee172b6c481c70e Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Fri, 29 Mar 2013 09:10:06 +0400
+Subject: [PATCH] 64-bit multilib hack.
+
+GCC has internal multilib handling code but it assumes a very specific rigid directory
+layout. The build system implementation of multilib layout is very generic and allows
+complete customisation of the library directories.
+
+This patch is a partial solution to allow any custom directories to be passed into gcc
+and handled correctly. It forces gcc to use the base_libdir (which is the current
+directory, "."). We need to do this for each multilib that is configured as we don't
+know which compiler options may be being passed into the compiler. Since we have a compiler
+per mulitlib at this point that isn't an issue.
+
+The one problem is the target compiler is only going to work for the default multlilib at
+this point. Ideally we'd figure out which multilibs were being enabled with which paths
+and be able to patch these entries with a complete set of correct paths but this we
+don't have such code at this point. This is something the target gcc recipe should do
+and override these platform defaults in its build config.
+
+Do same for riscv64, aarch64 & arc
+
+RP 15/8/11
+
+Upstream-Status: Inappropriate [OE-Specific]
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+Signed-off-by: Elvis Dowson <elvis.dowson@gmail.com>
+Signed-off-by: Mark Hatle <mark.hatle@windriver.com>
+---
+ gcc/config/aarch64/t-aarch64-linux |  8 ++++----
+ gcc/config/arc/t-multilib-linux    |  4 ++--
+ gcc/config/i386/t-linux64          |  6 ++----
+ gcc/config/mips/t-linux64          | 10 +++-------
+ gcc/config/riscv/t-linux           |  6 ++++--
+ gcc/config/rs6000/t-linux64        |  5 ++---
+ 6 files changed, 17 insertions(+), 22 deletions(-)
+
+diff --git a/gcc/config/aarch64/t-aarch64-linux b/gcc/config/aarch64/t-aarch64-linux
+index 241b0ef20b6..a7dadb2d64f 100644
+--- a/gcc/config/aarch64/t-aarch64-linux
++++ b/gcc/config/aarch64/t-aarch64-linux
+@@ -21,8 +21,8 @@
+ LIB1ASMSRC   = aarch64/lib1funcs.asm
+ LIB1ASMFUNCS = _aarch64_sync_cache_range
+ 
+-AARCH_BE = $(if $(findstring TARGET_BIG_ENDIAN_DEFAULT=1, $(tm_defines)),_be)
+-MULTILIB_OSDIRNAMES = mabi.lp64=../lib64$(call if_multiarch,:aarch64$(AARCH_BE)-linux-gnu)
+-MULTIARCH_DIRNAME = $(call if_multiarch,aarch64$(AARCH_BE)-linux-gnu)
++#AARCH_BE = $(if $(findstring TARGET_BIG_ENDIAN_DEFAULT=1, $(tm_defines)),_be)
++#MULTILIB_OSDIRNAMES = mabi.lp64=../lib64$(call if_multiarch,:aarch64$(AARCH_BE)-linux-gnu)
++#MULTIARCH_DIRNAME = $(call if_multiarch,aarch64$(AARCH_BE)-linux-gnu)
+ 
+-MULTILIB_OSDIRNAMES += mabi.ilp32=../libilp32$(call if_multiarch,:aarch64$(AARCH_BE)-linux-gnu_ilp32)
++#MULTILIB_OSDIRNAMES += mabi.ilp32=../libilp32$(call if_multiarch,:aarch64$(AARCH_BE)-linux-gnu_ilp32)
+diff --git a/gcc/config/arc/t-multilib-linux b/gcc/config/arc/t-multilib-linux
+index fc3fff640a2..d58e28f6df8 100644
+--- a/gcc/config/arc/t-multilib-linux
++++ b/gcc/config/arc/t-multilib-linux
+@@ -16,9 +16,9 @@
+ # along with GCC; see the file COPYING3.  If not see
+ # <http://www.gnu.org/licenses/>.
+ 
+-MULTILIB_OPTIONS = mcpu=hs/mcpu=archs/mcpu=hs38/mcpu=hs38_linux/mcpu=arc700/mcpu=nps400
++#MULTILIB_OPTIONS = mcpu=hs/mcpu=archs/mcpu=hs38/mcpu=hs38_linux/mcpu=arc700/mcpu=nps400
+ 
+-MULTILIB_DIRNAMES = hs archs hs38 hs38_linux arc700 nps400
++#MULTILIB_DIRNAMES = hs archs hs38 hs38_linux arc700 nps400
+ 
+ # Aliases:
+ MULTILIB_MATCHES += mcpu?arc700=mA7
+diff --git a/gcc/config/i386/t-linux64 b/gcc/config/i386/t-linux64
+index d288b093522..7b5980a9d21 100644
+--- a/gcc/config/i386/t-linux64
++++ b/gcc/config/i386/t-linux64
+@@ -32,7 +32,5 @@
+ #
+ comma=,
+ MULTILIB_OPTIONS    = $(subst $(comma),/,$(TM_MULTILIB_CONFIG))
+-MULTILIB_DIRNAMES   = $(patsubst m%, %, $(subst /, ,$(MULTILIB_OPTIONS)))
+-MULTILIB_OSDIRNAMES = m64=../lib64$(call if_multiarch,:x86_64-linux-gnu)
+-MULTILIB_OSDIRNAMES+= m32=$(if $(wildcard $(shell echo $(SYSTEM_HEADER_DIR))/../../usr/lib32),../lib32,../lib)$(call if_multiarch,:i386-linux-gnu)
+-MULTILIB_OSDIRNAMES+= mx32=../libx32$(call if_multiarch,:x86_64-linux-gnux32)
++MULTILIB_DIRNAMES = . .
++MULTILIB_OSDIRNAMES = ../$(shell basename $(base_libdir)) ../$(shell basename $(base_libdir))
+diff --git a/gcc/config/mips/t-linux64 b/gcc/config/mips/t-linux64
+index 130e1f04707..3b7eb6b2a2f 100644
+--- a/gcc/config/mips/t-linux64
++++ b/gcc/config/mips/t-linux64
+@@ -17,10 +17,6 @@
+ # <http://www.gnu.org/licenses/>.
+ 
+ MULTILIB_OPTIONS = mabi=n32/mabi=32/mabi=64
+-MULTILIB_DIRNAMES = n32 32 64
+-MIPS_EL = $(if $(filter %el, $(firstword $(subst -, ,$(target)))),el)
+-MIPS_SOFT = $(if $(strip $(filter MASK_SOFT_FLOAT_ABI, $(target_cpu_default)) $(filter soft, $(with_float))),soft)
+-MULTILIB_OSDIRNAMES = \
+-	../lib32$(call if_multiarch,:mips64$(MIPS_EL)-linux-gnuabin32$(MIPS_SOFT)) \
+-	../lib$(call if_multiarch,:mips$(MIPS_EL)-linux-gnu$(MIPS_SOFT)) \
+-	../lib64$(call if_multiarch,:mips64$(MIPS_EL)-linux-gnuabi64$(MIPS_SOFT))
++MULTILIB_DIRNAMES = . . .
++MULTILIB_OSDIRNAMES = ../$(shell basename $(base_libdir)) ../$(shell basename $(base_libdir)) ../$(shell basename $(base_libdir))
++
+diff --git a/gcc/config/riscv/t-linux b/gcc/config/riscv/t-linux
+index 216d2776a18..e4d817621fc 100644
+--- a/gcc/config/riscv/t-linux
++++ b/gcc/config/riscv/t-linux
+@@ -1,3 +1,5 @@
+ # Only XLEN and ABI affect Linux multilib dir names, e.g. /lib32/ilp32d/
+-MULTILIB_DIRNAMES := $(patsubst rv32%,lib32,$(patsubst rv64%,lib64,$(MULTILIB_DIRNAMES)))
+-MULTILIB_OSDIRNAMES := $(patsubst lib%,../lib%,$(MULTILIB_DIRNAMES))
++#MULTILIB_DIRNAMES := $(patsubst rv32%,lib32,$(patsubst rv64%,lib64,$(MULTILIB_DIRNAMES)))
++MULTILIB_DIRNAMES := . .
++#MULTILIB_OSDIRNAMES := $(patsubst lib%,../lib%,$(MULTILIB_DIRNAMES))
++MULTILIB_OSDIRNAMES := ../$(shell basename $(base_libdir)) ../$(shell basename $(base_libdir))
+diff --git a/gcc/config/rs6000/t-linux64 b/gcc/config/rs6000/t-linux64
+index e11a118cb5f..4eaffb416fe 100644
+--- a/gcc/config/rs6000/t-linux64
++++ b/gcc/config/rs6000/t-linux64
+@@ -26,10 +26,9 @@
+ # MULTILIB_OSDIRNAMES according to what is found on the target.
+ 
+ MULTILIB_OPTIONS    := m64/m32
+-MULTILIB_DIRNAMES   := 64 32
++MULTILIB_DIRNAMES   := . .
+ MULTILIB_EXTRA_OPTS := 
+-MULTILIB_OSDIRNAMES := m64=../lib64$(call if_multiarch,:powerpc64-linux-gnu)
+-MULTILIB_OSDIRNAMES += m32=$(if $(wildcard $(shell echo $(SYSTEM_HEADER_DIR))/../../usr/lib32),../lib32,../lib)$(call if_multiarch,:powerpc-linux-gnu)
++MULTILIB_OSDIRNAMES := ../$(shell basename $(base_libdir)) ../$(shell basename $(base_libdir))
+ 
+ rs6000-linux.o: $(srcdir)/config/rs6000/rs6000-linux.c
+ 	$(COMPILE) $<
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0004-CVE-2021-42574.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0004-CVE-2021-42574.patch
new file mode 100644
index 0000000..4999c71
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0004-CVE-2021-42574.patch
@@ -0,0 +1,573 @@
+From bef32d4a28595e933f24fef378cf052a30b674a7 Mon Sep 17 00:00:00 2001
+From: David Malcolm <dmalcolm@redhat.com>
+Date: Tue, 2 Nov 2021 15:45:22 -0400
+Subject: [PATCH] libcpp: capture and underline ranges in -Wbidi-chars=
+ [PR103026]
+MIME-Version: 1.0
+Content-Type: text/plain; charset=utf8
+Content-Transfer-Encoding: 8bit
+
+This patch converts the bidi::vec to use a struct so that we can
+capture location_t values for the bidirectional control characters.
+
+Before:
+
+  Wbidi-chars-1.c: In function âmainâ:
+  Wbidi-chars-1.c:6:43: warning: unpaired UTF-8 bidirectional control character detected [-Wbidi-chars=]
+      6 |     /*<U+202E> } <U+2066>if (isAdmin)<U+2069> <U+2066> begin admins only */
+        |                                                                           ^
+  Wbidi-chars-1.c:9:28: warning: unpaired UTF-8 bidirectional control character detected [-Wbidi-chars=]
+      9 |     /* end admins only <U+202E> { <U+2066>*/
+        |                                            ^
+
+After:
+
+  Wbidi-chars-1.c: In function âmainâ:
+  Wbidi-chars-1.c:6:43: warning: unpaired UTF-8 bidirectional control characters detected [-Wbidi-chars=]
+      6 |     /*<U+202E> } <U+2066>if (isAdmin)<U+2069> <U+2066> begin admins only */
+        |       ~~~~~~~~                                ~~~~~~~~                    ^
+        |       |                                       |                           |
+        |       |                                       |                           end of bidirectional context
+        |       U+202E (RIGHT-TO-LEFT OVERRIDE)         U+2066 (LEFT-TO-RIGHT ISOLATE)
+  Wbidi-chars-1.c:9:28: warning: unpaired UTF-8 bidirectional control characters detected [-Wbidi-chars=]
+      9 |     /* end admins only <U+202E> { <U+2066>*/
+        |                        ~~~~~~~~   ~~~~~~~~ ^
+        |                        |          |        |
+        |                        |          |        end of bidirectional context
+        |                        |          U+2066 (LEFT-TO-RIGHT ISOLATE)
+        |                        U+202E (RIGHT-TO-LEFT OVERRIDE)
+
+Signed-off-by: David Malcolm <dmalcolm@redhat.com>
+
+gcc/testsuite/ChangeLog:
+	PR preprocessor/103026
+	* c-c++-common/Wbidi-chars-ranges.c: New test.
+
+libcpp/ChangeLog:
+	PR preprocessor/103026
+	* lex.c (struct bidi::context): New.
+	(bidi::vec): Convert to a vec of context rather than unsigned
+	char.
+	(bidi::ctx_at): Rename to...
+	(bidi::pop_kind_at): ...this and reimplement for above change.
+	(bidi::current_ctx): Update for change to vec.
+	(bidi::current_ctx_ucn_p): Likewise.
+	(bidi::current_ctx_loc): New.
+	(bidi::on_char): Update for usage of context struct.  Add "loc"
+	param and pass it when pushing contexts.
+	(get_location_for_byte_range_in_cur_line): New.
+	(get_bidi_utf8): Rename to...
+	(get_bidi_utf8_1): ...this, reintroducing...
+	(get_bidi_utf8): ...as a wrapper, setting *OUT when the result is
+	not NONE.
+	(get_bidi_ucn): Rename to...
+	(get_bidi_ucn_1): ...this, reintroducing...
+	(get_bidi_ucn): ...as a wrapper, setting *OUT when the result is
+	not NONE.
+	(class unpaired_bidi_rich_location): New.
+	(maybe_warn_bidi_on_close): Use unpaired_bidi_rich_location when
+	reporting on unpaired bidi chars.  Split into singular vs plural
+	spellings.
+	(maybe_warn_bidi_on_char): Pass in a location_t rather than a
+	const uchar * and use it when emitting warnings, and when calling
+	bidi::on_char.
+	(_cpp_skip_block_comment): Capture location when kind is not NONE
+	and pass it to maybe_warn_bidi_on_char.
+	(skip_line_comment): Likewise.
+	(forms_identifier_p): Likewise.
+	(lex_raw_string): Likewise.
+	(lex_string): Likewise.
+
+Signed-off-by: David Malcolm <dmalcolm@redhat.com>
+
+CVE: CVE-2021-42574
+Upstream-Status: Backport [https://gcc.gnu.org/git/gitweb.cgi?p=gcc.git;h=bef32d4a28595e933f24fef378cf052a30b674a7]
+Signed-off-by: Pgowda <pgowda.cve@gmail.com>
+
+---
+ .../c-c++-common/Wbidi-chars-ranges.c         |  54 ++++
+ libcpp/lex.c                                  | 251 ++++++++++++++----
+ 2 files changed, 257 insertions(+), 48 deletions(-)
+ create mode 100644 gcc/testsuite/c-c++-common/Wbidi-chars-ranges.c
+
+diff --git a/gcc/testsuite/c-c++-common/Wbidi-chars-ranges.c b/gcc/testsuite/c-c++-common/Wbidi-chars-ranges.c
+new file mode 100644
+index 00000000000..298750a2a64
+--- /dev/null
++++ b/gcc/testsuite/c-c++-common/Wbidi-chars-ranges.c
+@@ -0,0 +1,54 @@
++/* PR preprocessor/103026 */
++/* { dg-do compile } */
++/* { dg-options "-Wbidi-chars=unpaired -fdiagnostics-show-caret" } */
++/* Verify that we escape and underline pertinent bidirectional
++   control characters when quoting the source.  */
++
++int test_unpaired_bidi () {
++    int isAdmin = 0;
++    /*â® } â¦if (isAdmin)â© â¦ begin admins only */
++/* { dg-warning "bidirectional" "" { target *-*-* } .-1 } */
++#if 0
++   { dg-begin-multiline-output "" }
++     /*<U+202E> } <U+2066>if (isAdmin)<U+2069> <U+2066> begin admins only */
++       ~~~~~~~~                                ~~~~~~~~                    ^
++       |                                       |                           |
++       |                                       |                           end of bidirectional context
++       U+202E (RIGHT-TO-LEFT OVERRIDE)         U+2066 (LEFT-TO-RIGHT ISOLATE)
++   { dg-end-multiline-output "" }
++#endif
++
++        __builtin_printf("You are an admin.\n");
++    /* end admins only â® { â¦*/
++/* { dg-warning "bidirectional" "" { target *-*-* } .-1 } */
++#if 0
++   { dg-begin-multiline-output "" }
++     /* end admins only <U+202E> { <U+2066>*/
++                        ~~~~~~~~   ~~~~~~~~ ^
++                        |          |        |
++                        |          |        end of bidirectional context
++                        |          U+2066 (LEFT-TO-RIGHT ISOLATE)
++                        U+202E (RIGHT-TO-LEFT OVERRIDE)
++   { dg-end-multiline-output "" }
++#endif
++
++    return 0;
++}
++
++int LRE_âª_PDF_\u202c;
++/* { dg-warning "mismatch" "" { target *-*-* } .-1 } */
++#if 0
++   { dg-begin-multiline-output "" }
++ int LRE_<U+202A>_PDF_\u202c;
++         ~~~~~~~~     ^~~~~~
++   { dg-end-multiline-output "" }
++#endif
++
++const char *s1 = "LRE_âª_PDF_\u202c";
++/* { dg-warning "mismatch" "" { target *-*-* } .-1 } */
++#if 0
++   { dg-begin-multiline-output "" }
++ const char *s1 = "LRE_<U+202A>_PDF_\u202c";
++                       ~~~~~~~~     ^~~~~~
++   { dg-end-multiline-output "" }
++#endif
+diff --git a/libcpp/lex.c b/libcpp/lex.c
+index 2421d6c0f40..94c36f0d014 100644
+--- a/libcpp/lex.c
++++ b/libcpp/lex.c
+@@ -1172,11 +1172,34 @@ namespace bidi {
+   /* All the UTF-8 encodings of bidi characters start with E2.  */
+   constexpr uchar utf8_start = 0xe2;
+ 
++  struct context
++  {
++    context () {}
++    context (location_t loc, kind k, bool pdf, bool ucn)
++    : m_loc (loc), m_kind (k), m_pdf (pdf), m_ucn (ucn)
++    {
++    }
++
++    kind get_pop_kind () const
++    {
++      return m_pdf ? kind::PDF : kind::PDI;
++    }
++    bool ucn_p () const
++    {
++      return m_ucn;
++    }
++
++    location_t m_loc;
++    kind m_kind;
++    unsigned m_pdf : 1;
++    unsigned m_ucn : 1;
++  };
++
+   /* A vector holding currently open bidi contexts.  We use a char for
+      each context, its LSB is 1 if it represents a PDF context, 0 if it
+      represents a PDI context.  The next bit is 1 if this context was open
+      by a bidi character written as a UCN, and 0 when it was UTF-8.  */
+-  semi_embedded_vec <unsigned char, 16> vec;
++  semi_embedded_vec <context, 16> vec;
+ 
+   /* Close the whole comment/identifier/string literal/character constant
+      context.  */
+@@ -1193,19 +1216,19 @@ namespace bidi {
+     vec.truncate (len - 1);
+   }
+ 
+-  /* Return the context of the Ith element.  */
+-  kind ctx_at (unsigned int i)
++  /* Return the pop kind of the context of the Ith element.  */
++  kind pop_kind_at (unsigned int i)
+   {
+-    return (vec[i] & 1) ? kind::PDF : kind::PDI;
++    return vec[i].get_pop_kind ();
+   }
+ 
+-  /* Return which context is currently opened.  */
++  /* Return the pop kind of the context that is currently opened.  */
+   kind current_ctx ()
+   {
+     unsigned int len = vec.count ();
+     if (len == 0)
+       return kind::NONE;
+-    return ctx_at (len - 1);
++    return vec[len - 1].get_pop_kind ();
+   }
+ 
+   /* Return true if the current context comes from a UCN origin, that is,
+@@ -1214,11 +1237,19 @@ namespace bidi {
+   {
+     unsigned int len = vec.count ();
+     gcc_checking_assert (len > 0);
+-    return (vec[len - 1] >> 1) & 1;
++    return vec[len - 1].m_ucn;
+   }
+ 
+-  /* We've read a bidi char, update the current vector as necessary.  */
+-  void on_char (kind k, bool ucn_p)
++  location_t current_ctx_loc ()
++  {
++    unsigned int len = vec.count ();
++    gcc_checking_assert (len > 0);
++    return vec[len - 1].m_loc;
++  }
++
++  /* We've read a bidi char, update the current vector as necessary.
++     LOC is only valid when K is not kind::NONE.  */
++  void on_char (kind k, bool ucn_p, location_t loc)
+   {
+     switch (k)
+       {
+@@ -1226,12 +1257,12 @@ namespace bidi {
+       case kind::RLE:
+       case kind::LRO:
+       case kind::RLO:
+-	vec.push (ucn_p ? 3u : 1u);
++	vec.push (context (loc, k, true, ucn_p));
+ 	break;
+       case kind::LRI:
+       case kind::RLI:
+       case kind::FSI:
+-	vec.push (ucn_p ? 2u : 0u);
++	vec.push (context (loc, k, false, ucn_p));
+ 	break;
+       /* PDF terminates the scope of the last LRE, RLE, LRO, or RLO
+ 	 whose scope has not yet been terminated.  */
+@@ -1245,7 +1276,7 @@ namespace bidi {
+ 	 yet been terminated.  */
+       case kind::PDI:
+ 	for (int i = vec.count () - 1; i >= 0; --i)
+-	  if (ctx_at (i) == kind::PDI)
++	  if (pop_kind_at (i) == kind::PDI)
+ 	    {
+ 	      vec.truncate (i);
+ 	      break;
+@@ -1295,10 +1326,47 @@ namespace bidi {
+   }
+ }
+ 
++/* Get location_t for the range of bytes [START, START + NUM_BYTES)
++   within the current line in FILE, with the caret at START.  */
++
++static location_t
++get_location_for_byte_range_in_cur_line (cpp_reader *pfile,
++					 const unsigned char *const start,
++					 size_t num_bytes)
++{
++  gcc_checking_assert (num_bytes > 0);
++
++  /* CPP_BUF_COLUMN and linemap_position_for_column both refer
++     to offsets in bytes, but CPP_BUF_COLUMN is 0-based,
++     whereas linemap_position_for_column is 1-based.  */
++
++  /* Get 0-based offsets within the line.  */
++  size_t start_offset = CPP_BUF_COLUMN (pfile->buffer, start);
++  size_t end_offset = start_offset + num_bytes - 1;
++
++  /* Now convert to location_t, where "columns" are 1-based byte offsets.  */
++  location_t start_loc = linemap_position_for_column (pfile->line_table,
++						      start_offset + 1);
++  location_t end_loc = linemap_position_for_column (pfile->line_table,
++						     end_offset + 1);
++
++  if (start_loc == end_loc)
++    return start_loc;
++
++  source_range src_range;
++  src_range.m_start = start_loc;
++  src_range.m_finish = end_loc;
++  location_t combined_loc = COMBINE_LOCATION_DATA (pfile->line_table,
++						   start_loc,
++						   src_range,
++						   NULL);
++  return combined_loc;
++}
++
+ /* Parse a sequence of 3 bytes starting with P and return its bidi code.  */
+ 
+ static bidi::kind
+-get_bidi_utf8 (const unsigned char *const p)
++get_bidi_utf8_1 (const unsigned char *const p)
+ {
+   gcc_checking_assert (p[0] == bidi::utf8_start);
+ 
+@@ -1340,10 +1408,25 @@ get_bidi_utf8 (const unsigned char *cons
+   return bidi::kind::NONE;
+ }
+ 
++/* Parse a sequence of 3 bytes starting with P and return its bidi code.
++   If the kind is not NONE, write the location to *OUT.*/
++
++static bidi::kind
++get_bidi_utf8 (cpp_reader *pfile, const unsigned char *const p, location_t *out)
++{
++  bidi::kind result = get_bidi_utf8_1 (p);
++  if (result != bidi::kind::NONE)
++    {
++      /* We have a sequence of 3 bytes starting at P.  */
++      *out = get_location_for_byte_range_in_cur_line (pfile, p, 3);
++    }
++  return result;
++}
++
+ /* Parse a UCN where P points just past \u or \U and return its bidi code.  */
+ 
+ static bidi::kind
+-get_bidi_ucn (const unsigned char *p, bool is_U)
++get_bidi_ucn_1 (const unsigned char *p, bool is_U)
+ {
+   /* 6.4.3 Universal Character Names
+       \u hex-quad
+@@ -1412,6 +1495,62 @@ get_bidi_ucn (const unsigned char *p, bo
+   return bidi::kind::NONE;
+ }
+ 
++/* Parse a UCN where P points just past \u or \U and return its bidi code.
++   If the kind is not NONE, write the location to *OUT.*/
++
++static bidi::kind
++get_bidi_ucn (cpp_reader *pfile,  const unsigned char *p, bool is_U,
++	      location_t *out)
++{
++  bidi::kind result = get_bidi_ucn_1 (p, is_U);
++  if (result != bidi::kind::NONE)
++    {
++      const unsigned char *start = p - 2;
++      size_t num_bytes = 2 + (is_U ? 8 : 4);
++      *out = get_location_for_byte_range_in_cur_line (pfile, start, num_bytes);
++    }
++  return result;
++}
++
++/* Subclass of rich_location for reporting on unpaired UTF-8
++   bidirectional control character(s).
++   Escape the source lines on output, and show all unclosed
++   bidi context, labelling everything.  */
++
++class unpaired_bidi_rich_location : public rich_location
++{
++ public:
++  class custom_range_label : public range_label
++  {
++   public:
++     label_text get_text (unsigned range_idx) const FINAL OVERRIDE
++     {
++       /* range 0 is the primary location; each subsequent range i + 1
++	  is for bidi::vec[i].  */
++       if (range_idx > 0)
++	 {
++	   const bidi::context &ctxt (bidi::vec[range_idx - 1]);
++	   return label_text::borrow (bidi::to_str (ctxt.m_kind));
++	 }
++       else
++	 return label_text::borrow (_("end of bidirectional context"));
++     }
++  };
++
++  unpaired_bidi_rich_location (cpp_reader *pfile, location_t loc)
++  : rich_location (pfile->line_table, loc, &m_custom_label)
++  {
++    set_escape_on_output (true);
++    for (unsigned i = 0; i < bidi::vec.count (); i++)
++      add_range (bidi::vec[i].m_loc,
++		 SHOW_RANGE_WITHOUT_CARET,
++		 &m_custom_label);
++  }
++
++ private:
++   custom_range_label m_custom_label;
++};
++
+ /* We're closing a bidi context, that is, we've encountered a newline,
+    are closing a C-style comment, or are at the end of a string literal,
+    character constant, or identifier.  Warn if this context was not
+@@ -1427,11 +1566,17 @@ maybe_warn_bidi_on_close (cpp_reader *pf
+       const location_t loc
+ 	= linemap_position_for_column (pfile->line_table,
+ 				       CPP_BUF_COLUMN (pfile->buffer, p));
+-      rich_location rich_loc (pfile->line_table, loc);
+-      rich_loc.set_escape_on_output (true);
+-      cpp_warning_at (pfile, CPP_W_BIDIRECTIONAL, &rich_loc,
+-		      "unpaired UTF-8 bidirectional control character "
+-		      "detected");
++      unpaired_bidi_rich_location rich_loc (pfile, loc);
++      /* cpp_callbacks doesn't yet have a way to handle singular vs plural
++	 forms of a diagnostic, so fake it for now.  */
++      if (bidi::vec.count () > 1)
++	cpp_warning_at (pfile, CPP_W_BIDIRECTIONAL, &rich_loc,
++			"unpaired UTF-8 bidirectional control characters "
++			"detected");
++      else
++	cpp_warning_at (pfile, CPP_W_BIDIRECTIONAL, &rich_loc,
++			"unpaired UTF-8 bidirectional control character "
++			"detected");
+     }
+   /* We're done with this context.  */
+   bidi::on_close ();
+@@ -1439,12 +1584,13 @@ maybe_warn_bidi_on_close (cpp_reader *pf
+ 
+ /* We're at the beginning or in the middle of an identifier/comment/string
+    literal/character constant.  Warn if we've encountered a bidi character.
+-   KIND says which bidi character it was; P points to it in the character
+-   stream.  UCN_P is true iff this bidi character was written as a UCN.  */
++   KIND says which bidi control character it was; UCN_P is true iff this bidi
++   control character was written as a UCN.  LOC is the location of the
++   character, but is only valid if KIND != bidi::kind::NONE.  */
+ 
+ static void
+-maybe_warn_bidi_on_char (cpp_reader *pfile, const uchar *p, bidi::kind kind,
+-			 bool ucn_p)
++maybe_warn_bidi_on_char (cpp_reader *pfile, bidi::kind kind,
++			 bool ucn_p, location_t loc)
+ {
+   if (__builtin_expect (kind == bidi::kind::NONE, 1))
+     return;
+@@ -1453,9 +1599,6 @@ maybe_warn_bidi_on_char (cpp_reader *pfi
+ 
+   if (warn_bidi != bidirectional_none)
+     {
+-      const location_t loc
+-	= linemap_position_for_column (pfile->line_table,
+-				       CPP_BUF_COLUMN (pfile->buffer, p));
+       rich_location rich_loc (pfile->line_table, loc);
+       rich_loc.set_escape_on_output (true);
+ 
+@@ -1467,9 +1610,12 @@ maybe_warn_bidi_on_char (cpp_reader *pfi
+ 	{
+ 	  if (warn_bidi == bidirectional_unpaired
+ 	      && bidi::current_ctx_ucn_p () != ucn_p)
+-	    cpp_warning_at (pfile, CPP_W_BIDIRECTIONAL, &rich_loc,
+-			    "UTF-8 vs UCN mismatch when closing "
+-			    "a context by \"%s\"", bidi::to_str (kind));
++	    {
++	      rich_loc.add_range (bidi::current_ctx_loc ());
++	      cpp_warning_at (pfile, CPP_W_BIDIRECTIONAL, &rich_loc,
++			      "UTF-8 vs UCN mismatch when closing "
++			      "a context by \"%s\"", bidi::to_str (kind));
++	    }
+ 	}
+       else if (warn_bidi == bidirectional_any)
+ 	{
+@@ -1484,7 +1630,7 @@ maybe_warn_bidi_on_char (cpp_reader *pfi
+ 	}
+     }
+   /* We're done with this context.  */
+-  bidi::on_char (kind, ucn_p);
++  bidi::on_char (kind, ucn_p, loc);
+ }
+ 
+ /* Skip a C-style block comment.  We find the end of the comment by
+@@ -1552,8 +1698,9 @@ _cpp_skip_block_comment (cpp_reader *pfi
+ 	 a bidirectional control character.  */
+       else if (__builtin_expect (c == bidi::utf8_start, 0) && warn_bidi_p)
+ 	{
+-	  bidi::kind kind = get_bidi_utf8 (cur - 1);
+-	  maybe_warn_bidi_on_char (pfile, cur, kind, /*ucn_p=*/false);
++	  location_t loc;
++	  bidi::kind kind = get_bidi_utf8 (pfile, cur - 1, &loc);
++	  maybe_warn_bidi_on_char (pfile, kind, /*ucn_p=*/false, loc);
+ 	}
+     }
+ 
+@@ -1586,9 +1733,9 @@ skip_line_comment (cpp_reader *pfile)
+ 	    {
+ 	      if (__builtin_expect (*buffer->cur == bidi::utf8_start, 0))
+ 		{
+-		  bidi::kind kind = get_bidi_utf8 (buffer->cur);
+-		  maybe_warn_bidi_on_char (pfile, buffer->cur, kind,
+-					   /*ucn_p=*/false);
++		  location_t loc;
++		  bidi::kind kind = get_bidi_utf8 (pfile, buffer->cur, &loc);
++		  maybe_warn_bidi_on_char (pfile, kind, /*ucn_p=*/false, loc);
+ 		}
+ 	      buffer->cur++;
+ 	    }
+@@ -1708,9 +1855,9 @@ forms_identifier_p (cpp_reader *pfile, i
+ 	  if (__builtin_expect (*buffer->cur == bidi::utf8_start, 0)
+ 	      && warn_bidi_p)
+ 	    {
+-	      bidi::kind kind = get_bidi_utf8 (buffer->cur);
+-	      maybe_warn_bidi_on_char (pfile, buffer->cur, kind,
+-				       /*ucn_p=*/false);
++	      location_t loc;
++	      bidi::kind kind = get_bidi_utf8 (pfile, buffer->cur, &loc);
++	      maybe_warn_bidi_on_char (pfile, kind, /*ucn_p=*/false, loc);
+ 	    }
+ 	  if (_cpp_valid_utf8 (pfile, &buffer->cur, buffer->rlimit, 1 + !first,
+ 			       state, &s))
+@@ -1722,10 +1869,12 @@ forms_identifier_p (cpp_reader *pfile, i
+ 	  buffer->cur += 2;
+ 	  if (warn_bidi_p)
+ 	    {
+-	      bidi::kind kind = get_bidi_ucn (buffer->cur,
+-					      buffer->cur[-1] == 'U');
+-	      maybe_warn_bidi_on_char (pfile, buffer->cur, kind,
+-				       /*ucn_p=*/true);
++	      location_t loc;
++	      bidi::kind kind = get_bidi_ucn (pfile,
++					      buffer->cur,
++					      buffer->cur[-1] == 'U',
++					      &loc);
++	      maybe_warn_bidi_on_char (pfile, kind, /*ucn_p=*/true, loc);
+ 	    }
+ 	  if (_cpp_valid_ucn (pfile, &buffer->cur, buffer->rlimit, 1 + !first,
+ 			      state, &s, NULL, NULL))
+@@ -2336,8 +2485,11 @@ lex_raw_string (cpp_reader *pfile, cpp_t
+ 	}
+       else if (__builtin_expect ((unsigned char) c == bidi::utf8_start, 0)
+ 	       && warn_bidi_p)
+-	maybe_warn_bidi_on_char (pfile, pos - 1, get_bidi_utf8 (pos - 1),
+-				 /*ucn_p=*/false);
++	{
++	  location_t loc;
++	  bidi::kind kind = get_bidi_utf8 (pfile, pos - 1, &loc);
++	  maybe_warn_bidi_on_char (pfile, kind, /*ucn_p=*/false, loc);
++	}
+     }
+ 
+   if (warn_bidi_p)
+@@ -2447,8 +2599,10 @@ lex_string (cpp_reader *pfile, cpp_token
+ 	{
+ 	  if ((cur[0] == 'u' || cur[0] == 'U') && warn_bidi_p)
+ 	    {
+-	      bidi::kind kind = get_bidi_ucn (cur + 1, cur[0] == 'U');
+-	      maybe_warn_bidi_on_char (pfile, cur, kind, /*ucn_p=*/true);
++	      location_t loc;
++	      bidi::kind kind = get_bidi_ucn (pfile, cur + 1, cur[0] == 'U',
++					      &loc);
++	      maybe_warn_bidi_on_char (pfile, kind, /*ucn_p=*/true, loc);
+ 	    }
+ 	  cur++;
+ 	}
+@@ -2476,8 +2630,9 @@ lex_string (cpp_reader *pfile, cpp_token
+ 	saw_NUL = true;
+       else if (__builtin_expect (c == bidi::utf8_start, 0) && warn_bidi_p)
+ 	{
+-	  bidi::kind kind = get_bidi_utf8 (cur - 1);
+-	  maybe_warn_bidi_on_char (pfile, cur - 1, kind, /*ucn_p=*/false);
++	  location_t loc;
++	  bidi::kind kind = get_bidi_utf8 (pfile, cur - 1, &loc);
++	  maybe_warn_bidi_on_char (pfile, kind, /*ucn_p=*/false, loc);
+ 	}
+     }
+ 
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0006-If-CXXFLAGS-contains-something-unsupported-by-the-bu.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0006-If-CXXFLAGS-contains-something-unsupported-by-the-bu.patch
new file mode 100644
index 0000000..98a1bb2
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0006-If-CXXFLAGS-contains-something-unsupported-by-the-bu.patch
@@ -0,0 +1,63 @@
+From e700190743fa29ddaebd6ee075298a24b1688773 Mon Sep 17 00:00:00 2001
+From: Richard Purdie <richard.purdie@linuxfoundation.org>
+Date: Thu, 28 Oct 2021 11:33:40 +0100
+Subject: [PATCH 6/6] If CXXFLAGS contains something unsupported by the build
+ CXX, we see build failures (e.g. using -fmacro-prefix-map for the target).
+
+Pass CXXFLAGS_FOR_BUILD in a couple of places to avoid these errors.
+
+2021-10-28 Richard Purdie <richard.purdie@linuxfoundation.org>
+
+ChangeLog:
+
+    * Makefile.in: Regenerate.
+    * Makefile.tpl: Add missing CXXFLAGS_FOR_BUILD overrides
+
+Signed-off-by: Richard Purdie <richard.purdie@linuxfoundation.org>
+---
+ Makefile.in  | 2 ++
+ Makefile.tpl | 2 ++
+ 2 files changed, 4 insertions(+)
+
+Upstream-Status: Pending [should be submittable]
+
+Index: gcc-11.2.0/Makefile.in
+===================================================================
+--- gcc-11.2.0.orig/Makefile.in
++++ gcc-11.2.0/Makefile.in
+@@ -172,6 +172,7 @@ BUILD_EXPORTS = \
+ # built for the build system to override those in BASE_FLAGS_TO_PASS.
+ EXTRA_BUILD_FLAGS = \
+ 	CFLAGS="$(CFLAGS_FOR_BUILD)" \
++	CXXFLAGS="$(CXXFLAGS_FOR_BUILD)" \
+ 	LDFLAGS="$(LDFLAGS_FOR_BUILD)"
+ 
+ # This is the list of directories to built for the host system.
+@@ -203,6 +204,7 @@ HOST_EXPORTS = \
+ 	CPP_FOR_BUILD="$(CPP_FOR_BUILD)"; export CPP_FOR_BUILD; \
+ 	CPPFLAGS_FOR_BUILD="$(CPPFLAGS_FOR_BUILD)"; export CPPFLAGS_FOR_BUILD; \
+ 	CXX_FOR_BUILD="$(CXX_FOR_BUILD)"; export CXX_FOR_BUILD; \
++	CXXFLAGS_FOR_BUILD="$(CXXFLAGS_FOR_BUILD)"; export CXXFLAGS_FOR_BUILD; \
+ 	DLLTOOL="$(DLLTOOL)"; export DLLTOOL; \
+ 	LD="$(LD)"; export LD; \
+ 	LDFLAGS="$(STAGE1_LDFLAGS) $(LDFLAGS)"; export LDFLAGS; \
+Index: gcc-11.2.0/Makefile.tpl
+===================================================================
+--- gcc-11.2.0.orig/Makefile.tpl
++++ gcc-11.2.0/Makefile.tpl
+@@ -175,6 +175,7 @@ BUILD_EXPORTS = \
+ # built for the build system to override those in BASE_FLAGS_TO_PASS.
+ EXTRA_BUILD_FLAGS = \
+ 	CFLAGS="$(CFLAGS_FOR_BUILD)" \
++	CXXFLAGS="$(CXXFLAGS_FOR_BUILD)" \
+ 	LDFLAGS="$(LDFLAGS_FOR_BUILD)"
+ 
+ # This is the list of directories to built for the host system.
+@@ -206,6 +207,7 @@ HOST_EXPORTS = \
+ 	CPP_FOR_BUILD="$(CPP_FOR_BUILD)"; export CPP_FOR_BUILD; \
+ 	CPPFLAGS_FOR_BUILD="$(CPPFLAGS_FOR_BUILD)"; export CPPFLAGS_FOR_BUILD; \
+ 	CXX_FOR_BUILD="$(CXX_FOR_BUILD)"; export CXX_FOR_BUILD; \
++	CXXFLAGS_FOR_BUILD="$(CXXFLAGS_FOR_BUILD)"; export CXXFLAGS_FOR_BUILD; \
+ 	DLLTOOL="$(DLLTOOL)"; export DLLTOOL; \
+ 	LD="$(LD)"; export LD; \
+ 	LDFLAGS="$(STAGE1_LDFLAGS) $(LDFLAGS)"; export LDFLAGS; \
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0007-Use-the-defaults.h-in-B-instead-of-S-and-t-oe-in-B.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0007-Use-the-defaults.h-in-B-instead-of-S-and-t-oe-in-B.patch
new file mode 100644
index 0000000..9844c2a
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0007-Use-the-defaults.h-in-B-instead-of-S-and-t-oe-in-B.patch
@@ -0,0 +1,92 @@
+From e64342df647829d3e1c52fd480a5be7c1281ced0 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Fri, 29 Mar 2013 09:17:25 +0400
+Subject: [PATCH] Use the defaults.h in ${B} instead of ${S}, and t-oe in ${B}
+
+Use the defaults.h in ${B} instead of ${S}, and t-oe in ${B}, so that
+the source can be shared between gcc-cross-initial,
+gcc-cross-intermediate, gcc-cross, gcc-runtime, and also the sdk build.
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+
+Upstream-Status: Pending
+
+While compiling gcc-crosssdk-initial-x86_64 on some host, there is
+occasionally failure that test the existance of default.h doesn't
+work, the reason is tm_include_list='** defaults.h' rather than
+tm_include_list='** ./defaults.h'
+
+So we add the test condition for this situation.
+Signed-off-by: Hongxu Jia <hongxu.jia@windriver.com>
+---
+ gcc/Makefile.in  | 2 +-
+ gcc/configure    | 4 ++--
+ gcc/configure.ac | 4 ++--
+ gcc/mkconfig.sh  | 4 ++--
+ 4 files changed, 7 insertions(+), 7 deletions(-)
+
+diff --git a/gcc/Makefile.in b/gcc/Makefile.in
+index 8a5fb3fd99c..7da6f439fff 100644
+--- a/gcc/Makefile.in
++++ b/gcc/Makefile.in
+@@ -552,7 +552,7 @@ TARGET_SYSTEM_ROOT = @TARGET_SYSTEM_ROOT@
+ TARGET_SYSTEM_ROOT_DEFINE = @TARGET_SYSTEM_ROOT_DEFINE@
+ 
+ xmake_file=@xmake_file@
+-tmake_file=@tmake_file@
++tmake_file=@tmake_file@ ./t-oe
+ TM_ENDIAN_CONFIG=@TM_ENDIAN_CONFIG@
+ TM_MULTILIB_CONFIG=@TM_MULTILIB_CONFIG@
+ TM_MULTILIB_EXCEPTIONS_CONFIG=@TM_MULTILIB_EXCEPTIONS_CONFIG@
+diff --git a/gcc/configure b/gcc/configure
+index 3f0734bff11..c5d3bc4ee2d 100755
+--- a/gcc/configure
++++ b/gcc/configure
+@@ -13090,8 +13090,8 @@ for f in $tm_file; do
+        tm_include_list="${tm_include_list} $f"
+        ;;
+     defaults.h )
+-       tm_file_list="${tm_file_list} \$(srcdir)/$f"
+-       tm_include_list="${tm_include_list} $f"
++       tm_file_list="${tm_file_list} ./$f"
++       tm_include_list="${tm_include_list} ./$f"
+        ;;
+     * )
+        tm_file_list="${tm_file_list} \$(srcdir)/config/$f"
+diff --git a/gcc/configure.ac b/gcc/configure.ac
+index 54e21764b3e..f83420326d6 100644
+--- a/gcc/configure.ac
++++ b/gcc/configure.ac
+@@ -2263,8 +2263,8 @@ for f in $tm_file; do
+        tm_include_list="${tm_include_list} $f"
+        ;;
+     defaults.h )
+-       tm_file_list="${tm_file_list} \$(srcdir)/$f"
+-       tm_include_list="${tm_include_list} $f"
++       tm_file_list="${tm_file_list} ./$f"
++       tm_include_list="${tm_include_list} ./$f"
+        ;;
+     * )
+        tm_file_list="${tm_file_list} \$(srcdir)/config/$f"
+diff --git a/gcc/mkconfig.sh b/gcc/mkconfig.sh
+index c49acd8f7e4..a0a657bdbb9 100644
+--- a/gcc/mkconfig.sh
++++ b/gcc/mkconfig.sh
+@@ -77,7 +77,7 @@ if [ -n "$HEADERS" ]; then
+     if [ $# -ge 1 ]; then
+ 	echo '#ifdef IN_GCC' >> ${output}T
+ 	for file in "$@"; do
+-	    if test x"$file" = x"defaults.h"; then
++	    if test x"$file" = x"./defaults.h" -o x"$file" = x"defaults.h"; then
+ 		postpone_defaults_h="yes"
+ 	    else
+ 		echo "# include \"$file\"" >> ${output}T
+@@ -106,7 +106,7 @@ esac
+ 
+ # If we postponed including defaults.h, add the #include now.
+ if test x"$postpone_defaults_h" = x"yes"; then
+-    echo "# include \"defaults.h\"" >> ${output}T
++    echo "# include \"./defaults.h\"" >> ${output}T
+ fi
+ 
+ # Add multiple inclusion protection guard, part two.
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0009-cpp-honor-sysroot.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0009-cpp-honor-sysroot.patch
new file mode 100644
index 0000000..37f26f0
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0009-cpp-honor-sysroot.patch
@@ -0,0 +1,59 @@
+From: Richard Purdie <richard.purdie@linuxfoundation.org>
+Subject: [PATCH 5/5] gcc: Pass sysroot options to cpp for preprocessed source
+
+OpenEmbedded/Yocto Project extensively uses the --sysroot support within gcc.
+We discovered that when compiling preprocessed source (.i or .ii files), the
+compiler will try and access the builtin sysroot location rather than the
+--sysroot option specified on the commandline. If access to that directory is
+permission denied (unreadable), gcc will error. This is particularly problematic
+when ccache is involved.
+
+This patch adds %I to the cpp-output spec macro so the default substitutions for
+-iprefix, -isystem, -isysroot happen and the correct sysroot is used.
+
+2021-10-27 Richard Purdie <richard.purdie@linuxfoundation.org>
+
+gcc/cp/ChangeLog:
+
+    * lang-specs.h: Pass sysroot options to cpp for preprocessed source
+
+gcc/ChangeLog:
+
+    * gcc.c: Pass sysroot options to cpp for preprocessed source
+
+Signed-off-by: Richard Purdie <richard.purdie@linuxfoundation.org>
+
+[YOCTO #2074]
+
+Upstream-Status: Submitted [https://gcc.gnu.org/pipermail/gcc-patches/2021-October/582725.html]
+---
+ gcc/cp/lang-specs.h | 2 +-
+ gcc/gcc.c           | 2 +-
+ 2 files changed, 2 insertions(+), 2 deletions(-)
+
+Index: gcc-11.2.0/gcc/cp/lang-specs.h
+===================================================================
+--- gcc-11.2.0.orig/gcc/cp/lang-specs.h
++++ gcc-11.2.0/gcc/cp/lang-specs.h
+@@ -116,7 +116,7 @@ along with GCC; see the file COPYING3.
+   {".ii", "@c++-cpp-output", 0, 0, 0},
+   {"@c++-cpp-output",
+       "%{!E:%{!M:%{!MM:"
+-      "  cc1plus -fpreprocessed %i %(cc1_options) %2"
++      "  cc1plus -fpreprocessed %i %I %(cc1_options) %2"
+       "  %{!fsyntax-only:"
+       "    %{fmodule-only:%{!S:-o %g.s%V}}"
+       "    %{!fmodule-only:%{!fmodule-header*:%(invoke_as)}}}"
+Index: gcc-11.2.0/gcc/gcc.c
+===================================================================
+--- gcc-11.2.0.orig/gcc/gcc.c
++++ gcc-11.2.0/gcc/gcc.c
+@@ -1470,7 +1470,7 @@ static const struct compiler default_com
+ 					   %W{o*:--output-pch=%*}}%V}}}}}}}", 0, 0, 0},
+   {".i", "@cpp-output", 0, 0, 0},
+   {"@cpp-output",
+-   "%{!M:%{!MM:%{!E:cc1 -fpreprocessed %i %(cc1_options) %{!fsyntax-only:%(invoke_as)}}}}", 0, 0, 0},
++   "%{!M:%{!MM:%{!E:cc1 -fpreprocessed %i %I %(cc1_options) %{!fsyntax-only:%(invoke_as)}}}}", 0, 0, 0},
+   {".s", "@assembler", 0, 0, 0},
+   {"@assembler",
+    "%{!M:%{!MM:%{!E:%{!S:as %(asm_debug) %(asm_options) %i %A }}}}", 0, 0, 0},
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0011-Define-GLIBC_DYNAMIC_LINKER-and-UCLIBC_DYNAMIC_LINKE.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0011-Define-GLIBC_DYNAMIC_LINKER-and-UCLIBC_DYNAMIC_LINKE.patch
new file mode 100644
index 0000000..0884730
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0011-Define-GLIBC_DYNAMIC_LINKER-and-UCLIBC_DYNAMIC_LINKE.patch
@@ -0,0 +1,247 @@
+From 8e9d6efecdc8c42f47e3a012dee764b13c3dad59 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Fri, 29 Mar 2013 09:24:50 +0400
+Subject: [PATCH] Define GLIBC_DYNAMIC_LINKER and UCLIBC_DYNAMIC_LINKER
+ relative to SYSTEMLIBS_DIR
+
+This patch defines GLIBC_DYNAMIC_LINKER and UCLIBC_DYNAMIC_LINKER
+relative to SYSTEMLIBS_DIR which can be set in generated headers
+This breaks the assumption of hardcoded multilib in gcc
+Change is only for the supported architectures in OE including
+SH, sparc, alpha for possible future support (if any)
+
+Removes the do_headerfix task in metadata
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+
+Upstream-Status: Inappropriate [OE configuration]
+---
+ gcc/config/alpha/linux-elf.h |  4 ++--
+ gcc/config/arm/linux-eabi.h  |  4 ++--
+ gcc/config/arm/linux-elf.h   |  2 +-
+ gcc/config/i386/linux.h      |  2 +-
+ gcc/config/i386/linux64.h    |  6 +++---
+ gcc/config/linux.h           |  8 ++++----
+ gcc/config/mips/linux.h      | 12 ++++++------
+ gcc/config/riscv/linux.h     |  2 +-
+ gcc/config/rs6000/linux64.h  | 15 +++++----------
+ gcc/config/sh/linux.h        |  2 +-
+ gcc/config/sparc/linux.h     |  2 +-
+ gcc/config/sparc/linux64.h   |  4 ++--
+ 12 files changed, 29 insertions(+), 34 deletions(-)
+
+unchanged:
+--- a/gcc/config/alpha/linux-elf.h
++++ b/gcc/config/alpha/linux-elf.h
+@@ -23,8 +23,8 @@ along with GCC; see the file COPYING3.  If not see
+ #define EXTRA_SPECS \
+ { "elf_dynamic_linker", ELF_DYNAMIC_LINKER },
+ 
+-#define GLIBC_DYNAMIC_LINKER	"/lib/ld-linux.so.2"
+-#define UCLIBC_DYNAMIC_LINKER "/lib/ld-uClibc.so.0"
++#define GLIBC_DYNAMIC_LINKER	SYSTEMLIBS_DIR "ld-linux.so.2"
++#define UCLIBC_DYNAMIC_LINKER  SYSTEMLIBS_DIR "ld-uClibc.so.0"
+ #if DEFAULT_LIBC == LIBC_UCLIBC
+ #define CHOOSE_DYNAMIC_LINKER(G, U) "%{mglibc:" G ";:" U "}"
+ #elif DEFAULT_LIBC == LIBC_GLIBC
+unchanged:
+--- a/gcc/config/arm/linux-eabi.h
++++ b/gcc/config/arm/linux-eabi.h
+@@ -65,8 +65,8 @@
+    GLIBC_DYNAMIC_LINKER_DEFAULT and TARGET_DEFAULT_FLOAT_ABI.  */
+ 
+ #undef  GLIBC_DYNAMIC_LINKER
+-#define GLIBC_DYNAMIC_LINKER_SOFT_FLOAT "/lib/ld-linux.so.3"
+-#define GLIBC_DYNAMIC_LINKER_HARD_FLOAT "/lib/ld-linux-armhf.so.3"
++#define GLIBC_DYNAMIC_LINKER_SOFT_FLOAT SYSTEMLIBS_DIR "ld-linux.so.3"
++#define GLIBC_DYNAMIC_LINKER_HARD_FLOAT SYSTEMLIBS_DIR "ld-linux-armhf.so.3"
+ #define GLIBC_DYNAMIC_LINKER_DEFAULT GLIBC_DYNAMIC_LINKER_SOFT_FLOAT
+ 
+ #define GLIBC_DYNAMIC_LINKER \
+unchanged:
+--- a/gcc/config/arm/linux-elf.h
++++ b/gcc/config/arm/linux-elf.h
+@@ -60,7 +60,7 @@
+ 
+ #define LIBGCC_SPEC "%{mfloat-abi=soft*:-lfloat} -lgcc"
+ 
+-#define GLIBC_DYNAMIC_LINKER "/lib/ld-linux.so.2"
++#define GLIBC_DYNAMIC_LINKER SYSTEMLIBS_DIR "ld-linux.so.2"
+ 
+ #define LINUX_TARGET_LINK_SPEC  "%{h*} \
+    %{static:-Bstatic} \
+unchanged:
+--- a/gcc/config/i386/linux.h
++++ b/gcc/config/i386/linux.h
+@@ -20,7 +20,7 @@ along with GCC; see the file COPYING3.  If not see
+ <http://www.gnu.org/licenses/>.  */
+ 
+ #define GNU_USER_LINK_EMULATION "elf_i386"
+-#define GLIBC_DYNAMIC_LINKER "/lib/ld-linux.so.2"
++#define GLIBC_DYNAMIC_LINKER SYSTEMLIBS_DIR "ld-linux.so.2"
+ 
+ #undef MUSL_DYNAMIC_LINKER
+ #define MUSL_DYNAMIC_LINKER "/lib/ld-musl-i386.so.1"
+unchanged:
+--- a/gcc/config/i386/linux64.h
++++ b/gcc/config/i386/linux64.h
+@@ -27,9 +27,9 @@ see the files COPYING3 and COPYING.RUNTIME respectively.  If not, see
+ #define GNU_USER_LINK_EMULATION64 "elf_x86_64"
+ #define GNU_USER_LINK_EMULATIONX32 "elf32_x86_64"
+ 
+-#define GLIBC_DYNAMIC_LINKER32 "/lib/ld-linux.so.2"
+-#define GLIBC_DYNAMIC_LINKER64 "/lib64/ld-linux-x86-64.so.2"
+-#define GLIBC_DYNAMIC_LINKERX32 "/libx32/ld-linux-x32.so.2"
++#define GLIBC_DYNAMIC_LINKER32 SYSTEMLIBS_DIR "ld-linux.so.2"
++#define GLIBC_DYNAMIC_LINKER64 SYSTEMLIBS_DIR "ld-linux-x86-64.so.2"
++#define GLIBC_DYNAMIC_LINKERX32 SYSTEMLIBS_DIR "ld-linux-x32.so.2"
+ 
+ #undef MUSL_DYNAMIC_LINKER32
+ #define MUSL_DYNAMIC_LINKER32 "/lib/ld-musl-i386.so.1"
+unchanged:
+--- a/gcc/config/linux.h
++++ b/gcc/config/linux.h
+@@ -94,10 +94,10 @@ see the files COPYING3 and COPYING.RUNTIME respectively.  If not, see
+    GLIBC_DYNAMIC_LINKER must be defined for each target using them, or
+    GLIBC_DYNAMIC_LINKER32 and GLIBC_DYNAMIC_LINKER64 for targets
+    supporting both 32-bit and 64-bit compilation.  */
+-#define UCLIBC_DYNAMIC_LINKER "/lib/ld-uClibc.so.0"
+-#define UCLIBC_DYNAMIC_LINKER32 "/lib/ld-uClibc.so.0"
+-#define UCLIBC_DYNAMIC_LINKER64 "/lib/ld64-uClibc.so.0"
+-#define UCLIBC_DYNAMIC_LINKERX32 "/lib/ldx32-uClibc.so.0"
++#define UCLIBC_DYNAMIC_LINKER SYSTEMLIBS_DIR "ld-uClibc.so.0"
++#define UCLIBC_DYNAMIC_LINKER32 SYSTEMLIBS_DIR "ld-uClibc.so.0"
++#define UCLIBC_DYNAMIC_LINKER64 SYSTEMLIBS_DIR "ld64-uClibc.so.0"
++#define UCLIBC_DYNAMIC_LINKERX32 SYSTEMLIBS_DIR "ldx32-uClibc.so.0"
+ #define BIONIC_DYNAMIC_LINKER "/system/bin/linker"
+ #define BIONIC_DYNAMIC_LINKER32 "/system/bin/linker"
+ #define BIONIC_DYNAMIC_LINKER64 "/system/bin/linker64"
+unchanged:
+--- a/gcc/config/mips/linux.h
++++ b/gcc/config/mips/linux.h
+@@ -22,20 +22,20 @@ along with GCC; see the file COPYING3.  If not see
+ #define GNU_USER_LINK_EMULATIONN32 "elf32%{EB:b}%{EL:l}tsmipn32"
+ 
+ #define GLIBC_DYNAMIC_LINKER32 \
+-  "%{mnan=2008:/lib/ld-linux-mipsn8.so.1;:/lib/ld.so.1}"
++  "%{mnan=2008:" SYSTEMLIBS_DIR "ld-linux-mipsn8.so.1;:" SYSTEMLIBS_DIR "ld.so.1}"
+ #define GLIBC_DYNAMIC_LINKER64 \
+-  "%{mnan=2008:/lib64/ld-linux-mipsn8.so.1;:/lib64/ld.so.1}"
++  "%{mnan=2008:" SYSTEMLIBS_DIR "ld-linux-mipsn8.so.1;:" SYSTEMLIBS_DIR "ld.so.1}"
+ #define GLIBC_DYNAMIC_LINKERN32 \
+-  "%{mnan=2008:/lib32/ld-linux-mipsn8.so.1;:/lib32/ld.so.1}"
++  "%{mnan=2008:" SYSTEMLIBS_DIR "ld-linux-mipsn8.so.1;:" SYSTEMLIBS_DIR "ld.so.1}"
+ 
+ #undef UCLIBC_DYNAMIC_LINKER32
+ #define UCLIBC_DYNAMIC_LINKER32 \
+-  "%{mnan=2008:/lib/ld-uClibc-mipsn8.so.0;:/lib/ld-uClibc.so.0}"
++  "%{mnan=2008:" SYSTEMLIBS_DIR "ld-uClibc-mipsn8.so.0;:" SYSTEMLIBS_DIR "ld-uClibc.so.0}"
+ #undef UCLIBC_DYNAMIC_LINKER64
+ #define UCLIBC_DYNAMIC_LINKER64 \
+-  "%{mnan=2008:/lib/ld64-uClibc-mipsn8.so.0;:/lib/ld64-uClibc.so.0}"
++  "%{mnan=2008:" SYSTEMLIBS_DIR "ld64-uClibc-mipsn8.so.0;:" SYSTEMLIBS_DIR "ld64-uClibc.so.0}"
+ #define UCLIBC_DYNAMIC_LINKERN32 \
+-  "%{mnan=2008:/lib32/ld-uClibc-mipsn8.so.0;:/lib32/ld-uClibc.so.0}"
++  "%{mnan=2008:" SYSTEMLIBS_DIR "ld-uClibc-mipsn8.so.0;:" SYSTEMLIBS_DIR "ld-uClibc.so.0}"
+ 
+ #undef MUSL_DYNAMIC_LINKER32
+ #define MUSL_DYNAMIC_LINKER32 \
+unchanged:
+--- a/gcc/config/riscv/linux.h
++++ b/gcc/config/riscv/linux.h
+@@ -22,7 +22,7 @@ along with GCC; see the file COPYING3.  If not see
+     GNU_USER_TARGET_OS_CPP_BUILTINS();				\
+   } while (0)
+ 
+-#define GLIBC_DYNAMIC_LINKER "/lib/ld-linux-riscv" XLEN_SPEC "-" ABI_SPEC ".so.1"
++#define GLIBC_DYNAMIC_LINKER SYSTEMLIBS_DIR "ld-linux-riscv" XLEN_SPEC "-" ABI_SPEC ".so.1"
+ 
+ #define MUSL_ABI_SUFFIX \
+   "%{mabi=ilp32:-sf}" \
+unchanged:
+--- a/gcc/config/rs6000/linux64.h
++++ b/gcc/config/rs6000/linux64.h
+@@ -336,24 +336,19 @@ extern int dot_symbols;
+ #undef	LINK_OS_DEFAULT_SPEC
+ #define LINK_OS_DEFAULT_SPEC "%(link_os_linux)"
+ 
+-#define GLIBC_DYNAMIC_LINKER32 "%(dynamic_linker_prefix)/lib/ld.so.1"
+-
++#define GLIBC_DYNAMIC_LINKER32 SYSTEMLIBS_DIR "ld.so.1"
+ #ifdef LINUX64_DEFAULT_ABI_ELFv2
+-#define GLIBC_DYNAMIC_LINKER64 \
+-"%{mabi=elfv1:%(dynamic_linker_prefix)/lib64/ld64.so.1;" \
+-":%(dynamic_linker_prefix)/lib64/ld64.so.2}"
++#define GLIBC_DYNAMIC_LINKER64 "%{mabi=elfv1:" SYSTEMLIBS_DIR "ld64.so.1;:" SYSTEMLIBS_DIR "ld64.so.2}"
+ #else
+-#define GLIBC_DYNAMIC_LINKER64 \
+-"%{mabi=elfv2:%(dynamic_linker_prefix)/lib64/ld64.so.2;" \
+-":%(dynamic_linker_prefix)/lib64/ld64.so.1}"
++#define GLIBC_DYNAMIC_LINKER64 "%{mabi=elfv2:" SYSTEMLIBS_DIR "ld64.so.2;:" SYSTEMLIBS_DIR "ld64.so.1}"
+ #endif
+ 
+ #undef MUSL_DYNAMIC_LINKER32
+ #define MUSL_DYNAMIC_LINKER32 \
+-  "/lib/ld-musl-powerpc" MUSL_DYNAMIC_LINKER_E "%{msoft-float:-sf}.so.1"
++  SYSTEMLIBS_DIR "ld-musl-powerpc" MUSL_DYNAMIC_LINKER_E "%{msoft-float:-sf}.so.1"
+ #undef MUSL_DYNAMIC_LINKER64
+ #define MUSL_DYNAMIC_LINKER64 \
+-  "/lib/ld-musl-powerpc64" MUSL_DYNAMIC_LINKER_E "%{msoft-float:-sf}.so.1"
++  SYSTEMLIBS_DIR "ld-musl-powerpc64" MUSL_DYNAMIC_LINKER_E "%{msoft-float:-sf}.so.1"
+ 
+ #undef  DEFAULT_ASM_ENDIAN
+ #if (TARGET_DEFAULT & MASK_LITTLE_ENDIAN)
+unchanged:
+--- a/gcc/config/sh/linux.h
++++ b/gcc/config/sh/linux.h
+@@ -64,7 +64,7 @@ along with GCC; see the file COPYING3.  If not see
+   "/lib/ld-musl-sh" MUSL_DYNAMIC_LINKER_E MUSL_DYNAMIC_LINKER_FP \
+   "%{mfdpic:-fdpic}.so.1"
+ 
+-#define GLIBC_DYNAMIC_LINKER "/lib/ld-linux.so.2"
++#define GLIBC_DYNAMIC_LINKER SYSTEMLIBS_DIR "ld-linux.so.2"
+ 
+ #undef SUBTARGET_LINK_EMUL_SUFFIX
+ #define SUBTARGET_LINK_EMUL_SUFFIX "%{mfdpic:_fd;:_linux}"
+unchanged:
+--- a/gcc/config/sparc/linux.h
++++ b/gcc/config/sparc/linux.h
+@@ -78,7 +78,7 @@ extern const char *host_detect_local_cpu (int argc, const char **argv);
+    When the -shared link option is used a final link is not being
+    done.  */
+ 
+-#define GLIBC_DYNAMIC_LINKER "/lib/ld-linux.so.2"
++#define GLIBC_DYNAMIC_LINKER SYSTEMLIBS_DIR "ld-linux.so.2"
+ 
+ #undef  LINK_SPEC
+ #define LINK_SPEC "-m elf32_sparc %{shared:-shared} \
+unchanged:
+--- a/gcc/config/sparc/linux64.h
++++ b/gcc/config/sparc/linux64.h
+@@ -78,8 +78,8 @@ along with GCC; see the file COPYING3.  If not see
+    When the -shared link option is used a final link is not being
+    done.  */
+ 
+-#define GLIBC_DYNAMIC_LINKER32 "/lib/ld-linux.so.2"
+-#define GLIBC_DYNAMIC_LINKER64 "/lib64/ld-linux.so.2"
++#define GLIBC_DYNAMIC_LINKER32 SYSTEMLIBS_DIR "ld-linux.so.2"
++#define GLIBC_DYNAMIC_LINKER64 SYSTEMLIBS_DIR "ld-linux.so.2"
+ 
+ #ifdef SPARC_BI_ARCH
+ 
+diff -u b/gcc/config/aarch64/aarch64-linux.h b/gcc/config/aarch64/aarch64-linux.h
+--- b/gcc/config/aarch64/aarch64-linux.h
++++ b/gcc/config/aarch64/aarch64-linux.h
+@@ -21,10 +21,10 @@
+ #ifndef GCC_AARCH64_LINUX_H
+ #define GCC_AARCH64_LINUX_H
+ 
+-#define GLIBC_DYNAMIC_LINKER "/lib/ld-linux-aarch64%{mbig-endian:_be}%{mabi=ilp32:_ilp32}.so.1"
++#define GLIBC_DYNAMIC_LINKER  SYSTEMLIBS_DIR "ld-linux-aarch64%{mbig-endian:_be}%{mabi=ilp32:_ilp32}.so.1"
+ 
+ #undef MUSL_DYNAMIC_LINKER
+-#define MUSL_DYNAMIC_LINKER "/lib/ld-musl-aarch64%{mbig-endian:_be}%{mabi=ilp32:_ilp32}.so.1"
++#define MUSL_DYNAMIC_LINKER  SYSTEMLIBS_DIR "ld-musl-aarch64%{mbig-endian:_be}%{mabi=ilp32:_ilp32}.so.1"
+ 
+ #undef  ASAN_CC1_SPEC
+ #define ASAN_CC1_SPEC "%{%:sanitize(address):-funwind-tables}"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0012-gcc-Fix-argument-list-too-long-error.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0012-gcc-Fix-argument-list-too-long-error.patch
new file mode 100644
index 0000000..ebee30b
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0012-gcc-Fix-argument-list-too-long-error.patch
@@ -0,0 +1,38 @@
+From: Richard Purdie <richard.purdie@linuxfoundation.org>
+Subject: [PATCH 2/5] gcc: Fix "argument list too long" from install-plugins
+
+When building in longer build paths (200+ characters), the
+"echo $(PLUGIN_HEADERS)" from the install-plugins target would cause an
+"argument list too long error" on some systems.
+
+Avoid this by calling make's sort function on the list which removes
+duplicates and stops the overflow from reaching the echo command.
+The original sort is left to handle the the .h and .def files.
+
+2021-10-26 Richard Purdie <richard.purdie@linuxfoundation.org>
+
+gcc/ChangeLog:
+
+    * Makefile.in: Fix "argument list too long" from install-plugins
+
+Signed-off-by: Richard Purdie <richard.purdie@linuxfoundation.org>
+
+Submitted [https://gcc.gnu.org/pipermail/gcc-patches/2021-October/582722.html]
+Upstream-Status: Backport [https://gcc.gnu.org/git/?p=gcc.git;a=commit;h=66e157188bd2f789809e17e85f917534c9381599]
+---
+ gcc/Makefile.in | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+Index: gcc-11.2.0/gcc/Makefile.in
+===================================================================
+--- gcc-11.2.0.orig/gcc/Makefile.in
++++ gcc-11.2.0/gcc/Makefile.in
+@@ -3678,7 +3678,7 @@ install-plugin: installdirs lang.install
+ # We keep the directory structure for files in config, common/config or
+ # c-family and .def files. All other files are flattened to a single directory.
+ 	$(mkinstalldirs) $(DESTDIR)$(plugin_includedir)
+-	headers=`echo $(PLUGIN_HEADERS) $$(cd $(srcdir); echo *.h *.def) | tr ' ' '\012' | sort -u`; \
++	headers=`echo $(sort $(PLUGIN_HEADERS)) $$(cd $(srcdir); echo *.h *.def) | tr ' ' '\012' | sort -u`; \
+ 	srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*|]/\\\\&/g'`; \
+ 	for file in $$headers; do \
+ 	  if [ -f $$file ] ; then \
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0014-libtool.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0014-libtool.patch
new file mode 100644
index 0000000..f2b8924
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0014-libtool.patch
@@ -0,0 +1,39 @@
+From a5b984db9669cf38a7bd76c3fcdb41c086f740db Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Fri, 29 Mar 2013 09:29:11 +0400
+Subject: [PATCH] libtool
+
+libstdc++ from gcc-runtime gets created with -rpath=/usr/lib/../lib for qemux86-64
+when running on am x86_64 build host.
+
+This patch stops this speading to libdir in the libstdc++.la file within libtool.
+Arguably, it shouldn't be passing this into libtool in the first place but
+for now this resolves the nastiest problems this causes.
+
+func_normal_abspath would resolve an empty path to `pwd` so we need
+to filter the zero case.
+
+RP 2012/8/24
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+
+Upstream-Status: Pending
+---
+ ltmain.sh | 4 ++++
+ 1 file changed, 4 insertions(+)
+
+diff --git a/ltmain.sh b/ltmain.sh
+index 70990740b6c..ee938056bef 100644
+--- a/ltmain.sh
++++ b/ltmain.sh
+@@ -6359,6 +6359,10 @@ func_mode_link ()
+ 	func_warning "ignoring multiple \`-rpath's for a libtool library"
+ 
+       install_libdir="$1"
++      if test -n "$install_libdir"; then
++	func_normal_abspath "$install_libdir"
++	install_libdir=$func_normal_abspath_result
++      fi
+ 
+       oldlibs=
+       if test -z "$rpath"; then
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0015-gcc-armv4-pass-fix-v4bx-to-linker-to-support-EABI.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0015-gcc-armv4-pass-fix-v4bx-to-linker-to-support-EABI.patch
new file mode 100644
index 0000000..1ccf39c
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0015-gcc-armv4-pass-fix-v4bx-to-linker-to-support-EABI.patch
@@ -0,0 +1,40 @@
+From 0ae7090dec149cb2ff0c87eef87c043d4000ece1 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Fri, 29 Mar 2013 09:30:32 +0400
+Subject: [PATCH] gcc: armv4: pass fix-v4bx to linker to support EABI.
+
+The LINK_SPEC for linux gets overwritten by linux-eabi.h which
+means the value of TARGET_FIX_V4BX_SPEC gets lost and as a result
+the option is not passed to linker when chosing march=armv4
+This patch redefines this in linux-eabi.h and reinserts it
+for eabi defaulting toolchains.
+
+We might want to send it upstream.
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+
+Upstream-Status: Pending
+---
+ gcc/config/arm/linux-eabi.h | 6 +++++-
+ 1 file changed, 5 insertions(+), 1 deletion(-)
+
+diff --git a/gcc/config/arm/linux-eabi.h b/gcc/config/arm/linux-eabi.h
+index 6bd95855827..77befab5da8 100644
+--- a/gcc/config/arm/linux-eabi.h
++++ b/gcc/config/arm/linux-eabi.h
+@@ -91,10 +91,14 @@
+ #define MUSL_DYNAMIC_LINKER \
+   "/lib/ld-musl-arm" MUSL_DYNAMIC_LINKER_E "%{mfloat-abi=hard:hf}%{mfdpic:-fdpic}.so.1"
+ 
++/* For armv4 we pass --fix-v4bx to linker to support EABI */
++#undef TARGET_FIX_V4BX_SPEC
++#define TARGET_FIX_V4BX_SPEC "%{mcpu=arm8|mcpu=arm810|mcpu=strongarm*|march=armv4: --fix-v4bx}"
++
+ /* At this point, bpabi.h will have clobbered LINK_SPEC.  We want to
+    use the GNU/Linux version, not the generic BPABI version.  */
+ #undef  LINK_SPEC
+-#define LINK_SPEC EABI_LINK_SPEC					\
++#define LINK_SPEC TARGET_FIX_V4BX_SPEC EABI_LINK_SPEC			\
+   LINUX_OR_ANDROID_LD (LINUX_TARGET_LINK_SPEC,				\
+ 		       LINUX_TARGET_LINK_SPEC " " ANDROID_LINK_SPEC)
+ 
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0016-Use-the-multilib-config-files-from-B-instead-of-usin.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0016-Use-the-multilib-config-files-from-B-instead-of-usin.patch
new file mode 100644
index 0000000..bde7ec6
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0016-Use-the-multilib-config-files-from-B-instead-of-usin.patch
@@ -0,0 +1,99 @@
+From 5ce8aa4615a3816d12c43752323154744517ae9f Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Fri, 29 Mar 2013 09:33:04 +0400
+Subject: [PATCH] Use the multilib config files from ${B} instead of using the
+ ones from ${S}
+
+Use the multilib config files from ${B} instead of using the ones from ${S}
+so that the source can be shared between gcc-cross-initial,
+gcc-cross-intermediate, gcc-cross, gcc-runtime, and also the sdk build.
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+Signed-off-by: Constantin Musca <constantinx.musca@intel.com>
+
+Upstream-Status: Inappropriate [configuration]
+---
+ gcc/configure    | 22 ++++++++++++++++++----
+ gcc/configure.ac | 22 ++++++++++++++++++----
+ 2 files changed, 36 insertions(+), 8 deletions(-)
+
+diff --git a/gcc/configure b/gcc/configure
+index 5160917d73e..e663052cad2 100755
+--- a/gcc/configure
++++ b/gcc/configure
+@@ -13070,10 +13070,20 @@ done
+ tmake_file_=
+ for f in ${tmake_file}
+ do
+-	if test -f ${srcdir}/config/$f
+-	then
+-		tmake_file_="${tmake_file_} \$(srcdir)/config/$f"
+-	fi
++  case $f in
++    */t-linux64 )
++       if test -f ./config/$f
++       then
++         tmake_file_="${tmake_file_} ./config/$f"
++       fi
++       ;;
++    * )
++       if test -f ${srcdir}/config/$f
++       then
++         tmake_file_="${tmake_file_} \$(srcdir)/config/$f"
++       fi
++       ;;
++  esac
+ done
+ tmake_file="${tmake_file_}${omp_device_property_tmake_file}"
+ 
+@@ -13084,6 +13094,10 @@ tm_file_list="options.h"
+ tm_include_list="options.h insn-constants.h"
+ for f in $tm_file; do
+   case $f in
++    */linux64.h )
++       tm_file_list="${tm_file_list} ./config/$f"
++       tm_include_list="${tm_include_list} ./config/$f"
++       ;;
+     ./* )
+        f=`echo $f | sed 's/^..//'`
+        tm_file_list="${tm_file_list} $f"
+diff --git a/gcc/configure.ac b/gcc/configure.ac
+index 50f78308e8a..2b84875b028 100644
+--- a/gcc/configure.ac
++++ b/gcc/configure.ac
+@@ -2243,10 +2243,20 @@ done
+ tmake_file_=
+ for f in ${tmake_file}
+ do
+-	if test -f ${srcdir}/config/$f
+-	then
+-		tmake_file_="${tmake_file_} \$(srcdir)/config/$f"
+-	fi
++  case $f in
++    */t-linux64 )
++       if test -f ./config/$f
++       then
++         tmake_file_="${tmake_file_} ./config/$f"
++       fi
++       ;;
++    * )
++       if test -f ${srcdir}/config/$f
++       then
++         tmake_file_="${tmake_file_} \$(srcdir)/config/$f"
++       fi
++       ;;
++  esac
+ done
+ tmake_file="${tmake_file_}${omp_device_property_tmake_file}"
+ 
+@@ -2257,6 +2267,10 @@ tm_file_list="options.h"
+ tm_include_list="options.h insn-constants.h"
+ for f in $tm_file; do
+   case $f in
++    */linux64.h )
++       tm_file_list="${tm_file_list} ./config/$f"
++       tm_include_list="${tm_include_list} ./config/$f"
++       ;;
+     ./* )
+        f=`echo $f | sed 's/^..//'`
+        tm_file_list="${tm_file_list} $f"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0017-Avoid-using-libdir-from-.la-which-usually-points-to-.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0017-Avoid-using-libdir-from-.la-which-usually-points-to-.patch
new file mode 100644
index 0000000..3e0a1b4
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0017-Avoid-using-libdir-from-.la-which-usually-points-to-.patch
@@ -0,0 +1,28 @@
+From 75a1ce0265ca123f74f17a40ad8bd8f26d9c2ab3 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Fri, 20 Feb 2015 09:39:38 +0000
+Subject: [PATCH] Avoid using libdir from .la which usually points to a host
+ path
+
+Upstream-Status: Inappropriate [embedded specific]
+
+Signed-off-by: Jonathan Liu <net147@gmail.com>
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ ltmain.sh | 3 +++
+ 1 file changed, 3 insertions(+)
+
+diff --git a/ltmain.sh b/ltmain.sh
+index ee938056bef..9ebc7e3d1e0 100644
+--- a/ltmain.sh
++++ b/ltmain.sh
+@@ -5628,6 +5628,9 @@ func_mode_link ()
+ 	    absdir="$abs_ladir"
+ 	    libdir="$abs_ladir"
+ 	  else
++	    # Instead of using libdir from .la which usually points to a host path,
++	    # use the path the .la is contained in.
++	    libdir="$abs_ladir"
+ 	    dir="$libdir"
+ 	    absdir="$libdir"
+ 	  fi
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0018-export-CPP.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0018-export-CPP.patch
new file mode 100644
index 0000000..86ab657
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0018-export-CPP.patch
@@ -0,0 +1,199 @@
+From: Richard Purdie <richard.purdie@linuxfoundation.org>
+Subject: [PATCH 1/5] Makefile.in: Ensure build CPP/CPPFLAGS is used for build targets
+
+During cross compiling, CPP is being set to the target compiler even for
+build targets. As an example, when building a cross compiler targetting
+mingw, the config.log for libiberty in
+build.x86_64-pokysdk-mingw32.i586-poky-linux/build-x86_64-linux/libiberty/config.log
+shows:
+
+configure:3786: checking how to run the C preprocessor
+configure:3856: result: x86_64-pokysdk-mingw32-gcc -E --sysroot=[sysroot]/x86_64-nativesdk-mingw32-pokysdk-mingw32
+configure:3876: x86_64-pokysdk-mingw32-gcc -E --sysroot=[sysroot]/x86_64-nativesdk-mingw32-pokysdk-mingw32 conftest.c
+configure:3876: $? = 0
+
+This is libiberty being built for the build environment, not the target one
+(i.e. in build-x86_64-linux). As such it should be using the build environment's
+gcc and not the target one. In the mingw case the system headers are quite
+different leading to build failures related to not being able to include a
+process.h file for pem-unix.c.
+
+Further analysis shows the same issue occuring for CPPFLAGS too.
+
+Fix this by adding support for CPP_FOR_BUILD and CPPFLAGS_FOR_BUILD which
+for example, avoids mixing the mingw headers for host binaries on linux
+systems.
+
+2021-10-27 Richard Purdie <richard.purdie@linuxfoundation.org>
+
+ChangeLog:
+
+    * Makefile.tpl: Add CPP_FOR_BUILD and CPPFLAGS_FOR_BUILD support
+    * Makefile.in: Regenerate.
+    * configure: Regenerate.
+    * configure.ac: Add CPP_FOR_BUILD and CPPFLAGS_FOR_BUILD support
+
+gcc/ChangeLog:
+
+    * configure: Regenerate.
+    * configure.ac: Use CPPFLAGS_FOR_BUILD for GMPINC
+
+Signed-off-by: Richard Purdie <richard.purdie@linuxfoundation.org>
+
+Submitted [https://gcc.gnu.org/pipermail/gcc-patches/2021-October/582727.html]
+Upstream-Status: Backport [https://gcc.gnu.org/git/?p=gcc.git;a=commitdiff;h=84401ce5fb4ecab55decb472b168100e7593e01f]
+---
+ Makefile.in      | 6 ++++++
+ Makefile.tpl     | 6 ++++++
+ configure        | 4 ++++
+ configure.ac     | 4 ++++
+ gcc/configure    | 2 +-
+ gcc/configure.ac | 2 +-
+ 6 files changed, 22 insertions(+), 2 deletions(-)
+
+Index: gcc-11.2.0/Makefile.in
+===================================================================
+--- gcc-11.2.0.orig/Makefile.in
++++ gcc-11.2.0/Makefile.in
+@@ -151,6 +151,8 @@ BUILD_EXPORTS = \
+ 	CC="$(CC_FOR_BUILD)"; export CC; \
+ 	CFLAGS="$(CFLAGS_FOR_BUILD)"; export CFLAGS; \
+ 	CONFIG_SHELL="$(SHELL)"; export CONFIG_SHELL; \
++	CPP="$(CPP_FOR_BUILD)"; export CPP; \
++	CPPFLAGS="$(CPPFLAGS_FOR_BUILD)"; export CPPFLAGS; \
+ 	CXX="$(CXX_FOR_BUILD)"; export CXX; \
+ 	CXXFLAGS="$(CXXFLAGS_FOR_BUILD)"; export CXXFLAGS; \
+ 	GFORTRAN="$(GFORTRAN_FOR_BUILD)"; export GFORTRAN; \
+@@ -198,6 +200,8 @@ HOST_EXPORTS = \
+ 	AR="$(AR)"; export AR; \
+ 	AS="$(AS)"; export AS; \
+ 	CC_FOR_BUILD="$(CC_FOR_BUILD)"; export CC_FOR_BUILD; \
++	CPP_FOR_BUILD="$(CPP_FOR_BUILD)"; export CPP_FOR_BUILD; \
++	CPPFLAGS_FOR_BUILD="$(CPPFLAGS_FOR_BUILD)"; export CPPFLAGS_FOR_BUILD; \
+ 	CXX_FOR_BUILD="$(CXX_FOR_BUILD)"; export CXX_FOR_BUILD; \
+ 	DLLTOOL="$(DLLTOOL)"; export DLLTOOL; \
+ 	LD="$(LD)"; export LD; \
+@@ -353,6 +357,8 @@ AR_FOR_BUILD = @AR_FOR_BUILD@
+ AS_FOR_BUILD = @AS_FOR_BUILD@
+ CC_FOR_BUILD = @CC_FOR_BUILD@
+ CFLAGS_FOR_BUILD = @CFLAGS_FOR_BUILD@
++CPP_FOR_BUILD = @CPP_FOR_BUILD@
++CPPFLAGS_FOR_BUILD = @CPPFLAGS_FOR_BUILD@
+ CXXFLAGS_FOR_BUILD = @CXXFLAGS_FOR_BUILD@
+ CXX_FOR_BUILD = @CXX_FOR_BUILD@
+ DLLTOOL_FOR_BUILD = @DLLTOOL_FOR_BUILD@
+Index: gcc-11.2.0/Makefile.tpl
+===================================================================
+--- gcc-11.2.0.orig/Makefile.tpl
++++ gcc-11.2.0/Makefile.tpl
+@@ -154,6 +154,8 @@ BUILD_EXPORTS = \
+ 	CC="$(CC_FOR_BUILD)"; export CC; \
+ 	CFLAGS="$(CFLAGS_FOR_BUILD)"; export CFLAGS; \
+ 	CONFIG_SHELL="$(SHELL)"; export CONFIG_SHELL; \
++	CPP="$(CPP_FOR_BUILD)"; export CPP; \
++	CPPFLAGS="$(CPPFLAGS_FOR_BUILD)"; export CPPFLAGS; \
+ 	CXX="$(CXX_FOR_BUILD)"; export CXX; \
+ 	CXXFLAGS="$(CXXFLAGS_FOR_BUILD)"; export CXXFLAGS; \
+ 	GFORTRAN="$(GFORTRAN_FOR_BUILD)"; export GFORTRAN; \
+@@ -201,6 +203,8 @@ HOST_EXPORTS = \
+ 	AR="$(AR)"; export AR; \
+ 	AS="$(AS)"; export AS; \
+ 	CC_FOR_BUILD="$(CC_FOR_BUILD)"; export CC_FOR_BUILD; \
++	CPP_FOR_BUILD="$(CPP_FOR_BUILD)"; export CPP_FOR_BUILD; \
++	CPPFLAGS_FOR_BUILD="$(CPPFLAGS_FOR_BUILD)"; export CPPFLAGS_FOR_BUILD; \
+ 	CXX_FOR_BUILD="$(CXX_FOR_BUILD)"; export CXX_FOR_BUILD; \
+ 	DLLTOOL="$(DLLTOOL)"; export DLLTOOL; \
+ 	LD="$(LD)"; export LD; \
+@@ -356,6 +360,8 @@ AR_FOR_BUILD = @AR_FOR_BUILD@
+ AS_FOR_BUILD = @AS_FOR_BUILD@
+ CC_FOR_BUILD = @CC_FOR_BUILD@
+ CFLAGS_FOR_BUILD = @CFLAGS_FOR_BUILD@
++CPP_FOR_BUILD = @CPP_FOR_BUILD@
++CPPFLAGS_FOR_BUILD = @CPPFLAGS_FOR_BUILD@
+ CXXFLAGS_FOR_BUILD = @CXXFLAGS_FOR_BUILD@
+ CXX_FOR_BUILD = @CXX_FOR_BUILD@
+ DLLTOOL_FOR_BUILD = @DLLTOOL_FOR_BUILD@
+Index: gcc-11.2.0/configure
+===================================================================
+--- gcc-11.2.0.orig/configure
++++ gcc-11.2.0/configure
+@@ -652,6 +652,8 @@ GFORTRAN_FOR_BUILD
+ DLLTOOL_FOR_BUILD
+ CXX_FOR_BUILD
+ CXXFLAGS_FOR_BUILD
++CPPFLAGS_FOR_BUILD
++CPP_FOR_BUILD
+ CFLAGS_FOR_BUILD
+ CC_FOR_BUILD
+ AS_FOR_BUILD
+@@ -4092,6 +4094,7 @@ if test "${build}" != "${host}" ; then
+   AR_FOR_BUILD=${AR_FOR_BUILD-ar}
+   AS_FOR_BUILD=${AS_FOR_BUILD-as}
+   CC_FOR_BUILD=${CC_FOR_BUILD-gcc}
++  CPP_FOR_BUILD="${CPP_FOR_BUILD-\$(CPP)}"
+   CXX_FOR_BUILD=${CXX_FOR_BUILD-g++}
+   GFORTRAN_FOR_BUILD=${GFORTRAN_FOR_BUILD-gfortran}
+   GOC_FOR_BUILD=${GOC_FOR_BUILD-gccgo}
+@@ -9809,6 +9812,7 @@ esac
+ # our build compiler if desired.
+ if test x"${build}" = x"${host}" ; then
+   CFLAGS_FOR_BUILD=${CFLAGS_FOR_BUILD-${CFLAGS}}
++  CPPFLAGS_FOR_BUILD=${CPPFLAGS_FOR_BUILD-${CPPFLAGS}}
+   CXXFLAGS_FOR_BUILD=${CXXFLAGS_FOR_BUILD-${CXXFLAGS}}
+   LDFLAGS_FOR_BUILD=${LDFLAGS_FOR_BUILD-${LDFLAGS}}
+ fi
+Index: gcc-11.2.0/configure.ac
+===================================================================
+--- gcc-11.2.0.orig/configure.ac
++++ gcc-11.2.0/configure.ac
+@@ -1347,6 +1347,7 @@ if test "${build}" != "${host}" ; then
+   AR_FOR_BUILD=${AR_FOR_BUILD-ar}
+   AS_FOR_BUILD=${AS_FOR_BUILD-as}
+   CC_FOR_BUILD=${CC_FOR_BUILD-gcc}
++  CPP_FOR_BUILD="${CPP_FOR_BUILD-\$(CPP)}"
+   CXX_FOR_BUILD=${CXX_FOR_BUILD-g++}
+   GFORTRAN_FOR_BUILD=${GFORTRAN_FOR_BUILD-gfortran}
+   GOC_FOR_BUILD=${GOC_FOR_BUILD-gccgo}
+@@ -3321,6 +3322,7 @@ esac
+ # our build compiler if desired.
+ if test x"${build}" = x"${host}" ; then
+   CFLAGS_FOR_BUILD=${CFLAGS_FOR_BUILD-${CFLAGS}}
++  CPPFLAGS_FOR_BUILD=${CPPFLAGS_FOR_BUILD-${CPPFLAGS}}
+   CXXFLAGS_FOR_BUILD=${CXXFLAGS_FOR_BUILD-${CXXFLAGS}}
+   LDFLAGS_FOR_BUILD=${LDFLAGS_FOR_BUILD-${LDFLAGS}}
+ fi
+@@ -3387,6 +3389,8 @@ AC_SUBST(AR_FOR_BUILD)
+ AC_SUBST(AS_FOR_BUILD)
+ AC_SUBST(CC_FOR_BUILD)
+ AC_SUBST(CFLAGS_FOR_BUILD)
++AC_SUBST(CPP_FOR_BUILD)
++AC_SUBST(CPPFLAGS_FOR_BUILD)
+ AC_SUBST(CXXFLAGS_FOR_BUILD)
+ AC_SUBST(CXX_FOR_BUILD)
+ AC_SUBST(DLLTOOL_FOR_BUILD)
+Index: gcc-11.2.0/gcc/configure
+===================================================================
+--- gcc-11.2.0.orig/gcc/configure
++++ gcc-11.2.0/gcc/configure
+@@ -12699,7 +12699,7 @@ else
+ 	CC="${CC_FOR_BUILD}" CFLAGS="${CFLAGS_FOR_BUILD}" \
+ 	CXX="${CXX_FOR_BUILD}" CXXFLAGS="${CXXFLAGS_FOR_BUILD}" \
+ 	LD="${LD_FOR_BUILD}" LDFLAGS="${LDFLAGS_FOR_BUILD}" \
+-	GMPINC="" CPPFLAGS="${CPPFLAGS} -DGENERATOR_FILE" \
++	GMPINC="" CPPFLAGS="${CPPFLAGS_FOR_BUILD} -DGENERATOR_FILE" \
+ 	${realsrcdir}/configure \
+ 		--enable-languages=${enable_languages-all} \
+ 		${enable_obsolete+--enable-obsolete="$enable_obsolete"} \
+Index: gcc-11.2.0/gcc/configure.ac
+===================================================================
+--- gcc-11.2.0.orig/gcc/configure.ac
++++ gcc-11.2.0/gcc/configure.ac
+@@ -2023,7 +2023,7 @@ else
+ 	CC="${CC_FOR_BUILD}" CFLAGS="${CFLAGS_FOR_BUILD}" \
+ 	CXX="${CXX_FOR_BUILD}" CXXFLAGS="${CXXFLAGS_FOR_BUILD}" \
+ 	LD="${LD_FOR_BUILD}" LDFLAGS="${LDFLAGS_FOR_BUILD}" \
+-	GMPINC="" CPPFLAGS="${CPPFLAGS} -DGENERATOR_FILE" \
++	GMPINC="" CPPFLAGS="${CPPFLAGS_FOR_BUILD} -DGENERATOR_FILE" \
+ 	${realsrcdir}/configure \
+ 		--enable-languages=${enable_languages-all} \
+ 		${enable_obsolete+--enable-obsolete="$enable_obsolete"} \
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0019-Ensure-target-gcc-headers-can-be-included.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0019-Ensure-target-gcc-headers-can-be-included.patch
new file mode 100644
index 0000000..120d5a2
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0019-Ensure-target-gcc-headers-can-be-included.patch
@@ -0,0 +1,113 @@
+From 6f410ed8fb7eee11ba7a25634c2257666b98ef52 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Fri, 20 Feb 2015 10:25:11 +0000
+Subject: [PATCH] Ensure target gcc headers can be included
+
+There are a few headers installed as part of the OpenEmbedded
+gcc-runtime target (omp.h, ssp/*.h). Being installed from a recipe
+built for the target architecture, these are within the target
+sysroot and not cross/nativesdk; thus they weren't able to be
+found by gcc with the existing search paths. Add support for
+picking up these headers under the sysroot supplied on the gcc
+command line in order to resolve this.
+
+Extend target gcc headers search to musl too
+
+Upstream-Status: Pending
+
+Signed-off-by: Paul Eggleton <paul.eggleton@linux.intel.com>
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ gcc/Makefile.in           | 2 ++
+ gcc/config/linux.h        | 8 ++++++++
+ gcc/config/rs6000/sysv4.h | 8 ++++++++
+ gcc/cppdefault.c          | 4 ++++
+ 4 files changed, 22 insertions(+)
+
+diff --git a/gcc/Makefile.in b/gcc/Makefile.in
+index 59c45c81393..9b17d120aa1 100644
+--- a/gcc/Makefile.in
++++ b/gcc/Makefile.in
+@@ -630,6 +630,7 @@ libexecdir = @libexecdir@
+ 
+ # Directory in which the compiler finds libraries etc.
+ libsubdir = $(libdir)/gcc/$(real_target_noncanonical)/$(version)$(accel_dir_suffix)
++libsubdir_target = $(target_noncanonical)/$(version)
+ # Directory in which the compiler finds executables
+ libexecsubdir = $(libexecdir)/gcc/$(real_target_noncanonical)/$(version)$(accel_dir_suffix)
+ # Directory in which all plugin resources are installed
+@@ -3016,6 +3017,7 @@ CFLAGS-intl.o += -DLOCALEDIR=\"$(localedir)\"
+ 
+ PREPROCESSOR_DEFINES = \
+   -DGCC_INCLUDE_DIR=\"$(libsubdir)/include\" \
++  -DGCC_INCLUDE_SUBDIR_TARGET=\"$(libsubdir_target)/include\" \
+   -DFIXED_INCLUDE_DIR=\"$(libsubdir)/include-fixed\" \
+   -DGPLUSPLUS_INCLUDE_DIR=\"$(gcc_gxx_include_dir)\" \
+   -DGPLUSPLUS_INCLUDE_DIR_ADD_SYSROOT=$(gcc_gxx_include_dir_add_sysroot) \
+diff --git a/gcc/config/linux.h b/gcc/config/linux.h
+index 87efc5f69fe..b525bcd56b3 100644
+--- a/gcc/config/linux.h
++++ b/gcc/config/linux.h
+@@ -165,6 +165,13 @@ see the files COPYING3 and COPYING.RUNTIME respectively.  If not, see
+ #define INCLUDE_DEFAULTS_MUSL_TOOL
+ #endif
+ 
++#ifdef GCC_INCLUDE_SUBDIR_TARGET
++#define INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET		\
++    { STANDARD_STARTFILE_PREFIX_2 GCC_INCLUDE_SUBDIR_TARGET, "GCC", 0, 0, 1, 0},
++#else
++#define INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET
++#endif
++
+ #ifdef NATIVE_SYSTEM_HEADER_DIR
+ #define INCLUDE_DEFAULTS_MUSL_NATIVE			\
+     { NATIVE_SYSTEM_HEADER_DIR, 0, 0, 0, 1, 2 },	\
+@@ -191,6 +198,7 @@ see the files COPYING3 and COPYING.RUNTIME respectively.  If not, see
+     INCLUDE_DEFAULTS_MUSL_PREFIX			\
+     INCLUDE_DEFAULTS_MUSL_CROSS				\
+     INCLUDE_DEFAULTS_MUSL_TOOL				\
++    INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET			\
+     INCLUDE_DEFAULTS_MUSL_NATIVE			\
+     { GCC_INCLUDE_DIR, "GCC", 0, 1, 0, 0 },		\
+     { 0, 0, 0, 0, 0, 0 }				\
+diff --git a/gcc/config/rs6000/sysv4.h b/gcc/config/rs6000/sysv4.h
+index 510abe169c5..0c2bba5ea32 100644
+--- a/gcc/config/rs6000/sysv4.h
++++ b/gcc/config/rs6000/sysv4.h
+@@ -995,6 +995,13 @@ ncrtn.o%s"
+ #define INCLUDE_DEFAULTS_MUSL_TOOL
+ #endif
+ 
++#ifdef GCC_INCLUDE_SUBDIR_TARGET
++#define INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET            \
++    { STANDARD_STARTFILE_PREFIX_2 GCC_INCLUDE_SUBDIR_TARGET, "GCC", 0, 0, 1, 0},
++#else
++#define INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET
++#endif
++
+ #ifdef NATIVE_SYSTEM_HEADER_DIR
+ #define INCLUDE_DEFAULTS_MUSL_NATIVE			\
+     { NATIVE_SYSTEM_HEADER_DIR, 0, 0, 0, 1, 2 },	\
+@@ -1021,6 +1028,7 @@ ncrtn.o%s"
+     INCLUDE_DEFAULTS_MUSL_PREFIX			\
+     INCLUDE_DEFAULTS_MUSL_CROSS				\
+     INCLUDE_DEFAULTS_MUSL_TOOL				\
++    INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET			\
+     INCLUDE_DEFAULTS_MUSL_NATIVE			\
+     { GCC_INCLUDE_DIR, "GCC", 0, 1, 0, 0 },		\
+     { 0, 0, 0, 0, 0, 0 }				\
+diff --git a/gcc/cppdefault.c b/gcc/cppdefault.c
+index c503d14fc3f..d54d6ce0076 100644
+--- a/gcc/cppdefault.c
++++ b/gcc/cppdefault.c
+@@ -64,6 +64,10 @@ const struct default_include cpp_include_defaults[]
+     /* This is the dir for gcc's private headers.  */
+     { GCC_INCLUDE_DIR, "GCC", 0, 0, 0, 0 },
+ #endif
++#ifdef GCC_INCLUDE_SUBDIR_TARGET
++    /* This is the dir for gcc's private headers under the specified sysroot.  */
++    { STANDARD_STARTFILE_PREFIX_2 GCC_INCLUDE_SUBDIR_TARGET, "GCC", 0, 0, 1, 0 },
++#endif
+ #ifdef LOCAL_INCLUDE_DIR
+     /* /usr/local/include comes before the fixincluded header files.  */
+     { LOCAL_INCLUDE_DIR, 0, 0, 1, 1, 2 },
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0020-Don-t-search-host-directory-during-relink-if-inst_pr.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0020-Don-t-search-host-directory-during-relink-if-inst_pr.patch
new file mode 100644
index 0000000..bb2a367
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0020-Don-t-search-host-directory-during-relink-if-inst_pr.patch
@@ -0,0 +1,35 @@
+From cd09adb1e7f2a7e81a9f595a17c36f24911b90ac Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Tue, 3 Mar 2015 08:21:19 +0000
+Subject: [PATCH] Don't search host directory during "relink" if $inst_prefix
+ is provided
+
+http://lists.gnu.org/archive/html/libtool-patches/2011-01/msg00026.html
+
+Upstream-Status: Submitted
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ ltmain.sh | 5 +++--
+ 1 file changed, 3 insertions(+), 2 deletions(-)
+
+diff --git a/ltmain.sh b/ltmain.sh
+index 9ebc7e3d1e0..7ea79fa8be6 100644
+--- a/ltmain.sh
++++ b/ltmain.sh
+@@ -6004,12 +6004,13 @@ func_mode_link ()
+ 	      fi
+ 	    else
+ 	      # We cannot seem to hardcode it, guess we'll fake it.
++	      # Default if $libdir is not relative to the prefix:
+ 	      add_dir="-L$libdir"
+-	      # Try looking first in the location we're being installed to.
++
+ 	      if test -n "$inst_prefix_dir"; then
+ 		case $libdir in
+ 		  [\\/]*)
+-		    add_dir="$add_dir -L$inst_prefix_dir$libdir"
++		    add_dir="-L$inst_prefix_dir$libdir"
+ 		    ;;
+ 		esac
+ 	      fi
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0023-libcc1-fix-libcc1-s-install-path-and-rpath.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0023-libcc1-fix-libcc1-s-install-path-and-rpath.patch
new file mode 100644
index 0000000..fb4511c
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0023-libcc1-fix-libcc1-s-install-path-and-rpath.patch
@@ -0,0 +1,51 @@
+From 2cfc5ee38b3f5ec1d7965aae0991bcd48b5dc2e6 Mon Sep 17 00:00:00 2001
+From: Robert Yang <liezhi.yang@windriver.com>
+Date: Sun, 5 Jul 2015 20:25:18 -0700
+Subject: [PATCH] libcc1: fix libcc1's install path and rpath
+
+* Install libcc1.so and libcc1plugin.so into
+  $(libexecdir)/gcc/$(target_noncanonical)/$(gcc_version), as what we
+  had done to lto-plugin.
+* Fix bad RPATH iussue:
+  gcc-5.2.0: package gcc-plugins contains bad RPATH /patht/to/tmp/sysroots/qemux86-64/usr/lib64/../lib64 in file
+ /path/to/gcc/5.2.0-r0/packages-split/gcc-plugins/usr/lib64/gcc/x86_64-poky-linux/5.2.0/plugin/libcc1plugin.so.0.0.0
+ [rpaths]
+
+Upstream-Status: Inappropriate [OE configuration]
+
+Signed-off-by: Robert Yang <liezhi.yang@windriver.com>
+---
+ libcc1/Makefile.am | 4 ++--
+ libcc1/Makefile.in | 4 ++--
+ 2 files changed, 4 insertions(+), 4 deletions(-)
+
+diff --git a/libcc1/Makefile.am b/libcc1/Makefile.am
+index f148fdd7aa2..0b6eb8f2855 100644
+--- a/libcc1/Makefile.am
++++ b/libcc1/Makefile.am
+@@ -40,8 +40,8 @@ libiberty = $(if $(wildcard $(libiberty_noasan)),$(Wc)$(libiberty_noasan), \
+ 	    $(Wc)$(libiberty_normal)))
+ libiberty_dep = $(patsubst $(Wc)%,%,$(libiberty))
+ 
+-plugindir = $(libdir)/gcc/$(target_noncanonical)/$(gcc_version)/plugin
+-cc1libdir = $(libdir)/$(libsuffix)
++cc1libdir = $(libexecdir)/gcc/$(target_noncanonical)/$(gcc_version)
++plugindir = $(cc1libdir)
+ 
+ if ENABLE_PLUGIN
+ plugin_LTLIBRARIES = libcc1plugin.la libcp1plugin.la
+diff --git a/libcc1/Makefile.in b/libcc1/Makefile.in
+index 753d435c9cb..455627b0a3b 100644
+--- a/libcc1/Makefile.in
++++ b/libcc1/Makefile.in
+@@ -398,8 +398,8 @@ libiberty = $(if $(wildcard $(libiberty_noasan)),$(Wc)$(libiberty_noasan), \
+ 	    $(Wc)$(libiberty_normal)))
+ 
+ libiberty_dep = $(patsubst $(Wc)%,%,$(libiberty))
+-plugindir = $(libdir)/gcc/$(target_noncanonical)/$(gcc_version)/plugin
+-cc1libdir = $(libdir)/$(libsuffix)
++cc1libdir = $(libexecdir)/gcc/$(target_noncanonical)/$(gcc_version)
++plugindir = $(cc1libdir)
+ @ENABLE_PLUGIN_TRUE@plugin_LTLIBRARIES = libcc1plugin.la libcp1plugin.la
+ @ENABLE_PLUGIN_TRUE@cc1lib_LTLIBRARIES = libcc1.la
+ shared_source = callbacks.cc callbacks.hh connection.cc connection.hh \
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0024-handle-sysroot-support-for-nativesdk-gcc.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0024-handle-sysroot-support-for-nativesdk-gcc.patch
new file mode 100644
index 0000000..b1054fa
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0024-handle-sysroot-support-for-nativesdk-gcc.patch
@@ -0,0 +1,510 @@
+From 354682ad8f71f62643dcd83f64b51b5979615a0c Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Mon, 7 Dec 2015 23:39:54 +0000
+Subject: [PATCH] handle sysroot support for nativesdk-gcc
+
+Being able to build a nativesdk gcc is useful, particularly in cases
+where the host compiler may be of an incompatible version (or a 32
+bit compiler is needed).
+
+Sadly, building nativesdk-gcc is not straight forward. We install
+nativesdk-gcc into a relocatable location and this means that its
+library locations can change. "Normal" sysroot support doesn't help
+in this case since the values of paths like "libdir" change, not just
+base root directory of the system.
+
+In order to handle this we do two things:
+
+a) Add %r into spec file markup which can be used for injected paths
+   such as SYSTEMLIBS_DIR (see gcc_multilib_setup()).
+b) Add other paths which need relocation into a .gccrelocprefix section
+   which the relocation code will notice and adjust automatically.
+
+Upstream-Status: Inappropriate
+RP 2015/7/28
+
+Extend the gccrelocprefix support to musl config too, this ensures
+that gcc will get right bits in SDK installations
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+
+Added PREFIXVAR and EXEC_PREFIXVAR to support runtime relocation.  Without
+these as part of the gccrelocprefix the system can't do runtime relocation
+if the executable is moved.  (These paths were missed in the original
+implementation.)
+
+Signed-off-by: Mark Hatle <mark.hatle@kernel.crashing.org>
+---
+ gcc/c-family/c-opts.c     |  4 +--
+ gcc/config/linux.h        | 24 +++++++--------
+ gcc/config/rs6000/sysv4.h | 24 +++++++--------
+ gcc/cppdefault.c          | 63 ++++++++++++++++++++++++---------------
+ gcc/cppdefault.h          | 13 ++++----
+ gcc/gcc.c                 | 20 +++++++++----
+ gcc/incpath.c             | 12 ++++----
+ gcc/prefix.c              |  6 ++--
+ 8 files changed, 94 insertions(+), 72 deletions(-)
+
+diff --git a/gcc/c-family/c-opts.c b/gcc/c-family/c-opts.c
+index 89e05a4c551..5577383665d 100644
+--- a/gcc/c-family/c-opts.c
++++ b/gcc/c-family/c-opts.c
+@@ -1436,8 +1436,8 @@ add_prefixed_path (const char *suffix, incpath_kind chain)
+   size_t prefix_len, suffix_len;
+ 
+   suffix_len = strlen (suffix);
+-  prefix     = iprefix ? iprefix : cpp_GCC_INCLUDE_DIR;
+-  prefix_len = iprefix ? strlen (iprefix) : cpp_GCC_INCLUDE_DIR_len;
++  prefix     = iprefix ? iprefix : GCC_INCLUDE_DIRVAR;
++  prefix_len = iprefix ? strlen (iprefix) : strlen(GCC_INCLUDE_DIRVAR) - 7;
+ 
+   path = (char *) xmalloc (prefix_len + suffix_len + 1);
+   memcpy (path, prefix, prefix_len);
+diff --git a/gcc/config/linux.h b/gcc/config/linux.h
+index b525bcd56b3..ba02c013e30 100644
+--- a/gcc/config/linux.h
++++ b/gcc/config/linux.h
+@@ -129,53 +129,53 @@ see the files COPYING3 and COPYING.RUNTIME respectively.  If not, see
+  * Unfortunately, this is mostly duplicated from cppdefault.c */
+ #if DEFAULT_LIBC == LIBC_MUSL
+ #define INCLUDE_DEFAULTS_MUSL_GPP			\
+-    { GPLUSPLUS_INCLUDE_DIR, "G++", 1, 1,		\
++    { GPLUSPLUS_INCLUDE_DIRVAR, "G++", 1, 1,		\
+       GPLUSPLUS_INCLUDE_DIR_ADD_SYSROOT, 0 },		\
+-    { GPLUSPLUS_TOOL_INCLUDE_DIR, "G++", 1, 1,		\
++    { GPLUSPLUS_TOOL_INCLUDE_DIRVAR, "G++", 1, 1,	\
+       GPLUSPLUS_INCLUDE_DIR_ADD_SYSROOT, 1 },		\
+-    { GPLUSPLUS_BACKWARD_INCLUDE_DIR, "G++", 1, 1,	\
++    { GPLUSPLUS_BACKWARD_INCLUDE_DIRVAR, "G++", 1, 1,	\
+       GPLUSPLUS_INCLUDE_DIR_ADD_SYSROOT, 0 },
+ 
+ #ifdef LOCAL_INCLUDE_DIR
+ #define INCLUDE_DEFAULTS_MUSL_LOCAL			\
+-    { LOCAL_INCLUDE_DIR, 0, 0, 1, 1, 2 },		\
+-    { LOCAL_INCLUDE_DIR, 0, 0, 1, 1, 0 },
++    { LOCAL_INCLUDE_DIRVAR, 0, 0, 1, 1, 2 },		\
++    { LOCAL_INCLUDE_DIRVAR, 0, 0, 1, 1, 0 },
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_LOCAL
+ #endif
+ 
+ #ifdef PREFIX_INCLUDE_DIR
+ #define INCLUDE_DEFAULTS_MUSL_PREFIX			\
+-    { PREFIX_INCLUDE_DIR, 0, 0, 1, 0, 0},
++    { PREFIX_INCLUDE_DIRVAR, 0, 0, 1, 0, 0},
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_PREFIX
+ #endif
+ 
+ #ifdef CROSS_INCLUDE_DIR
+ #define INCLUDE_DEFAULTS_MUSL_CROSS			\
+-    { CROSS_INCLUDE_DIR, "GCC", 0, 0, 0, 0},
++    { CROSS_INCLUDE_DIRVAR, "GCC", 0, 0, 0, 0},
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_CROSS
+ #endif
+ 
+ #ifdef TOOL_INCLUDE_DIR
+ #define INCLUDE_DEFAULTS_MUSL_TOOL			\
+-    { TOOL_INCLUDE_DIR, "BINUTILS", 0, 1, 0, 0},
++    { TOOL_INCLUDE_DIRVAR, "BINUTILS", 0, 1, 0, 0},
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_TOOL
+ #endif
+ 
+ #ifdef GCC_INCLUDE_SUBDIR_TARGET
+ #define INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET		\
+-    { STANDARD_STARTFILE_PREFIX_2 GCC_INCLUDE_SUBDIR_TARGET, "GCC", 0, 0, 1, 0},
++    { STANDARD_STARTFILE_PREFIX_2VAR, "GCC", 0, 0, 1, 0},
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET
+ #endif
+ 
+ #ifdef NATIVE_SYSTEM_HEADER_DIR
+ #define INCLUDE_DEFAULTS_MUSL_NATIVE			\
+-    { NATIVE_SYSTEM_HEADER_DIR, 0, 0, 0, 1, 2 },	\
+-    { NATIVE_SYSTEM_HEADER_DIR, 0, 0, 0, 1, 0 },
++    { NATIVE_SYSTEM_HEADER_DIRVAR, 0, 0, 0, 1, 2 },	\
++    { NATIVE_SYSTEM_HEADER_DIRVAR, 0, 0, 0, 1, 0 },
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_NATIVE
+ #endif
+@@ -200,7 +200,7 @@ see the files COPYING3 and COPYING.RUNTIME respectively.  If not, see
+     INCLUDE_DEFAULTS_MUSL_TOOL				\
+     INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET			\
+     INCLUDE_DEFAULTS_MUSL_NATIVE			\
+-    { GCC_INCLUDE_DIR, "GCC", 0, 1, 0, 0 },		\
++    { GCC_INCLUDE_DIRVAR, "GCC", 0, 1, 0, 0 },		\
+     { 0, 0, 0, 0, 0, 0 }				\
+   }
+ #endif
+diff --git a/gcc/config/rs6000/sysv4.h b/gcc/config/rs6000/sysv4.h
+index 0c2bba5ea32..313a8de4417 100644
+--- a/gcc/config/rs6000/sysv4.h
++++ b/gcc/config/rs6000/sysv4.h
+@@ -959,53 +959,53 @@ ncrtn.o%s"
+ /* Include order changes for musl, same as in generic linux.h.  */
+ #if DEFAULT_LIBC == LIBC_MUSL
+ #define INCLUDE_DEFAULTS_MUSL_GPP			\
+-    { GPLUSPLUS_INCLUDE_DIR, "G++", 1, 1,		\
++    { GPLUSPLUS_INCLUDE_DIRVAR, "G++", 1, 1,		\
+       GPLUSPLUS_INCLUDE_DIR_ADD_SYSROOT, 0 },		\
+-    { GPLUSPLUS_TOOL_INCLUDE_DIR, "G++", 1, 1,		\
++    { GPLUSPLUS_TOOL_INCLUDE_DIRVAR, "G++", 1, 1,	\
+       GPLUSPLUS_INCLUDE_DIR_ADD_SYSROOT, 1 },		\
+-    { GPLUSPLUS_BACKWARD_INCLUDE_DIR, "G++", 1, 1,	\
++    { GPLUSPLUS_BACKWARD_INCLUDE_DIRVAR, "G++", 1, 1,	\
+       GPLUSPLUS_INCLUDE_DIR_ADD_SYSROOT, 0 },
+ 
+ #ifdef LOCAL_INCLUDE_DIR
+ #define INCLUDE_DEFAULTS_MUSL_LOCAL			\
+-    { LOCAL_INCLUDE_DIR, 0, 0, 1, 1, 2 },		\
+-    { LOCAL_INCLUDE_DIR, 0, 0, 1, 1, 0 },
++    { LOCAL_INCLUDE_DIRVAR, 0, 0, 1, 1, 2 },		\
++    { LOCAL_INCLUDE_DIRVAR, 0, 0, 1, 1, 0 },
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_LOCAL
+ #endif
+ 
+ #ifdef PREFIX_INCLUDE_DIR
+ #define INCLUDE_DEFAULTS_MUSL_PREFIX			\
+-    { PREFIX_INCLUDE_DIR, 0, 0, 1, 0, 0},
++    { PREFIX_INCLUDE_DIRVAR, 0, 0, 1, 0, 0},
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_PREFIX
+ #endif
+ 
+ #ifdef CROSS_INCLUDE_DIR
+ #define INCLUDE_DEFAULTS_MUSL_CROSS			\
+-    { CROSS_INCLUDE_DIR, "GCC", 0, 0, 0, 0},
++    { CROSS_INCLUDE_DIRVAR, "GCC", 0, 0, 0, 0},
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_CROSS
+ #endif
+ 
+ #ifdef TOOL_INCLUDE_DIR
+ #define INCLUDE_DEFAULTS_MUSL_TOOL			\
+-    { TOOL_INCLUDE_DIR, "BINUTILS", 0, 1, 0, 0},
++    { TOOL_INCLUDE_DIRVAR, "BINUTILS", 0, 1, 0, 0},
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_TOOL
+ #endif
+ 
+ #ifdef GCC_INCLUDE_SUBDIR_TARGET
+ #define INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET            \
+-    { STANDARD_STARTFILE_PREFIX_2 GCC_INCLUDE_SUBDIR_TARGET, "GCC", 0, 0, 1, 0},
++    { STANDARD_STARTFILE_PREFIX_2VAR, "GCC", 0, 0, 1, 0},
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET
+ #endif
+ 
+ #ifdef NATIVE_SYSTEM_HEADER_DIR
+ #define INCLUDE_DEFAULTS_MUSL_NATIVE			\
+-    { NATIVE_SYSTEM_HEADER_DIR, 0, 0, 0, 1, 2 },	\
+-    { NATIVE_SYSTEM_HEADER_DIR, 0, 0, 0, 1, 0 },
++    { NATIVE_SYSTEM_HEADER_DIRVAR, 0, 0, 0, 1, 2 },	\
++    { NATIVE_SYSTEM_HEADER_DIRVAR, 0, 0, 0, 1, 0 },
+ #else
+ #define INCLUDE_DEFAULTS_MUSL_NATIVE
+ #endif
+@@ -1030,7 +1030,7 @@ ncrtn.o%s"
+     INCLUDE_DEFAULTS_MUSL_TOOL				\
+     INCLUDE_DEFAULTS_MUSL_SUBDIR_TARGET			\
+     INCLUDE_DEFAULTS_MUSL_NATIVE			\
+-    { GCC_INCLUDE_DIR, "GCC", 0, 1, 0, 0 },		\
++    { GCC_INCLUDE_DIRVAR, "GCC", 0, 1, 0, 0 },		\
+     { 0, 0, 0, 0, 0, 0 }				\
+   }
+ #endif
+diff --git a/gcc/cppdefault.c b/gcc/cppdefault.c
+index d54d6ce0076..784a92a0c24 100644
+--- a/gcc/cppdefault.c
++++ b/gcc/cppdefault.c
+@@ -35,6 +35,30 @@
+ # undef CROSS_INCLUDE_DIR
+ #endif
+ 
++static char GPLUSPLUS_INCLUDE_DIRVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = GPLUSPLUS_INCLUDE_DIR;
++char GCC_INCLUDE_DIRVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = GCC_INCLUDE_DIR;
++static char GPLUSPLUS_TOOL_INCLUDE_DIRVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = GPLUSPLUS_TOOL_INCLUDE_DIR;
++static char GPLUSPLUS_BACKWARD_INCLUDE_DIRVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = GPLUSPLUS_BACKWARD_INCLUDE_DIR;
++static char STANDARD_STARTFILE_PREFIX_2VAR[4096] __attribute__ ((section (".gccrelocprefix"))) = STANDARD_STARTFILE_PREFIX_2 GCC_INCLUDE_SUBDIR_TARGET;
++#ifdef LOCAL_INCLUDE_DIR
++static char LOCAL_INCLUDE_DIRVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = LOCAL_INCLUDE_DIR;
++#endif
++#ifdef PREFIX_INCLUDE_DIR
++static char PREFIX_INCLUDE_DIRVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = PREFIX_INCLUDE_DIR;
++#endif
++#ifdef FIXED_INCLUDE_DIR
++static char FIXED_INCLUDE_DIRVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = FIXED_INCLUDE_DIR;
++#endif
++#ifdef CROSS_INCLUDE_DIR
++static char CROSS_INCLUDE_DIRVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = CROSS_INCLUDE_DIR;
++#endif
++#ifdef TOOL_INCLUDE_DIR
++static char TOOL_INCLUDE_DIRVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = TOOL_INCLUDE_DIR;
++#endif
++#ifdef NATIVE_SYSTEM_HEADER_DIR
++static char NATIVE_SYSTEM_HEADER_DIRVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = NATIVE_SYSTEM_HEADER_DIR;
++#endif
++
+ const struct default_include cpp_include_defaults[]
+ #ifdef INCLUDE_DEFAULTS
+ = INCLUDE_DEFAULTS;
+@@ -42,17 +66,17 @@ const struct default_include cpp_include_defaults[]
+ = {
+ #ifdef GPLUSPLUS_INCLUDE_DIR
+     /* Pick up GNU C++ generic include files.  */
+-    { GPLUSPLUS_INCLUDE_DIR, "G++", 1, 1,
++    { GPLUSPLUS_INCLUDE_DIRVAR, "G++", 1, 1,
+       GPLUSPLUS_INCLUDE_DIR_ADD_SYSROOT, 0 },
+ #endif
+ #ifdef GPLUSPLUS_TOOL_INCLUDE_DIR
+     /* Pick up GNU C++ target-dependent include files.  */
+-    { GPLUSPLUS_TOOL_INCLUDE_DIR, "G++", 1, 1,
++    { GPLUSPLUS_TOOL_INCLUDE_DIRVAR, "G++", 1, 1,
+       GPLUSPLUS_INCLUDE_DIR_ADD_SYSROOT, 1 },
+ #endif
+ #ifdef GPLUSPLUS_BACKWARD_INCLUDE_DIR
+     /* Pick up GNU C++ backward and deprecated include files.  */
+-    { GPLUSPLUS_BACKWARD_INCLUDE_DIR, "G++", 1, 1,
++    { GPLUSPLUS_BACKWARD_INCLUDE_DIRVAR, "G++", 1, 1,
+       GPLUSPLUS_INCLUDE_DIR_ADD_SYSROOT, 0 },
+ #endif
+ #ifdef GPLUSPLUS_LIBCXX_INCLUDE_DIR
+@@ -62,23 +86,23 @@ const struct default_include cpp_include_defaults[]
+ #endif
+ #ifdef GCC_INCLUDE_DIR
+     /* This is the dir for gcc's private headers.  */
+-    { GCC_INCLUDE_DIR, "GCC", 0, 0, 0, 0 },
++    { GCC_INCLUDE_DIRVAR, "GCC", 0, 0, 0, 0 },
+ #endif
+ #ifdef GCC_INCLUDE_SUBDIR_TARGET
+     /* This is the dir for gcc's private headers under the specified sysroot.  */
+-    { STANDARD_STARTFILE_PREFIX_2 GCC_INCLUDE_SUBDIR_TARGET, "GCC", 0, 0, 1, 0 },
++    { STANDARD_STARTFILE_PREFIX_2VAR, "GCC", 0, 0, 1, 0 },
+ #endif
+ #ifdef LOCAL_INCLUDE_DIR
+     /* /usr/local/include comes before the fixincluded header files.  */
+-    { LOCAL_INCLUDE_DIR, 0, 0, 1, 1, 2 },
+-    { LOCAL_INCLUDE_DIR, 0, 0, 1, 1, 0 },
++    { LOCAL_INCLUDE_DIRVAR, 0, 0, 1, 1, 2 },
++    { LOCAL_INCLUDE_DIRVAR, 0, 0, 1, 1, 0 },
+ #endif
+ #ifdef PREFIX_INCLUDE_DIR
+-    { PREFIX_INCLUDE_DIR, 0, 0, 1, 0, 0 },
++    { PREFIX_INCLUDE_DIRVAR, 0, 0, 1, 0, 0 },
+ #endif
+ #ifdef FIXED_INCLUDE_DIR
+     /* This is the dir for fixincludes.  */
+-    { FIXED_INCLUDE_DIR, "GCC", 0, 0, 0,
++    { FIXED_INCLUDE_DIRVAR, "GCC", 0, 0, 0,
+       /* A multilib suffix needs adding if different multilibs use
+ 	 different headers.  */
+ #ifdef SYSROOT_HEADERS_SUFFIX_SPEC
+@@ -90,33 +114,24 @@ const struct default_include cpp_include_defaults[]
+ #endif
+ #ifdef CROSS_INCLUDE_DIR
+     /* One place the target system's headers might be.  */
+-    { CROSS_INCLUDE_DIR, "GCC", 0, 0, 0, 0 },
++    { CROSS_INCLUDE_DIRVAR, "GCC", 0, 0, 0, 0 },
+ #endif
+ #ifdef TOOL_INCLUDE_DIR
+     /* Another place the target system's headers might be.  */
+-    { TOOL_INCLUDE_DIR, "BINUTILS", 0, 1, 0, 0 },
++    { TOOL_INCLUDE_DIRVAR, "BINUTILS", 0, 1, 0, 0 },
+ #endif
+ #ifdef NATIVE_SYSTEM_HEADER_DIR
+     /* /usr/include comes dead last.  */
+-    { NATIVE_SYSTEM_HEADER_DIR, NATIVE_SYSTEM_HEADER_COMPONENT, 0, 0, 1, 2 },
+-    { NATIVE_SYSTEM_HEADER_DIR, NATIVE_SYSTEM_HEADER_COMPONENT, 0, 0, 1, 0 },
++    { NATIVE_SYSTEM_HEADER_DIRVAR, NATIVE_SYSTEM_HEADER_COMPONENT, 0, 0, 1, 2 },
++    { NATIVE_SYSTEM_HEADER_DIRVAR, NATIVE_SYSTEM_HEADER_COMPONENT, 0, 0, 1, 0 },
+ #endif
+     { 0, 0, 0, 0, 0, 0 }
+   };
+ #endif /* no INCLUDE_DEFAULTS */
+ 
+-#ifdef GCC_INCLUDE_DIR
+-const char cpp_GCC_INCLUDE_DIR[] = GCC_INCLUDE_DIR;
+-const size_t cpp_GCC_INCLUDE_DIR_len = sizeof GCC_INCLUDE_DIR - 8;
+-#else
+-const char cpp_GCC_INCLUDE_DIR[] = "";
+-const size_t cpp_GCC_INCLUDE_DIR_len = 0;
+-#endif
+-
+ /* The configured prefix.  */
+-const char cpp_PREFIX[] = PREFIX;
+-const size_t cpp_PREFIX_len = sizeof PREFIX - 1;
+-const char cpp_EXEC_PREFIX[] = STANDARD_EXEC_PREFIX;
++char PREFIXVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = PREFIX;
++char EXEC_PREFIXVAR[4096] __attribute__ ((section (".gccrelocprefix"))) = STANDARD_EXEC_PREFIX;
+ 
+ /* This value is set by cpp_relocated at runtime */
+ const char *gcc_exec_prefix;
+diff --git a/gcc/cppdefault.h b/gcc/cppdefault.h
+index fd3c655db1c..20669ac427d 100644
+--- a/gcc/cppdefault.h
++++ b/gcc/cppdefault.h
+@@ -33,7 +33,8 @@
+ 
+ struct default_include
+ {
+-  const char *const fname;	/* The name of the directory.  */
++  const char *fname;     /* The name of the directory.  */
++
+   const char *const component;	/* The component containing the directory
+ 				   (see update_path in prefix.c) */
+   const char cplusplus;		/* When this is non-zero, we should only
+@@ -55,17 +56,13 @@ struct default_include
+ };
+ 
+ extern const struct default_include cpp_include_defaults[];
+-extern const char cpp_GCC_INCLUDE_DIR[];
+-extern const size_t cpp_GCC_INCLUDE_DIR_len;
++extern char GCC_INCLUDE_DIRVAR[] __attribute__ ((section (".gccrelocprefix")));
+ 
+ /* The configure-time prefix, i.e., the value supplied as the argument
+    to --prefix=.  */
+-extern const char cpp_PREFIX[];
++extern char PREFIXVAR[] __attribute__ ((section (".gccrelocprefix")));
+ /* The length of the configure-time prefix.  */
+-extern const size_t cpp_PREFIX_len;
+-/* The configure-time execution prefix.  This is typically the lib/gcc
+-   subdirectory of cpp_PREFIX.  */
+-extern const char cpp_EXEC_PREFIX[];
++extern char EXEC_PREFIXVAR[] __attribute__ ((section (".gccrelocprefix")));
+ /* The run-time execution prefix.  This is typically the lib/gcc
+    subdirectory of the actual installation.  */
+ extern const char *gcc_exec_prefix;
+diff --git a/gcc/gcc.c b/gcc/gcc.c
+index 8737bae5353..aa6fbe43965 100644
+--- a/gcc/gcc.c
++++ b/gcc/gcc.c
+@@ -252,6 +252,8 @@ FILE *report_times_to_file = NULL;
+ #endif
+ static const char *target_system_root = DEFAULT_TARGET_SYSTEM_ROOT;
+ 
++static char target_relocatable_prefix[4096] __attribute__ ((section (".gccrelocprefix"))) = SYSTEMLIBS_DIR;
++
+ /* Nonzero means pass the updated target_system_root to the compiler.  */
+ 
+ static int target_system_root_changed;
+@@ -568,6 +570,7 @@ or with constant text in a single argument.
+  %G     process LIBGCC_SPEC as a spec.
+  %R     Output the concatenation of target_system_root and
+         target_sysroot_suffix.
++ %r     Output the base path target_relocatable_prefix
+  %S     process STARTFILE_SPEC as a spec.  A capital S is actually used here.
+  %E     process ENDFILE_SPEC as a spec.  A capital E is actually used here.
+  %C     process CPP_SPEC as a spec.
+@@ -1621,10 +1624,10 @@ static const char *gcc_libexec_prefix;
+    gcc_exec_prefix is set because, in that case, we know where the
+    compiler has been installed, and use paths relative to that
+    location instead.  */
+-static const char *const standard_exec_prefix = STANDARD_EXEC_PREFIX;
+-static const char *const standard_libexec_prefix = STANDARD_LIBEXEC_PREFIX;
+-static const char *const standard_bindir_prefix = STANDARD_BINDIR_PREFIX;
+-static const char *const standard_startfile_prefix = STANDARD_STARTFILE_PREFIX;
++static char standard_exec_prefix[4096] __attribute__ ((section (".gccrelocprefix"))) = STANDARD_EXEC_PREFIX;
++static char standard_libexec_prefix[4096] __attribute__ ((section (".gccrelocprefix"))) = STANDARD_LIBEXEC_PREFIX;
++static char standard_bindir_prefix[4096] __attribute__ ((section (".gccrelocprefix"))) = STANDARD_BINDIR_PREFIX;
++static char *const standard_startfile_prefix = STANDARD_STARTFILE_PREFIX;
+ 
+ /* For native compilers, these are well-known paths containing
+    components that may be provided by the system.  For cross
+@@ -1632,9 +1635,9 @@ static const char *const standard_startfile_prefix = STANDARD_STARTFILE_PREFIX;
+ static const char *md_exec_prefix = MD_EXEC_PREFIX;
+ static const char *md_startfile_prefix = MD_STARTFILE_PREFIX;
+ static const char *md_startfile_prefix_1 = MD_STARTFILE_PREFIX_1;
+-static const char *const standard_startfile_prefix_1
++static char standard_startfile_prefix_1[4096] __attribute__ ((section (".gccrelocprefix")))
+   = STANDARD_STARTFILE_PREFIX_1;
+-static const char *const standard_startfile_prefix_2
++static char standard_startfile_prefix_2[4096] __attribute__ ((section (".gccrelocprefix")))
+   = STANDARD_STARTFILE_PREFIX_2;
+ 
+ /* A relative path to be used in finding the location of tools
+@@ -6564,6 +6567,11 @@ do_spec_1 (const char *spec, int inswitch, const char *soft_matched_part)
+ 	      }
+ 	    break;
+ 
++          case 'r':
++              obstack_grow (&obstack, target_relocatable_prefix,
++		      strlen (target_relocatable_prefix));
++            break;
++
+ 	  case 'S':
+ 	    value = do_spec_1 (startfile_spec, 0, NULL);
+ 	    if (value != 0)
+diff --git a/gcc/incpath.c b/gcc/incpath.c
+index fbfc0ce03b8..a82e543428b 100644
+--- a/gcc/incpath.c
++++ b/gcc/incpath.c
+@@ -131,7 +131,7 @@ add_standard_paths (const char *sysroot, const char *iprefix,
+   int relocated = cpp_relocated ();
+   size_t len;
+ 
+-  if (iprefix && (len = cpp_GCC_INCLUDE_DIR_len) != 0)
++  if (iprefix && (len = strlen(GCC_INCLUDE_DIRVAR) - 7) != 0)
+     {
+       /* Look for directories that start with the standard prefix.
+ 	 "Translate" them, i.e. replace /usr/local/lib/gcc... with
+@@ -146,7 +146,7 @@ add_standard_paths (const char *sysroot, const char *iprefix,
+ 		 now.  */
+ 	      if (sysroot && p->add_sysroot)
+ 		continue;
+-	      if (!filename_ncmp (p->fname, cpp_GCC_INCLUDE_DIR, len))
++	      if (!filename_ncmp (p->fname, GCC_INCLUDE_DIRVAR, len))
+ 		{
+ 		  char *str = concat (iprefix, p->fname + len, NULL);
+ 		  if (p->multilib == 1 && imultilib)
+@@ -187,7 +187,7 @@ add_standard_paths (const char *sysroot, const char *iprefix,
+ 	      free (sysroot_no_trailing_dir_separator);
+ 	    }
+ 	  else if (!p->add_sysroot && relocated
+-		   && !filename_ncmp (p->fname, cpp_PREFIX, cpp_PREFIX_len))
++		   && !filename_ncmp (p->fname, PREFIXVAR, strlen(PREFIXVAR)))
+ 	    {
+  	      static const char *relocated_prefix;
+ 	      char *ostr;
+@@ -204,12 +204,12 @@ add_standard_paths (const char *sysroot, const char *iprefix,
+ 		  dummy = concat (gcc_exec_prefix, "dummy", NULL);
+ 		  relocated_prefix
+ 		    = make_relative_prefix (dummy,
+-					    cpp_EXEC_PREFIX,
+-					    cpp_PREFIX);
++					    EXEC_PREFIXVAR,
++					    PREFIXVAR);
+ 		  free (dummy);
+ 		}
+ 	      ostr = concat (relocated_prefix,
+-			     p->fname + cpp_PREFIX_len,
++			     p->fname + strlen(PREFIXVAR),
+ 			     NULL);
+ 	      str = update_path (ostr, p->component);
+ 	      free (ostr);
+diff --git a/gcc/prefix.c b/gcc/prefix.c
+index 747c09de638..f728638dc65 100644
+--- a/gcc/prefix.c
++++ b/gcc/prefix.c
+@@ -72,7 +72,9 @@ License along with GCC; see the file COPYING3.  If not see
+ #include "prefix.h"
+ #include "common/common-target.h"
+ 
+-static const char *std_prefix = PREFIX;
++char PREFIXVAR1[4096] __attribute__ ((section (".gccrelocprefix"))) = PREFIX;
++
++static const char *std_prefix = PREFIXVAR1;
+ 
+ static const char *get_key_value (char *);
+ static char *translate_name (char *);
+@@ -212,7 +214,7 @@ translate_name (char *name)
+ 	prefix = getenv (key);
+ 
+       if (prefix == 0)
+-	prefix = PREFIX;
++	prefix = PREFIXVAR1;
+ 
+       /* We used to strip trailing DIR_SEPARATORs here, but that can
+ 	 sometimes yield a result with no separator when one was coded
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0025-Search-target-sysroot-gcc-version-specific-dirs-with.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0025-Search-target-sysroot-gcc-version-specific-dirs-with.patch
new file mode 100644
index 0000000..ff622b1
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0025-Search-target-sysroot-gcc-version-specific-dirs-with.patch
@@ -0,0 +1,99 @@
+From c55e24459370ad96577496ecd87475e3a9de7dad Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Mon, 7 Dec 2015 23:41:45 +0000
+Subject: [PATCH] Search target sysroot gcc version specific dirs with
+ multilib.
+
+We install the gcc libraries (such as crtbegin.p) into
+<sysroot><libdir>/<target-sys>/5.2.0/
+which is a default search path for GCC (aka multi_suffix in the
+code below). <target-sys> is 'machine' in gcc's terminology. We use
+these directories so that multiple gcc versions could in theory
+co-exist on target.
+
+We only want to build one gcc-cross-canadian per arch and have this work
+for all multilibs. <target-sys> can be handled by mapping the multilib
+<target-sys> to the one used by gcc-cross-canadian, e.g.
+mips64-polkmllib32-linux
+is symlinked to by mips64-poky-linux.
+
+The default gcc search path in the target sysroot for a "lib64" mutlilib
+is:
+
+<sysroot>/lib32/mips64-poky-linux/5.2.0/
+<sysroot>/lib32/../lib64/
+<sysroot>/usr/lib32/mips64-poky-linux/5.2.0/
+<sysroot>/usr/lib32/../lib64/
+<sysroot>/lib32/
+<sysroot>/usr/lib32/
+
+which means that the lib32 crtbegin.o will be found and the lib64 ones
+will not which leads to compiler failures.
+
+This patch injects a multilib version of that path first so the lib64
+binaries can be found first. With this change the search path becomes:
+
+<sysroot>/lib32/../lib64/mips64-poky-linux/5.2.0/
+<sysroot>/lib32/mips64-poky-linux/5.2.0/
+<sysroot>/lib32/../lib64/
+<sysroot>/usr/lib32/../lib64/mips64-poky-linux/5.2.0/
+<sysroot>/usr/lib32/mips64-poky-linux/5.2.0/
+<sysroot>/usr/lib32/../lib64/
+<sysroot>/lib32/
+<sysroot>/usr/lib32/
+
+Upstream-Status: Pending
+RP 2015/7/31
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ gcc/gcc.c | 29 ++++++++++++++++++++++++++++-
+ 1 file changed, 28 insertions(+), 1 deletion(-)
+
+diff --git a/gcc/gcc.c b/gcc/gcc.c
+index aa6fbe43965..f8a71a13826 100644
+--- a/gcc/gcc.c
++++ b/gcc/gcc.c
+@@ -2811,7 +2811,7 @@ for_each_path (const struct path_prefix *paths,
+       if (path == NULL)
+ 	{
+ 	  len = paths->max_len + extra_space + 1;
+-	  len += MAX (MAX (suffix_len, multi_os_dir_len), multiarch_len);
++	  len += MAX ((suffix_len + multi_os_dir_len), multiarch_len);
+ 	  path = XNEWVEC (char, len);
+ 	}
+ 
+@@ -2823,6 +2823,33 @@ for_each_path (const struct path_prefix *paths,
+ 	  /* Look first in MACHINE/VERSION subdirectory.  */
+ 	  if (!skip_multi_dir)
+ 	    {
++	      if (!(pl->os_multilib ? skip_multi_os_dir : skip_multi_dir))
++	        {
++	          const char *this_multi;
++	          size_t this_multi_len;
++
++	          if (pl->os_multilib)
++		    {
++		      this_multi = multi_os_dir;
++		      this_multi_len = multi_os_dir_len;
++		    }
++	          else
++		    {
++		      this_multi = multi_dir;
++		      this_multi_len = multi_dir_len;
++		    }
++
++	          /* Look in multilib MACHINE/VERSION subdirectory first */
++	          if (this_multi_len)
++	            {
++		      memcpy (path + len, this_multi, this_multi_len + 1);
++	              memcpy (path + len + this_multi_len, multi_suffix, suffix_len + 1);
++	              ret = callback (path, callback_info);
++	                if (ret)
++		          break;
++	            }
++	        }
++
+ 	      memcpy (path + len, multi_suffix, suffix_len + 1);
+ 	      ret = callback (path, callback_info);
+ 	      if (ret)
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0027-nios2-Define-MUSL_DYNAMIC_LINKER.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0027-nios2-Define-MUSL_DYNAMIC_LINKER.patch
new file mode 100644
index 0000000..97c2713
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0027-nios2-Define-MUSL_DYNAMIC_LINKER.patch
@@ -0,0 +1,31 @@
+From: Richard Purdie <richard.purdie@linuxfoundation.org>
+Subject: [PATCH 4/5] gcc/nios2: Define the musl linker
+
+Add a definition of the musl linker used on the nios2 platform.
+
+2021-10-26 Richard Purdie <richard.purdie@linuxfoundation.org>
+
+gcc/ChangeLog:
+
+    * config/nios2/linux.h (MUSL_DYNAMIC_LINKER): Add musl linker
+
+Signed-off-by: Richard Purdie <richard.purdie@linuxfoundation.org>
+
+Submitted [https://gcc.gnu.org/pipermail/gcc-patches/2021-October/582723.html]
+Upstream-Status: Backport [https://gcc.gnu.org/git/?p=gcc.git;a=commitdiff;h=e5ddbbf992b909d8e38851bd3179d29389e6ac97]
+---
+ gcc/config/nios2/linux.h | 1 +
+ 1 file changed, 1 insertion(+)
+
+Index: gcc-11.2.0/gcc/config/nios2/linux.h
+===================================================================
+--- gcc-11.2.0.orig/gcc/config/nios2/linux.h
++++ gcc-11.2.0/gcc/config/nios2/linux.h
+@@ -30,6 +30,7 @@
+ #define CPP_SPEC "%{posix:-D_POSIX_SOURCE} %{pthread:-D_REENTRANT}"
+ 
+ #define GLIBC_DYNAMIC_LINKER "/lib/ld-linux-nios2.so.1"
++#define MUSL_DYNAMIC_LINKER  "/lib/ld-musl-nios2.so.1"
+ 
+ #undef LINK_SPEC
+ #define LINK_SPEC LINK_SPEC_ENDIAN \
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0028-Add-ssp_nonshared-to-link-commandline-for-musl-targe.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0028-Add-ssp_nonshared-to-link-commandline-for-musl-targe.patch
new file mode 100644
index 0000000..9c616d2
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0028-Add-ssp_nonshared-to-link-commandline-for-musl-targe.patch
@@ -0,0 +1,78 @@
+From 745a2ac7825c73102b888226c54397d21512f86b Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Tue, 27 Jun 2017 18:10:54 -0700
+Subject: [PATCH] Add ssp_nonshared to link commandline for musl targets
+
+when -fstack-protector options are enabled we need to
+link with ssp_shared on musl since it does not provide
+the __stack_chk_fail_local() so essentially it provides
+libssp but not libssp_nonshared something like
+TARGET_LIBC_PROVIDES_SSP_BUT_NOT_SSP_NONSHARED
+ where-as for glibc the needed symbols
+are already present in libc_nonshared library therefore
+we do not need any library helper on glibc based systems
+but musl needs the libssp_noshared from gcc
+
+Upstream-Status: Pending
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ gcc/config/linux.h          |  7 +++++++
+ gcc/config/rs6000/linux.h   | 10 ++++++++++
+ gcc/config/rs6000/linux64.h | 10 ++++++++++
+ 3 files changed, 27 insertions(+)
+
+--- a/gcc/config/linux.h
++++ b/gcc/config/linux.h
+@@ -203,6 +203,13 @@ see the files COPYING3 and COPYING.RUNTI
+     { GCC_INCLUDE_DIRVAR, "GCC", 0, 1, 0, 0 },		\
+     { 0, 0, 0, 0, 0, 0 }				\
+   }
++#ifdef TARGET_LIBC_PROVIDES_SSP
++#undef LINK_SSP_SPEC
++#define LINK_SSP_SPEC "%{fstack-protector|fstack-protector-all" \
++		       "|fstack-protector-strong|fstack-protector-explicit" \
++		       ":-lssp_nonshared}"
++#endif
++
+ #endif
+ 
+ #if (DEFAULT_LIBC == LIBC_UCLIBC) && defined (SINGLE_LIBC) /* uClinux */
+--- a/gcc/config/rs6000/linux.h
++++ b/gcc/config/rs6000/linux.h
+@@ -94,6 +94,16 @@
+ 					 " -m elf32ppclinux")
+ #endif
+ 
++/* link libssp_nonshared.a with musl */
++#if DEFAULT_LIBC == LIBC_MUSL
++#ifdef TARGET_LIBC_PROVIDES_SSP
++#undef LINK_SSP_SPEC
++#define LINK_SSP_SPEC "%{fstack-protector|fstack-protector-all" \
++		       "|fstack-protector-strong|fstack-protector-explicit" \
++		       ":-lssp_nonshared}"
++#endif
++#endif
++
+ #undef LINK_OS_LINUX_SPEC
+ #define LINK_OS_LINUX_SPEC LINK_OS_LINUX_EMUL " %{!shared: %{!static: \
+   %{!static-pie: \
+--- a/gcc/config/rs6000/linux64.h
++++ b/gcc/config/rs6000/linux64.h
+@@ -369,6 +369,16 @@ extern int dot_symbols;
+ 					   " -m elf64ppc")
+ #endif
+ 
++/* link libssp_nonshared.a with musl */
++#if DEFAULT_LIBC == LIBC_MUSL
++#ifdef TARGET_LIBC_PROVIDES_SSP
++#undef LINK_SSP_SPEC
++#define LINK_SSP_SPEC "%{fstack-protector|fstack-protector-all" \
++		       "|fstack-protector-strong|fstack-protector-explicit" \
++		       ":-lssp_nonshared}"
++#endif
++#endif
++
+ #define LINK_OS_LINUX_SPEC32 LINK_OS_LINUX_EMUL32 " %{!shared: %{!static: \
+   %{!static-pie: \
+     %{rdynamic:-export-dynamic} \
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0029-Link-libgcc-using-LDFLAGS-not-just-SHLIB_LDFLAGS.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0029-Link-libgcc-using-LDFLAGS-not-just-SHLIB_LDFLAGS.patch
new file mode 100644
index 0000000..b74aeb8
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0029-Link-libgcc-using-LDFLAGS-not-just-SHLIB_LDFLAGS.patch
@@ -0,0 +1,26 @@
+From 695adb4dffb23c6f5cbc757e05cf4187a2bd6528 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Wed, 4 May 2016 21:11:34 -0700
+Subject: [PATCH] Link libgcc using LDFLAGS, not just SHLIB_LDFLAGS
+
+Upstream-Status: Pending
+
+Signed-off-by: Christopher Larson <chris_larson@mentor.com>
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ libgcc/config/t-slibgcc | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/libgcc/config/t-slibgcc b/libgcc/config/t-slibgcc
+index c59b43b7b69..ca4c141f526 100644
+--- a/libgcc/config/t-slibgcc
++++ b/libgcc/config/t-slibgcc
+@@ -32,7 +32,7 @@ SHLIB_INSTALL_SOLINK = $(LN_S) $(SHLIB_SONAME) \
+ 	$(DESTDIR)$(slibdir)$(SHLIB_SLIBDIR_QUAL)/$(SHLIB_SOLINK)
+ 
+ SHLIB_LINK = $(CC) $(LIBGCC2_CFLAGS) -shared -nodefaultlibs \
+-	$(SHLIB_LDFLAGS) \
++	$(LDFLAGS) $(SHLIB_LDFLAGS) \
+ 	-o $(SHLIB_DIR)/$(SHLIB_SONAME).tmp @multilib_flags@ \
+ 	$(SHLIB_OBJS) $(SHLIB_LC) && \
+ 	rm -f $(SHLIB_DIR)/$(SHLIB_SOLINK) && \
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0030-sync-gcc-stddef.h-with-musl.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0030-sync-gcc-stddef.h-with-musl.patch
new file mode 100644
index 0000000..fd7d604
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0030-sync-gcc-stddef.h-with-musl.patch
@@ -0,0 +1,88 @@
+From a9173429ae256c4b4a3ab4d758a6adf42f8c4239 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Fri, 3 Feb 2017 12:56:00 -0800
+Subject: [PATCH] sync gcc stddef.h with musl
+
+musl defines ptrdiff_t size_t and wchar_t
+so dont define them here if musl is definining them
+
+Upstream-Status: Backport [https://gcc.gnu.org/git/?p=gcc.git;a=commit;h=85a438fc78dd12249ca854a3e5c577fefeb1a5cd]
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ gcc/ginclude/stddef.h | 9 +++++++++
+ 1 file changed, 9 insertions(+)
+
+diff --git a/gcc/ginclude/stddef.h b/gcc/ginclude/stddef.h
+index 66619fe43b1..3f843d6f365 100644
+--- a/gcc/ginclude/stddef.h
++++ b/gcc/ginclude/stddef.h
+@@ -128,6 +128,7 @@ _TYPE_wchar_t;
+ #ifndef ___int_ptrdiff_t_h
+ #ifndef _GCC_PTRDIFF_T
+ #ifndef _PTRDIFF_T_DECLARED /* DragonFly */
++#ifndef __DEFINED_ptrdiff_t /* musl */
+ #define _PTRDIFF_T
+ #define _T_PTRDIFF_
+ #define _T_PTRDIFF
+@@ -137,10 +138,12 @@ _TYPE_wchar_t;
+ #define ___int_ptrdiff_t_h
+ #define _GCC_PTRDIFF_T
+ #define _PTRDIFF_T_DECLARED
++#define __DEFINED_ptrdiff_t /* musl */
+ #ifndef __PTRDIFF_TYPE__
+ #define __PTRDIFF_TYPE__ long int
+ #endif
+ typedef __PTRDIFF_TYPE__ ptrdiff_t;
++#endif /* __DEFINED_ptrdiff_t */
+ #endif /* _PTRDIFF_T_DECLARED */
+ #endif /* _GCC_PTRDIFF_T */
+ #endif /* ___int_ptrdiff_t_h */
+@@ -178,6 +181,7 @@ typedef __PTRDIFF_TYPE__ ptrdiff_t;
+ #ifndef _GCC_SIZE_T
+ #ifndef _SIZET_
+ #ifndef __size_t
++#ifndef __DEFINED_size_t /* musl */
+ #define __size_t__	/* BeOS */
+ #define __SIZE_T__	/* Cray Unicos/Mk */
+ #define _SIZE_T
+@@ -194,6 +198,7 @@ typedef __PTRDIFF_TYPE__ ptrdiff_t;
+ #define ___int_size_t_h
+ #define _GCC_SIZE_T
+ #define _SIZET_
++#define __DEFINED_size_t /* musl */
+ #if defined (__FreeBSD__) \
+   || defined(__DragonFly__) \
+   || defined(__FreeBSD_kernel__) \
+@@ -228,6 +233,7 @@ typedef long ssize_t;
+ #endif /* _SIZE_T */
+ #endif /* __SIZE_T__ */
+ #endif /* __size_t__ */
++#endif /* __DEFINED_size_t */
+ #undef	__need_size_t
+ #endif /* _STDDEF_H or __need_size_t.  */
+ 
+@@ -257,6 +263,7 @@ typedef long ssize_t;
+ #ifndef ___int_wchar_t_h
+ #ifndef __INT_WCHAR_T_H
+ #ifndef _GCC_WCHAR_T
++#ifndef __DEFINED_wchar_t /* musl */
+ #define __wchar_t__	/* BeOS */
+ #define __WCHAR_T__	/* Cray Unicos/Mk */
+ #define _WCHAR_T
+@@ -272,6 +279,7 @@ typedef long ssize_t;
+ #define __INT_WCHAR_T_H
+ #define _GCC_WCHAR_T
+ #define _WCHAR_T_DECLARED
++#define __DEFINED_wchar_t /* musl */
+ 
+ /* On BSD/386 1.1, at least, machine/ansi.h defines _BSD_WCHAR_T_
+    instead of _WCHAR_T_, and _BSD_RUNE_T_ (which, unlike the other
+@@ -337,6 +345,7 @@ typedef __WCHAR_TYPE__ wchar_t;
+ #endif
+ #endif /* __WCHAR_T__ */
+ #endif /* __wchar_t__ */
++#endif /* __DEFINED_wchar_t musl */
+ #undef	__need_wchar_t
+ #endif /* _STDDEF_H or __need_wchar_t.  */
+ 
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0033-Re-introduce-spe-commandline-options.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0033-Re-introduce-spe-commandline-options.patch
new file mode 100644
index 0000000..129f555
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0033-Re-introduce-spe-commandline-options.patch
@@ -0,0 +1,39 @@
+From 0b900d6410b7c1938e86eceb87b032fd538566a9 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Wed, 6 Jun 2018 12:10:22 -0700
+Subject: [PATCH] Re-introduce spe commandline options
+
+This should ensure that we keep accepting
+spe options
+
+Upstream-Status: Inappropriate [SPE port is removed from rs600 port]
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ gcc/config/rs6000/rs6000.opt | 13 +++++++++++++
+ 1 file changed, 13 insertions(+)
+
+diff --git a/gcc/config/rs6000/rs6000.opt b/gcc/config/rs6000/rs6000.opt
+index 0dbdf753673..b273eb65c35 100644
+--- a/gcc/config/rs6000/rs6000.opt
++++ b/gcc/config/rs6000/rs6000.opt
+@@ -352,6 +352,19 @@ mdebug=
+ Target RejectNegative Joined
+ -mdebug=	Enable debug output.
+ 
++; PPC SPE ABI
++mspe
++Target Var(rs6000_spe) Save
++Generate SPE SIMD instructions on E500.
++
++mabi=spe
++Target RejectNegative Var(rs6000_spe_abi) Save
++Use the SPE ABI extensions.
++
++mabi=no-spe
++Target RejectNegative Var(rs6000_spe_abi, 0)
++Do not use the SPE ABI extensions.
++
+ ; Altivec ABI
+ mabi=altivec
+ Target RejectNegative Var(rs6000_altivec_abi) Save
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0034-libgcc_s-Use-alias-for-__cpu_indicator_init-instead-.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0034-libgcc_s-Use-alias-for-__cpu_indicator_init-instead-.patch
new file mode 100644
index 0000000..3f666dc
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0034-libgcc_s-Use-alias-for-__cpu_indicator_init-instead-.patch
@@ -0,0 +1,83 @@
+From ea9154338cb3acbd75945fddde4202e73c20dd1a Mon Sep 17 00:00:00 2001
+From: Szabolcs Nagy <nsz@port70.net>
+Date: Sat, 24 Oct 2015 20:09:53 +0000
+Subject: [PATCH] libgcc_s: Use alias for __cpu_indicator_init instead of
+ symver
+
+Adapter from
+
+https://gcc.gnu.org/ml/gcc-patches/2015-05/msg00899.html
+
+This fix was debated but hasnt been applied gcc upstream since
+they expect musl to support '@' in symbol versioning which is
+a sun/gnu versioning extention. This patch however avoids the
+need for the '@' symbols at all
+
+libgcc/Changelog:
+
+2015-05-11  Szabolcs Nagy  <szabolcs.nagy@arm.com>
+
+	* config/i386/cpuinfo.c (__cpu_indicator_init_local): Add.
+	(__cpu_indicator_init@GCC_4.8.0, __cpu_model@GCC_4.8.0): Remove.
+
+	* config/i386/t-linux (HOST_LIBGCC2_CFLAGS): Remove -DUSE_ELF_SYMVER.
+
+gcc/Changelog:
+
+2015-05-11  Szabolcs Nagy  <szabolcs.nagy@arm.com>
+
+	* config/i386/i386-expand.c (ix86_expand_builtin): Make __builtin_cpu_init
+	call __cpu_indicator_init_local instead of __cpu_indicator_init.
+
+Upstream-Status: Pending
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ gcc/config/i386/i386-expand.c | 4 ++--
+ libgcc/config/i386/cpuinfo.c  | 6 +++---
+ libgcc/config/i386/t-linux    | 2 +-
+ 3 files changed, 6 insertions(+), 6 deletions(-)
+
+diff --git a/gcc/config/i386/i386-expand.c b/gcc/config/i386/i386-expand.c
+index ac69eed4d32..ffaa44a16fc 100644
+--- a/gcc/config/i386/i386-expand.c
++++ b/gcc/config/i386/i386-expand.c
+@@ -11038,10 +11038,10 @@ ix86_expand_builtin (tree exp, rtx target, rtx subtarget,
+     {
+     case IX86_BUILTIN_CPU_INIT:
+       {
+-	/* Make it call __cpu_indicator_init in libgcc. */
++	/* Make it call __cpu_indicator_init_local in libgcc.a. */
+ 	tree call_expr, fndecl, type;
+         type = build_function_type_list (integer_type_node, NULL_TREE); 
+-	fndecl = build_fn_decl ("__cpu_indicator_init", type);
++	fndecl = build_fn_decl ("__cpu_indicator_init_local", type);
+ 	call_expr = build_call_expr (fndecl, 0); 
+ 	return expand_expr (call_expr, target, mode, EXPAND_NORMAL);
+       }
+diff --git a/libgcc/config/i386/cpuinfo.c b/libgcc/config/i386/cpuinfo.c
+index ef463848f9d..1a3de052c80 100644
+--- a/libgcc/config/i386/cpuinfo.c
++++ b/libgcc/config/i386/cpuinfo.c
+@@ -63,7 +63,7 @@ __cpu_indicator_init (void)
+ 			     __cpu_features2);
+ }
+ 
+-#if defined SHARED && defined USE_ELF_SYMVER
+-__asm__ (".symver __cpu_indicator_init, __cpu_indicator_init@GCC_4.8.0");
+-__asm__ (".symver __cpu_model, __cpu_model@GCC_4.8.0");
++#ifndef SHARED
++int __cpu_indicator_init_local (void)
++  __attribute__ ((weak, alias ("__cpu_indicator_init")));
+ #endif
+diff --git a/libgcc/config/i386/t-linux b/libgcc/config/i386/t-linux
+index 8506a635790..564296f788e 100644
+--- a/libgcc/config/i386/t-linux
++++ b/libgcc/config/i386/t-linux
+@@ -3,5 +3,5 @@
+ # t-slibgcc-elf-ver and t-linux
+ SHLIB_MAPFILES = libgcc-std.ver $(srcdir)/config/i386/libgcc-glibc.ver
+ 
+-HOST_LIBGCC2_CFLAGS += -mlong-double-80 -DUSE_ELF_SYMVER $(CET_FLAGS)
++HOST_LIBGCC2_CFLAGS += -mlong-double-80 $(CET_FLAGS)
+ CRTSTUFF_T_CFLAGS += $(CET_FLAGS)
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0035-gentypes-genmodes-Do-not-use-__LINE__-for-maintainin.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0035-gentypes-genmodes-Do-not-use-__LINE__-for-maintainin.patch
new file mode 100644
index 0000000..874cd77
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0035-gentypes-genmodes-Do-not-use-__LINE__-for-maintainin.patch
@@ -0,0 +1,182 @@
+From 520411cf364ee4b0b5a8f0857498aaabd790afb3 Mon Sep 17 00:00:00 2001
+From: Richard Purdie <richard.purdie@linuxfoundation.org>
+Date: Tue, 10 Mar 2020 08:26:53 -0700
+Subject: [PATCH] gentypes/genmodes: Do not use __LINE__ for maintaining
+ reproducibility
+
+Inserting line numbers into generated code means its not always reproducible wth
+differing versions of host gcc. Void the issue by not adding these.
+
+Upstream-Status: Inappropriate [OE Reproducibility specific]
+
+Signed-off-by: Richard Purdie <richard.purdie@linuxfoundation.org>
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ gcc/gengtype.c |  6 +++---
+ gcc/genmodes.c | 32 ++++++++++++++++----------------
+ 2 files changed, 19 insertions(+), 19 deletions(-)
+
+diff --git a/gcc/gengtype.c b/gcc/gengtype.c
+index 5f50242e857..cbaca9b8cd0 100644
+--- a/gcc/gengtype.c
++++ b/gcc/gengtype.c
+@@ -991,7 +991,7 @@ create_field_at (pair_p next, type_p type, const char *name, options_p opt,
+ /* Create a fake field with the given type and name.  NEXT is the next
+    field in the chain.  */
+ #define create_field(next,type,name) \
+-    create_field_all (next,type,name, 0, this_file, __LINE__)
++    create_field_all (next,type,name, 0, this_file, 0)
+ 
+ /* Like create_field, but the field is only valid when condition COND
+    is true.  */
+@@ -1024,7 +1024,7 @@ create_optional_field_ (pair_p next, type_p type, const char *name,
+ }
+ 
+ #define create_optional_field(next,type,name,cond)	\
+-       create_optional_field_(next,type,name,cond,__LINE__)
++       create_optional_field_(next,type,name,cond,0)
+ 
+ /* Reverse a linked list of 'struct pair's in place.  */
+ pair_p
+@@ -5189,7 +5189,7 @@ main (int argc, char **argv)
+       /* These types are set up with #define or else outside of where
+          we can see them.  We should initialize them before calling
+          read_input_list.  */
+-#define POS_HERE(Call) do { pos.file = this_file; pos.line = __LINE__; \
++#define POS_HERE(Call) do { pos.file = this_file; pos.line = 0; \
+ 	Call;} while (0)
+       POS_HERE (do_scalar_typedef ("CUMULATIVE_ARGS", &pos));
+       POS_HERE (do_scalar_typedef ("REAL_VALUE_TYPE", &pos));
+diff --git a/gcc/genmodes.c b/gcc/genmodes.c
+index c268ebc4c6e..4361f3f1563 100644
+--- a/gcc/genmodes.c
++++ b/gcc/genmodes.c
+@@ -438,7 +438,7 @@ complete_all_modes (void)
+ }
+ 
+ /* For each mode in class CLASS, construct a corresponding complex mode.  */
+-#define COMPLEX_MODES(C) make_complex_modes (MODE_##C, __FILE__, __LINE__)
++#define COMPLEX_MODES(C) make_complex_modes (MODE_##C, __FILE__, 0)
+ static void
+ make_complex_modes (enum mode_class cl,
+ 		    const char *file, unsigned int line)
+@@ -497,7 +497,7 @@ make_complex_modes (enum mode_class cl,
+    having as many components as necessary.  ORDER is the sorting order
+    of the mode, with smaller numbers indicating a higher priority.  */
+ #define VECTOR_MODES_WITH_PREFIX(PREFIX, C, W, ORDER) \
+-  make_vector_modes (MODE_##C, #PREFIX, W, ORDER, __FILE__, __LINE__)
++  make_vector_modes (MODE_##C, #PREFIX, W, ORDER, __FILE__, 0)
+ #define VECTOR_MODES(C, W) VECTOR_MODES_WITH_PREFIX (V, C, W, 0)
+ static void ATTRIBUTE_UNUSED
+ make_vector_modes (enum mode_class cl, const char *prefix, unsigned int width,
+@@ -549,7 +549,7 @@ make_vector_modes (enum mode_class cl, const char *prefix, unsigned int width,
+ /* Create a vector of booleans called NAME with COUNT elements and
+    BYTESIZE bytes in total.  */
+ #define VECTOR_BOOL_MODE(NAME, COUNT, BYTESIZE) \
+-  make_vector_bool_mode (#NAME, COUNT, BYTESIZE, __FILE__, __LINE__)
++  make_vector_bool_mode (#NAME, COUNT, BYTESIZE, __FILE__, 0)
+ static void ATTRIBUTE_UNUSED
+ make_vector_bool_mode (const char *name, unsigned int count,
+ 		       unsigned int bytesize, const char *file,
+@@ -571,7 +571,7 @@ make_vector_bool_mode (const char *name, unsigned int count,
+ /* Input.  */
+ 
+ #define _SPECIAL_MODE(C, N) \
+-  make_special_mode (MODE_##C, #N, __FILE__, __LINE__)
++  make_special_mode (MODE_##C, #N, __FILE__, 0)
+ #define RANDOM_MODE(N) _SPECIAL_MODE (RANDOM, N)
+ #define CC_MODE(N) _SPECIAL_MODE (CC, N)
+ 
+@@ -584,7 +584,7 @@ make_special_mode (enum mode_class cl, const char *name,
+ 
+ #define INT_MODE(N, Y) FRACTIONAL_INT_MODE (N, -1U, Y)
+ #define FRACTIONAL_INT_MODE(N, B, Y) \
+-  make_int_mode (#N, B, Y, __FILE__, __LINE__)
++  make_int_mode (#N, B, Y, __FILE__, 0)
+ 
+ static void
+ make_int_mode (const char *name,
+@@ -611,16 +611,16 @@ make_opaque_mode (const char *name,
+ }
+ 
+ #define FRACT_MODE(N, Y, F) \
+-	make_fixed_point_mode (MODE_FRACT, #N, Y, 0, F, __FILE__, __LINE__)
++	make_fixed_point_mode (MODE_FRACT, #N, Y, 0, F, __FILE__, 0)
+ 
+ #define UFRACT_MODE(N, Y, F) \
+-	make_fixed_point_mode (MODE_UFRACT, #N, Y, 0, F, __FILE__, __LINE__)
++	make_fixed_point_mode (MODE_UFRACT, #N, Y, 0, F, __FILE__, 0)
+ 
+ #define ACCUM_MODE(N, Y, I, F) \
+-	make_fixed_point_mode (MODE_ACCUM, #N, Y, I, F, __FILE__, __LINE__)
++	make_fixed_point_mode (MODE_ACCUM, #N, Y, I, F, __FILE__, 0)
+ 
+ #define UACCUM_MODE(N, Y, I, F) \
+-	make_fixed_point_mode (MODE_UACCUM, #N, Y, I, F, __FILE__, __LINE__)
++	make_fixed_point_mode (MODE_UACCUM, #N, Y, I, F, __FILE__, 0)
+ 
+ /* Create a fixed-point mode by setting CL, NAME, BYTESIZE, IBIT, FBIT,
+    FILE, and LINE.  */
+@@ -641,7 +641,7 @@ make_fixed_point_mode (enum mode_class cl,
+ 
+ #define FLOAT_MODE(N, Y, F)             FRACTIONAL_FLOAT_MODE (N, -1U, Y, F)
+ #define FRACTIONAL_FLOAT_MODE(N, B, Y, F) \
+-  make_float_mode (#N, B, Y, #F, __FILE__, __LINE__)
++  make_float_mode (#N, B, Y, #F, __FILE__, 0)
+ 
+ static void
+ make_float_mode (const char *name,
+@@ -658,7 +658,7 @@ make_float_mode (const char *name,
+ #define DECIMAL_FLOAT_MODE(N, Y, F)	\
+ 	FRACTIONAL_DECIMAL_FLOAT_MODE (N, -1U, Y, F)
+ #define FRACTIONAL_DECIMAL_FLOAT_MODE(N, B, Y, F)	\
+-  make_decimal_float_mode (#N, B, Y, #F, __FILE__, __LINE__)
++  make_decimal_float_mode (#N, B, Y, #F, __FILE__, 0)
+ 
+ static void
+ make_decimal_float_mode (const char *name,
+@@ -673,7 +673,7 @@ make_decimal_float_mode (const char *name,
+ }
+ 
+ #define RESET_FLOAT_FORMAT(N, F) \
+-  reset_float_format (#N, #F, __FILE__, __LINE__)
++  reset_float_format (#N, #F, __FILE__, 0)
+ static void ATTRIBUTE_UNUSED
+ reset_float_format (const char *name, const char *format,
+ 		    const char *file, unsigned int line)
+@@ -694,7 +694,7 @@ reset_float_format (const char *name, const char *format,
+ 
+ /* __intN support.  */
+ #define INT_N(M,PREC)				\
+-  make_int_n (#M, PREC, __FILE__, __LINE__)
++  make_int_n (#M, PREC, __FILE__, 0)
+ static void ATTRIBUTE_UNUSED
+ make_int_n (const char *m, int bitsize,
+             const char *file, unsigned int line)
+@@ -723,7 +723,7 @@ make_int_n (const char *m, int bitsize,
+ /* Partial integer modes are specified by relation to a full integer
+    mode.  */
+ #define PARTIAL_INT_MODE(M,PREC,NAME)				\
+-  make_partial_integer_mode (#M, #NAME, PREC, __FILE__, __LINE__)
++  make_partial_integer_mode (#M, #NAME, PREC, __FILE__, 0)
+ static void ATTRIBUTE_UNUSED
+ make_partial_integer_mode (const char *base, const char *name,
+ 			   unsigned int precision,
+@@ -750,7 +750,7 @@ make_partial_integer_mode (const char *base, const char *name,
+ /* A single vector mode can be specified by naming its component
+    mode and the number of components.  */
+ #define VECTOR_MODE(C, M, N) \
+-  make_vector_mode (MODE_##C, #M, N, __FILE__, __LINE__);
++  make_vector_mode (MODE_##C, #M, N, __FILE__, 0);
+ static void ATTRIBUTE_UNUSED
+ make_vector_mode (enum mode_class bclass,
+ 		  const char *base,
+@@ -793,7 +793,7 @@ make_vector_mode (enum mode_class bclass,
+ 
+ /* Adjustability.  */
+ #define _ADD_ADJUST(A, M, X, C1, C2) \
+-  new_adjust (#M, &adj_##A, #A, #X, MODE_##C1, MODE_##C2, __FILE__, __LINE__)
++  new_adjust (#M, &adj_##A, #A, #X, MODE_##C1, MODE_##C2, __FILE__, 0)
+ 
+ #define ADJUST_NUNITS(M, X)    _ADD_ADJUST (nunits, M, X, RANDOM, RANDOM)
+ #define ADJUST_BYTESIZE(M, X)  _ADD_ADJUST (bytesize, M, X, RANDOM, RANDOM)
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0036-mingw32-Enable-operation_not_supported.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0036-mingw32-Enable-operation_not_supported.patch
new file mode 100644
index 0000000..3a7618c
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0036-mingw32-Enable-operation_not_supported.patch
@@ -0,0 +1,26 @@
+From 96d895c8d5dc895d24fe37aa2b4f201a2566b4cc Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Tue, 12 May 2020 10:39:09 -0700
+Subject: [PATCH] mingw32: Enable operation_not_supported
+
+Fixes nativesdk build errors on mingw32 gcc-runtime
+
+Upstream-Status: Pending
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ libstdc++-v3/config/os/mingw32/error_constants.h | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/libstdc++-v3/config/os/mingw32/error_constants.h b/libstdc++-v3/config/os/mingw32/error_constants.h
+index eca06a97014..933cfab49cf 100644
+--- a/libstdc++-v3/config/os/mingw32/error_constants.h
++++ b/libstdc++-v3/config/os/mingw32/error_constants.h
+@@ -107,7 +107,7 @@ _GLIBCXX_BEGIN_NAMESPACE_VERSION
+ #ifdef EPERM
+       operation_not_permitted = 		EPERM,
+ #endif
+-//    operation_not_supported = 		EOPNOTSUPP,
++      operation_not_supported = 		EOPNOTSUPP,
+ #ifdef EWOULDBLOCK
+       operation_would_block = 			EWOULDBLOCK,
+ #endif
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0037-libatomic-Do-not-enforce-march-on-aarch64.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0037-libatomic-Do-not-enforce-march-on-aarch64.patch
new file mode 100644
index 0000000..4c9a79c
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0037-libatomic-Do-not-enforce-march-on-aarch64.patch
@@ -0,0 +1,42 @@
+From 36d4fdbc99e69f9d70a29e2bada40cc3c1534557 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Wed, 13 May 2020 15:10:38 -0700
+Subject: [PATCH] libatomic: Do not enforce march on aarch64
+
+OE passes the right options via gcc compiler cmdline via TUNE_CCARGS
+this can conflict between -mcpu settings and -march setting here, since
+-mcpu will translate into an appropriate -march, lets depend on that
+instead of setting it explicitly
+
+Upstream-Status: Inappropriate [OE-Specific]
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ libatomic/Makefile.am | 1 -
+ libatomic/Makefile.in | 1 -
+ 2 files changed, 2 deletions(-)
+
+diff --git a/libatomic/Makefile.am b/libatomic/Makefile.am
+index 0f3cd6f7121..c8124c1d5aa 100644
+--- a/libatomic/Makefile.am
++++ b/libatomic/Makefile.am
+@@ -125,7 +125,6 @@ libatomic_la_LIBADD = $(foreach s,$(SIZES),$(addsuffix _$(s)_.lo,$(SIZEOBJS)))
+ ## On a target-specific basis, include alternates to be selected by IFUNC.
+ if HAVE_IFUNC
+ if ARCH_AARCH64_LINUX
+-IFUNC_OPTIONS	     = -march=armv8-a+lse
+ libatomic_la_LIBADD += $(foreach s,$(SIZES),$(addsuffix _$(s)_1_.lo,$(SIZEOBJS)))
+ endif
+ if ARCH_ARM_LINUX
+diff --git a/libatomic/Makefile.in b/libatomic/Makefile.in
+index 0a51bd55f01..6d5b1581706 100644
+--- a/libatomic/Makefile.in
++++ b/libatomic/Makefile.in
+@@ -432,7 +432,6 @@ M_SRC = $(firstword $(filter %/$(M_FILE), $(all_c_files)))
+ libatomic_la_LIBADD = $(foreach s,$(SIZES),$(addsuffix \
+ 	_$(s)_.lo,$(SIZEOBJS))) $(am__append_1) $(am__append_2) \
+ 	$(am__append_3) $(am__append_4)
+-@ARCH_AARCH64_LINUX_TRUE@@HAVE_IFUNC_TRUE@IFUNC_OPTIONS = -march=armv8-a+lse
+ @ARCH_ARM_LINUX_TRUE@@HAVE_IFUNC_TRUE@IFUNC_OPTIONS = -march=armv7-a+fp -DHAVE_KERNEL64
+ @ARCH_I386_TRUE@@HAVE_IFUNC_TRUE@IFUNC_OPTIONS = -march=i586
+ @ARCH_X86_64_TRUE@@HAVE_IFUNC_TRUE@IFUNC_OPTIONS = -mcx16
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0041-apply-debug-prefix-maps-before-checksumming-DIEs.patch b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0041-apply-debug-prefix-maps-before-checksumming-DIEs.patch
new file mode 100644
index 0000000..c8dcd74
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-11.2/0041-apply-debug-prefix-maps-before-checksumming-DIEs.patch
@@ -0,0 +1,95 @@
+From 7cc2df084b7977653a9b59cbc34a9ad500ae619c Mon Sep 17 00:00:00 2001
+From: Richard Biener <rguenther@suse.de>
+Date: Tue, 20 Jul 2021 11:00:33 +0200
+Subject: [PATCH] debug/101473 - apply debug prefix maps before checksumming DIEs
+
+The following makes sure to apply the debug prefix maps to filenames
+before checksumming DIEs to create the global symbol for the CU DIE
+used by LTO to link the late debug to the early debug.  This avoids
+binary differences (in said symbol) when compiling with toolchains
+installed under a different path and that compensated with appropriate
+-fdebug-prefix-map options.
+
+The easiest and most scalable way is to record both the unmapped
+and the remapped filename in the dwarf_file_data so the remapping
+process takes place at a single point and only once (otherwise it
+creates GC garbage at each point doing that).
+
+2021-07-20  Richard Biener  <rguenther@suse.de>
+
+	PR debug/101473
+	* dwarf2out.h (dwarf_file_data): Add key member.
+	* dwarf2out.c (dwarf_file_hasher::equal): Compare key.
+	(dwarf_file_hasher::hash): Hash key.
+	(lookup_filename): Remap the filename and store it in the
+	filename member of dwarf_file_data when creating a new
+	dwarf_file_data.
+	(file_name_acquire): Do not remap the filename again.
+	(maybe_emit_file): Likewise.
+
+[YOCTO #14481]
+
+Upstream-Status: Backport [https://gcc.gnu.org/git/?p=gcc.git;a=commit;h=7cc2df084b7977653a9b59cbc34a9ad500ae619c]
+
+The upstream patch was modified to compensate for the definition of
+"struct dwarf_file_data" being in dwarf2out.c rather than dwarf2out.h in
+this version of gcc.
+
+Signed-off-by: Tony Battersby <tonyb@cybernetics.com>
+---
+diff -urpN a/gcc/dwarf2out.c b/gcc/dwarf2out.c
+--- a/gcc/dwarf2out.c	2021-04-27 06:00:13.000000000 -0400
++++ b/gcc/dwarf2out.c	2021-07-23 16:40:06.141886167 -0400
+@@ -1283,6 +1283,7 @@ dwarf2out_switch_text_section (void)
+ 
+ /* Data about a single source file.  */
+ struct GTY((for_user)) dwarf_file_data {
++  const char * key;
+   const char * filename;
+   int emitted_number;
+ };
+@@ -12334,7 +12335,7 @@ file_name_acquire (dwarf_file_data **slo
+ 
+   fi = fnad->files + fnad->used_files++;
+ 
+-  f = remap_debug_filename (d->filename);
++  f = d->filename;
+ 
+   /* Skip all leading "./".  */
+   while (f[0] == '.' && IS_DIR_SEPARATOR (f[1]))
+@@ -27231,13 +27232,13 @@ dwarf2out_ignore_block (const_tree block
+ bool
+ dwarf_file_hasher::equal (dwarf_file_data *p1, const char *p2)
+ {
+-  return filename_cmp (p1->filename, p2) == 0;
++  return filename_cmp (p1->key, p2) == 0;
+ }
+ 
+ hashval_t
+ dwarf_file_hasher::hash (dwarf_file_data *p)
+ {
+-  return htab_hash_string (p->filename);
++  return htab_hash_string (p->key);
+ }
+ 
+ /* Lookup FILE_NAME (in the list of filenames that we know about here in
+@@ -27267,7 +27268,8 @@ lookup_filename (const char *file_name)
+     return *slot;
+ 
+   created = ggc_alloc<dwarf_file_data> ();
+-  created->filename = file_name;
++  created->key = file_name;
++  created->filename = remap_debug_filename (file_name);
+   created->emitted_number = 0;
+   *slot = created;
+   return created;
+@@ -27293,8 +27295,7 @@ maybe_emit_file (struct dwarf_file_data
+       if (output_asm_line_debug_info ())
+ 	{
+ 	  fprintf (asm_out_file, "\t.file %u ", fd->emitted_number);
+-	  output_quoted_string (asm_out_file,
+-				remap_debug_filename (fd->filename));
++	  output_quoted_string (asm_out_file, fd->filename);
+ 	  fputc ('\n', asm_out_file);
+ 	}
+     }
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-common.inc b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-common.inc
new file mode 100644
index 0000000..d7cc4e7
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-arm-common.inc
@@ -0,0 +1,27 @@
+# gcc-target.inc in OE-Core has these 2 lines in do_install():
+#
+# | # Add unwind.h, it comes from libgcc which we don't want to build again
+# | install ${STAGING_LIBDIR_NATIVE}/${TARGET_SYS}/gcc/${TARGET_SYS}/${BINV}/include/unwind.h ${D}${libdir}/gcc/${TARGET_SYS}/${BINV}/include/
+#
+# When TCMODE="external-arm" libgcc is provided by external-arm-toolchain.bb
+# And while it stages the necessary unwind.h file, it ends up in slightly
+# different location. While this is a kludge, be very conservative - only
+# copy the file for target build in recipe-specific sysroot, in external-arm
+# toolchain mode and if the required file does not exist already.
+
+do_install:prepend:class-target () {
+	if [ "${TCMODE}" = "external-arm" -a ! -f ${STAGING_LIBDIR_NATIVE}/${TARGET_SYS}/gcc/${TARGET_SYS}/${BINV}/include/unwind.h ]; then
+		install -d ${STAGING_LIBDIR_NATIVE}/${TARGET_SYS}/gcc/${TARGET_SYS}/${BINV}/include/
+		install ${STAGING_LIBDIR}/gcc/${TARGET_SYS}/${EAT_VER_GCC}/include/unwind.h ${STAGING_LIBDIR_NATIVE}/${TARGET_SYS}/gcc/${TARGET_SYS}/${BINV}/include/
+	fi
+}
+
+# When TCMODE="external-arm" and TARGET_SYS is different from EAT_TARGET_SYS,
+# gcc installs additional aliases as ${TARGET_SYS}-${EAT_TARGET_SYS}-gcc, etc.
+# Since those are not packaged and not too useful, let's remove them to avoid
+# QA issues
+do_install:append () {
+	for f in g++ gcc gcc-ar gcc-ranlib gcc-nm; do
+		rm -f ${D}${bindir}/${TARGET_SYS}-${EAT_TARGET_SYS}-$f
+	done
+}
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-cross-canadian_arm-11.2.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-cross-canadian_arm-11.2.bb
new file mode 100644
index 0000000..d4bf7bf
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-cross-canadian_arm-11.2.bb
@@ -0,0 +1,2 @@
+require recipes-devtools/gcc/gcc-${PV}.inc
+require recipes-devtools/gcc/gcc-cross-canadian.inc
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-cross_arm-11.2.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-cross_arm-11.2.bb
new file mode 100644
index 0000000..0a8aa75
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-cross_arm-11.2.bb
@@ -0,0 +1,3 @@
+require recipes-devtools/gcc/gcc-${PV}.inc
+require recipes-devtools/gcc/gcc-cross.inc
+
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-crosssdk_arm-11.2.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-crosssdk_arm-11.2.bb
new file mode 100644
index 0000000..0a9f98a
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-crosssdk_arm-11.2.bb
@@ -0,0 +1,2 @@
+require recipes-devtools/gcc/gcc-cross_${PV}.bb
+require recipes-devtools/gcc/gcc-crosssdk.inc
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-runtime_arm-11.2.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-runtime_arm-11.2.bb
new file mode 100644
index 0000000..b755f55
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-runtime_arm-11.2.bb
@@ -0,0 +1,2 @@
+require recipes-devtools/gcc/gcc-${PV}.inc
+require recipes-devtools/gcc/gcc-runtime.inc
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-sanitizers_arm-11.2.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-sanitizers_arm-11.2.bb
new file mode 100644
index 0000000..e88ebe1
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-sanitizers_arm-11.2.bb
@@ -0,0 +1,7 @@
+require recipes-devtools/gcc/gcc-${PV}.inc
+require recipes-devtools/gcc/gcc-sanitizers.inc
+
+# Building with thumb enabled on armv4t armv5t fails with
+# sanitizer_linux.s:5749: Error: lo register required -- `ldr ip,[sp],#8'
+ARM_INSTRUCTION_SET:armv4 = "arm"
+ARM_INSTRUCTION_SET:armv5 = "arm"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-source_arm-11.2.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-source_arm-11.2.bb
new file mode 100644
index 0000000..b890fa3
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc-source_arm-11.2.bb
@@ -0,0 +1,4 @@
+require recipes-devtools/gcc/gcc-${PV}.inc
+require recipes-devtools/gcc/gcc-source.inc
+
+EXCLUDE_FROM_WORLD = "1"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc_arm-11.2.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc_arm-11.2.bb
new file mode 100644
index 0000000..9296730
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/gcc_arm-11.2.bb
@@ -0,0 +1,15 @@
+require recipes-devtools/gcc/gcc-${PV}.inc
+require recipes-devtools/gcc/gcc-target.inc
+require recipes-devtools/gcc/gcc-arm-common.inc
+
+# Building with thumb enabled on armv4t armv5t fails with
+# | gcc-4.8.1-r0/gcc-4.8.1/gcc/cp/decl.c:7438:(.text.unlikely+0x2fa): relocation truncated to fit: R_ARM_THM_CALL against symbol `fancy_abort(char const*, int, char const*)' defined in .glue_7 section in linker stubs
+# | gcc-4.8.1-r0/gcc-4.8.1/gcc/cp/decl.c:7442:(.text.unlikely+0x318): additional relocation overflows omitted from the output
+ARM_INSTRUCTION_SET:armv4 = "arm"
+ARM_INSTRUCTION_SET:armv5 = "arm"
+
+ARMFPARCHEXT:armv6 = "${@'+fp' if d.getVar('TARGET_FPU') == 'hard' else ''}"
+ARMFPARCHEXT:armv7a = "${@'+fp' if d.getVar('TARGET_FPU') == 'hard' else ''}"
+ARMFPARCHEXT:armv7ve = "${@'+fp' if d.getVar('TARGET_FPU') == 'hard' else ''}"
+
+BBCLASSEXTEND = "nativesdk"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/libgcc-initial_arm-11.2.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/libgcc-initial_arm-11.2.bb
new file mode 100644
index 0000000..ca1159c
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/libgcc-initial_arm-11.2.bb
@@ -0,0 +1,5 @@
+require recipes-devtools/gcc/gcc-${PV}.inc
+require recipes-devtools/gcc/libgcc-initial.inc
+
+# Building with thumb enabled on armv6t fails
+ARM_INSTRUCTION_SET:armv6 = "arm"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/libgcc_arm-11.2.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/libgcc_arm-11.2.bb
new file mode 100644
index 0000000..35ac5df
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/libgcc_arm-11.2.bb
@@ -0,0 +1,5 @@
+require recipes-devtools/gcc/gcc-${PV}.inc
+require recipes-devtools/gcc/libgcc.inc
+
+# Building with thumb enabled on armv6t fails
+ARM_INSTRUCTION_SET:armv6 = "arm"
diff --git a/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/libgfortran_arm-11.2.bb b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/libgfortran_arm-11.2.bb
new file mode 100644
index 0000000..fe63d0a
--- /dev/null
+++ b/meta-arm/meta-arm-toolchain/recipes-devtools/gcc/libgfortran_arm-11.2.bb
@@ -0,0 +1,2 @@
+require recipes-devtools/gcc/gcc-${PV}.inc
+require recipes-devtools/gcc/libgfortran.inc
diff --git a/meta-arm/meta-arm/README.md b/meta-arm/meta-arm/README.md
new file mode 100644
index 0000000..490ddca
--- /dev/null
+++ b/meta-arm/meta-arm/README.md
@@ -0,0 +1 @@
+See ../README.md
diff --git a/meta-arm/meta-arm/classes/fvpboot.bbclass b/meta-arm/meta-arm/classes/fvpboot.bbclass
new file mode 100644
index 0000000..ec9d4f5
--- /dev/null
+++ b/meta-arm/meta-arm/classes/fvpboot.bbclass
@@ -0,0 +1,84 @@
+# Image class to write .fvpconf files for use with runfvp. If this is desired
+# then add fvpboot to IMAGE_CLASSES, and set the variables below in your machine
+# configuration as appropriate.
+
+# Name of recipe providing FVP executable. If unset then the executable must be installed on the host.
+FVP_PROVIDER ?= ""
+# Name of FVP executable to run
+FVP_EXE ?= ""
+# Flags for --parameter/-C
+FVP_CONFIG ?= ""
+# Flags for --data
+FVP_DATA ?= ""
+# Flags for --application
+FVP_APPLICATIONS ?= ""
+# Flags to name serial terminals. Flag name is the terminal id (such as
+# terminal_0), value is a human-readable name. If the name is not set
+# then runfvp will hide the terminal.
+FVP_TERMINALS ?= ""
+# What terminal should be considered the primary console
+FVP_CONSOLE ?= ""
+# Flags for console names, as they appear in the FVP output. Flag name is an
+# application-specific id for the console for use in test cases
+FVP_CONSOLES[default] ?= "${FVP_CONSOLE}"
+# Arbitrary extra arguments
+FVP_EXTRA_ARGS ?= ""
+
+EXTRA_IMAGEDEPENDS += "${FVP_PROVIDER}"
+
+inherit image-artifact-names
+
+IMAGE_POSTPROCESS_COMMAND += "do_write_fvpboot_conf;"
+python do_write_fvpboot_conf() {
+    # Note that currently this JSON file is in development and the format may
+    # change at any point, so it should always be used with a matching runfvp.
+
+    import json, shlex
+
+    if not d.getVar("FVP_EXE"):
+        return
+
+    conffile = os.path.join(d.getVar("IMGDEPLOYDIR"), d.getVar("IMAGE_NAME") + ".fvpconf")
+    conffile_link = os.path.join(d.getVar("IMGDEPLOYDIR"), d.getVar("IMAGE_LINK_NAME") + ".fvpconf")
+
+    data = {}
+    provider = d.getVar("FVP_PROVIDER")
+    if provider:
+        data["provider"] = provider
+        data["fvp-bindir"] = os.path.join(d.getVar("COMPONENTS_DIR"),
+                                            d.getVar("BUILD_ARCH"),
+                                            provider,
+                                            "usr", "bin")
+
+    def getFlags(varname):
+        flags = d.getVarFlags(varname)
+        # For unexplained reasons, getVarFlags() returns None if there are no flags
+        if flags is None:
+            return {}
+        # For other reasons, you can't pass expand=True
+        return {key: d.expand(value) for key, value in flags.items()}
+
+    data["exe"] = d.getVar("FVP_EXE")
+    data["parameters"] = getFlags("FVP_CONFIG")
+    data["data"] = shlex.split(d.getVar("FVP_DATA") or "")
+    data["applications"] = getFlags("FVP_APPLICATIONS")
+    data["consoles"] = getFlags("FVP_CONSOLES")
+    data["terminals"] = getFlags("FVP_TERMINALS")
+    data["args"] = shlex.split(d.getVar("FVP_EXTRA_ARGS") or "")
+
+    os.makedirs(os.path.dirname(conffile), exist_ok=True)
+    with open(conffile, "wt") as f:
+        json.dump(data, f)
+
+    if conffile_link != conffile:
+        if os.path.lexists(conffile_link):
+           os.remove(conffile_link)
+        os.symlink(os.path.basename(conffile), conffile_link)
+}
+
+def fvpboot_vars(d):
+    vars = ['DEPLOY_DIR_IMAGE', 'IMAGE_NAME', 'IMAGE_LINK_NAME', 'COMPONENTS_DIR', 'BUILD_ARCH']
+    vars.extend((k for k in d.keys() if k.startswith('FVP_')))
+    return " ".join(vars)
+
+do_write_fvpboot_conf[vardeps] += "${@fvpboot_vars(d)}"
diff --git a/meta-arm/meta-arm/conf/layer.conf b/meta-arm/meta-arm/conf/layer.conf
new file mode 100644
index 0000000..c23ea56
--- /dev/null
+++ b/meta-arm/meta-arm/conf/layer.conf
@@ -0,0 +1,19 @@
+# We have a conf and classes directory, add to BBPATH
+BBPATH .= ":${LAYERDIR}"
+
+# We have recipes-* directories, add to BBFILES
+BBFILES += "${LAYERDIR}/recipes-*/*/*.bb \
+            ${LAYERDIR}/recipes-*/*/*.bbappend"
+
+BBFILE_COLLECTIONS += "meta-arm"
+BBFILE_PATTERN_meta-arm = "^${LAYERDIR}/"
+BBFILE_PRIORITY_meta-arm = "5"
+
+LAYERDEPENDS_meta-arm = " \
+    core \
+    arm-toolchain \
+"
+LAYERSERIES_COMPAT_meta-arm = "kirkstone"
+
+# runfvp --console needs telnet, so pull this in for testimage.
+HOSTTOOLS_NONFATAL += "telnet"
diff --git a/meta-arm/meta-arm/conf/machine/generic-arm64.conf b/meta-arm/meta-arm/conf/machine/generic-arm64.conf
new file mode 100644
index 0000000..2ee7805
--- /dev/null
+++ b/meta-arm/meta-arm/conf/machine/generic-arm64.conf
@@ -0,0 +1,18 @@
+#@TYPE: Machine
+#@NAME: generic-arm64
+#@DESCRIPTION: Generic Arm64 machine for typical SystemReady platforms, which
+#have working firmware and boot via EFI.
+
+require conf/machine/include/arm/arch-armv8a.inc
+
+PREFERRED_PROVIDER_virtual/kernel ?= "linux-yocto"
+KBUILD_DEFCONFIG = "defconfig"
+KCONFIG_MODE = "--alldefconfig"
+KERNEL_IMAGETYPE = "Image"
+MACHINE_EXTRA_RRECOMMENDS += "kernel-modules linux-firmware"
+
+IMAGE_FSTYPES ?= "wic"
+WKS_FILE ?= "efi-disk.wks.in"
+EFI_PROVIDER ?= "${@bb.utils.contains("DISTRO_FEATURES", "systemd", "systemd-boot", "grub-efi", d)}"
+
+MACHINE_FEATURES:append = " alsa bluetooth efi qemu-usermode rtc screen usbhost vfat wifi"
diff --git a/meta-arm/meta-arm/conf/machine/microbit-v1.conf b/meta-arm/meta-arm/conf/machine/microbit-v1.conf
new file mode 100644
index 0000000..ef3872f
--- /dev/null
+++ b/meta-arm/meta-arm/conf/machine/microbit-v1.conf
@@ -0,0 +1,24 @@
+#@TYPE: Machine
+#@NAME: microbit_v1
+#@DESCRIPTION: Machine for BBC Microbit v1, Zephyr BOARD qemu_cortex_m0
+
+require conf/machine/include/qemu.inc
+require conf/machine/include/arm/armv6m/tune-cortexm0.inc
+
+MACHINEOVERRIDES =. "nordic:"
+
+# GLIBC will not work with Cortex-M.
+TCLIBC = "newlib"
+
+# For runqemu
+QB_SYSTEM_NAME = "qemu-system-arm"
+QB_MACHINE = "-machine microbit"
+QB_CPU = "-cpu cortex-m0"
+QB_GRAPHICS = "-nographic -vga none"
+QB_RNG = ""
+QB_OPT_APPEND = "-icount shift=6,align=off,sleep=on -rtc clock=vm"
+
+# Zephyr RTOS settings
+ZEPHYR_BOARD = "qemu_cortex_m0"
+ZEPHYR_INHERIT_CLASSES += "zephyr-qemuboot"
+ARCH:qemu-cortex-m0 = "arm"
diff --git a/meta-arm/meta-arm/conf/machine/qemu-cortex-a53.conf b/meta-arm/meta-arm/conf/machine/qemu-cortex-a53.conf
new file mode 100644
index 0000000..7147fac
--- /dev/null
+++ b/meta-arm/meta-arm/conf/machine/qemu-cortex-a53.conf
@@ -0,0 +1,19 @@
+#@TYPE: Machine
+#@NAME: qemu-cortex-a53
+#@DESCRIPTION: Machine for Zephyr BOARD qemu_cortex_a53
+
+require conf/machine/include/qemu.inc
+require conf/machine/include/arm/armv8a/tune-cortexa53.inc
+
+TCLIBC = "newlib"
+
+# For runqemu
+QB_SYSTEM_NAME = "qemu-system-aarch64"
+QB_MACHINE = "-machine virt"
+QB_CPU = "-cpu cortex-a53"
+QB_GRAPHICS = "-nographic -vga none"
+
+# Zephyr RTOS settings
+ZEPHYR_BOARD = "qemu_cortex_a53"
+ZEPHYR_INHERIT_CLASSES += "zephyr-qemuboot"
+ARCH:qemu-cortex-a53 = "aarch64"
diff --git a/meta-arm/meta-arm/conf/machine/qemu-generic-arm64.conf b/meta-arm/meta-arm/conf/machine/qemu-generic-arm64.conf
new file mode 100644
index 0000000..717a45c
--- /dev/null
+++ b/meta-arm/meta-arm/conf/machine/qemu-generic-arm64.conf
@@ -0,0 +1,35 @@
+#@TYPE: Machine
+#@NAME: qemu-generic-arm64
+#@DESCRIPTION: Generic Arm64 machine for typical SystemReady platforms, which
+#have working firmware and boot via EFI.
+
+MACHINEOVERRIDES =. "generic-arm64:"
+
+require conf/machine/generic-arm64.conf
+require conf/machine/include/qemu.inc
+
+SERIAL_CONSOLES ?= "115200;ttyAMA0 115200;hvc0"
+SERIAL_CONSOLES_CHECK = "${SERIAL_CONSOLES}"
+
+EXTRA_IMAGEDEPENDS += "edk2-firmware"
+
+# This unique WIC file is necessary because kernel boot args cannot be passed
+# because there is no default kernel (see below).  There is no default kernel
+# because QEMU will only allow firmware or kernel to be passed in as a
+# parameter, and we need the firmware.  So, to allow for "ip=dhcp" as a kernel
+# boot arg (which we need for testimage), we have to have a WIC file unique to
+# this platform.
+WKS_FILE = "qemu-efi-disk.wks.in"
+IMAGE_FSTYPES += "wic.qcow2"
+
+QB_SYSTEM_NAME = "qemu-system-aarch64"
+QB_MACHINE = "-machine sbsa-ref"
+QB_MEM = "-m 1024"
+QB_DEFAULT_FSTYPE = "wic.qcow2"
+QB_NETWORK_DEVICE = "-device virtio-net-pci,netdev=net0,mac=@MAC@"
+QB_DRIVE_TYPE = "/dev/hd"
+QB_ROOTFS_OPT = "-drive file=@ROOTFS@,if=ide,format=qcow2"
+QB_DEFAULT_KERNEL = "none"
+QB_OPT_APPEND = "-device qemu-xhci -device usb-tablet -device usb-kbd -pflash @DEPLOY_DIR_IMAGE@/SBSA_FLASH0.fd -pflash @DEPLOY_DIR_IMAGE@/SBSA_FLASH1.fd"
+QB_SERIAL_OPT = "-device virtio-serial-pci -chardev null,id=virtcon -device virtconsole,chardev=virtcon"
+QB_TCPSERIAL_OPT = "-device virtio-serial-pci -chardev socket,id=virtcon,port=@PORT@,host=127.0.0.1 -device virtconsole,chardev=virtcon"
diff --git a/meta-arm/meta-arm/conf/machine/qemuarm-secureboot.conf b/meta-arm/meta-arm/conf/machine/qemuarm-secureboot.conf
new file mode 100644
index 0000000..a459f3f
--- /dev/null
+++ b/meta-arm/meta-arm/conf/machine/qemuarm-secureboot.conf
@@ -0,0 +1,25 @@
+MACHINEOVERRIDES =. "qemuarm:"
+
+require ${COREBASE}/meta/conf/machine/qemuarm.conf
+
+# secure=on can't ever use KVM, so force it off
+QEMU_USE_KVM = ""
+
+QB_MACHINE = "-machine virt,highmem=off,secure=on"
+QB_MEM = "-m 1024"
+QB_DEFAULT_FSTYPE = "wic.qcow2"
+QB_DEFAULT_BIOS = "flash.bin"
+QB_FSINFO = "wic:no-kernel-in-fs"
+QB_ROOTFS_OPT = ""
+QB_KERNEL_ROOT = "/dev/vda2"
+
+IMAGE_FSTYPES += "wic wic.qcow2"
+
+WKS_FILE ?= "qemuarm.wks"
+WKS_FILE_DEPENDS = "trusted-firmware-a"
+IMAGE_BOOT_FILES = "${KERNEL_IMAGETYPE}"
+
+MACHINE_FEATURES += "optee-ftpm"
+
+# FIXME - CPUs 2-4 don't start in the newer OPTEE
+PREFERRED_VERSION_optee-os ?= "3.14%"
diff --git a/meta-arm/meta-arm/conf/machine/qemuarm64-secureboot.conf b/meta-arm/meta-arm/conf/machine/qemuarm64-secureboot.conf
new file mode 100644
index 0000000..55c4cab
--- /dev/null
+++ b/meta-arm/meta-arm/conf/machine/qemuarm64-secureboot.conf
@@ -0,0 +1,25 @@
+MACHINEOVERRIDES =. "qemuarm64:"
+
+require ${COREBASE}/meta/conf/machine/qemuarm64.conf
+
+KMACHINE = "qemuarm64"
+
+# secure=on can't ever use KVM, so force it off
+QEMU_USE_KVM = ""
+
+QB_MACHINE = "-machine virt,secure=on"
+QB_OPT_APPEND += "-no-acpi"
+QB_MEM = "-m 1024"
+QB_DEFAULT_FSTYPE = "wic.qcow2"
+QB_DEFAULT_BIOS = "flash.bin"
+QB_FSINFO = "wic:no-kernel-in-fs"
+QB_ROOTFS_OPT = ""
+QB_KERNEL_ROOT = "/dev/vda2"
+
+IMAGE_FSTYPES += "wic wic.qcow2"
+
+WKS_FILE ?= "qemuarm64.wks"
+WKS_FILE_DEPENDS = "trusted-firmware-a"
+IMAGE_BOOT_FILES = "${KERNEL_IMAGETYPE}"
+
+MACHINE_FEATURES += "optee-ftpm"
diff --git a/meta-arm/meta-arm/lib/fvp/__init__.py b/meta-arm/meta-arm/lib/fvp/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/meta-arm/meta-arm/lib/fvp/__init__.py
diff --git a/meta-arm/meta-arm/lib/fvp/conffile.py b/meta-arm/meta-arm/lib/fvp/conffile.py
new file mode 100644
index 0000000..acede40
--- /dev/null
+++ b/meta-arm/meta-arm/lib/fvp/conffile.py
@@ -0,0 +1,58 @@
+import json
+import pathlib
+import os
+
+
+def get_image_directory(machine=None):
+    """
+    Get the DEPLOY_DIR_IMAGE for the specified machine
+    (or the configured machine if not set).
+    """
+    try:
+        import bb.tinfoil
+    except ImportError as e:
+        raise RuntimeError("Cannot connect to BitBake, did you oe-init-build-env?") from e
+
+    if machine:
+        os.environ["MACHINE"] = machine
+
+    with bb.tinfoil.Tinfoil() as tinfoil:
+        tinfoil.prepare(config_only=True)
+        image_dir = tinfoil.config_data.getVar("DEPLOY_DIR_IMAGE")
+        return pathlib.Path(image_dir)
+
+def find(machine):
+    image_dir = get_image_directory(machine)
+    # All .fvpconf configuration files
+    configs = image_dir.glob("*.fvpconf")
+    # Just the files
+    configs = [p for p in configs if p.is_file() and not p.is_symlink()]
+    if not configs:
+            print(f"Cannot find any .fvpconf in {image_dir}")
+            raise RuntimeError()
+    # Sorted by modification time
+    configs = sorted(configs, key=lambda p: p.stat().st_mtime)
+    return configs[-1]
+
+
+def load(config_file):
+    with open(config_file) as f:
+        config = json.load(f)
+
+    # Ensure that all expected keys are present
+    def sanitise(key, value):
+        if key not in config or config[key] is None:
+            config[key] = value
+    sanitise("fvp-bindir", "")
+    sanitise("exe", "")
+    sanitise("parameters", {})
+    sanitise("data", {})
+    sanitise("applications", {})
+    sanitise("terminals", {})
+    sanitise("args", [])
+    sanitise("consoles", {})
+
+    if not config["exe"]:
+        raise ValueError("Required value FVP_EXE not set in machine configuration")
+
+    return config
diff --git a/meta-arm/meta-arm/lib/fvp/runner.py b/meta-arm/meta-arm/lib/fvp/runner.py
new file mode 100644
index 0000000..7641cd6
--- /dev/null
+++ b/meta-arm/meta-arm/lib/fvp/runner.py
@@ -0,0 +1,139 @@
+import asyncio
+import re
+import subprocess
+import os
+import shutil
+import sys
+
+from .terminal import terminals
+
+
+def cli_from_config(config, terminal_choice):
+    cli = []
+    if config["fvp-bindir"]:
+        cli.append(os.path.join(config["fvp-bindir"], config["exe"]))
+    else:
+        cli.append(config["exe"])
+
+    for param, value in config["parameters"].items():
+        cli.extend(["--parameter", f"{param}={value}"])
+
+    for value in config["data"]:
+        cli.extend(["--data", value])
+
+    for param, value in config["applications"].items():
+        cli.extend(["--application", f"{param}={value}"])
+
+    for terminal, name in config["terminals"].items():
+        # If terminals are enabled and this terminal has been named
+        if terminal_choice != "none" and name:
+            # TODO if raw mode
+            # cli.extend(["--parameter", f"{terminal}.mode=raw"])
+            # TODO put name into terminal title
+            cli.extend(["--parameter", f"{terminal}.terminal_command={terminals[terminal_choice].command}"])
+        else:
+            # Disable terminal
+            cli.extend(["--parameter", f"{terminal}.start_telnet=0"])
+
+    cli.extend(config["args"])
+
+    return cli
+
+def check_telnet():
+    # Check that telnet is present
+    if not bool(shutil.which("telnet")):
+        raise RuntimeError("Cannot find telnet, this is needed to connect to the FVP.")
+
+class FVPRunner:
+    def __init__(self, logger):
+        self._terminal_ports = {}
+        self._line_callbacks = []
+        self._logger = logger
+        self._fvp_process = None
+        self._telnets = []
+        self._pexpects = []
+
+    def add_line_callback(self, callback):
+        self._line_callbacks.append(callback)
+
+    async def start(self, config, extra_args=[], terminal_choice="none"):
+        cli = cli_from_config(config, terminal_choice)
+        cli += extra_args
+        self._logger.debug(f"Constructed FVP call: {cli}")
+        self._fvp_process = await asyncio.create_subprocess_exec(*cli, stdin=subprocess.DEVNULL, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
+
+        def detect_terminals(line):
+            m = re.match(r"^(\S+): Listening for serial connection on port (\d+)$", line)
+            if m:
+                terminal = m.group(1)
+                port = int(m.group(2))
+                self._terminal_ports[terminal] = port
+        self.add_line_callback(detect_terminals)
+
+    async def stop(self):
+        if self._fvp_process:
+            self._logger.debug(f"Terminating FVP PID {self._fvp_process.pid}")
+            try:
+                self._fvp_process.terminate()
+                await asyncio.wait_for(self._fvp_process.wait(), 10.0)
+            except asyncio.TimeoutError:
+                self._logger.debug(f"Killing FVP PID {self._fvp_process.pid}")
+                self._fvp_process.kill()
+            except ProcessLookupError:
+                pass
+
+        for telnet in self._telnets:
+            try:
+                telnet.terminate()
+                await asyncio.wait_for(telnet.wait(), 10.0)
+            except asyncio.TimeoutError:
+                telnet.kill()
+            except ProcessLookupError:
+                pass
+
+        for console in self._pexpects:
+            import pexpect
+            # Ensure pexpect logs all remaining output to the logfile
+            console.expect(pexpect.EOF, timeout=5.0)
+            console.close()
+
+        if self._fvp_process and self._fvp_process.returncode:
+            self._logger.info(f"FVP quit with code {self._fvp_process.returncode}")
+            return self._fvp_process.returncode
+        else:
+            return 0
+
+    async def run(self, until=None):
+        if until and until():
+            return
+
+        async for line in self._fvp_process.stdout:
+            line = line.strip().decode("utf-8", errors="replace")
+            for callback in self._line_callbacks:
+                callback(line)
+            if until and until():
+                return
+
+    async def _get_terminal_port(self, terminal, timeout):
+        def terminal_exists():
+            return terminal in self._terminal_ports
+        await asyncio.wait_for(self.run(terminal_exists), timeout)
+        return self._terminal_ports[terminal]
+
+    async def create_telnet(self, terminal, timeout=15.0):
+        check_telnet()
+        port = await self._get_terminal_port(terminal, timeout)
+        telnet = await asyncio.create_subprocess_exec("telnet", "localhost", str(port), stdin=sys.stdin, stdout=sys.stdout)
+        self._telnets.append(telnet)
+        return telnet
+
+    async def create_pexpect(self, terminal, timeout=15.0, **kwargs):
+        check_telnet()
+        import pexpect
+        port = await self._get_terminal_port(terminal, timeout)
+        instance = pexpect.spawn(f"telnet localhost {port}", **kwargs)
+        self._pexpects.append(instance)
+        return instance
+
+    def pid(self):
+        return self._fvp_process.pid
diff --git a/meta-arm/meta-arm/lib/fvp/terminal.py b/meta-arm/meta-arm/lib/fvp/terminal.py
new file mode 100644
index 0000000..6f40815
--- /dev/null
+++ b/meta-arm/meta-arm/lib/fvp/terminal.py
@@ -0,0 +1,59 @@
+import shutil
+import collections
+import pathlib
+import os
+
+from typing import List, Optional
+
+
+def get_config_dir() -> pathlib.Path:
+    value = os.environ.get("XDG_CONFIG_HOME")
+    if value and os.path.isabs(value):
+        return pathlib.Path(value)
+    else:
+        return pathlib.Path.home() / ".config"
+
+class Terminals:
+    Terminal = collections.namedtuple("Terminal", ["priority", "name", "command"])
+
+    def __init__(self):
+        self.terminals = []
+
+    def add_terminal(self, priority, name, command):
+        self.terminals.append(Terminals.Terminal(priority, name, command))
+        # Keep this list sorted by priority
+        self.terminals.sort(reverse=True, key=lambda t: t.priority)
+        self.name_map = {t.name: t for t in self.terminals}
+
+    def configured_terminal(self) -> Optional[str]:
+        import configparser
+
+        config = configparser.ConfigParser()
+        config.read(get_config_dir() / "runfvp.conf")
+        return config.get("RunFVP", "Terminal", fallback=None)
+
+    def preferred_terminal(self) -> str:
+        import shlex
+
+        preferred = self.configured_terminal()
+        if preferred:
+            return preferred
+
+        for t in self.terminals:
+            if t.command and shutil.which(shlex.split(t.command)[0]):
+                return t.name
+        return self.terminals[-1].name
+
+    def all_terminals(self) -> List[str]:
+        return self.name_map.keys()
+
+    def __getitem__(self, name: str):
+        return self.name_map[name]
+
+terminals = Terminals()
+# TODO: option to switch between telnet and netcat
+connect_command = "telnet localhost %port"
+terminals.add_terminal(2, "tmux", f"tmux new-window -n \"%title\" \"{connect_command}\""),
+terminals.add_terminal(2, "gnome-terminal", f"gnome-terminal --window --title \"%title\" --command \"{connect_command}\""),
+terminals.add_terminal(1, "xterm", f"xterm -title \"%title\" -e {connect_command}"),
+terminals.add_terminal(0, "none", None)
diff --git a/meta-arm/meta-arm/lib/oeqa/controllers/__init__.py b/meta-arm/meta-arm/lib/oeqa/controllers/__init__.py
new file mode 100644
index 0000000..df3c142
--- /dev/null
+++ b/meta-arm/meta-arm/lib/oeqa/controllers/__init__.py
@@ -0,0 +1,3 @@
+# This is needed so that multiple locations can provide the same package
+from pkgutil import extend_path
+__path__ = extend_path(__path__, __name__)
diff --git a/meta-arm/meta-arm/lib/oeqa/controllers/fvp.py b/meta-arm/meta-arm/lib/oeqa/controllers/fvp.py
new file mode 100644
index 0000000..c8dcf29
--- /dev/null
+++ b/meta-arm/meta-arm/lib/oeqa/controllers/fvp.py
@@ -0,0 +1,147 @@
+import asyncio
+import pathlib
+import pexpect
+import os
+
+from oeqa.core.target.ssh import OESSHTarget
+from fvp import conffile, runner
+
+
+class OEFVPSSHTarget(OESSHTarget):
+    """
+    Base class for meta-arm FVP targets.
+    Contains common logic to start and stop an FVP.
+    """
+    def __init__(self, logger, target_ip, server_ip, timeout=300, user='root',
+                 port=None, dir_image=None, rootfs=None, **kwargs):
+        super().__init__(logger, target_ip, server_ip, timeout, user, port)
+        image_dir = pathlib.Path(dir_image)
+        # rootfs may have multiple extensions so we need to strip *all* suffixes
+        basename = pathlib.Path(rootfs)
+        basename = basename.name.replace("".join(basename.suffixes), "")
+        self.fvpconf = image_dir / (basename + ".fvpconf")
+        self.config = conffile.load(self.fvpconf)
+
+        if not self.fvpconf.exists():
+            raise FileNotFoundError(f"Cannot find {self.fvpconf}")
+
+    async def boot_fvp(self):
+        self.fvp = runner.FVPRunner(self.logger)
+        await self.fvp.start(self.config)
+        self.logger.debug(f"Started FVP PID {self.fvp.pid()}")
+        await self._after_start()
+
+    async def _after_start(self):
+        pass
+
+    async def _after_stop(self):
+        pass
+
+    async def stop_fvp(self):
+        returncode = await self.fvp.stop()
+        await self._after_stop()
+
+        self.logger.debug(f"Stopped FVP with return code {returncode}")
+
+    def start(self, **kwargs):
+        # When we can assume Py3.7+, this can simply be asyncio.run()
+        loop = asyncio.get_event_loop()
+        loop.run_until_complete(asyncio.gather(self.boot_fvp()))
+
+    def stop(self, **kwargs):
+        loop = asyncio.get_event_loop()
+        loop.run_until_complete(asyncio.gather(self.stop_fvp()))
+
+
+class OEFVPTarget(OEFVPSSHTarget):
+    """
+    For compatibility with OE-core test cases, this target's start() method
+    waits for a Linux shell before returning to ensure that SSH commands work
+    with the default test dependencies.
+    """
+    def __init__(self, logger, target_ip, server_ip, bootlog=None, **kwargs):
+        super().__init__(logger, target_ip, server_ip, **kwargs)
+        self.logfile = bootlog and open(bootlog, "wb") or None
+
+        # FVPs boot slowly, so allow ten minutes
+        self.boot_timeout = 10 * 60
+
+    async def _after_start(self):
+        self.logger.debug(f"Awaiting console on terminal {self.config['consoles']['default']}")
+        console = await self.fvp.create_pexpect(self.config['consoles']['default'])
+        try:
+            console.expect("login\\:", timeout=self.boot_timeout)
+            self.logger.debug("Found login prompt")
+        except pexpect.TIMEOUT:
+            self.logger.info("Timed out waiting for login prompt.")
+            self.logger.info("Boot log follows:")
+            self.logger.info(b"\n".join(console.before.splitlines()[-200:]).decode("utf-8", errors="replace"))
+            raise RuntimeError("Failed to start FVP.")
+
+
+class OEFVPSerialTarget(OEFVPSSHTarget):
+    """
+    This target is intended for interaction with the target over one or more
+    telnet consoles using pexpect.
+    
+    This still depends on OEFVPSSHTarget so SSH commands can still be run on
+    the target, but note that this class does not inherently guarantee that
+    the SSH server is running prior to running test cases. Test cases that use
+    SSH should first validate that SSH is available, e.g. by depending on the
+    "linuxboot" test case in meta-arm.
+    """
+    DEFAULT_CONSOLE = "default"
+
+    def __init__(self, logger, target_ip, server_ip, bootlog=None, **kwargs):
+        super().__init__(logger, target_ip, server_ip, **kwargs)
+        self.terminals = {}
+
+        self.test_log_path = pathlib.Path(bootlog).parent
+        self.test_log_suffix = pathlib.Path(bootlog).suffix
+        self.bootlog = bootlog
+
+    async def _add_terminal(self, name, fvp_name):
+        logfile = self._create_logfile(name)
+        self.logger.info(f'Creating terminal {name} on {fvp_name}')
+        self.terminals[name] = \
+            await self.fvp.create_pexpect(fvp_name, logfile=logfile)
+
+    def _create_logfile(self, name):
+        fvp_log_file = f"{name}_log{self.test_log_suffix}"
+        fvp_log_path = pathlib.Path(self.test_log_path, fvp_log_file)
+        fvp_log_symlink = pathlib.Path(self.test_log_path, f"{name}_log")
+        try:
+            os.remove(fvp_log_symlink)
+        except:
+            pass
+        os.symlink(fvp_log_file, fvp_log_symlink)
+        return open(fvp_log_path, 'wb')
+
+    async def _after_start(self):
+        for name, console in self.config["consoles"].items():
+            await self._add_terminal(name, console)
+
+            # testimage.bbclass expects to see a log file at `bootlog`,
+            # so make a symlink to the 'default' log file
+            if name == 'default':
+                default_test_file = f"{name}_log{self.test_log_suffix}"
+                os.symlink(default_test_file, self.bootlog)
+
+    def _get_terminal(self, name):
+        return self.terminals[name]
+
+    def __getattr__(self, name):
+        """
+        Magic method which automatically exposes the whole pexpect API on the
+        target, with the first argument being the terminal name.
+
+        e.g. self.target.expect(self.target.DEFAULT_CONSOLE, "login\\:")
+        """
+        def call_pexpect(terminal, *args, **kwargs):
+            attr = getattr(self.terminals[terminal], name)
+            if callable(attr):
+                return attr(*args, **kwargs)
+            else:
+                return attr
+
+        return call_pexpect
diff --git a/meta-arm/meta-arm/lib/oeqa/runtime/cases/linuxboot.py b/meta-arm/meta-arm/lib/oeqa/runtime/cases/linuxboot.py
new file mode 100644
index 0000000..8994405
--- /dev/null
+++ b/meta-arm/meta-arm/lib/oeqa/runtime/cases/linuxboot.py
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: MIT
+
+from oeqa.runtime.case import OERuntimeTestCase
+
+
+class LinuxBootTest(OERuntimeTestCase):
+    """
+    This test case is only compatible with the OEFVPSerialTarget as it uses
+    the pexpect interface. It waits for a Linux login prompt on the default
+    console.
+    """
+
+    def setUp(self):
+        self.console = self.target.DEFAULT_CONSOLE
+
+    def test_linux_boot(self):
+        self.logger.info(f"{self.console}: Waiting for login prompt")
+        self.target.expect(self.console, r"login\:", timeout=10*60)
diff --git a/meta-arm/meta-arm/lib/oeqa/selftest/cases/runfvp.py b/meta-arm/meta-arm/lib/oeqa/selftest/cases/runfvp.py
new file mode 100644
index 0000000..d1e452f
--- /dev/null
+++ b/meta-arm/meta-arm/lib/oeqa/selftest/cases/runfvp.py
@@ -0,0 +1,109 @@
+import asyncio
+import os
+import pathlib
+import subprocess
+import tempfile
+import unittest.mock
+
+from oeqa.selftest.case import OESelftestTestCase
+
+runfvp = pathlib.Path(__file__).parents[5] / "scripts" / "runfvp"
+testdir = pathlib.Path(__file__).parent / "tests"
+
+class RunFVPTests(OESelftestTestCase):
+    def setUpLocal(self):
+        self.assertTrue(runfvp.exists())
+
+    def run_fvp(self, *args, should_succeed=True):
+        """
+        Call runfvp passing any arguments. If check is True verify return stdout
+        on exit code 0 or fail the test, otherwise return the CompletedProcess
+        instance.
+        """
+        # Put the test directory in PATH so that any mock FVPs are found first
+        newenv = {"PATH": str(testdir) + ":" + os.environ["PATH"]}
+        cli = [runfvp,] + list(args)
+        print(f"Calling {cli}")
+        ret = subprocess.run(cli, env=newenv, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, universal_newlines=True)
+        if should_succeed:
+            self.assertEqual(ret.returncode, 0, f"runfvp exit {ret.returncode}, output: {ret.stdout}")
+            return ret.stdout
+        else:
+            self.assertNotEqual(ret.returncode, 0, f"runfvp exit {ret.returncode}, output: {ret.stdout}")
+            return ret.stdout
+
+    def test_help(self):
+        output = self.run_fvp("--help")
+        self.assertIn("Run images in a FVP", output)
+
+    def test_bad_options(self):
+        self.run_fvp("--this-is-an-invalid-option", should_succeed=False)
+
+    def test_run_auto_tests(self):
+        newenv = {"PATH": str(testdir) + ":" + os.environ["PATH"]}
+
+        cases = list(testdir.glob("auto-*.json"))
+        if not cases:
+            self.fail("No tests found")
+        for case in cases:
+            with self.subTest(case=case.stem):
+                self.run_fvp(case)
+
+    def test_fvp_options(self):
+        # test-parameter sets one argument, add another manually
+        self.run_fvp(testdir / "test-parameter.json", "--", "--parameter", "board.dog=woof")
+
+class ConfFileTests(OESelftestTestCase):
+    def test_no_exe(self):
+        from fvp import conffile
+        with tempfile.NamedTemporaryFile('w') as tf:
+            tf.write('{}')
+            tf.flush()
+
+            with self.assertRaises(ValueError):
+                conffile.load(tf.name)
+
+    def test_minimal(self):
+        from fvp import conffile
+        with tempfile.NamedTemporaryFile('w') as tf:
+            tf.write('{"exe": "FVP_Binary"}')
+            tf.flush()
+
+            conf = conffile.load(tf.name)
+            self.assertTrue('fvp-bindir' in conf)
+            self.assertTrue('fvp-bindir' in conf)
+            self.assertTrue("exe" in conf)
+            self.assertTrue("parameters" in conf)
+            self.assertTrue("data" in conf)
+            self.assertTrue("applications" in conf)
+            self.assertTrue("terminals" in conf)
+            self.assertTrue("args" in conf)
+            self.assertTrue("consoles" in conf)
+
+
+class RunnerTests(OESelftestTestCase):
+    def create_mock(self):
+        return unittest.mock.patch("asyncio.create_subprocess_exec")
+
+    def test_start(self):
+        from fvp import runner
+        with self.create_mock() as m:
+            fvp = runner.FVPRunner(self.logger)
+            asyncio.run(fvp.start({
+                "fvp-bindir": "/usr/bin",
+                "exe": "FVP_Binary",
+                "parameters": {'foo': 'bar'},
+                "data": ['data1'],
+                "applications": {'a1': 'file'},
+                "terminals": {},
+                "args": ['--extra-arg'],
+            }))
+
+            m.assert_called_once_with('/usr/bin/FVP_Binary',
+                '--parameter', 'foo=bar',
+                '--data', 'data1',
+                '--application', 'a1=file',
+                '--extra-arg',
+                stdin=unittest.mock.ANY,
+                stdout=unittest.mock.ANY,
+                stderr=unittest.mock.ANY)
diff --git a/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/auto-basic.json b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/auto-basic.json
new file mode 100644
index 0000000..476eb57
--- /dev/null
+++ b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/auto-basic.json
@@ -0,0 +1,3 @@
+{
+    "exe": "auto-basic.sh"
+}
diff --git a/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/auto-basic.sh b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/auto-basic.sh
new file mode 100755
index 0000000..ea9abac
--- /dev/null
+++ b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/auto-basic.sh
@@ -0,0 +1,11 @@
+#! /bin/sh
+
+set -e -u
+
+if [ $# = 0 ]; then
+    echo No arguments as expected
+    exit 0
+else
+    echo Unexpected arguments: $*
+    exit 1
+fi
diff --git a/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/auto-parameters.json b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/auto-parameters.json
new file mode 100644
index 0000000..0c7d4ef
--- /dev/null
+++ b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/auto-parameters.json
@@ -0,0 +1,7 @@
+{
+    "exe": "test-parameters.py",
+    "parameters": {
+        "board.cow": "moo",
+        "board.dog": "woof"
+    }
+}
diff --git a/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/mock-fvp.py b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/mock-fvp.py
new file mode 100755
index 0000000..2213c9f
--- /dev/null
+++ b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/mock-fvp.py
@@ -0,0 +1,22 @@
+#! /usr/bin/env python3
+
+import argparse
+import sys
+
+def do_test_parameters(args):
+    if not args.parameter or set(args.parameter) != set(("board.cow=moo", "board.dog=woof")):
+        print(f"Unexpected arguments: {args}")
+        sys.exit(1)
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-C", "--parameter", action="append")
+    args = parser.parse_args()
+
+    function = "do_" + parser.prog.replace("-", "_").replace(".py", "")
+    if function in locals():
+        locals()[function](args)
+    else:
+        print(f"Unknown mock mode {parser.prog}")
+        sys.exit(1)
diff --git a/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/test-parameter.json b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/test-parameter.json
new file mode 100644
index 0000000..9b565f2
--- /dev/null
+++ b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/test-parameter.json
@@ -0,0 +1,6 @@
+{
+    "exe": "test-parameters.py",
+    "parameters": {
+        "board.cow": "moo"
+    }
+}
diff --git a/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/test-parameters.py b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/test-parameters.py
new file mode 120000
index 0000000..c734eec
--- /dev/null
+++ b/meta-arm/meta-arm/lib/oeqa/selftest/cases/tests/test-parameters.py
@@ -0,0 +1 @@
+mock-fvp.py
\ No newline at end of file
diff --git a/meta-arm/meta-arm/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_git.bb b/meta-arm/meta-arm/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_git.bb
new file mode 100644
index 0000000..74adaf3
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_git.bb
@@ -0,0 +1,92 @@
+SUMMARY = "Linux aarch64 boot wrapper with FDT support"
+LICENSE = "BSD-3-Clause"
+
+LIC_FILES_CHKSUM = "file://LICENSE.txt;md5=bb63326febfb5fb909226c8e7ebcef5c"
+
+SRC_URI = "git://git.kernel.org/pub/scm/linux/kernel/git/mark/boot-wrapper-aarch64.git;branch=master"
+SRCREV = "1044c77062573985f7c994c3b6cef5695f57e955"
+
+PV = "git${SRCPV}"
+
+S = "${WORKDIR}/git"
+
+inherit autotools deploy
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+COMPATIBLE_MACHINE ?= "invalid"
+
+# Device tree to put in the image
+# by default use the standard kernel devicetree
+# This should be overwritten if the devicetree is not generated
+# by the kernel.
+# This should point to a file in the deploy image directory
+BOOT_WRAPPER_AARCH64_DEVICETREE ??= "${KERNEL_DEVICETREE}"
+
+# Kernel image to put in the image
+# This should point to a file in the deploy image directory
+BOOT_WRAPPER_AARCH64_KERNEL ??= "Image"
+
+# Kernel command line for the image
+BOOT_WRAPPER_AARCH64_CMDLINE ??= "rw"
+
+# Image generated by boot wrapper
+BOOT_WRAPPER_AARCH64_IMAGE ??= "linux-system.axf"
+
+DEPENDS += "virtual/kernel dtc-native"
+
+EXTRA_OECONF += "--with-kernel-dir=${WORKDIR}/kernel"
+EXTRA_OECONF += "--with-dtb=${WORKDIR}/kernel/dummy.dtb"
+EXTRA_OECONF += "--with-cmdline=\"\""
+EXTRA_OECONF += "--enable-psci --enable-gicv3"
+
+# unset LDFLAGS solves this error when compiling kernel modules:
+# aarch64-poky-linux-ld: unrecognized option '-Wl,-O1'
+EXTRA_OEMAKE += "'LDFLAGS= --gc-sections '"
+
+# Strip prefix if any
+REAL_DTB = "${@os.path.basename(d.getVar('BOOT_WRAPPER_AARCH64_DEVICETREE'))}"
+
+EXTRA_OEMAKE += "'KERNEL_DTB=${DEPLOY_DIR_IMAGE}/${REAL_DTB}'"
+EXTRA_OEMAKE += "'KERNEL_IMAGE=${DEPLOY_DIR_IMAGE}/${BOOT_WRAPPER_AARCH64_KERNEL}'"
+EXTRA_OEMAKE += "'CMDLINE=${BOOT_WRAPPER_AARCH64_CMDLINE}'"
+
+
+do_configure:prepend() {
+    # Create dummy files to make configure happy.
+    # We will pass the generated ones directly to make.
+    mkdir -p ${WORKDIR}/kernel/arch/arm64/boot
+    echo "dummy" > ${WORKDIR}/kernel/arch/arm64/boot/Image
+    echo "dummy" > ${WORKDIR}/kernel/dummy.dtb
+
+    # Generate configure
+    (cd ${S} && autoreconf -i || exit 1)
+}
+
+do_compile[noexec] = "1"
+do_install[noexec] = "1"
+
+# We need the kernel to create an image
+do_deploy[depends] += "virtual/kernel:do_deploy"
+
+do_deploy() {
+    if [ ! -f ${DEPLOY_DIR_IMAGE}/${REAL_DTB} ]; then
+        echo "ERROR: cannot find ${REAL_DTB} in ${DEPLOY_DIR_IMAGE}" >&2
+        echo "Please check your BOOT_WRAPPER_AARCH64_DEVICETREE settings" >&2
+        exit 1
+    fi
+
+    if [ ! -f ${DEPLOY_DIR_IMAGE}/${BOOT_WRAPPER_AARCH64_KERNEL} ]; then
+        echo "ERROR: cannot find ${BOOT_WRAPPER_AARCH64_KERNEL}" \
+            " in ${DEPLOY_DIR_IMAGE}" >&2
+        echo "Please check your BOOT_WRAPPER_AARCH64_KERNEL settings" >&2
+        exit 1
+    fi
+
+    oe_runmake clean
+    oe_runmake all
+
+    install -D -p -m 644 ${BOOT_WRAPPER_AARCH64_IMAGE} \
+        ${DEPLOYDIR}/linux-system.axf
+}
+addtask deploy before do_build after do_compile
diff --git a/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/0001-define-_Noreturn-if-needed.patch b/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/0001-define-_Noreturn-if-needed.patch
new file mode 100644
index 0000000..b73c533
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/0001-define-_Noreturn-if-needed.patch
@@ -0,0 +1,33 @@
+From 0d941ba32a082023575fd0d14d52a12b7547b367 Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Tue, 19 Apr 2022 22:32:56 -0700
+Subject: [PATCH] define _Noreturn if needed
+
+The new _Noreturn function specifier is not recognized by the parser and shows as a syntax error:
+
+Fixes
+../git/inc/hf/panic.h:13:1: error: '_Noreturn' is a C11 extension [-Werror,-Wc11-extensions]
+noreturn void panic(const char *fmt, ...);
+^
+
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ inc/hf/panic.h | 4 ++++
+ 1 file changed, 4 insertions(+)
+
+diff --git a/inc/hf/panic.h b/inc/hf/panic.h
+index ec864e4f..588f1193 100644
+--- a/inc/hf/panic.h
++++ b/inc/hf/panic.h
+@@ -10,4 +10,8 @@
+ 
+ #include <stdnoreturn.h>
+ 
++#ifndef _Noreturn
++#define _Noreturn __attribute__ ((noreturn))
++#endif
++
+ noreturn void panic(const char *fmt, ...);
+-- 
+2.36.0
+
diff --git a/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/host-ld.patch b/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/host-ld.patch
new file mode 100644
index 0000000..040d61b
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/host-ld.patch
@@ -0,0 +1,21 @@
+We need to be sure that the host linker flags are passed to the kernel build,
+as otherwise it is possible that binaries are incorrectly linked. For example:
+
+HOSTCC scripts/extract-cert
+ld: .../recipe-sysroot-native/usr/lib/pkgconfig/../../../usr/lib/libcrypto.so: undefined reference to `pthread_once@GLIBC_2.34'
+
+Upstream-Status: Inappropriate
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+diff --git a/build/linux/linux.gni b/build/linux/linux.gni
+index 65cc9df..19adbfb 100644
+--- a/build/linux/linux.gni
++++ b/build/linux/linux.gni
+@@ -60,6 +60,7 @@ template("linux_kernel") {
+     "LLVM=1",
+     "LLVM_IAS=1",
+     "CROSS_COMPILE=aarch64-linux-gnu-",
++    "HOSTLDFLAGS=" + getenv("BUILD_LDFLAGS"),
+
+     # Build out-of-tree in `target_out_dir`.
+     "O=" + rebase_path(target_out_dir),
diff --git a/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/native-dtc.patch b/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/native-dtc.patch
new file mode 100644
index 0000000..840c0bc
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/native-dtc.patch
@@ -0,0 +1,16 @@
+Use our dtc tools instead of the prebuilt (x86-64-only) binaries.
+
+Upstream-Status: Pending [part of a larger effort to remove prebuilt]
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+diff --git a/build/image/dtc.py b/build/image/dtc.py
+index d077818..1513120 100755
+--- a/build/image/dtc.py
++++ b/build/image/dtc.py
+@@ -16,4 +16,2 @@ import sys
+-HF_ROOT = os.path.dirname(os.path.dirname(os.path.dirname(__file__)))
+-DTC_ROOT = os.path.join(HF_ROOT, "prebuilts", "linux-x64", "dtc")
+-DTC = os.path.join(DTC_ROOT, "dtc")
+-FDTOVERLAY = os.path.join(DTC_ROOT, "fdtoverlay")
++DTC = "dtc"
++FDTOVERLAY = "fdtoverlay"
diff --git a/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/pkg-config-native.patch b/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/pkg-config-native.patch
new file mode 100644
index 0000000..bc03195
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium/pkg-config-native.patch
@@ -0,0 +1,20 @@
+Use pkg-config-native to find the libssl headers.
+
+Upstream-Status: Inappropriate
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+diff --git a/scripts/Makefile b/scripts/Makefile
+index b4b7d8b58..26a5160ee 100644
+--- a/third_party/linux/scripts/Makefile
++++ b/third_party/linux/scripts/Makefile
+@@ -10,8 +10,8 @@
+
+ HOST_EXTRACFLAGS += -I$(srctree)/tools/include
+
+-CRYPTO_LIBS = $(shell pkg-config --libs libcrypto 2> /dev/null || echo -lcrypto)
+-CRYPTO_CFLAGS = $(shell pkg-config --cflags libcrypto 2> /dev/null)
++CRYPTO_LIBS = $(shell pkg-config-native --libs libcrypto 2> /dev/null || echo -lcrypto)
++CRYPTO_CFLAGS = $(shell pkg-config-native --cflags libcrypto 2> /dev/null)
+
+ hostprogs-$(CONFIG_BUILD_BIN2C)  += bin2c
+ hostprogs-$(CONFIG_KALLSYMS)     += kallsyms
diff --git a/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium_2.6.bb b/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium_2.6.bb
new file mode 100644
index 0000000..ae89e8d
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/hafnium/hafnium_2.6.bb
@@ -0,0 +1,74 @@
+SUMMARY = "Hafnium"
+DESCRIPTION = "A reference Secure Partition Manager (SPM) for systems that implement the Armv8.4-A Secure-EL2 extension"
+DEPENDS = "gn-native ninja-native bison-native bc-native dtc-native openssl-native"
+
+LICENSE = "BSD-3-Clause & GPL-2.0-only"
+LIC_FILES_CHKSUM = "file://LICENSE;md5=782b40c14bad5294672c500501edc103"
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+inherit deploy python3native pkgconfig
+
+SRC_URI = "gitsm://git.trustedfirmware.org/hafnium/hafnium.git;protocol=https;branch=master \
+           file://0001-define-_Noreturn-if-needed.patch \
+           file://host-ld.patch \
+           file://pkg-config-native.patch \
+           file://native-dtc.patch"
+SRCREV = "55b74f893948dd08d2782dd8fa9e903c143a6704"
+S = "${WORKDIR}/git"
+B = "${WORKDIR}/build"
+
+COMPATIBLE_MACHINE ?= "invalid"
+COMPATIBLE_MACHINE:qemuarm64 = "qemuarm64"
+
+# Default build 'reference'
+HAFNIUM_PROJECT ?= "reference"
+
+# Platform must be set for each machine
+HAFNIUM_PLATFORM ?= "invalid"
+HAFNIUM_PLATFORM:qemuarm64 = "qemu_aarch64"
+
+# do_deploy will install everything listed in this variable. It is set by
+# default to hafnium
+HAFNIUM_INSTALL_TARGET ?= "hafnium"
+
+# set project to build
+EXTRA_OEMAKE += "PROJECT=${HAFNIUM_PROJECT}"
+
+EXTRA_OEMAKE += "OUT_DIR=${B}"
+
+# Don't use prebuilt binaries for gn and ninja
+EXTRA_OEMAKE += "GN=${STAGING_BINDIR_NATIVE}/gn NINJA=${STAGING_BINDIR_NATIVE}/ninja"
+
+do_configure[cleandirs] += "${B}"
+
+do_compile() {
+    oe_runmake -C ${S}
+}
+
+do_install() {
+    cd ${B}/${HAFNIUM_PLATFORM}_clang
+    install -d -m 755 ${D}/firmware
+    for bldfile in ${HAFNIUM_INSTALL_TARGET}; do
+        install -m 0755 $bldfile.bin $bldfile.elf ${D}/firmware/
+    done
+}
+
+FILES:${PN} = "/firmware/*.bin"
+FILES:${PN}-dbg = "/firmware/*.elf"
+SYSROOT_DIRS += "/firmware"
+INSANE_SKIP:${PN} = "ldflags"
+INSANE_SKIP:${PN}-dbg = "ldflags"
+# Build paths are currently embedded
+INSANE_SKIP:${PN}-dbg += "buildpaths"
+
+do_deploy() {
+    cp -rf ${D}/firmware/* ${DEPLOYDIR}/
+}
+addtask deploy after do_install
+
+python() {
+    # https://developer.trustedfirmware.org/T898
+    if d.getVar("BUILD_ARCH") != "x86_64":
+        raise bb.parse.SkipRecipe("Cannot be built on non-x86-64 hosts")
+}
diff --git a/meta-arm/meta-arm/recipes-bsp/scp-firmware/scp-firmware_2.10.0.bb b/meta-arm/meta-arm/recipes-bsp/scp-firmware/scp-firmware_2.10.0.bb
new file mode 100644
index 0000000..4828fb5
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/scp-firmware/scp-firmware_2.10.0.bb
@@ -0,0 +1,107 @@
+SUMMARY = "SCP and MCP Firmware"
+DESCRIPTION = "Firmware for SCP and MCP software reference implementation"
+HOMEPAGE = "https://github.com/ARM-software/SCP-firmware"
+
+LICENSE = "BSD-3-Clause & Apache-2.0"
+LIC_FILES_CHKSUM = "file://license.md;beginline=5;md5=9db9e3d2fb8d9300a6c3d15101b19731 \
+                    file://contrib/cmsis/git/LICENSE.txt;md5=e3fc50a88d0a364313df4b21ef20c29e"
+
+SRC_URI = "gitsm://github.com/ARM-software/SCP-firmware.git;protocol=https;branch=master"
+
+SRCREV  = "673d014f3861ad81cc5ab06d2884a314a610799b"
+
+PROVIDES += "virtual/control-processor-firmware"
+
+SCP_BUILD_RELEASE   ?= "1"
+SCP_PLATFORM        ?= "invalid"
+SCP_COMPILER        ?= "arm-none-eabi"
+SCP_LOG_LEVEL       ?= "WARN"
+SCP_PLATFORM_FEATURE_SET ?= "0"
+
+INHIBIT_DEFAULT_DEPS = "1"
+DEPENDS = "virtual/arm-none-eabi-gcc-native \
+           cmake-native \
+           ninja-native \
+          "
+
+# For now we only build with GCC, so stop meta-clang trying to get involved
+TOOLCHAIN = "gcc"
+
+SCP_BUILD_STR = "${@bb.utils.contains('SCP_BUILD_RELEASE', '1', 'release', 'debug', d)}"
+
+inherit deploy
+
+B = "${WORKDIR}/build"
+S = "${WORKDIR}/git"
+
+# Allow platform specific copying of only scp or both scp & mcp, default to both
+FW_TARGETS ?= "scp mcp"
+FW_INSTALL ?= "ramfw romfw"
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+COMPATIBLE_MACHINE ?= "invalid"
+
+LDFLAGS[unexport] = "1"
+CFLAGS[unexport] = "1"
+
+EXTRA_OECMAKE = "-D CMAKE_BUILD_TYPE=${SCP_BUILD_STR} \
+                 -D SCP_LOG_LEVEL=${SCP_LOG_LEVEL} \
+                 -D SCP_PLATFORM_FEATURE_SET=${SCP_PLATFORM_FEATURE_SET} \
+                "
+
+do_configure() {
+     for FW in ${FW_TARGETS}; do
+         for TYPE in ${FW_INSTALL}; do
+             cmake -GNinja ${EXTRA_OECMAKE} -S ${S} -B "${B}/${TYPE}/${FW}" -D SCP_FIRMWARE_SOURCE_DIR="${SCP_PLATFORM}/${FW}_${TYPE}"
+         done
+     done
+}
+
+do_configure[cleandirs] += "${B}"
+
+do_compile() {
+     for FW in ${FW_TARGETS}; do
+         for TYPE in ${FW_INSTALL}; do
+             cmake --build ${B}/${TYPE}/${FW} --target all
+         done
+     done
+}
+
+do_install() {
+     install -d ${D}/firmware
+     for TYPE in ${FW_INSTALL}; do
+         for FW in ${FW_TARGETS}; do
+            if [ "$TYPE" = "romfw" ]; then
+                if [ "$FW" = "scp" ]; then
+                    install -D "${B}/${TYPE}/${FW}/bin/${SCP_PLATFORM}-bl1.bin" "${D}/firmware/${FW}_${TYPE}.bin"
+                    install -D "${B}/${TYPE}/${FW}/bin/${SCP_PLATFORM}-bl1" "${D}/firmware/${FW}_${TYPE}.elf"
+                elif [ "$FW" = "mcp" ]; then
+                    install -D "${B}/${TYPE}/${FW}/bin/${SCP_PLATFORM}-mcp-bl1.bin" "${D}/firmware/${FW}_${TYPE}.bin"
+                    install -D "${B}/${TYPE}/${FW}/bin/${SCP_PLATFORM}-mcp-bl1" "${D}/firmware/${FW}_${TYPE}.elf"
+                fi
+            elif [ "$TYPE" = "ramfw" ]; then
+                if [ "$FW" = "scp" ]; then
+                    install -D "${B}/${TYPE}/${FW}/bin/${SCP_PLATFORM}-bl2.bin" "${D}/firmware/${FW}_${TYPE}.bin"
+                    install -D "${B}/${TYPE}/${FW}/bin/${SCP_PLATFORM}-bl2" "${D}/firmware/${FW}_${TYPE}.elf"
+                elif [ "$FW" = "mcp" ]; then
+                    install -D "${B}/${TYPE}/${FW}/bin/${SCP_PLATFORM}-mcp-bl2.bin" "${D}/firmware/${FW}_${TYPE}.bin"
+                    install -D "${B}/${TYPE}/${FW}/bin/${SCP_PLATFORM}-mcp-bl2" "${D}/firmware/${FW}_${TYPE}.elf"
+                fi
+            fi
+        done
+     done
+}
+
+FILES:${PN} = "/firmware"
+SYSROOT_DIRS += "/firmware"
+
+FILES:${PN}-dbg += "/firmware/*.elf"
+# Skip QA check for relocations in .text of elf binaries
+INSANE_SKIP:${PN}-dbg = "arch textrel"
+INHIBIT_PACKAGE_DEBUG_SPLIT = "1"
+INHIBIT_PACKAGE_STRIP = "1"
+
+do_deploy() {
+    # Copy the images to deploy directory
+    cp -rf ${D}/firmware/* ${DEPLOYDIR}/
+}
+addtask deploy after do_install
diff --git a/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/files/build-deps-upgrade-to-mbed-TLS-2.28.0.patch b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/files/build-deps-upgrade-to-mbed-TLS-2.28.0.patch
new file mode 100644
index 0000000..058423c
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/files/build-deps-upgrade-to-mbed-TLS-2.28.0.patch
@@ -0,0 +1,72 @@
+Upstream-Status: Backport
+Signed-off-by: Emekcan Aras <emekcan.aras@arm.com>
+
+From a93084be95634b66b917f1c8baf403067dc75c5d Mon Sep 17 00:00:00 2001
+From: Sandrine Bailleux <sandrine.bailleux@arm.com>
+Date: Thu, 21 Apr 2022 10:21:29 +0200
+Subject: [PATCH] build(deps): upgrade to mbed TLS 2.28.0
+
+Upgrade to the latest and greatest 2.x release of Mbed TLS library
+(i.e. v2.28.0) to take advantage of their bug fixes.
+
+Note that the Mbed TLS project published version 3.x some time
+ago. However, as this is a major release with API breakages, upgrading
+to 3.x might require some more involved changes in TF-A, which we are
+not ready to do. We shall upgrade to mbed TLS 3.x after the v2.7
+release of TF-A.
+
+Actually, the upgrade this time simply boils down to including the new
+source code module 'constant_time.c' into the firmware.
+
+To quote mbed TLS v2.28.0 release notes [1]:
+
+  The mbedcrypto library includes a new source code module
+  constant_time.c, containing various functions meant to resist timing
+  side channel attacks. This module does not have a separate
+  configuration option, and functions from this module will be
+  included in the build as required.
+
+As a matter of fact, if one is attempting to link TF-A against mbed
+TLS v2.28.0 without the present patch, one gets some linker errors
+due to missing symbols from this new module.
+
+Apart from this, none of the items listed in mbed TLS release
+notes [1] directly affect TF-A. Special note on the following one:
+
+  Fix a bug in mbedtls_gcm_starts() when the bit length of the iv
+  exceeds 2^32.
+
+In TF-A, we do use mbedtls_gcm_starts() when the firmware decryption
+feature is enabled with AES-GCM as the authenticated decryption
+algorithm (DECRYPTION_SUPPORT=aes_gcm). However, the iv_len variable
+which gets passed to mbedtls_gcm_starts() is an unsigned int, i.e. a
+32-bit value which by definition is always less than 2**32. Therefore,
+we are immune to this bug.
+
+With this upgrade, the size of BL1 and BL2 binaries does not appear to
+change on a standard sample test build (with trusted boot and measured
+boot enabled).
+
+[1] https://github.com/Mbed-TLS/mbedtls/releases/tag/v2.28.0
+
+Change-Id: Icd5dbf527395e9e22c8fd6b77427188bd7237fd6
+Signed-off-by: Sandrine Bailleux <sandrine.bailleux@arm.com>
+---
+ drivers/auth/mbedtls/mbedtls_common.mk | 1 +
+ 1 file changed, 1 insertion(+)
+
+diff --git a/drivers/auth/mbedtls/mbedtls_common.mk b/drivers/auth/mbedtls/mbedtls_common.mk
+index 0a4775d00..3eb41617f 100644
+--- a/drivers/auth/mbedtls/mbedtls_common.mk
++++ b/drivers/auth/mbedtls/mbedtls_common.mk
+@@ -48,6 +48,7 @@ LIBMBEDTLS_SRCS		:= $(addprefix ${MBEDTLS_DIR}/library/,	\
+ 					rsa_internal.c				\
+ 					x509.c 					\
+ 					x509_crt.c 				\
++					constant_time.c 			\
+ 					)
+ 
+ # The platform may define the variable 'TF_MBEDTLS_KEY_ALG' to select the key
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/files/ssl.patch b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/files/ssl.patch
new file mode 100644
index 0000000..cdabd1b
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/files/ssl.patch
@@ -0,0 +1,52 @@
+fiptool: respect OPENSSL_DIR
+
+fiptool links to libcrypto, so as with the other tools it should respect
+OPENSSL_DIR for include/library paths.
+
+Upstream-Status: Submitted
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+diff --git a/Makefile b/Makefile
+index ec6f88585..2d3b9fc26 100644
+--- a/Makefile
++++ b/Makefile
+@@ -1388,7 +1388,7 @@ fwu_fip: ${BUILD_PLAT}/${FWU_FIP_NAME}
+ 
+ ${FIPTOOL}: FORCE
+ ifdef UNIX_MK
+-	${Q}${MAKE} CPPFLAGS="-DVERSION='\"${VERSION_STRING}\"'" FIPTOOL=${FIPTOOL} --no-print-directory -C ${FIPTOOLPATH}
++	${Q}${MAKE} CPPFLAGS="-DVERSION='\"${VERSION_STRING}\"'" FIPTOOL=${FIPTOOL} OPENSSL_DIR=${OPENSSL_DIR} --no-print-directory -C ${FIPTOOLPATH}
+ else
+ # Clear the MAKEFLAGS as we do not want
+ # to pass the gnumake flags to nmake.
+diff --git a/tools/fiptool/Makefile b/tools/fiptool/Makefile
+index 11d2e7b0b..7c2a08379 100644
+--- a/tools/fiptool/Makefile
++++ b/tools/fiptool/Makefile
+@@ -12,6 +12,8 @@ FIPTOOL ?= fiptool${BIN_EXT}
+ PROJECT := $(notdir ${FIPTOOL})
+ OBJECTS := fiptool.o tbbr_config.o
+ V ?= 0
++OPENSSL_DIR := /usr
++
+ 
+ override CPPFLAGS += -D_GNU_SOURCE -D_XOPEN_SOURCE=700
+ HOSTCCFLAGS := -Wall -Werror -pedantic -std=c99
+@@ -20,7 +22,7 @@ ifeq (${DEBUG},1)
+ else
+   HOSTCCFLAGS += -O2
+ endif
+-LDLIBS := -lcrypto
++LDLIBS := -L${OPENSSL_DIR}/lib -lcrypto
+ 
+ ifeq (${V},0)
+   Q := @
+@@ -28,7 +30,7 @@ else
+   Q :=
+ endif
+ 
+-INCLUDE_PATHS := -I../../include/tools_share
++INCLUDE_PATHS := -I../../include/tools_share  -I${OPENSSL_DIR}/include
+ 
+ HOSTCC ?= gcc
+ 
diff --git a/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/tf-a-tests_2.7.0.bb b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/tf-a-tests_2.7.0.bb
new file mode 100644
index 0000000..0c06440
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/tf-a-tests_2.7.0.bb
@@ -0,0 +1,51 @@
+DESCRIPTION = "Trusted Firmware-A tests(aka TFTF)"
+LICENSE = "BSD-3-Clause & NCSA"
+
+LIC_FILES_CHKSUM += "file://docs/license.rst;md5=6175cc0aa2e63b6d21a32aa0ee7d1b4a"
+
+inherit deploy
+
+COMPATIBLE_MACHINE ?= "invalid"
+
+SRC_URI = "git://git.trustedfirmware.org/TF-A/tf-a-tests.git;protocol=https;branch=master"
+SRCREV ?= "5f591f67738a1bbe6b262c53d9dad46ed8bbcd67"
+
+DEPENDS += "optee-os"
+
+EXTRA_OEMAKE += "USE_NVM=0"
+EXTRA_OEMAKE += "SHELL_COLOR=1"
+EXTRA_OEMAKE += "DEBUG=1"
+
+# Platform must be set for each machine
+TFA_PLATFORM ?= "invalid"
+
+EXTRA_OEMAKE += "ARCH=aarch64"
+EXTRA_OEMAKE += "LOG_LEVEL=50"
+
+S = "${WORKDIR}/git"
+B = "${WORKDIR}/build"
+
+# Add platform parameter
+EXTRA_OEMAKE += "BUILD_BASE=${B} PLAT=${TFA_PLATFORM}"
+
+# Requires CROSS_COMPILE set by hand as there is no configure script
+export CROSS_COMPILE="${TARGET_PREFIX}"
+
+do_compile() {
+    oe_runmake -C ${S} tftf
+}
+
+do_compile[cleandirs] = "${B}"
+
+FILES:${PN} = "/firmware/tftf.bin"
+SYSROOT_DIRS += "/firmware"
+
+do_install() {
+    install -d -m 755 ${D}/firmware
+    install -m 0644 ${B}/${TFA_PLATFORM}/debug/tftf.bin ${D}/firmware/tftf.bin
+}
+
+do_deploy() {
+    cp -rf ${D}/firmware/* ${DEPLOYDIR}/
+}
+addtask deploy after do_install
diff --git a/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/trusted-firmware-a.inc b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/trusted-firmware-a.inc
new file mode 100644
index 0000000..c5b695e
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/trusted-firmware-a.inc
@@ -0,0 +1,247 @@
+DESCRIPTION = "Trusted Firmware-A"
+LICENSE = "BSD-3-Clause & MIT"
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+inherit deploy
+
+SRC_URI = "git://git.trustedfirmware.org/TF-A/trusted-firmware-a.git;protocol=https;name=tfa;branch=master"
+
+UPSTREAM_CHECK_GITTAGREGEX = "^v(?P<pver>\d+(\.\d+)+)$"
+
+SRCREV_FORMAT = "tfa"
+
+COMPATIBLE_MACHINE ?= "invalid"
+
+# Platform must be set for each machine
+TFA_PLATFORM ?= "invalid"
+
+# Some platforms can have multiple board configurations
+# Leave empty for default behavior
+TFA_BOARD ?= ""
+
+# Some platforms use SPD (Secure Payload Dispatcher) services
+# Few options are "opteed", "tlkd", "trusty", "tspd", "spmd"...
+# Leave empty to not use SPD
+TFA_SPD ?= ""
+
+# Variable used when TFA_SPD=spmd
+TFA_SPMD_SPM_AT_SEL2 ?= "1"
+
+# SP layout file location. Used when TFA_SPD=spmd and TFA_SPMD_SPM_AT_SEL2=1
+TFA_SP_LAYOUT_FILE ?= ""
+
+# SPMC manifest file location. Used when TFA_SPD=spmd and TFA_SPMD_SPM_AT_SEL2=1
+TFA_ARM_SPMC_MANIFEST_DTS ?= ""
+
+# Build for debug (set TFA_DEBUG to 1 to activate)
+TFA_DEBUG ?= "0"
+
+S = "${WORKDIR}/git"
+B = "${WORKDIR}/build"
+
+# mbed TLS support (set TFA_MBEDTLS to 1 to activate)
+TFA_MBEDTLS ?= "0"
+# sub-directory in which mbedtls will be downloaded
+TFA_MBEDTLS_DIR ?= "mbedtls"
+# This should be set to MBEDTLS download URL if MBEDTLS is needed
+SRC_URI_MBEDTLS ??= ""
+# This should be set to MBEDTLS LIC FILES checksum
+LIC_FILES_CHKSUM_MBEDTLS ??= ""
+# add MBEDTLS to our sources if activated
+SRC_URI:append = " ${@bb.utils.contains('TFA_MBEDTLS', '1', '${SRC_URI_MBEDTLS}', '', d)}"
+# Update license variables
+LICENSE:append = "${@bb.utils.contains('TFA_MBEDTLS', '1', ' & Apache-2.0', '', d)}"
+LIC_FILES_CHKSUM:append = "${@bb.utils.contains('TFA_MBEDTLS', '1', ' ${LIC_FILES_CHKSUM_MBEDTLS}', '', d)}"
+# add mbed TLS to version
+SRCREV_FORMAT:append = "${@bb.utils.contains('TFA_MBEDTLS', '1', '_mbedtls', '', d)}"
+
+# U-boot support (set TFA_UBOOT to 1 to activate)
+# When U-Boot support is activated BL33 is activated with u-boot.bin file
+TFA_UBOOT ??= "0"
+
+# UEFI support (set TFA_UEFI to 1 to activate)
+# When UEFI support is activated BL33 is activated with uefi.bin file
+TFA_UEFI ??= "0"
+
+# What to build
+# By default we only build bl1, do_deploy will copy
+# everything listed in this variable (by default bl1.bin)
+TFA_BUILD_TARGET ?= "bl1"
+
+# What to install
+# do_install and do_deploy will install everything listed in this
+# variable. It is set by default to TFA_BUILD_TARGET
+TFA_INSTALL_TARGET ?= "${TFA_BUILD_TARGET}"
+
+# Requires CROSS_COMPILE set by hand as there is no configure script
+export CROSS_COMPILE="${TARGET_PREFIX}"
+
+# Let the Makefile handle setting up the CFLAGS and LDFLAGS as it is a standalone application
+CFLAGS[unexport] = "1"
+LDFLAGS[unexport] = "1"
+AS[unexport] = "1"
+LD[unexport] = "1"
+
+# No configure
+do_configure[noexec] = "1"
+
+# Baremetal, just need a compiler
+DEPENDS:remove = "virtual/${TARGET_PREFIX}compilerlibs virtual/libc"
+
+# We need dtc for dtbs compilation
+# We need openssl for fiptool
+DEPENDS = "dtc-native openssl-native"
+DEPENDS:append:toolchain-clang = " compiler-rt"
+
+# CC and LD introduce arguments which conflict with those otherwise provided by
+# this recipe. The heads of these variables excluding those arguments
+# are therefore used instead.
+def remove_options_tail (in_string):
+    from itertools import takewhile
+    return ' '.join(takewhile(lambda x: not x.startswith('-'), in_string.split(' ')))
+
+EXTRA_OEMAKE += "LD=${@remove_options_tail(d.getVar('LD'))}"
+
+EXTRA_OEMAKE += "CC=${@remove_options_tail(d.getVar('CC'))}"
+
+# Verbose builds, no -Werror
+EXTRA_OEMAKE += "V=1 E=0"
+
+# Add platform parameter
+EXTRA_OEMAKE += "BUILD_BASE=${B} PLAT=${TFA_PLATFORM}"
+
+# Handle TFA_BOARD parameter
+EXTRA_OEMAKE += "${@'TARGET_BOARD=${TFA_BOARD}' if d.getVar('TFA_BOARD') else ''}"
+
+# Handle TFA_SPD parameter
+EXTRA_OEMAKE += "${@'SPD=${TFA_SPD}' if d.getVar('TFA_SPD') else ''}"
+
+# If TFA_SPD is spmd, set SPMD_SPM_AT_SEL2
+EXTRA_OEMAKE += "${@'SPMD_SPM_AT_SEL2=${TFA_SPMD_SPM_AT_SEL2}' if d.getVar('TFA_SPD', True) == 'spmd' else ''}"
+
+# Handle TFA_DEBUG parameter
+EXTRA_OEMAKE += "${@bb.utils.contains('TFA_DEBUG', '1', 'DEBUG=${TFA_DEBUG}', '', d)}"
+
+# Handle MBEDTLS
+EXTRA_OEMAKE += "${@bb.utils.contains('TFA_MBEDTLS', '1', 'MBEDTLS_DIR=${TFA_MBEDTLS_DIR}', '', d)}"
+
+# Uboot support
+DEPENDS += " ${@bb.utils.contains('TFA_UBOOT', '1', 'u-boot', '', d)}"
+do_compile[depends] += " ${@bb.utils.contains('TFA_UBOOT', '1', 'u-boot:do_deploy', '', d)}"
+EXTRA_OEMAKE += "${@bb.utils.contains('TFA_UBOOT', '1', 'BL33=${DEPLOY_DIR_IMAGE}/u-boot.bin', '', d)}"
+
+# UEFI support
+DEPENDS += " ${@bb.utils.contains('TFA_UEFI', '1', 'edk2-firmware', '', d)}"
+EXTRA_OEMAKE += "${@bb.utils.contains('TFA_UEFI', '1', 'BL33=${RECIPE_SYSROOT}/firmware/uefi.bin', '', d)}"
+
+# TFTF test support
+DEPENDS += " ${@bb.utils.contains('TFTF_TESTS', '1', 'tf-a-tests', '', d)}"
+EXTRA_OEMAKE += "${@bb.utils.contains('TFTF_TESTS', '1', 'BL33=${RECIPE_SYSROOT}/firmware/tftf.bin', '',d)}"
+
+# Hafnium support
+SEL2_SPMC = "${@'${TFA_SPMD_SPM_AT_SEL2}' if d.getVar('TFA_SPD', True) == 'spmd' else ''}"
+
+DEPENDS += " ${@bb.utils.contains('SEL2_SPMC', '1', 'hafnium', '', d)}"
+
+EXTRA_OEMAKE += "${@bb.utils.contains('SEL2_SPMC', '1', 'CTX_INCLUDE_EL2_REGS=1 ARM_ARCH_MINOR=4 BL32=${RECIPE_SYSROOT}/firmware/hafnium.bin', '', d)}"
+
+# Add SP layout file and spmc manifest for hafnium
+EXTRA_OEMAKE += "${@bb.utils.contains('SEL2_SPMC', '1', 'SP_LAYOUT_FILE=${TFA_SP_LAYOUT_FILE}' if d.getVar('TFA_SP_LAYOUT_FILE') else '', '', d)}"
+
+EXTRA_OEMAKE += "${@bb.utils.contains('SEL2_SPMC', '1', 'ARM_SPMC_MANIFEST_DTS=${TFA_ARM_SPMC_MANIFEST_DTS}' if d.getVar('TFA_ARM_SPMC_MANIFEST_DTS') else '', '', d)}"
+
+# Tell the tools where the native OpenSSL is located
+EXTRA_OEMAKE += "OPENSSL_DIR=${STAGING_DIR_NATIVE}/${prefix_native}"
+# Use the correct native compiler
+EXTRA_OEMAKE += "HOSTCC='${BUILD_CC}'"
+
+# Runtime variables
+EXTRA_OEMAKE += "RUNTIME_SYSROOT=${STAGING_DIR_HOST}"
+
+BUILD_DIR = "${B}/${TFA_PLATFORM}"
+BUILD_DIR .= "${@'/${TFA_BOARD}' if d.getVar('TFA_BOARD') else ''}"
+BUILD_DIR .= "/${@'debug' if d.getVar("TFA_DEBUG") == '1' else 'release'}"
+
+do_compile() {
+    # This is still needed to have the native tools executing properly by
+    # setting the RPATH
+    sed -i '/^LDLIBS/ s,$, \$\{BUILD_LDFLAGS},' ${S}/tools/fiptool/Makefile
+    sed -i '/^INCLUDE_PATHS/ s,$, \$\{BUILD_CFLAGS},' ${S}/tools/fiptool/Makefile
+    sed -i '/^LIB/ s,$, \$\{BUILD_LDFLAGS},' ${S}/tools/cert_create/Makefile
+
+    # Currently there are races if you build all the targets at once in parallel
+    for T in ${TFA_BUILD_TARGET}; do
+        oe_runmake -C ${S} $T
+    done
+}
+do_compile[cleandirs] = "${B}"
+
+do_install() {
+    install -d -m 755 ${D}/firmware
+    for atfbin in ${TFA_INSTALL_TARGET}; do
+        processed="0"
+        if [ "$atfbin" = "all" ]; then
+            # Target all is not handled by default
+            bberror "all as TFA_INSTALL_TARGET is not handled by do_install"
+            bberror "Please specify valid targets in TFA_INSTALL_TARGET or"
+            bberror "rewrite or turn off do_install"
+            exit 1
+        fi
+
+        if [ -f ${BUILD_DIR}/$atfbin.bin ]; then
+            echo "Install $atfbin.bin"
+            install -m 0644 ${BUILD_DIR}/$atfbin.bin \
+                ${D}/firmware/$atfbin-${TFA_PLATFORM}.bin
+            ln -sf $atfbin-${TFA_PLATFORM}.bin ${D}/firmware/$atfbin.bin
+            processed="1"
+        fi
+        if [ -f ${BUILD_DIR}/$atfbin/$atfbin.elf ]; then
+            echo "Install $atfbin.elf"
+            install -m 0644 ${BUILD_DIR}/$atfbin/$atfbin.elf \
+                ${D}/firmware/$atfbin-${TFA_PLATFORM}.elf
+            ln -sf $atfbin-${TFA_PLATFORM}.elf ${D}/firmware/$atfbin.elf
+            processed="1"
+        fi
+        if [ -f ${BUILD_DIR}/$atfbin ]; then
+            echo "Install $atfbin"
+            install -m 0644 ${BUILD_DIR}/$atfbin \
+                ${D}/firmware/$atfbin-${TFA_PLATFORM}
+            ln -sf $atfbin-${TFA_PLATFORM} ${D}/firmware/$atfbin
+            processed="1"
+        fi
+        if [ -f ${BUILD_DIR}/fdts/$atfbin.dtb ]; then
+            echo "Install $atfbin.dtb"
+            install -m 0644 "${BUILD_DIR}/fdts/$atfbin.dtb" \
+                "${D}/firmware/$atfbin.dtb"
+            processed="1"
+        elif [ "$atfbin" = "dtbs" ]; then
+            echo "dtbs install, skipped: set dtbs in TFA_INSTALL_TARGET"
+        elif [ -f ${B}/tools/$atfbin/$atfbin ]; then
+            echo "Tools $atfbin install, skipped"
+        elif [ "$processed" = "0" ]; then
+            bberror "Unsupported TFA_INSTALL_TARGET target $atfbin"
+            exit 1
+        fi
+    done
+}
+
+FILES:${PN} = "/firmware"
+SYSROOT_DIRS += "/firmware"
+
+FILES:${PN}-dbg = "/firmware/*.elf"
+# Skip QA check for relocations in .text of elf binaries
+INSANE_SKIP:${PN}-dbg += "textrel"
+# Build paths are currently embedded
+INSANE_SKIP:${PN} += "buildpaths"
+INSANE_SKIP:${PN}-dbg += "buildpaths"
+
+do_deploy() {
+    cp -rf ${D}/firmware/* ${DEPLOYDIR}/
+}
+addtask deploy after do_install
+
+CVE_PRODUCT = "arm:arm-trusted-firmware \
+               arm:trusted_firmware-a \
+               arm:arm_trusted_firmware \
+               arm_trusted_firmware_project:arm_trusted_firmware"
diff --git a/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/trusted-firmware-a_%.bbappend b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/trusted-firmware-a_%.bbappend
new file mode 100644
index 0000000..8815510
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/trusted-firmware-a_%.bbappend
@@ -0,0 +1,56 @@
+COMPATIBLE_MACHINE:qemuarm64-secureboot = "qemuarm64-secureboot"
+COMPATIBLE_MACHINE:qemu-generic-arm64 = "qemu-generic-arm64"
+COMPATIBLE_MACHINE:qemuarm-secureboot = "qemuarm-secureboot"
+
+#FIXME - clang fails to build tfa for qemuarm-secureboot, and possibly other
+# arm/aarch32.  This is a known testing hole in TF-A.
+TOOLCHAIN:qemuarm-secureboot = "gcc"
+
+TFA_PLATFORM:qemuarm64-secureboot = "qemu"
+TFA_PLATFORM:qemu-generic-arm64 = "qemu_sbsa"
+TFA_PLATFORM:qemuarm-secureboot = "qemu"
+
+TFA_SPD:qemuarm64-secureboot = "opteed"
+
+TFA_UBOOT:qemuarm64-secureboot = "1"
+TFA_UBOOT:qemuarm-secureboot = "1"
+TFA_BUILD_TARGET:aarch64:qemuall = "all fip"
+TFA_BUILD_TARGET:arm:qemuall = "all fip"
+
+TFA_INSTALL_TARGET:qemuarm64-secureboot = "flash.bin"
+TFA_INSTALL_TARGET:qemu-generic-arm64 = "bl1 fip"
+TFA_INSTALL_TARGET:qemuarm-secureboot = "flash.bin"
+
+DEPENDS:append:aarch64:qemuall = " optee-os"
+DEPENDS:append:arm:qemuall = " optee-os"
+
+EXTRA_OEMAKE:append:aarch64:qemuall = " \
+    BL32=${STAGING_DIR_TARGET}${nonarch_base_libdir}/firmware/tee-header_v2.bin \
+    BL32_EXTRA1=${STAGING_DIR_TARGET}${nonarch_base_libdir}/firmware/tee-pager_v2.bin \
+    BL32_EXTRA2=${STAGING_DIR_TARGET}${nonarch_base_libdir}/firmware/tee-pageable_v2.bin \
+    BL32_RAM_LOCATION=tdram \
+    "
+
+EXTRA_OEMAKE:append:arm:qemuall = " \
+    BL32=${STAGING_DIR_TARGET}${nonarch_base_libdir}/firmware/tee-header_v2.bin \
+    BL32_EXTRA1=${STAGING_DIR_TARGET}${nonarch_base_libdir}/firmware/tee-pager_v2.bin \
+    BL32_EXTRA2=${STAGING_DIR_TARGET}${nonarch_base_libdir}/firmware/tee-pageable_v2.bin \
+    ARM_ARCH_MAJOR=7 \
+    ARCH=aarch32 \
+    BL32_RAM_LOCATION=tdram \
+    AARCH32_SP=optee \
+    "
+
+do_compile:append:qemuarm64-secureboot() {
+    # Create a secure flash image for booting AArch64 Qemu. See:
+    # https://git.trustedfirmware.org/TF-A/trusted-firmware-a.git/tree/docs/plat/qemu.rst
+    dd if=${BUILD_DIR}/bl1.bin of=${BUILD_DIR}/flash.bin bs=4096 conv=notrunc
+    dd if=${BUILD_DIR}/fip.bin of=${BUILD_DIR}/flash.bin seek=64 bs=4096 conv=notrunc
+}
+
+do_compile:append:qemuarm-secureboot() {
+    # Create a secure flash image for booting AArch64 Qemu. See:
+    # https://git.trustedfirmware.org/TF-A/trusted-firmware-a.git/tree/docs/plat/qemu.rst
+    dd if=${BUILD_DIR}/bl1.bin of=${BUILD_DIR}/flash.bin bs=4096 conv=notrunc
+    dd if=${BUILD_DIR}/fip.bin of=${BUILD_DIR}/flash.bin seek=64 bs=4096 conv=notrunc
+}
diff --git a/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/trusted-firmware-a_2.7.0.bb b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/trusted-firmware-a_2.7.0.bb
new file mode 100644
index 0000000..537ec32
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-a/trusted-firmware-a_2.7.0.bb
@@ -0,0 +1,12 @@
+require trusted-firmware-a.inc
+
+# TF-A v2.7
+SRCREV_tfa = "35f4c7295bafeb32c8bcbdfb6a3f2e74a57e732b"
+
+LIC_FILES_CHKSUM += "file://docs/license.rst;md5=b2c740efedc159745b9b31f88ff03dde"
+
+# mbed TLS v2.28.0
+SRC_URI_MBEDTLS = "git://github.com/ARMmbed/mbedtls.git;name=mbedtls;protocol=https;destsuffix=git/mbedtls;branch=mbedtls-2.28"
+SRCREV_mbedtls = "8b3f26a5ac38d4fdccbc5c5366229f3e01dafcc0"
+
+LIC_FILES_CHKSUM_MBEDTLS = "file://mbedtls/LICENSE;md5=3b83ef96387f14655fc854ddc3c6bd57"
diff --git a/meta-arm/meta-arm/recipes-bsp/trusted-firmware-m/trusted-firmware-m_1.6.0.bb b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-m/trusted-firmware-m_1.6.0.bb
new file mode 100644
index 0000000..bda2771
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/trusted-firmware-m/trusted-firmware-m_1.6.0.bb
@@ -0,0 +1,142 @@
+# SPDX-License-Identifier: MIT
+#
+# Copyright (c) 2020 Arm Limited
+#
+
+SUMMARY = "Trusted Firmware for Cortex-M"
+DESCRIPTION = "Trusted Firmware-M"
+HOMEPAGE = "https://git.trustedfirmware.org/trusted-firmware-m.git"
+PROVIDES = "virtual/trusted-firmware-m"
+
+LICENSE = "BSD-3-Clause & Apache-2.0"
+
+LIC_FILES_CHKSUM = "file://license.rst;md5=07f368487da347f3c7bd0fc3085f3afa \
+                    file://../tf-m-tests/license.rst;md5=02d06ffb8d9f099ff4961c0cb0183a18 \
+                    file://../mbedtls/LICENSE;md5=3b83ef96387f14655fc854ddc3c6bd57 \
+                    file://../mcuboot/LICENSE;md5=b6ee33f1d12a5e6ee3de1e82fb51eeb8"
+
+SRC_URI  = "git://git.trustedfirmware.org/TF-M/trusted-firmware-m.git;protocol=https;branch=${SRCBRANCH_tfm};name=tfm;destsuffix=git/tfm \
+            git://git.trustedfirmware.org/TF-M/tf-m-tests.git;protocol=https;branch=release/1.6.x;name=tfm-tests;destsuffix=git/tf-m-tests \
+            git://github.com/ARMmbed/mbedtls.git;protocol=https;branch=master;name=mbedtls;destsuffix=git/mbedtls \
+            git://github.com/mcu-tools/mcuboot.git;protocol=https;branch=main;name=mcuboot;destsuffix=git/mcuboot \
+            "
+
+# The required dependencies are documented in tf-m/config/config_default.cmake
+# TF-Mv1.6.0
+SRCBRANCH_tfm = "release/1.6.x"
+SRCREV_tfm = "7387d88158701a3c51ad51c90a05326ee12847a8"
+# mbedtls-3.1.0
+SRCREV_mbedtls = "d65aeb37349ad1a50e0f6c9b694d4b5290d60e49"
+# TF-Mv1.6.0
+SRCREV_tfm-tests = "723905d46019596f3f2df66d79b5d6bff6f3f213"
+# v1.9.0
+SRCREV_mcuboot = "c657cbea75f2bb1faf1fceacf972a0537a8d26dd"
+
+UPSTREAM_CHECK_GITTAGREGEX = "^TF-Mv(?P<pver>\d+(\.\d+)+)$"
+
+# Note to future readers of this recipe: until the CMakeLists don't abuse
+# installation (see do_install) there is no point in trying to inherit
+# cmake here. You can easily short-circuit the toolchain but the install
+# is so convoluted there's no gain.
+
+inherit python3native deploy
+
+# Baremetal and we bring a compiler below
+INHIBIT_DEFAULT_DEPS = "1"
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+DEPENDS += "cmake-native \
+            ninja-native \
+            gcc-arm-none-eabi-native \
+            python3-intelhex-native \
+            python3-jinja2-native \
+            python3-pyyaml-native \
+            python3-click-native \
+            python3-cryptography-native \
+            python3-cbor2-native"
+
+S = "${WORKDIR}/git/tfm"
+B = "${WORKDIR}/build"
+
+# Build for debug (set TFM_DEBUG to 1 to activate)
+TFM_DEBUG ?= "0"
+
+# Platform must be set, ideally in the machine configuration.
+TFM_PLATFORM ?= ""
+python() {
+    if not d.getVar("TFM_PLATFORM"):
+        raise bb.parse.SkipRecipe("TFM_PLATFORM needs to be set")
+}
+
+PACKAGECONFIG ??= ""
+# Whether to integrate the test suite
+PACKAGECONFIG[test-secure] = "-DTEST_S=ON,-DTEST_S=OFF"
+PACKAGECONFIG[test-nonsecure] = "-DTEST_NS=ON,-DTEST_NS=OFF"
+
+# Currently we only support using the Arm binary GCC
+EXTRA_OECMAKE += "-DTFM_TOOLCHAIN_FILE=${S}/toolchain_GNUARM.cmake"
+
+# Don't let FetchContent download more sources during do_configure
+EXTRA_OECMAKE += "-DFETCHCONTENT_FULLY_DISCONNECTED=ON"
+
+# Add platform parameters
+EXTRA_OECMAKE += "-DTFM_PLATFORM=${TFM_PLATFORM}"
+
+# Handle TFM_DEBUG parameter
+EXTRA_OECMAKE += "${@bb.utils.contains('TFM_DEBUG', '1', '-DCMAKE_BUILD_TYPE=Debug', '', d)}"
+
+# Verbose builds
+EXTRA_OECMAKE += "-DCMAKE_VERBOSE_MAKEFILE:BOOL=ON"
+
+EXTRA_OECMAKE += "-DMBEDCRYPTO_PATH=${S}/../mbedtls -DTFM_TEST_REPO_PATH=${S}/../tf-m-tests -DMCUBOOT_PATH=${S}/../mcuboot"
+
+export CMAKE_BUILD_PARALLEL_LEVEL = "${@oe.utils.parallel_make(d, False)}"
+
+# Let the Makefile handle setting up the CFLAGS and LDFLAGS as it is a standalone application
+CFLAGS[unexport] = "1"
+LDFLAGS[unexport] = "1"
+AS[unexport] = "1"
+LD[unexport] = "1"
+
+# python3-cryptography needs the legacy provider, so set OPENSSL_MODULES to the
+# right path until this is relocated automatically.
+export OPENSSL_MODULES="${STAGING_LIBDIR_NATIVE}/ossl-modules"
+
+# TF-M ships patches that it needs applied to mbedcrypto, so apply them
+# as part of do_patch.
+apply_local_patches() {
+    cat ${S}/lib/ext/mbedcrypto/*.patch | patch -p1 -d ${S}/../mbedtls
+}
+do_patch[postfuncs] += "apply_local_patches"
+
+do_configure[cleandirs] = "${B}"
+do_configure() {
+    cmake -GNinja -S ${S} -B ${B} ${EXTRA_OECMAKE} ${PACKAGECONFIG_CONFARGS}
+}
+
+# Invoke install here as there's no point in splitting compile from install: the
+# first thing the build does is 'install' inside the build tree thus causing a
+# rebuild. It also overrides the install prefix to be in the build tree, so you
+# can't use the usual install prefix variables.
+do_compile() {
+    cmake --build ${B} -- install
+}
+do_compile[progress] = "outof:^\[(\d+)/(\d+)\]\s+"
+
+do_install() {
+    # TODO install headers and static libraries when we know how they're used
+    install -d -m 755 ${D}/firmware
+    install -m 0644 ${B}/bin/* ${D}/firmware/
+}
+
+FILES:${PN} = "/firmware"
+SYSROOT_DIRS += "/firmware"
+
+addtask deploy after do_install
+do_deploy() {
+    cp -rf ${D}/firmware/* ${DEPLOYDIR}/
+}
+
+# Build paths are currently embedded
+INSANE_SKIP:${PN} += "buildpaths"
diff --git a/meta-arm/meta-arm/recipes-bsp/u-boot/u-boot/qemuarm.cfg b/meta-arm/meta-arm/recipes-bsp/u-boot/u-boot/qemuarm.cfg
new file mode 100644
index 0000000..db8dfec
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/u-boot/u-boot/qemuarm.cfg
@@ -0,0 +1,6 @@
+# This must match the address that TF-A jumps to for BL33
+CONFIG_SYS_TEXT_BASE=0x60000000
+CONFIG_ENV_IS_NOWHERE=y
+# CONFIG_ENV_IS_IN_FLASH is not set
+# CONFIG_MTD is not set
+# CONFIG_MTD_NOR_FLASH is not set
diff --git a/meta-arm/meta-arm/recipes-bsp/u-boot/u-boot/qemuarm64.cfg b/meta-arm/meta-arm/recipes-bsp/u-boot/u-boot/qemuarm64.cfg
new file mode 100644
index 0000000..de0c6ec
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/u-boot/u-boot/qemuarm64.cfg
@@ -0,0 +1,4 @@
+CONFIG_TFABOOT=y
+# This must match the address that TF-A jumps to for BL33
+CONFIG_SYS_TEXT_BASE=0x60000000
+
diff --git a/meta-arm/meta-arm/recipes-bsp/u-boot/u-boot_%.bbappend b/meta-arm/meta-arm/recipes-bsp/u-boot/u-boot_%.bbappend
new file mode 100644
index 0000000..0683a78
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/u-boot/u-boot_%.bbappend
@@ -0,0 +1,4 @@
+FILESEXTRAPATHS:prepend := "${THISDIR}/${PN}:"
+
+SRC_URI:append:qemuarm64-secureboot = " file://qemuarm64.cfg"
+SRC_URI:append:qemuarm-secureboot = " file://qemuarm.cfg"
diff --git a/meta-arm/meta-arm/recipes-bsp/uefi/edk2-firmware.inc b/meta-arm/meta-arm/recipes-bsp/uefi/edk2-firmware.inc
new file mode 100644
index 0000000..e0dfa28
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/uefi/edk2-firmware.inc
@@ -0,0 +1,125 @@
+SUMMARY = "UEFI EDK2 Firmware"
+DESCRIPTION = "UEFI EDK2 Firmware for Arm reference platforms"
+HOMEPAGE = "https://github.com/tianocore/edk2"
+LICENSE = "BSD-2-Clause-Patent"
+
+PROVIDES = "virtual/bootloader"
+
+# EDK2
+LIC_FILES_CHKSUM = "file://License.txt;md5=2b415520383f7964e96700ae12b4570a"
+# EDK2 Platforms
+LIC_FILES_CHKSUM += "file://edk2-platforms/License.txt;md5=2b415520383f7964e96700ae12b4570a"
+
+# These can be overridden as needed
+EDK2_SRC_URI = "gitsm://github.com/tianocore/edk2.git;branch=master;protocol=https"
+EDK2_PLATFORMS_SRC_URI = "git://github.com/tianocore/edk2-platforms.git;branch=master;protocol=https"
+
+SRC_URI = "\
+    ${EDK2_SRC_URI};name=edk2;destsuffix=edk2;nobranch=1 \
+    ${EDK2_PLATFORMS_SRC_URI};name=edk2-platforms;destsuffix=edk2/edk2-platforms;nobranch=1 \
+    file://unaligned.patch \
+    file://default.patch;patchdir=edk2-platforms \
+"
+
+SRCREV_FORMAT         = "edk2_edk2-platforms"
+UPSTREAM_CHECK_GITTAGREGEX = "^edk2-stable(?P<pver>\d+)$"
+
+COMPATIBLE_MACHINE ?= "invalid"
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+EDK2_BUILD_RELEASE = "1"
+
+EDK2_PLATFORM      = "unset"
+# build --platform
+EDK2_PLATFORM_DSC  = "unset"
+EDK2_BIN_NAME      = ""
+# build --arch
+EDK2_ARCH         ?= "unset"
+EDK2_ARCH:arm      = "ARM"
+EDK2_ARCH:aarch64  = "AARCH64"
+EDK2_ARCH:x86      = "IA32"
+EDK2_ARCH:x86-64   = "X64"
+EDK2_ARCH:riscv64  = "RISCV64"
+
+# Extra arguments passed to build
+EDK2_EXTRA_BUILD   = ""
+
+# build --buildtarget
+EDK2_BUILD_MODE ?= "${@bb.utils.contains('EDK2_BUILD_RELEASE', '1', 'RELEASE', 'DEBUG', d)}"
+
+# Baremetal, no need for a C library
+DEPENDS:remove = "virtual/${TARGET_PREFIX}compilerlibs virtual/libc"
+DEPENDS += "util-linux-native iasl-native"
+
+inherit deploy
+
+S = "${WORKDIR}/edk2"
+B = "${WORKDIR}/build"
+
+LDFLAGS[unexport] = "1"
+
+do_configure[cleandirs] += "${B}"
+
+# Set variables as per envsetup
+export PACKAGES_PATH       = "${S}:${S}/edk2-platforms"
+export WORKSPACE           = "${B}"
+export EDK_TOOLS_PATH      = "${S}/BaseTools"
+export PYTHON_COMMAND      = "python3"
+export CONF_PATH           = "${S}/Conf"
+
+export BTOOLS_PATH = "${EDK_TOOLS_PATH}/BinWrappers/PosixLike"
+
+EDK_COMPILER ?= "GCC5"
+export GCC5_AARCH64_PREFIX = "${TARGET_PREFIX}"
+export GCC5_ARM_PREFIX = "${TARGET_PREFIX}"
+
+EDK_COMPILER:toolchain-clang = "CLANG38"
+export CLANG38_AARCH64_PREFIX = "${TARGET_PREFIX}"
+export CLANG38_ARM_PREFIX = "${TARGET_PREFIX}"
+
+#FIXME - arm32 doesn't work with clang due to a linker issue
+TOOLCHAIN:arm = "gcc"
+
+do_configure:prepend() {
+    sed -i -e "s#-target ${HOST_ARCH}-linux-gnu*#-target ${HOST_SYS}#" ${S}/BaseTools/Conf/tools_def.template
+}
+
+do_compile() {
+    sed -i -e 's:-I \.\.:-I \.\. ${BUILD_CFLAGS} :' ${EDK_TOOLS_PATH}/Source/C/Makefiles/header.makefile
+    sed -i -e 's: -luuid: -luuid ${BUILD_LDFLAGS}:g' ${EDK_TOOLS_PATH}/Source/C/*/GNUmakefile
+
+    # Copy the templates as we don't run envsetup
+    cp ${EDK_TOOLS_PATH}/Conf/build_rule.template ${S}/Conf/build_rule.txt
+    cp ${EDK_TOOLS_PATH}/Conf/tools_def.template ${S}/Conf/tools_def.txt
+    cp ${EDK_TOOLS_PATH}/Conf/target.template ${S}/Conf/target.txt
+
+    # Build basetools
+    oe_runmake -C ${S}/BaseTools
+
+    PATH="${WORKSPACE}:${BTOOLS_PATH}:$PATH" \
+    build \
+       --arch "${EDK2_ARCH}" \
+       --buildtarget ${EDK2_BUILD_MODE} \
+       --tagname ${EDK_COMPILER} \
+       --platform ${EDK2_PLATFORM_DSC} \
+       ${@oe.utils.parallel_make_argument(d, "-n %d")} \
+       ${EDK2_EXTRA_BUILD}
+}
+
+do_install() {
+    install -d ${D}/firmware
+    install ${B}/Build/${EDK2_PLATFORM}/${EDK2_BUILD_MODE}_${EDK_COMPILER}/FV/${EDK2_BIN_NAME} ${D}/firmware/uefi.bin
+}
+
+FILES:${PN} = "/firmware"
+SYSROOT_DIRS += "/firmware"
+# Skip QA check for relocations in .text of elf binaries
+INSANE_SKIP:${PN} += "textrel"
+# Build paths are currently embedded
+INSANE_SKIP:${PN} += "buildpaths"
+
+do_deploy() {
+    # Copy the images to deploy directory
+    cp -rf ${D}/firmware/* ${DEPLOYDIR}/
+}
+addtask deploy after do_install
diff --git a/meta-arm/meta-arm/recipes-bsp/uefi/edk2-firmware_%.bbappend b/meta-arm/meta-arm/recipes-bsp/uefi/edk2-firmware_%.bbappend
new file mode 100644
index 0000000..7a39bb0
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/uefi/edk2-firmware_%.bbappend
@@ -0,0 +1,40 @@
+COMPATIBLE_MACHINE:qemuarm64-secureboot = "qemuarm64-secureboot"
+EDK2_PLATFORM:qemuarm64-secureboot      = "ArmVirtQemu-AARCH64"
+EDK2_PLATFORM_DSC:qemuarm64-secureboot  = "ArmVirtPkg/ArmVirtQemu.dsc"
+EDK2_BIN_NAME:qemuarm64-secureboot      = "QEMU_EFI.fd"
+
+COMPATIBLE_MACHINE:qemuarm64 = "qemuarm64"
+EDK2_PLATFORM:qemuarm64      = "ArmVirtQemu-AARCH64"
+EDK2_PLATFORM_DSC:qemuarm64  = "ArmVirtPkg/ArmVirtQemu.dsc"
+EDK2_BIN_NAME:qemuarm64      = "QEMU_EFI.fd"
+
+COMPATIBLE_MACHINE:qemuarm = "qemuarm"
+EDK2_PLATFORM:qemuarm      = "ArmVirtQemu-ARM"
+EDK2_PLATFORM_DSC:qemuarm  = "ArmVirtPkg/ArmVirtQemu.dsc"
+EDK2_BIN_NAME:qemuarm      = "QEMU_EFI.fd"
+
+COMPATIBLE_MACHINE:qemu-generic-arm64   = "qemu-generic-arm64"
+DEPENDS:append:qemu-generic-arm64       = " trusted-firmware-a coreutils-native"
+EDK2_PLATFORM:qemu-generic-arm64        = "SbsaQemu"
+EDK2_PLATFORM_DSC:qemu-generic-arm64    = "Platform/Qemu/SbsaQemu/SbsaQemu.dsc"
+EDK2_BIN_NAME:qemu-generic-arm64        = "SBSA_FLASH0.fd"
+
+do_compile:prepend:qemu-generic-arm64() {
+    mkdir -p ${B}/Platform/Qemu/Sbsa/
+    cp ${RECIPE_SYSROOT}/firmware/bl1.bin ${B}/Platform/Qemu/Sbsa/
+    cp ${RECIPE_SYSROOT}/firmware/fip.bin ${B}/Platform/Qemu/Sbsa/
+}
+
+do_install:append:qemu-generic-arm64() {
+    install ${B}/Build/${EDK2_PLATFORM}/${EDK2_BUILD_MODE}_${EDK_COMPILER}/FV/SBSA_FLASH*.fd ${D}/firmware/
+    # QEMU requires that the images be minimum of 256M in size
+    truncate -s 256M ${D}/firmware/SBSA_FLASH*.fd
+}
+
+do_install:append:qemuarm64() {
+    install ${B}/Build/${EDK2_PLATFORM}/${EDK2_BUILD_MODE}_${EDK_COMPILER}/FV/${EDK2_BIN_NAME} ${D}/firmware/
+}
+
+do_install:append:qemuarm() {
+    install ${B}/Build/${EDK2_PLATFORM}/${EDK2_BUILD_MODE}_${EDK_COMPILER}/FV/${EDK2_BIN_NAME} ${D}/firmware/
+}
diff --git a/meta-arm/meta-arm/recipes-bsp/uefi/edk2-firmware_202205.bb b/meta-arm/meta-arm/recipes-bsp/uefi/edk2-firmware_202205.bb
new file mode 100644
index 0000000..e260665
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/uefi/edk2-firmware_202205.bb
@@ -0,0 +1,4 @@
+SRCREV_edk2           ?= "16779ede2d366bfc6b702e817356ccf43425bcc8"
+SRCREV_edk2-platforms ?= "3b896d1a325686de3942723c42f286090453e37a"
+
+require edk2-firmware.inc
diff --git a/meta-arm/meta-arm/recipes-bsp/uefi/files/default.patch b/meta-arm/meta-arm/recipes-bsp/uefi/files/default.patch
new file mode 100644
index 0000000..fca232f
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/uefi/files/default.patch
@@ -0,0 +1,23 @@
+Platform/ARM: fix uninitialized variable FileSize in RunAxf
+
+Clang 14 detects a potentially uninitialized variable FileSize:
+
+RunAxf.c:216:11: error: variable 'FileSize' is used uninitialized
+                        whenever 'if' condition is false
+RunAxf.c:281:38: note: uninitialized use occurs here
+WriteBackDataCacheRange (FileData, FileSize);
+                                    ^~~~~~~~
+
+Reading the code it doesn't look like this can actually happen, but we
+can keep clang happy by initialising FileSize to 0.
+
+Upstream-Status: Pending
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+diff --git a/Platform/ARM/Library/ArmShellCmdRunAxf/RunAxf.c b/Platform/ARM/Library/ArmShellCmdRunAxf/RunAxf.c
+index d23739ad38..fba5e0ba30 100644
+--- a/Platform/ARM/Library/ArmShellCmdRunAxf/RunAxf.c
++++ b/Platform/ARM/Library/ArmShellCmdRunAxf/RunAxf.c
+@@ -136,1 +136,1 @@ ShellDynCmdRunAxfHandler (
+-  UINTN                       FileSize;
++  UINTN                       FileSize = 0;
diff --git a/meta-arm/meta-arm/recipes-bsp/uefi/files/unaligned.patch b/meta-arm/meta-arm/recipes-bsp/uefi/files/unaligned.patch
new file mode 100644
index 0000000..fa13956
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/uefi/files/unaligned.patch
@@ -0,0 +1,21 @@
+Latest clang is causing build failures because -Werror is used:
+
+  edk2/MdeModulePkg/Include/Guid/ExtendedFirmwarePerformance.h:78:47:
+  error: field Guid within 'FPDT_GUID_EVENT_RECORD' is less aligned than 'EFI_GUID'
+  and is usually due to 'FPDT_GUID_EVENT_RECORD' being packed, which can lead to
+  unaligned accesses [-Werror,-Wunaligned-access]
+
+This has been reported upstream[1] so until this is resolved, ignore the warnings.
+
+[1] https://edk2.groups.io/g/devel/message/86838
+
+Upstream-Status: Pending
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+diff --git a/BaseTools/Conf/tools_def.template b/BaseTools/Conf/tools_def.template
+index f2bb6247e8..ca2b449f0a 100755
+--- a/BaseTools/Conf/tools_def.template
++++ b/BaseTools/Conf/tools_def.template
+@@ -2570 +2570 @@ DEFINE CLANG38_X64_TARGET           = -target x86_64-pc-linux-gnu
+-DEFINE CLANG38_WARNING_OVERRIDES    = -Wno-parentheses-equality -Wno-tautological-compare -Wno-tautological-constant-out-of-range-compare -Wno-empty-body -Wno-unused-const-variable -Wno-varargs -Wno-unknown-warning-option -Wno-unused-but-set-variable -Wno-unused-const-variable
++DEFINE CLANG38_WARNING_OVERRIDES    = -Wno-parentheses-equality -Wno-tautological-compare -Wno-tautological-constant-out-of-range-compare -Wno-empty-body -Wno-unused-const-variable -Wno-varargs -Wno-unknown-warning-option -Wno-unused-but-set-variable -Wno-unused-const-variable -Wno-error=unaligned-access
diff --git a/meta-arm/meta-arm/recipes-bsp/uefi/sbsa-acs/shell.patch b/meta-arm/meta-arm/recipes-bsp/uefi/sbsa-acs/shell.patch
new file mode 100644
index 0000000..2f56547
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/uefi/sbsa-acs/shell.patch
@@ -0,0 +1,18 @@
+Patch in the paths to the SBSA test suite
+
+Upstream-Status: Inappropriate (required action)
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+diff --git a/ShellPkg/ShellPkg.dsc b/ShellPkg/ShellPkg.dsc
+index c42bc9464a..ea21f07a31 100644
+--- a/ShellPkg/ShellPkg.dsc
++++ b/ShellPkg/ShellPkg.dsc
+@@ -24,2 +24,4 @@
+ [LibraryClasses.common]
++  SbsaValLib|ShellPkg/Application/sbsa-acs/val/SbsaValLib.inf
++  SbsaPalLib|ShellPkg/Application/sbsa-acs/platform/pal_uefi/SbsaPalLib.inf
+   UefiApplicationEntryPoint|MdePkg/Library/UefiApplicationEntryPoint/UefiApplicationEntryPoint.inf
+@@ -88,2 +90,3 @@
+   #
++  ShellPkg/Application/sbsa-acs/uefi_app/SbsaAvs.inf
+   ShellPkg/Library/UefiShellLib/UefiShellLib.inf
diff --git a/meta-arm/meta-arm/recipes-bsp/uefi/sbsa-acs/use_bfd_linker.patch b/meta-arm/meta-arm/recipes-bsp/uefi/sbsa-acs/use_bfd_linker.patch
new file mode 100644
index 0000000..04c50ac
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/uefi/sbsa-acs/use_bfd_linker.patch
@@ -0,0 +1,22 @@
+Enforce using good old BFD linker
+
+some distros may use gold as system linker and it crashes while linking the app
+
+TOPDIR/build/tmpfs/work/qemuarm64-yoe-linux/sbsa-acs/3.0-r0/recipe-sysroot-native/usr/bin/aarch64-yoe-linux/../../libexec/aarch64-yoe-linux/gcc/aarch64-yoe-linux/11.0.1/ld: error: TOPDIR/build/tmpfs/work/qemuarm64-yoe-linux/sbsa-acs/3.0-r0/edk2/BaseTools/Scripts/GccBase.lds:54:10: INFO section type is unsupported
+TOPDIR/build/tmpfs/work/qemuarm64-yoe-linux/sbsa-acs/3.0-r0/recipe-sysroot-native/usr/bin/aarch64-yoe-linux/../../libexec/aarch64-yoe-linux/gcc/aarch64-yoe-linux/11.0.1/ld: error: TOPDIR/build/tmpfs/work/qemuarm64-yoe-linux/sbsa-acs/3.0-r0/edk2/BaseTools/Scripts/GccBase.lds:66:14: INFO section type is unsupported
+TOPDIR/build/tmpfs/work/qemuarm64-yoe-linux/sbsa-acs/3.0-r0/recipe-sysroot-native/usr/bin/aarch64-yoe-linux/../../libexec/aarch64-yoe-linux/gcc/aarch64-yoe-linux/11.0.1/ld: internal error in do_layout, at ../../gold/object.cc:1939
+collect2: error: ld returned 1 exit status
+
+Upstream-Status: Pending
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+--- a/BaseTools/Conf/tools_def.template
++++ b/BaseTools/Conf/tools_def.template
+@@ -1926,7 +1926,7 @@ DEFINE GCC_ARM_CC_XIPFLAGS         = -mn
+ DEFINE GCC_AARCH64_CC_FLAGS        = DEF(GCC_ALL_CC_FLAGS) -mlittle-endian -fno-short-enums -fverbose-asm -funsigned-char  -ffunction-sections -fdata-sections -Wno-address -fno-asynchronous-unwind-tables -fno-unwind-tables -fno-pic -fno-pie -ffixed-x18
+ DEFINE GCC_AARCH64_CC_XIPFLAGS     = -mstrict-align -mgeneral-regs-only
+ DEFINE GCC_DLINK_FLAGS_COMMON      = -nostdlib --pie
+-DEFINE GCC_DLINK2_FLAGS_COMMON     = -Wl,--script=$(EDK_TOOLS_PATH)/Scripts/GccBase.lds
++DEFINE GCC_DLINK2_FLAGS_COMMON     = -fuse-ld=bfd -Wl,--script=$(EDK_TOOLS_PATH)/Scripts/GccBase.lds
+ DEFINE GCC_IA32_X64_DLINK_COMMON   = DEF(GCC_DLINK_FLAGS_COMMON) --gc-sections
+ DEFINE GCC_ARM_AARCH64_DLINK_COMMON= -Wl,--emit-relocs -nostdlib -Wl,--gc-sections -u $(IMAGE_ENTRY_POINT) -Wl,-e,$(IMAGE_ENTRY_POINT),-Map,$(DEST_DIR_DEBUG)/$(BASE_NAME).map
+ DEFINE GCC_ARM_DLINK_FLAGS         = DEF(GCC_ARM_AARCH64_DLINK_COMMON) -z common-page-size=0x20 -Wl,--pic-veneer
diff --git a/meta-arm/meta-arm/recipes-bsp/uefi/sbsa-acs_3.1.bb b/meta-arm/meta-arm/recipes-bsp/uefi/sbsa-acs_3.1.bb
new file mode 100644
index 0000000..217760c
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-bsp/uefi/sbsa-acs_3.1.bb
@@ -0,0 +1,33 @@
+require recipes-bsp/uefi/edk2-firmware_202205.bb
+PROVIDES:remove = "virtual/bootloader"
+
+LICENSE += "& Apache-2.0"
+LIC_FILES_CHKSUM += "file://ShellPkg/Application/sbsa-acs/LICENSE.md;md5=2a944942e1496af1886903d274dedb13"
+
+SRC_URI += "git://github.com/ARM-software/sbsa-acs;destsuffix=edk2/ShellPkg/Application/sbsa-acs;protocol=https;branch=master;name=acs \
+            git://github.com/tianocore/edk2-libc;destsuffix=edk2/edk2-libc;protocol=https;branch=master;name=libc \
+            file://shell.patch \
+            file://use_bfd_linker.patch \
+            "
+
+SRCREV_acs = "ec02a7736ae5714326507c60595f4d5299e3dec8"
+SRCREV_libc = "61687168fe02ac4d933a36c9145fdd242ac424d1"
+
+# GCC12 trips on it
+#see https://src.fedoraproject.org/rpms/edk2/blob/rawhide/f/0032-Basetools-turn-off-gcc12-warning.patch
+BUILD_CFLAGS += "-Wno-error=stringop-overflow"
+
+COMPATIBLE_HOST = "aarch64.*-linux"
+COMPATIBLE_MACHINE = ""
+PACKAGE_ARCH = "${TUNE_PKGARCH}"
+
+EDK2_PLATFORM = "Shell"
+EDK2_PLATFORM_DSC = "ShellPkg/ShellPkg.dsc"
+EDK2_EXTRA_BUILD = "--module ShellPkg/Application/sbsa-acs/uefi_app/SbsaAvs.inf"
+
+PACKAGES_PATH .= ":${S}/edk2-libc"
+
+do_install() {
+    install -d ${D}/firmware
+    install ${B}/Build/${EDK2_PLATFORM}/${EDK2_BUILD_MODE}_${EDK_COMPILER}/*/Sbsa.efi ${D}/firmware/
+}
diff --git a/meta-arm/meta-arm/recipes-devtools/fiptool/files/ssl.patch b/meta-arm/meta-arm/recipes-devtools/fiptool/files/ssl.patch
new file mode 100644
index 0000000..cdabd1b
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fiptool/files/ssl.patch
@@ -0,0 +1,52 @@
+fiptool: respect OPENSSL_DIR
+
+fiptool links to libcrypto, so as with the other tools it should respect
+OPENSSL_DIR for include/library paths.
+
+Upstream-Status: Submitted
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+diff --git a/Makefile b/Makefile
+index ec6f88585..2d3b9fc26 100644
+--- a/Makefile
++++ b/Makefile
+@@ -1388,7 +1388,7 @@ fwu_fip: ${BUILD_PLAT}/${FWU_FIP_NAME}
+ 
+ ${FIPTOOL}: FORCE
+ ifdef UNIX_MK
+-	${Q}${MAKE} CPPFLAGS="-DVERSION='\"${VERSION_STRING}\"'" FIPTOOL=${FIPTOOL} --no-print-directory -C ${FIPTOOLPATH}
++	${Q}${MAKE} CPPFLAGS="-DVERSION='\"${VERSION_STRING}\"'" FIPTOOL=${FIPTOOL} OPENSSL_DIR=${OPENSSL_DIR} --no-print-directory -C ${FIPTOOLPATH}
+ else
+ # Clear the MAKEFLAGS as we do not want
+ # to pass the gnumake flags to nmake.
+diff --git a/tools/fiptool/Makefile b/tools/fiptool/Makefile
+index 11d2e7b0b..7c2a08379 100644
+--- a/tools/fiptool/Makefile
++++ b/tools/fiptool/Makefile
+@@ -12,6 +12,8 @@ FIPTOOL ?= fiptool${BIN_EXT}
+ PROJECT := $(notdir ${FIPTOOL})
+ OBJECTS := fiptool.o tbbr_config.o
+ V ?= 0
++OPENSSL_DIR := /usr
++
+ 
+ override CPPFLAGS += -D_GNU_SOURCE -D_XOPEN_SOURCE=700
+ HOSTCCFLAGS := -Wall -Werror -pedantic -std=c99
+@@ -20,7 +22,7 @@ ifeq (${DEBUG},1)
+ else
+   HOSTCCFLAGS += -O2
+ endif
+-LDLIBS := -lcrypto
++LDLIBS := -L${OPENSSL_DIR}/lib -lcrypto
+ 
+ ifeq (${V},0)
+   Q := @
+@@ -28,7 +30,7 @@ else
+   Q :=
+ endif
+ 
+-INCLUDE_PATHS := -I../../include/tools_share
++INCLUDE_PATHS := -I../../include/tools_share  -I${OPENSSL_DIR}/include
+ 
+ HOSTCC ?= gcc
+ 
diff --git a/meta-arm/meta-arm/recipes-devtools/fiptool/fiptool-native_2.7.bb b/meta-arm/meta-arm/recipes-devtools/fiptool/fiptool-native_2.7.bb
new file mode 100644
index 0000000..ec531e4
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fiptool/fiptool-native_2.7.bb
@@ -0,0 +1,31 @@
+# Firmware Image Package (FIP)
+# It is a packaging format used by TF-A to package the
+# firmware images in a single binary.
+
+DESCRIPTION = "fiptool - Trusted Firmware tool for packaging"
+LICENSE = "BSD-3-Clause"
+
+SRC_URI = "git://git.trustedfirmware.org/TF-A/trusted-firmware-a.git;destsuffix=fiptool-${PV};protocol=https;branch=master"
+LIC_FILES_CHKSUM = "file://docs/license.rst;md5=b2c740efedc159745b9b31f88ff03dde"
+
+# Use fiptool from TF-A v2.7
+SRCREV = "35f4c7295bafeb32c8bcbdfb6a3f2e74a57e732b"
+
+DEPENDS += "openssl-native"
+
+inherit native
+
+EXTRA_OEMAKE = "V=1 HOSTCC='${BUILD_CC}' OPENSSL_DIR=${STAGING_DIR_NATIVE}/${prefix_native}"
+
+do_compile () {
+    # This is still needed to have the native fiptool executing properly by
+    # setting the RPATH
+    sed -i '/^LDLIBS/ s,$, \$\{BUILD_LDFLAGS},' ${S}/tools/fiptool/Makefile
+    sed -i '/^INCLUDE_PATHS/ s,$, \$\{BUILD_CFLAGS},' ${S}/tools/fiptool/Makefile
+
+    oe_runmake fiptool
+}
+
+do_install () {
+    install -D -p -m 0755 tools/fiptool/fiptool ${D}${bindir}/fiptool
+}
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-base-a-aem_11.18.16.bb b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-base-a-aem_11.18.16.bb
new file mode 100644
index 0000000..4d8519e
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-base-a-aem_11.18.16.bb
@@ -0,0 +1,10 @@
+require fvp-envelope.inc
+
+SUMMARY = "Arm Fixed Virtual Platform - Armv-A Base RevC Architecture Envelope Model FVP"
+LIC_FILES_CHKSUM = "file://license_terms/license_agreement.txt;md5=1a33828e132ba71861c11688dbb0bd16 \
+                    file://license_terms/third_party_licenses/third_party_licenses.txt;md5=34a1ba318d745f05e6197def68ea5411 \
+                    file://license_terms/third_party_licenses/arm_license_management_utilities/third_party_licenses.txt;md5=2e53bda6ff2db4c35d69944b93926c9f"
+
+SRC_URI[sha256sum] = "fe76f9ff217a44ed7680a0a6ccd6bd19758f5e0466ff98ccd82b2ae62289b9fe"
+
+MODEL_CODE = "FVP_Base_RevC-2xAEMvA"
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-base-r-aem_11.18.16.bb b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-base-r-aem_11.18.16.bb
new file mode 100644
index 0000000..e8704c2
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-base-r-aem_11.18.16.bb
@@ -0,0 +1,10 @@
+require fvp-envelope.inc
+
+SUMMARY = "Arm Fixed Virtual Platform - Armv8-R Base Architecture Envelope Model FVP"
+LIC_FILES_CHKSUM = "file://license_terms/license_agreement.txt;md5=1a33828e132ba71861c11688dbb0bd16 \
+                    file://license_terms/third_party_licenses/third_party_licenses.txt;md5=34a1ba318d745f05e6197def68ea5411 \
+                    file://license_terms/third_party_licenses/arm_license_management_utilities/third_party_licenses.txt;md5=2e53bda6ff2db4c35d69944b93926c9f"
+
+SRC_URI[sha256sum] = "a7a5ee0b7f3b84a2e98e136a6f3ab648e4b6b5ad365186a397595f3f7c69f558"
+
+MODEL_CODE = "FVP_Base_AEMv8R"
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-common.inc b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-common.inc
new file mode 100644
index 0000000..dd02a7c
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-common.inc
@@ -0,0 +1,56 @@
+HOMEPAGE = "https://developer.arm.com/tools-and-software/simulation-models/fixed-virtual-platforms"
+
+# FVP has an End User License Agreement. Add Arm-FVP-EULA to your
+# LICENSE_FLAGS_ACCEPTED if you agree to these terms.
+LICENSE_FLAGS = "Arm-FVP-EULA"
+
+LICENSE = "Proprietary & Apache-2.0 & Python-2.0 & GPL-3.0-with-GCC-exception & Zlib & NCSA & LGPL-2.0-or-later & MIT & BSD-3-Clause"
+
+COMPATIBLE_HOST = "x86_64.*-linux"
+
+# The architecture-specific download filename suffix
+FVP_ARCH:aarch64 = "Linux64_armv8l"
+FVP_ARCH:x86-64 = "Linux64"
+
+# The architecture-specific directory the binaries are installed under
+FVP_ARCH_DIR = "${FVP_ARCH}"
+
+def get_real_pv(d):
+    # FVP versions are like 11.12_43
+    pv = d.getVar("PV")
+    return "%s.%s_%s" % tuple(pv.split("."))
+
+# If PV is 1.2.3, VERSION=1.2, BUILD=3, PV_URL=1.2_3.
+VERSION = "${@oe.utils.trim_version(d.getVar('PV', -1))}"
+BUILD = "${@d.getVar('PV').split('.')[-1]}"
+PV_URL = "${@get_real_pv(d)}"
+
+# The directory the FVP is installed into
+FVPDIR = "${libdir}/fvp/${BPN}"
+
+# Used in do_install to create symlinks in $bindir to $FVPDIR
+fvp_link_binaries() {
+    DIR="${D}${FVPDIR}/models/${FVP_ARCH_DIR}*"
+
+    stat $DIR/FVP_* >/dev/null 2>&1 || bbfatal Cannot find FVP binaries in $DIR
+
+    for FVP in $DIR/FVP_*; do
+        ln -rs $FVP ${D}${bindir}/$(basename $FVP)
+    done
+    # But not the .so files too
+    rm -f ${D}${bindir}/*.so
+}
+
+FILES:${PN} = "${bindir} ${FVPDIR}"
+
+# Prebuilt binaries are already stripped
+INSANE_SKIP:${PN} += "already-stripped"
+# FVP can optionally have a GUI, but we can use the host libraries in native/nativesdk
+INSANE_SKIP:${PN} += "file-rdeps"
+# GNU_HASH warnings
+INSANE_SKIP:${PN} += "ldflags"
+
+# FVP brings its own standard library so don't let it be used as a shlib provider
+PRIVATE_LIBS = "libgcc_s.so.1 libstdc++.so.6"
+
+BBCLASSEXTEND = "native nativesdk"
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-corstone1000.bb b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-corstone1000.bb
new file mode 100644
index 0000000..35ffe0b
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-corstone1000.bb
@@ -0,0 +1,11 @@
+require fvp-ecosystem.inc
+
+MODEL = "Corstone-1000-23"
+MODEL_CODE = "FVP_Corstone_1000"
+PV = "11.17_23"
+
+SRC_URI = "https://developer.arm.com/-/media/Arm%20Developer%20Community/Downloads/OSS/FVP/${MODEL}/Linux/${MODEL_CODE}_${PV}.tgz;subdir=${BP}"
+SRC_URI[sha256sum] = "00ccb72d02c90e2424d24a625d275cabf8ea8dc024713985208f618bb88d1934"
+
+LIC_FILES_CHKSUM = "file://license_terms/license_agreement.txt;md5=1a33828e132ba71861c11688dbb0bd16 \
+                    file://license_terms/third_party_licenses.txt;md5=41029e71051b1c786bae3112a29905a7"
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-corstone500.bb b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-corstone500.bb
new file mode 100644
index 0000000..c80b94c
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-corstone500.bb
@@ -0,0 +1,10 @@
+require fvp-ecosystem.inc
+
+MODEL = "Corstone-500"
+MODEL_CODE = "FVP_Corstone_500"
+PV = "11.12.59"
+
+SRC_URI[sha256sum] = "26f0fbb52de2ccdb4c7b40b6f4ddb5eabdcb8173775fdd11c9a12173326f8614"
+
+LIC_FILES_CHKSUM = "file://license_terms/license_agreement.txt;md5=1a33828e132ba71861c11688dbb0bd16 \
+                    file://license_terms/third_party_licenses.txt;md5=47473b1e04b70938cf0a7ffea8ea4cc3"
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-ecosystem.inc b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-ecosystem.inc
new file mode 100644
index 0000000..365b39c
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-ecosystem.inc
@@ -0,0 +1,25 @@
+require fvp-common.inc
+
+# These need to be set
+MODEL ?= "unset"
+MODEL_CODE ?= "unset"
+PV ?= "unset"
+
+SUMMARY = "Arm Fixed Virtual Platform - ${MODEL} Ecosystem Reference Design"
+HOMEPAGE = "https://developer.arm.com/tools-and-software/open-source-software/arm-platforms-software/arm-ecosystem-fvps"
+
+SRC_URI = "https://developer.arm.com/-/media/Arm%20Developer%20Community/Downloads/OSS/FVP/${MODEL}/${MODEL_CODE}_${PV_URL}.tgz;subdir=${BP}"
+
+UPSTREAM_CHECK_URI = "${HOMEPAGE}"
+UPSTREAM_CHECK_REGEX = "${MODEL_CODE}_(?P<pver>(\d+[\.\-_]*)+).tgz"
+
+do_install() {
+    mkdir --parents ${D}${FVPDIR} ${D}${bindir}
+
+    ${S}/${MODEL_CODE}.sh \
+        --i-agree-to-the-contained-eula \
+        --no-interactive \
+        --destination ${D}${FVPDIR}
+
+    fvp_link_binaries
+}
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-envelope.inc b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-envelope.inc
new file mode 100644
index 0000000..887d9d2
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-envelope.inc
@@ -0,0 +1,16 @@
+require fvp-common.inc
+
+HOMEPAGE = "https://developer.arm.com/tools-and-software/simulation-models/fixed-virtual-platforms/arm-ecosystem-models"
+
+SRC_URI = "https://developer.arm.com/-/media/Files/downloads/ecosystem-models/${MODEL_CODE}_${PV_URL}_${FVP_ARCH}.tgz;subdir=${BP}"
+
+UPSTREAM_CHECK_URI = "${HOMEPAGE}"
+UPSTREAM_CHECK_REGEX = "${MODEL_CODE}_(?P<pver>(\d+[\.\-_]*)+).tgz"
+
+do_install() {
+    mkdir --parents ${D}${FVPDIR} ${D}${bindir}
+
+    cp --archive --no-preserve=ownership ${S}/*_pkg/* ${D}${FVPDIR}/
+
+    fvp_link_binaries
+}
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-library.bb b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-library.bb
new file mode 100644
index 0000000..1a4319e
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-library.bb
@@ -0,0 +1,22 @@
+require fvp-ecosystem.inc
+
+MODEL = "Library"
+MODEL_CODE = "FVP_ARM_Std_Library"
+PV = "11.18.16"
+
+HOMEPAGE = "https://developer.arm.com/tools-and-software/simulation-models/fixed-virtual-platforms"
+
+LIC_FILES_CHKSUM = "file://license_terms/license_agreement.txt;md5=a50d186fffa51ed55599183aad911298 \
+                    file://license_terms/third_party_licenses.txt;md5=3db0c4947b7e3405c40b943672d8de2f"
+
+
+# The FVP Library tarball cannot be downloaded directly, so download the it
+# yourself from from the homepage and set FVP_LIBRARY_TARBALL_URI appropriately
+# to the directory that contains the tarball (for example, "file:///home/user/").
+FVP_LIBRARY_TARBALL_URI ?= ""
+
+SRC_URI = "${FVP_LIBRARY_TARBALL_URI}/${MODEL_CODE}_${PV_URL}_${FVP_ARCH}.tgz;subdir=${BP}"
+python() {
+    if not d.getVar("FVP_LIBRARY_TARBALL_URI"):
+        raise bb.parse.SkipRecipe("FVP_LIBRARY_TARBALL_URI not set")
+}
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-n1-edge.bb b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-n1-edge.bb
new file mode 100644
index 0000000..7fc3949
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-n1-edge.bb
@@ -0,0 +1,11 @@
+require fvp-ecosystem.inc
+
+MODEL = "Neoverse-N1"
+MODEL_CODE = "FVP_RD_N1_edge"
+PV = "11.17.29"
+
+SRC_URI = "https://developer.arm.com/-/media/Arm%20Developer%20Community/Downloads/OSS/FVP/${MODEL}/${MODEL_CODE}_${PV_URL}_Linux64.tgz;subdir=${BP}"
+SRC_URI[sha256sum] = "76f5d6ec50b64fad6d8d901101d9ae2c62805f50fcfd0edb125bc2c68de8c8f2"
+
+LIC_FILES_CHKSUM = "file://license_terms/license_agreement.txt;md5=1a33828e132ba71861c11688dbb0bd16 \
+                    file://license_terms/third_party_licenses.txt;md5=41029e71051b1c786bae3112a29905a7"
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-sgi575.bb b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-sgi575.bb
new file mode 100644
index 0000000..efdd46f
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-sgi575.bb
@@ -0,0 +1,10 @@
+require fvp-ecosystem.inc
+
+MODEL = "SGI-575"
+MODEL_CODE = "FVP_CSS_SGI-575"
+PV = "11.15.26"
+
+SRC_URI[sha256sum] = "d07241112f6c146362deec789e782e10e83bc3560cf605ccd055a606d0b44e74"
+
+LIC_FILES_CHKSUM = "file://license_terms/license_agreement.txt;md5=1a33828e132ba71861c11688dbb0bd16 \
+                    file://license_terms/third_party_licenses.txt;md5=3db0c4947b7e3405c40b943672d8de2f"
diff --git a/meta-arm/meta-arm/recipes-devtools/fvp/fvp-tc0.bb b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-tc0.bb
new file mode 100644
index 0000000..78f2fc2
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/fvp/fvp-tc0.bb
@@ -0,0 +1,12 @@
+require fvp-ecosystem.inc
+
+MODEL = "TotalCompute"
+MODEL_CODE = "FVP_TC0"
+PV = "11.17.18"
+
+# Unconventional URI structure for this release
+SRC_URI = "https://developer.arm.com/-/media/Arm%20Developer%20Community/Downloads/OSS/FVP/TotalCompute/Total%20Compute%20Update%202022/FVP_TC0_11.17_18.tgz;subdir=${BP}"
+SRC_URI[sha256sum] = "0bd78354e036a7e92bd7f8cbd78cd2b5197dc0872fe2b25c95ea734929fe83b8"
+
+LIC_FILES_CHKSUM = "file://license_terms/license_agreement.txt;md5=1a33828e132ba71861c11688dbb0bd16 \
+                    file://license_terms/third_party_licenses.txt;md5=41029e71051b1c786bae3112a29905a7"
diff --git a/meta-arm/meta-arm/recipes-devtools/gator-daemon/gator-daemon/0001-Sources.mk-Remove-xml-PmuXMLParser.h-header-from-GAT.patch b/meta-arm/meta-arm/recipes-devtools/gator-daemon/gator-daemon/0001-Sources.mk-Remove-xml-PmuXMLParser.h-header-from-GAT.patch
new file mode 100644
index 0000000..8ac2bdc
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/gator-daemon/gator-daemon/0001-Sources.mk-Remove-xml-PmuXMLParser.h-header-from-GAT.patch
@@ -0,0 +1,32 @@
+From 6f62c77f0a51de17b7f81f225ef483ed8214640e Mon Sep 17 00:00:00 2001
+From: Khem Raj <raj.khem@gmail.com>
+Date: Sat, 26 Jun 2021 14:19:10 -0700
+Subject: [PATCH] Sources.mk: Remove xml/PmuXMLParser.h header from
+ GATORD_CXX_SRC_FILES
+
+This otherwise appears in final linker cmdline and clang is not happy
+since it sees this as an output file along with the real output file
+specified with -o and bails out
+
+| clang-13: error: cannot specify -o when generating multiple output files
+
+Upstream-Status: Backport [https://github.com/ARM-software/gator/commit/6ef311882a56e14c189d70c4d72945dd95e9b88e]
+Signed-off-by: Khem Raj <raj.khem@gmail.com>
+---
+ daemon/Sources.mk | 3 +--
+ 1 file changed, 1 insertion(+), 2 deletions(-)
+
+diff --git a/daemon/Sources.mk b/daemon/Sources.mk
+index 60d15ef..3e679f5 100644
+--- a/daemon/Sources.mk
++++ b/daemon/Sources.mk
+@@ -147,5 +147,4 @@ GATORD_CXX_SRC_FILES := \
+     xml/EventsXMLProcessor.cpp \
+     xml/MxmlUtils.cpp \
+     xml/PmuXML.cpp \
+-    xml/PmuXMLParser.cpp \
+-    xml/PmuXMLParser.h
++    xml/PmuXMLParser.cpp
+-- 
+2.32.0
+
diff --git a/meta-arm/meta-arm/recipes-devtools/gator-daemon/gator-daemon_7.7.0.bb b/meta-arm/meta-arm/recipes-devtools/gator-daemon/gator-daemon_7.7.0.bb
new file mode 100644
index 0000000..492d321
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/gator-daemon/gator-daemon_7.7.0.bb
@@ -0,0 +1,34 @@
+SUMMARY = "DS-5 Streamline Gator daemon"
+DESCRIPTION = "Target-side daemon gathering data for ARM Streamline \
+               Performance Analyzer."
+HOMEPAGE = "https://github.com/ARM-software/gator"
+
+# Note that Gator uses the Linux Perf API for
+# most of its data collection. Check that your Kernel follow the
+# configuration requirement specified here:
+# https://github.com/ARM-software/gator#kernel-configuration
+
+LICENSE = "GPL-2.0-only & LGPL-2.1-or-later & Apache-2.0"
+LIC_FILES_CHKSUM = "file://COPYING;md5=b234ee4d69f5fce4486a80fdaf4a4263 \
+                    file://libsensors/COPYING.LGPL;md5=4fbd65380cdd255951079008b364516c \
+                    file://mxml/LICENSE;md5=86d3f3a95c324c9479bd8986968f4327 \
+                    file://k/perf_event.h;endline=14;md5=e548bf30a60b2ed11ef2dcf7bfdac230 \
+                   "
+
+SRCREV = "9d8d75fa08352470c51abc23fe3b314879bd8b78"
+SRC_URI = "git://github.com/ARM-software/gator.git;protocol=http;branch=main;protocol=https \
+           file://0001-Sources.mk-Remove-xml-PmuXMLParser.h-header-from-GAT.patch;striplevel=2 \
+          "
+
+S = "${WORKDIR}/git/daemon"
+
+COMPATIBLE_HOST = "aarch64.*-linux"
+
+EXTRA_OEMAKE = "'CFLAGS=${CFLAGS} ${TARGET_CC_ARCH} -D_DEFAULT_SOURCE -DETCDIR=\"${sysconfdir}\"' \
+                'LDFLAGS=${LDFLAGS} ${TARGET_CC_ARCH}' 'CROSS_COMPILE=${TARGET_PREFIX}' \
+                'CXXFLAGS=${CXXFLAGS} ${TARGET_CC_ARCH} -fno-rtti' CC='${CC}' CXX='${CXX}' V=1"
+
+do_install() {
+        install -d ${D}${sbindir}
+        install -m 0755 ${S}/gatord ${D}${sbindir}/gatord
+}
diff --git a/meta-arm/meta-arm/recipes-devtools/gn/gn_git.bb b/meta-arm/meta-arm/recipes-devtools/gn/gn_git.bb
new file mode 100644
index 0000000..9b3906c
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/gn/gn_git.bb
@@ -0,0 +1,49 @@
+SUMMARY = "GN is a meta-build system that generates build files for Ninja"
+DEPENDS += "ninja-native"
+
+LICENSE = "BSD-3-Clause"
+LIC_FILES_CHKSUM = "file://LICENSE;md5=0fca02217a5d49a14dfe2d11837bb34d"
+
+SRC_URI = "git://gn.googlesource.com/gn;protocol=https;branch=main"
+SRCREV = "69ec4fca1fa69ddadae13f9e6b7507efa0675263"
+PV = "0+git${SRCPV}"
+
+S = "${WORKDIR}/git"
+B = "${WORKDIR}/build"
+
+# Currently fails to build with clang, eg:
+# https://errors.yoctoproject.org/Errors/Details/610602/
+# https://errors.yoctoproject.org/Errors/Details/610486/
+TOOLCHAIN = "gcc"
+
+# Map from our _OS strings to the GN's platform values.
+def gn_platform(variable, d):
+    os = d.getVar(variable)
+    if "linux" in os:
+        return "linux"
+    elif "mingw" in os:
+        return "mingw"
+    else:
+        return os
+
+do_configure[cleandirs] += "${B}"
+do_configure() {
+    python3 ${S}/build/gen.py \
+        --platform=${@gn_platform("TARGET_OS", d)} \
+        --out-path=${B} \
+        --no-static-libstdc++ \
+        --no-strip
+}
+
+do_compile() {
+    ninja -C ${B} --verbose
+}
+
+do_install() {
+    install -d ${D}${bindir}
+    install ${B}/gn ${D}${bindir}
+}
+
+BBCLASSEXTEND = "native"
+
+COMPATIBLE_HOST = "^(?!riscv32).*"
diff --git a/meta-arm/meta-arm/recipes-devtools/opencsd/opencsd_1.2.0.bb b/meta-arm/meta-arm/recipes-devtools/opencsd/opencsd_1.2.0.bb
new file mode 100644
index 0000000..0ff4b17
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-devtools/opencsd/opencsd_1.2.0.bb
@@ -0,0 +1,36 @@
+SUMMARY = "OpenCSD - An open source CoreSight(tm) Trace Decode library"
+HOMEPAGE = "https://github.com/Linaro/OpenCSD"
+LICENSE = "BSD-3-Clause"
+LIC_FILES_CHKSUM = "file://LICENSE;md5=ad8cb685eb324d2fa2530b985a43f3e5"
+
+SRC_URI = "git://github.com/Linaro/OpenCSD;protocol=https;branch=master"
+SRCREV = "dac554d62d514b202174506995afc0e109ef3fea"
+
+S = "${WORKDIR}/git"
+
+COMPATIBLE_HOST = "(i.86|x86_64|arm|aarch64).*-linux"
+
+EXTRA_OEMAKE = "ARCH='${TARGET_ARCH}' \
+                CROSS_COMPILE='${TARGET_SYS}-' \
+                CC='${CC}' \
+                CXX='${CXX}' \
+                LIB='${AR}' \
+                LINKER='${CXX}' \
+                LINUX64=1 \
+                DEBUG=1 \
+                PREFIX=${D}/usr \
+                INSTALL_BIN_DIR=${D}${bindir} \
+                INSTALL_INCLUDE_DIR=${D}${includedir} \
+                INSTALL_LIB_DIR=${D}${libdir} \
+                INSTALL_MAN_DIR=${D}${mandir}/man1 \
+                "
+
+do_compile() {
+    oe_runmake -C ${S}/decoder/build/linux
+}
+
+do_install() {
+    oe_runmake -C ${S}/decoder/build/linux install install_man
+}
+
+BBCLASSEXTEND = "native"
diff --git a/meta-arm/meta-arm/recipes-kernel/ffa-debugfs/ffa-debugfs-mod_2.1.0.bb b/meta-arm/meta-arm/recipes-kernel/ffa-debugfs/ffa-debugfs-mod_2.1.0.bb
new file mode 100644
index 0000000..4051c34
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/ffa-debugfs/ffa-debugfs-mod_2.1.0.bb
@@ -0,0 +1,39 @@
+SUMMARY = "FF-A Debugfs Linux kernel module"
+DESCRIPTION = "This out-of-tree kernel module exposes FF-A operations to user space \
+used for development purposes"
+LICENSE = "GPL-2.0-only"
+LIC_FILES_CHKSUM = "file://arm_ffa_user.c;beginline=1;endline=1;md5=fcab174c20ea2e2bc0be64b493708266"
+
+SRC_URI = "git://git.gitlab.arm.com/linux-arm/linux-trusted-services.git;branch=main;protocol=https"
+
+# ffa-debugfs v2.1.0
+SRCREV = "77967912d033144aff2695cecbd52d3be450deaa"
+
+S = "${WORKDIR}/git"
+
+inherit module
+
+SRC_URI:append = " \
+    file://0001-build-add-Yocto-support.patch   \
+    file://0002-script-loading-the-driver-in-a-generic-way.patch \
+  "
+
+FILES:${PN} += "${bindir}/load_ffa_debugfs.sh"
+FILES:${PN}-dev += "${includedir}/arm_ffa_user.h"
+
+do_install:append() {
+  install -D -p -m 0755 ${B}/load_ffa_debugfs.sh ${D}/${bindir}/load_ffa_debugfs.sh
+  install -m 0644 ${S}/arm_ffa_user.h ${D}/${includedir}/arm_ffa_user.h
+}
+
+COMPATIBLE_HOST = "(arm|aarch64).*-linux"
+
+# Kernel modules currently RDEPEND on the kernel, which is troublesome when you want to put a
+# kernel module into a initramfs without pulling the kernel into the initramfs, which would be
+# silly.  Until this is a recommends the easiest way to handle this is to remove the dependency
+# in this recipe.
+PACKAGESPLITFUNCS:append = " remove_kernel_dependency"
+python remove_kernel_dependency() {
+  key = "RDEPENDS:kernel-module-arm-ffa-user-" + d.getVar("KERNEL_VERSION")
+  d.delVar(key)
+}
diff --git a/meta-arm/meta-arm/recipes-kernel/ffa-debugfs/files/0001-build-add-Yocto-support.patch b/meta-arm/meta-arm/recipes-kernel/ffa-debugfs/files/0001-build-add-Yocto-support.patch
new file mode 100644
index 0000000..5d7e977
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/ffa-debugfs/files/0001-build-add-Yocto-support.patch
@@ -0,0 +1,79 @@
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Arpita S.K <arpita.s.k@arm.com>
+
+From 8a7bea4e7d08395036ffc2fde57c4fb44315e181 Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Mon, 25 Oct 2021 13:12:11 +0100
+Subject: [PATCH 1/2] build: add Yocto support
+
+This commit allows to build the driver under Yocto
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+---
+ Kbuild   |  4 ----
+ Makefile | 40 +++++++++++-----------------------------
+ 2 files changed, 11 insertions(+), 33 deletions(-)
+ delete mode 100644 Kbuild
+
+diff --git a/Kbuild b/Kbuild
+deleted file mode 100644
+index 330b019..0000000
+--- a/Kbuild
++++ /dev/null
+@@ -1,4 +0,0 @@
+-# SPDX-License-Identifier: GPL-2.0-only
+-
+-arm-ffa-user-objs := arm_ffa_user.o
+-obj-m := arm-ffa-user.o
+diff --git a/Makefile b/Makefile
+index 62dbfb1..90dfaef 100644
+--- a/Makefile
++++ b/Makefile
+@@ -1,33 +1,15 @@
+-# SPDX-License-Identifier: GPL-2.0-only
++arm-ffa-user-objs := arm_ffa_user.o
++obj-m := arm-ffa-user.o
+ 
+-ARCH			:= arm64
+-CROSS_COMPILE		?= aarch64-linux-gnu-
++SRC := $(shell pwd)
+ 
+-ROOT			?= $(CURDIR)/..
+-KDIR			?= $(ROOT)/linux
+-TARGET_DIR		?= $(ROOT)/shared
+-BUILD_DIR		?= $(CURDIR)/build
+-BUILD_DIR_MAKEFILE 	?= $(BUILD_DIR)/Makefile
++all:
++	$(MAKE) -C $(KERNEL_SRC) M=$(SRC)
+ 
+-all: module
++modules_install:
++	$(MAKE) -C $(KERNEL_SRC) M=$(SRC) modules_install
+ 
+-clean: module-clean
+-
+-install: all
+-	cp $(BUILD_DIR)/arm-ffa-user.ko $(TARGET_DIR)/
+-	cp load_module.sh $(TARGET_DIR)/
+-
+-module: $(BUILD_DIR_MAKEFILE)
+-	$(MAKE) -C $(KDIR) M=$(BUILD_DIR) src=$(CURDIR) modules \
+-		ARCH=$(ARCH) CROSS_COMPILE="$(CROSS_COMPILE)"
+-
+-module-clean:
+-	$(MAKE) -C $(KDIR) M=$(BUILD_DIR) src=$(CURDIR) clean \
+-		ARCH=$(ARCH) CROSS_COMPILE="$(CROSS_COMPILE)"
+-	rm $(BUILD_DIR_MAKEFILE)
+-
+-$(BUILD_DIR):
+-	mkdir -p "$@"
+-
+-$(BUILD_DIR_MAKEFILE): $(BUILD_DIR)
+-	touch "$@"
++clean:
++	rm -f *.o *~ core .depend .*.cmd *.ko *.mod.c
++	rm -f Module.markers Module.symvers modules.order
++	rm -rf .tmp_versions Modules.symvers
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm/recipes-kernel/ffa-debugfs/files/0002-script-loading-the-driver-in-a-generic-way.patch b/meta-arm/meta-arm/recipes-kernel/ffa-debugfs/files/0002-script-loading-the-driver-in-a-generic-way.patch
new file mode 100644
index 0000000..e2469d9
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/ffa-debugfs/files/0002-script-loading-the-driver-in-a-generic-way.patch
@@ -0,0 +1,46 @@
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Arpita S.K <arpita.s.k@arm.com>
+
+From e5d9dfa703a5a57e535b5dab4eda47a9707972d3 Mon Sep 17 00:00:00 2001
+From: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+Date: Mon, 25 Oct 2021 12:51:37 +0100
+Subject: [PATCH 2/2] script: loading the driver in a generic way
+
+Use the kernel module from the modules path.
+
+Signed-off-by: Abdellatif El Khlifi <abdellatif.elkhlifi@arm.com>
+---
+ load_module.sh => load_ffa_debugfs.sh | 8 ++++++--
+ 1 file changed, 6 insertions(+), 2 deletions(-)
+ rename load_module.sh => load_ffa_debugfs.sh (50%)
+ mode change 100755 => 100644
+
+diff --git a/load_module.sh b/load_ffa_debugfs.sh
+old mode 100755
+new mode 100644
+similarity index 50%
+rename from load_module.sh
+rename to load_ffa_debugfs.sh
+index 2137245..4f31ff3
+--- a/load_module.sh
++++ b/load_ffa_debugfs.sh
+@@ -1,10 +1,14 @@
+ #!/bin/sh
++#
++# Use:
++# load_ffa_debugfs.sh <folder containing sp_uuid_list.txt>
++#
+ 
+-[ ! -f $(dirname "$0")/sp_uuid_list.txt ] && \
++[ ! -f "$1"/sp_uuid_list.txt ] && \
+ 	{ echo "Error: missing SP UUID list"; exit 1; }
+ 
+ if ! grep -qs 'arm-ffa-user' /proc/modules; then
+-	insmod $(dirname "$0")/arm-ffa-user.ko uuid_str_list=$(cat $(dirname "$0")/sp_uuid_list.txt)
++	insmod /lib/modules/$(uname -r)/extra/arm-ffa-user.ko uuid_str_list=$(cat "$1"/sp_uuid_list.txt)
+ fi
+ 
+ if ! grep -qs 'debugfs' /proc/mounts; then
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack.inc b/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack.inc
new file mode 100644
index 0000000..6266c80
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack.inc
@@ -0,0 +1,21 @@
+# SPDX-License-Identifier: Apache-2.0
+#
+# Copyright (c) 2021 Arm Limited
+#
+
+DESCRIPTION = "Linux Android Common Kernel"
+SECTION = "kernel"
+LICENSE = "GPL-2.0-only"
+LIC_FILES_CHKSUM = "file://${S}/COPYING;md5=6bc538ed5bd9a7fc9398086aedcd7e46"
+
+require recipes-kernel/linux/linux-yocto.inc
+
+COMPATIBLE_MACHINE ?= "invalid"
+
+ARCH = "arm64"
+
+S = "${WORKDIR}/git"
+
+LINUX_VERSION ?= "${PV}"
+KERNEL_VERSION_SANITY_SKIP = "1"
+KBRANCH = ""
diff --git a/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack/0001-lib-build_OID_registry-fix-reproducibility-issues.patch b/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack/0001-lib-build_OID_registry-fix-reproducibility-issues.patch
new file mode 100644
index 0000000..d2a56a6
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack/0001-lib-build_OID_registry-fix-reproducibility-issues.patch
@@ -0,0 +1,46 @@
+Upstream-Status: Backport
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+From af8dffeef974b488fd0f12723080a72b1b5f5822 Mon Sep 17 00:00:00 2001
+From: Bruce Ashfield <bruce.ashfield@gmail.com>
+Date: Sun, 10 Jul 2022 22:56:53 -0400
+Subject: [PATCH 1/2] lib/build_OID_registry: fix reproducibility issues
+
+The script build_OID_registry captures the full path of itself
+in the generated data. This causes reproduciblity issues as the
+path is captured and packaged.
+
+We use the basename of the script instead, and that allows us
+to be reprodicible, with slightly less information captured in
+the output data (but the generating script can still easily
+be found).
+
+Signed-off-by: Bruce Ashfield <bruce.ashfield@gmail.com>
+---
+ lib/build_OID_registry | 3 ++-
+ 1 file changed, 2 insertions(+), 1 deletion(-)
+
+diff --git a/lib/build_OID_registry b/lib/build_OID_registry
+index d7fc32ea8ac2..f6de0a7f7457 100755
+--- a/lib/build_OID_registry
++++ b/lib/build_OID_registry
+@@ -8,6 +8,7 @@
+ #
+ 
+ use strict;
++use File::Basename;
+ 
+ my @names = ();
+ my @oids = ();
+@@ -35,7 +36,7 @@ close IN_FILE || die;
+ #
+ open C_FILE, ">$ARGV[1]" or die;
+ print C_FILE "/*\n";
+-print C_FILE " * Automatically generated by ", $0, ".  Do not edit\n";
++print C_FILE " * Automatically generated by ", basename $0, ".  Do not edit\n";
+ print C_FILE " */\n";
+ 
+ #
+-- 
+2.34.1
+
diff --git a/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack/0002-vt-conmakehash-improve-reproducibility.patch b/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack/0002-vt-conmakehash-improve-reproducibility.patch
new file mode 100644
index 0000000..5bb40ec
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack/0002-vt-conmakehash-improve-reproducibility.patch
@@ -0,0 +1,56 @@
+Upstream-Status: Backport
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+From 94b28f266f72c244051a2ec30ff4526a44b2ce85 Mon Sep 17 00:00:00 2001
+From: Bruce Ashfield <bruce.ashfield@gmail.com>
+Date: Sun, 10 Jul 2022 21:37:07 -0400
+Subject: [PATCH 2/2] vt/conmakehash: improve reproducibility
+
+The file generated by conmakehash capture the application
+path used to generate the file. While that can be informative,
+it varies based on where the kernel was built, as the full
+path is captured.
+
+We tweak the application to use a second input as the "capture
+name", and then modify the Makefile to pass the basename of
+the source, making it reproducible.
+
+This could be improved by using some sort of path mapping,
+or the application manipualing argv[1] itself, but for now
+this solves the reprodicibility issue.
+
+Signed-off-by: Bruce Ashfield <bruce.ashfield@gmail.com>
+---
+ drivers/tty/vt/Makefile      | 2 +-
+ drivers/tty/vt/conmakehash.c | 2 +-
+ 2 files changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/drivers/tty/vt/Makefile b/drivers/tty/vt/Makefile
+index fe30ce512819..cb51c21b58f9 100644
+--- a/drivers/tty/vt/Makefile
++++ b/drivers/tty/vt/Makefile
+@@ -15,7 +15,7 @@ clean-files := consolemap_deftbl.c defkeymap.c
+ hostprogs += conmakehash
+ 
+ quiet_cmd_conmk = CONMK   $@
+-      cmd_conmk = $(obj)/conmakehash $< > $@
++      cmd_conmk = $(obj)/conmakehash $< $(shell basename $<) > $@
+ 
+ $(obj)/consolemap_deftbl.c: $(src)/$(FONTMAPFILE) $(obj)/conmakehash
+ 	$(call cmd,conmk)
+diff --git a/drivers/tty/vt/conmakehash.c b/drivers/tty/vt/conmakehash.c
+index cddd789fe46e..d62510b280e9 100644
+--- a/drivers/tty/vt/conmakehash.c
++++ b/drivers/tty/vt/conmakehash.c
+@@ -253,7 +253,7 @@ int main(int argc, char *argv[])
+ #include <linux/types.h>\n\
+ \n\
+ u8 dfont_unicount[%d] = \n\
+-{\n\t", argv[1], fontlen);
++{\n\t", argv[2], fontlen);
+ 
+   for ( i = 0 ; i < fontlen ; i++ )
+     {
+-- 
+2.34.1
+
diff --git a/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack_5.10.bb b/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack_5.10.bb
new file mode 100644
index 0000000..7865b25
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/linux/linux-arm64-ack_5.10.bb
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: Apache-2.0
+#
+# Copyright (c) 2021 Arm Limited
+#
+require linux-arm64-ack.inc
+
+SRC_URI = " \
+    git://android.googlesource.com/kernel/common.git;protocol=https;branch=android12-5.10-lts \
+    file://0001-lib-build_OID_registry-fix-reproducibility-issues.patch \
+    file://0002-vt-conmakehash-improve-reproducibility.patch \
+    "
+
+# tag: ASB-2021-09-05_12-5.10
+SRCREV = "3d371f087c953c0e08a228169d4e5c44aea99416"
diff --git a/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/efi.cfg b/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/efi.cfg
new file mode 100644
index 0000000..00be1bc
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/efi.cfg
@@ -0,0 +1,2 @@
+CONFIG_ACPI=y
+CONFIG_EFI=y
diff --git a/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/generic-arm64-kmeta/generic-arm64-standard.scc b/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/generic-arm64-kmeta/generic-arm64-standard.scc
new file mode 100644
index 0000000..21bff02
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/generic-arm64-kmeta/generic-arm64-standard.scc
@@ -0,0 +1,5 @@
+define KMACHINE generic-arm64
+define KTYPE standard
+define KARCH arm64
+
+include ktypes/standard/standard.scc
diff --git a/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/skip-unavailable-memory.patch b/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/skip-unavailable-memory.patch
new file mode 100644
index 0000000..c5dac6a
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/skip-unavailable-memory.patch
@@ -0,0 +1,86 @@
+Backported to 5.15.
+
+Upstream-Status: Submitted [https://lore.kernel.org/linux-arm-kernel/20220517101410.3493781-1-andre.przywara@arm.com/T/#u]
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+From 7bfeda1c9224270af97adf799ce0b5a4292bceb6 Mon Sep 17 00:00:00 2001
+From: Andre Przywara <andre.przywara@arm.com>
+Date: Tue, 17 May 2022 11:14:10 +0100
+Subject: [PATCH] of/fdt: Ignore disabled memory nodes
+
+When we boot a machine using a devicetree, the generic DT code goes
+through all nodes with a 'device_type = "memory"' property, and collects
+all memory banks mentioned there. However it does not check for the
+status property, so any nodes which are explicitly "disabled" will still
+be added as a memblock.
+This ends up badly for QEMU, when booting with secure firmware on
+arm/arm64 machines, because QEMU adds a node describing secure-only
+memory:
+===================
+	secram@e000000 {
+		secure-status = "okay";
+		status = "disabled";
+		reg = <0x00 0xe000000 0x00 0x1000000>;
+		device_type = "memory";
+	};
+===================
+
+The kernel will eventually use that memory block (which is located below
+the main DRAM bank), but accesses to that will be answered with an
+SError:
+===================
+[    0.000000] Internal error: synchronous external abort: 96000050 [#1] PREEMPT SMP
+[    0.000000] Modules linked in:
+[    0.000000] CPU: 0 PID: 0 Comm: swapper Not tainted 5.18.0-rc6-00014-g10c8acb8b679 #524
+[    0.000000] Hardware name: linux,dummy-virt (DT)
+[    0.000000] pstate: 200000c5 (nzCv daIF -PAN -UAO -TCO -DIT -SSBS BTYPE=--)
+[    0.000000] pc : new_slab+0x190/0x340
+[    0.000000] lr : new_slab+0x184/0x340
+[    0.000000] sp : ffff80000a4b3d10
+....
+==================
+The actual crash location and call stack will be somewhat random, and
+depend on the specific allocation of that physical memory range.
+
+As the DT spec[1] explicitly mentions standard properties, add a simple
+check to skip over disabled memory nodes, so that we only use memory
+that is meant for non-secure code to use.
+
+That fixes booting a QEMU arm64 VM with EL3 enabled ("secure=on"), when
+not using UEFI. In this case the QEMU generated DT will be handed on
+to the kernel, which will see the secram node.
+This issue is reproducible when using TF-A together with U-Boot as
+firmware, then booting with the "booti" command.
+
+When using U-Boot as an UEFI provider, the code there [2] explicitly
+filters for disabled nodes when generating the UEFI memory map, so we
+are safe.
+EDK/2 only reads the first bank of the first DT memory node [3] to learn
+about memory, so we got lucky there.
+
+[1] https://github.com/devicetree-org/devicetree-specification/blob/main/source/chapter3-devicenodes.rst#memory-node (after the table)
+[2] https://source.denx.de/u-boot/u-boot/-/blob/master/lib/fdtdec.c#L1061-1063
+[3] https://github.com/tianocore/edk2/blob/master/ArmVirtPkg/PrePi/FdtParser.c
+
+Reported-by: Ross Burton <ross.burton@arm.com>
+Signed-off-by: Andre Przywara <andre.przywara@arm.com>
+---
+ drivers/of/fdt.c | 3 +++
+ 1 file changed, 3 insertions(+)
+
+diff --git a/drivers/of/fdt.c b/drivers/of/fdt.c
+index 59a7a9ee58ef..5439c899fe04 100644
+--- a/drivers/of/fdt.c
++++ b/drivers/of/fdt.c
+@@ -1102,6 +1102,9 @@ int __init early_init_dt_scan_memory(unsigned long node, const char *uname,
+ 	if (type == NULL || strcmp(type, "memory") != 0)
+ 		return 0;
+ 
++	if (!of_fdt_device_is_available(initial_boot_params, node))
++		return 0;
++
+ 	reg = of_get_flat_dt_prop(node, "linux,usable-memory", &l);
+ 	if (reg == NULL)
+ 		reg = of_get_flat_dt_prop(node, "reg", &l);
+-- 
+2.25.1
diff --git a/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/tee.cfg b/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/tee.cfg
new file mode 100644
index 0000000..9a25cd5
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto/tee.cfg
@@ -0,0 +1,12 @@
+CONFIG_HW_RANDOM_OPTEE=y
+CONFIG_TEE=y
+
+#
+# TEE drivers
+#
+CONFIG_OPTEE=y
+CONFIG_OPTEE_SHM_NUM_PRIV_PAGES=1
+# end of TEE drivers
+
+CONFIG_TCG_TPM=y
+CONFIG_TCG_FTPM_TEE=y
diff --git a/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto_%.bbappend b/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto_%.bbappend
new file mode 100644
index 0000000..1d01daa
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-kernel/linux/linux-yocto_%.bbappend
@@ -0,0 +1,24 @@
+ARMFILESPATHS := "${THISDIR}/${PN}:"
+
+COMPATIBLE_MACHINE:generic-arm64 = "generic-arm64"
+FILESEXTRAPATHS:prepend:generic-arm64 = "${ARMFILESPATHS}"
+SRC_URI:append:generic-arm64 =" \
+    file://generic-arm64-kmeta;type=kmeta;destsuffix=generic-arm64-kmeta \
+    "
+
+FILESEXTRAPATHS:prepend:qemuarm64-secureboot = "${ARMFILESPATHS}"
+SRC_URI:append:qemuarm64-secureboot = " \
+    file://skip-unavailable-memory.patch \
+    file://tee.cfg \
+    "
+
+FILESEXTRAPATHS:prepend:qemuarm-secureboot = "${ARMFILESPATHS}"
+SRC_URI:append:qemuarm-secureboot = " \
+    file://tee.cfg \
+    "
+
+FILESEXTRAPATHS:prepend:qemuarm64 = "${ARMFILESPATHS}"
+SRC_URI:append:qemuarm64 = " file://efi.cfg"
+
+FILESEXTRAPATHS:prepend:qemuarm = "${ARMFILESPATHS}"
+SRC_URI:append:qemuarm = " file://efi.cfg"
diff --git a/meta-arm/meta-arm/recipes-security/optee-ftpm/optee-ftpm/0001-add-enum-to-ta-flags.patch b/meta-arm/meta-arm/recipes-security/optee-ftpm/optee-ftpm/0001-add-enum-to-ta-flags.patch
new file mode 100644
index 0000000..94509dd
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee-ftpm/optee-ftpm/0001-add-enum-to-ta-flags.patch
@@ -0,0 +1,30 @@
+From 2d00f16058529eb9f4d4d2bcaeed91fd53b43989 Mon Sep 17 00:00:00 2001
+From: Maxim Uvarov <maxim.uvarov@linaro.org>
+Date: Fri, 17 Apr 2020 12:05:53 +0100
+Subject: [PATCH 2/2] add enum to ta flags
+
+If we compile this TA into OPTEE-OS we need to define a flag
+that this TA can be discovered on the optee bus.
+Upstream-Status: Submitted [https://github.com/microsoft/MSRSec/pull/34]
+
+Signed-off-by: Maxim Uvarov <maxim.uvarov@linaro.org>
+---
+ TAs/optee_ta/fTPM/user_ta_header_defines.h | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/Samples/ARM32-FirmwareTPM/optee_ta/fTPM/user_ta_header_defines.h b/Samples/ARM32-FirmwareTPM/optee_ta/fTPM/user_ta_header_defines.h
+index 72ecbf0cf1c7..e83619d55d3c 100644
+--- a/Samples/ARM32-FirmwareTPM/optee_ta/fTPM/user_ta_header_defines.h
++++ b/Samples/ARM32-FirmwareTPM/optee_ta/fTPM/user_ta_header_defines.h
+@@ -44,7 +44,7 @@
+ 
+ #define TA_UUID                     TA_FTPM_UUID
+ 
+-#define TA_FLAGS                    (TA_FLAG_SINGLE_INSTANCE | TA_FLAG_INSTANCE_KEEP_ALIVE) 
++#define TA_FLAGS                    (TA_FLAG_SINGLE_INSTANCE | TA_FLAG_INSTANCE_KEEP_ALIVE | TA_FLAG_DEVICE_ENUM_SUPP)
+ #define TA_STACK_SIZE               (64 * 1024)
+ #define TA_DATA_SIZE                (32 * 1024)
+
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-arm/recipes-security/optee-ftpm/optee-ftpm_git.bb b/meta-arm/meta-arm/recipes-security/optee-ftpm/optee-ftpm_git.bb
new file mode 100644
index 0000000..df1f3bd
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee-ftpm/optee-ftpm_git.bb
@@ -0,0 +1,79 @@
+SUMMARY = "OPTEE fTPM Microsoft TA"
+DESCRIPTION = "TCG reference implementation of the TPM 2.0 Specification."
+HOMEPAGE = "https://github.com/microsoft/ms-tpm-20-ref/"
+
+COMPATIBLE_MACHINE ?= "invalid"
+COMPATIBLE_MACHINE:qemuarm64 = "qemuarm64"
+COMPATIBLE_MACHINE:qemuarm64-secureboot = "qemuarm64"
+COMPATIBLE_MACHINE:qemu-generic-arm64 = "qemu-generic-arm64"
+COMPATIBLE_MACHINE:qemuarm-secureboot = "qemuarm"
+
+#FIXME - doesn't currently work with clang
+TOOLCHAIN = "gcc"
+
+inherit deploy python3native
+
+LICENSE = "MIT"
+LIC_FILES_CHKSUM = "file://LICENSE;md5=5a3925ece0806073ae9ebbb08ff6f11e"
+
+DEPENDS = "python3-pyelftools-native optee-os-tadevkit python3-cryptography-native "
+
+FTPM_UUID="bc50d971-d4c9-42c4-82cb-343fb7f37896"
+
+SRC_URI = "gitsm://github.com/Microsoft/ms-tpm-20-ref;branch=main;protocol=https \
+           file://0001-add-enum-to-ta-flags.patch"
+
+SRCREV = "d638536d0fe01acd5e39ffa1bd100b3da82d92c7"
+
+S = "${WORKDIR}/git"
+
+OPTEE_CLIENT_EXPORT = "${STAGING_DIR_HOST}${prefix}"
+TEEC_EXPORT = "${STAGING_DIR_HOST}${prefix}"
+TA_DEV_KIT_DIR = "${STAGING_INCDIR}/optee/export-user_ta"
+
+EXTRA_OEMAKE += '\
+    CFG_FTPM_USE_WOLF=y \
+    TA_DEV_KIT_DIR=${TA_DEV_KIT_DIR} \
+    TA_CROSS_COMPILE=${TARGET_PREFIX} \
+    CFLAGS="${CFLAGS} --sysroot=${STAGING_DIR_HOST} -I${WORKDIR}/optee-os" \
+'
+
+EXTRA_OEMAKE:append:aarch64:qemuall = "\
+    CFG_ARM64_ta_arm64=y \
+"
+
+# python3-cryptography needs the legacy provider, so set OPENSSL_MODULES to the
+# right path until this is relocated automatically.
+export OPENSSL_MODULES="${STAGING_LIBDIR_NATIVE}/ossl-modules"
+
+PARALLEL_MAKE = ""
+
+do_compile() {
+    # The internal ${CC} includes the correct -mcpu option
+    sed -i 's/-mcpu=$(TA_CPU)//' Samples/ARM32-FirmwareTPM/optee_ta/fTPM/sub.mk
+    # there's also a secure variable storage TA called authvars
+    cd ${S}/Samples/ARM32-FirmwareTPM/optee_ta
+    oe_runmake
+}
+
+do_install () {
+    mkdir -p ${D}/${nonarch_base_libdir}/optee_armtz
+    install -D -p -m 0644 ${S}/Samples/ARM32-FirmwareTPM/optee_ta/out/fTPM/${FTPM_UUID}.ta ${D}/${nonarch_base_libdir}/optee_armtz/
+    install -D -p -m 0644 ${S}/Samples/ARM32-FirmwareTPM/optee_ta/out/fTPM/${FTPM_UUID}.stripped.elf ${D}/${nonarch_base_libdir}/optee_armtz/
+}
+
+do_deploy () {
+    install -d ${DEPLOYDIR}/optee
+    install -D -p -m 0644 ${S}/Samples/ARM32-FirmwareTPM/optee_ta/out/fTPM/${FTPM_UUID}.stripped.elf ${DEPLOYDIR}/optee/
+}
+
+addtask deploy before do_build after do_install
+
+FILES:${PN} += " \
+               ${nonarch_base_libdir}/optee_armtz/${FTPM_UUID}.ta \
+               ${nonarch_base_libdir}/optee_armtz/${FTPM_UUID}.stripped.elf \
+               "
+
+# Imports machine specific configs from staging to build
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+INSANE_SKIP:${PN} += "ldflags"
diff --git a/meta-arm/meta-arm/recipes-security/optee-ftpm/optee-os_%.bbappend b/meta-arm/meta-arm/recipes-security/optee-ftpm/optee-os_%.bbappend
new file mode 100644
index 0000000..acea750
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee-ftpm/optee-os_%.bbappend
@@ -0,0 +1,14 @@
+FTPM_UUID="bc50d971-d4c9-42c4-82cb-343fb7f37896"
+
+DEPENDS:append = "\
+                  ${@bb.utils.contains('MACHINE_FEATURES', \
+                 'optee-ftpm', \
+                 'optee-ftpm', \
+                 '' , \
+                 d)}"
+
+EXTRA_OEMAKE:append = "${@bb.utils.contains('MACHINE_FEATURES', \
+                      'optee-ftpm', \
+                      'CFG_EARLY_TA=y EARLY_TA_PATHS="${STAGING_DIR_TARGET}/lib/optee_armtz/${FTPM_UUID}.stripped.elf"', \
+                      '', \
+                      d)} "
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-client.inc b/meta-arm/meta-arm/recipes-security/optee/optee-client.inc
new file mode 100644
index 0000000..3b9943c
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-client.inc
@@ -0,0 +1,40 @@
+SUMMARY = "OP-TEE Client API"
+DESCRIPTION = "Open Portable Trusted Execution Environment - Normal World Client side of the TEE"
+HOMEPAGE = "https://www.op-tee.org/"
+
+LICENSE = "BSD-2-Clause"
+LIC_FILES_CHKSUM = "file://LICENSE;md5=69663ab153298557a59c67a60a743e5b"
+
+inherit systemd update-rc.d cmake
+
+SRC_URI = " \
+    git://github.com/OP-TEE/optee_client.git;branch=master;protocol=https \
+    file://tee-supplicant.service \
+    file://tee-supplicant.sh \
+"
+
+UPSTREAM_CHECK_GITTAGREGEX = "^(?P<pver>\d+(\.\d+)+)$"
+
+S = "${WORKDIR}/git"
+
+EXTRA_OECMAKE = " \
+    -DBUILD_SHARED_LIBS=ON \
+    -DCFG_TEE_FS_PARENT_PATH='${localstatedir}/lib/tee' \
+"
+EXTRA_OECMAKE:append:toolchain-clang = " -DCFG_WERROR=0"
+
+do_install:append() {
+    install -D -p -m0644 ${WORKDIR}/tee-supplicant.service ${D}${systemd_system_unitdir}/tee-supplicant.service
+    install -D -p -m0755 ${WORKDIR}/tee-supplicant.sh ${D}${sysconfdir}/init.d/tee-supplicant
+
+    sed -i -e s:@sysconfdir@:${sysconfdir}:g \
+           -e s:@sbindir@:${sbindir}:g \
+              ${D}${systemd_system_unitdir}/tee-supplicant.service \
+              ${D}${sysconfdir}/init.d/tee-supplicant
+}
+
+SYSTEMD_SERVICE:${PN} = "tee-supplicant.service"
+
+INITSCRIPT_PACKAGES = "${PN}"
+INITSCRIPT_NAME:${PN} = "tee-supplicant"
+INITSCRIPT_PARAMS:${PN} = "start 10 1 2 3 4 5 . stop 90 0 6 ."
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-client/tee-supplicant.service b/meta-arm/meta-arm/recipes-security/optee/optee-client/tee-supplicant.service
new file mode 100644
index 0000000..c273832
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-client/tee-supplicant.service
@@ -0,0 +1,10 @@
+[Unit]
+Description=TEE Supplicant
+
+[Service]
+User=root
+EnvironmentFile=-@sysconfdir@/default/tee-supplicant
+ExecStart=@sbindir@/tee-supplicant $OPTARGS
+
+[Install]
+WantedBy=basic.target
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-client/tee-supplicant.sh b/meta-arm/meta-arm/recipes-security/optee/optee-client/tee-supplicant.sh
new file mode 100644
index 0000000..b4d2195
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-client/tee-supplicant.sh
@@ -0,0 +1,46 @@
+#!/bin/sh
+
+# Source function library
+. /etc/init.d/functions
+
+NAME=tee-supplicant
+PATH=/sbin:/bin:/usr/sbin:/usr/bin
+DESC="OP-TEE Supplicant"
+
+DAEMON=@sbindir@/$NAME
+
+test -f $DAEMON || exit 0
+
+test -f @sysconfdir@/default/$NAME && . @sysconfdir@/default/$NAME
+test -f @sysconfdir@/default/rcS && . @sysconfdir@/default/rcS
+
+SSD_OPTIONS="--oknodo --quiet --exec $DAEMON -- -d $OPTARGS"
+
+set -e
+
+case $1 in
+    start)
+	    echo -n "Starting $DESC: "
+	    start-stop-daemon --start $SSD_OPTIONS
+        echo "${DAEMON##*/}."
+        ;;
+    stop)
+	    echo -n "Stopping $DESC: "
+	    start-stop-daemon --stop $SSD_OPTIONS
+        echo "${DAEMON##*/}."
+        ;;
+    restart|force-reload)
+	    $0 stop
+	    sleep 1
+	    $0 start
+        ;;
+    status)
+        status ${DAEMON} || exit $?
+        ;;
+    *)
+        echo "Usage: $0 {start|stop|restart|force-reload|status}" >&2
+        exit 1
+        ;;
+esac
+
+exit 0
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-client_3.14.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-client_3.14.0.bb
new file mode 100644
index 0000000..be78b88
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-client_3.14.0.bb
@@ -0,0 +1,3 @@
+require optee-client.inc
+
+SRCREV = "06e1b32f6a7028e039c625b07cfc25fda0c17d53"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-client_3.17.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-client_3.17.0.bb
new file mode 100644
index 0000000..5de16e7
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-client_3.17.0.bb
@@ -0,0 +1,3 @@
+require optee-client.inc
+
+SRCREV = "9a337049c52495e5e16b4a94decaa3e58fce793e"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-examples.inc b/meta-arm/meta-arm/recipes-security/optee/optee-examples.inc
new file mode 100644
index 0000000..e6feb99
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-examples.inc
@@ -0,0 +1,46 @@
+SUMMARY = "OP-TEE examples"
+DESCRIPTION = "Open Portable Trusted Execution Environment - Sample Applications"
+HOMEPAGE = "https://github.com/linaro-swg/optee_examples"
+
+LICENSE = "BSD-2-Clause"
+LIC_FILES_CHKSUM = "file://LICENSE;md5=cd95ab417e23b94f381dafc453d70c30"
+
+DEPENDS = "optee-client optee-os-tadevkit python3-cryptography-native"
+
+inherit python3native
+
+require optee.inc
+
+SRC_URI = "git://github.com/linaro-swg/optee_examples.git;branch=master;protocol=https \
+           file://0001-Makefile-Fix-non-portable-sh-check-for-plugins.patch"
+
+EXTRA_OEMAKE += "TA_DEV_KIT_DIR=${TA_DEV_KIT_DIR} \
+                 HOST_CROSS_COMPILE=${HOST_PREFIX} \
+                 TA_CROSS_COMPILE=${HOST_PREFIX} \
+                 OUTPUT_DIR=${B} \
+               "
+
+S = "${WORKDIR}/git"
+B = "${WORKDIR}/build"
+
+
+do_compile() {
+    oe_runmake -C ${S}
+}
+do_compile[cleandirs] = "${B}"
+
+do_install () {
+    mkdir -p ${D}${nonarch_base_libdir}/optee_armtz
+    mkdir -p ${D}${bindir}
+    mkdir -p ${D}${libdir}/tee-supplicant/plugins
+    install -D -p -m0755 ${B}/ca/* ${D}${bindir}
+    install -D -p -m0444 ${B}/ta/* ${D}${nonarch_base_libdir}/optee_armtz
+    install -D -p -m0444 ${B}/plugins/* ${D}${libdir}/tee-supplicant/plugins
+}
+
+FILES:${PN} += "${nonarch_base_libdir}/optee_armtz/ \
+                ${libdir}/tee-supplicant/plugins/ \
+               "
+
+# Imports machine specific configs from staging to build
+PACKAGE_ARCH = "${MACHINE_ARCH}"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-examples/0001-Makefile-Fix-non-portable-sh-check-for-plugins.patch b/meta-arm/meta-arm/recipes-security/optee/optee-examples/0001-Makefile-Fix-non-portable-sh-check-for-plugins.patch
new file mode 100644
index 0000000..70add62
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-examples/0001-Makefile-Fix-non-portable-sh-check-for-plugins.patch
@@ -0,0 +1,46 @@
+From 11610debf750f15c7a104db7315dcd7d69e282a8 Mon Sep 17 00:00:00 2001
+From: Alejandro Enedino Hernandez Samaniego <alhe@linux.microsoft.com>
+Date: Sat, 26 Feb 2022 01:52:26 +0000
+Subject: [PATCH] Makefile: Fix non-portable sh check for plugins
+
+Upstream-Status: Pending
+
+We previously held a patch that used "=" for comparison, but when
+that patch got upstreamed it was changed to "==" which is non-portable,
+resulting in an error:
+
+/bin/sh: 6: [: acipher: unexpected operator
+/bin/sh: 6: [: plugins: unexpected operator
+/bin/sh: 6: [: hello_world: unexpected operator
+/bin/sh: 6: [: hotp: unexpected operator
+/bin/sh: 6: [: aes: unexpected operator
+/bin/sh: 6: [: random: unexpected operator
+/bin/sh: 6: [: secure_storage: unexpected operator
+
+if /bin/sh doesnt point to bash.
+
+Which in turn causes our do_install task to fail since plugins arent
+where we expect them to be.
+
+
+Signed-off-by: Alejandro Enedino Hernandez Samaniego <alhe@linux.microsoft.com>
+---
+ Makefile | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/Makefile b/Makefile
+index b3f16aa..9359d95 100644
+--- a/Makefile
++++ b/Makefile
+@@ -31,7 +31,7 @@ prepare-for-rootfs: examples
+ 			cp -p $$example/host/optee_example_$$example $(OUTPUT_DIR)/ca/; \
+ 		fi; \
+ 		cp -pr $$example/ta/*.ta $(OUTPUT_DIR)/ta/; \
+-		if [ $$example == plugins ]; then \
++		if [ $$example = plugins ]; then \
+ 			cp -p plugins/syslog/*.plugin $(OUTPUT_DIR)/plugins/; \
+ 		fi; \
+ 	done
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-examples_3.14.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-examples_3.14.0.bb
new file mode 100644
index 0000000..f2b5f7d
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-examples_3.14.0.bb
@@ -0,0 +1,4 @@
+require optee-examples.inc
+
+SRCREV = "e9c870525af8f7e7fccf575a0ca5394ce55adcec"
+
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-examples_3.17.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-examples_3.17.0.bb
new file mode 100644
index 0000000..b5f6269
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-examples_3.17.0.bb
@@ -0,0 +1,3 @@
+require optee-examples.inc
+
+SRCREV = "65fc74309e12189ad5b6ce3ffec37c8011088a5a"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-os-tadevkit_3.14.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-os-tadevkit_3.14.0.bb
new file mode 100644
index 0000000..0d37a52
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-os-tadevkit_3.14.0.bb
@@ -0,0 +1,20 @@
+FILESEXTRAPATHS:prepend := "${THISDIR}/optee-os:"
+require optee-os_3.14.0.bb
+
+SUMMARY = "OP-TEE Trusted OS TA devkit"
+DESCRIPTION = "OP-TEE TA devkit for build TAs"
+HOMEPAGE = "https://www.op-tee.org/"
+
+do_install() {
+    #install TA devkit
+    install -d ${D}${includedir}/optee/export-user_ta/
+    for f in ${B}/export-ta_${OPTEE_ARCH}/* ; do
+        cp -aR $f ${D}${includedir}/optee/export-user_ta/
+    done
+}
+
+do_deploy() {
+	echo "Do not inherit do_deploy from optee-os."
+}
+
+FILES:${PN} = "${includedir}/optee/"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-os-tadevkit_3.17.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-os-tadevkit_3.17.0.bb
new file mode 100644
index 0000000..5ff373a
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-os-tadevkit_3.17.0.bb
@@ -0,0 +1,25 @@
+FILESEXTRAPATHS:prepend := "${THISDIR}/optee-os:"
+require optee-os_3.17.0.bb
+
+SUMMARY = "OP-TEE Trusted OS TA devkit"
+DESCRIPTION = "OP-TEE TA devkit for build TAs"
+HOMEPAGE = "https://www.op-tee.org/"
+
+DEPENDS += "python3-pycryptodome-native"
+
+do_install() {
+    #install TA devkit
+    install -d ${D}${includedir}/optee/export-user_ta/
+    for f in ${B}/export-ta_${OPTEE_ARCH}/* ; do
+        cp -aR $f ${D}${includedir}/optee/export-user_ta/
+    done
+}
+
+do_deploy() {
+	echo "Do not inherit do_deploy from optee-os."
+}
+
+FILES:${PN} = "${includedir}/optee/"
+
+# Build paths are currently embedded
+INSANE_SKIP:${PN}-dev += "buildpaths"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-os.inc b/meta-arm/meta-arm/recipes-security/optee/optee-os.inc
new file mode 100644
index 0000000..11193dc
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-os.inc
@@ -0,0 +1,75 @@
+SUMMARY = "OP-TEE Trusted OS"
+DESCRIPTION = "Open Portable Trusted Execution Environment - Trusted side of the TEE"
+HOMEPAGE = "https://www.op-tee.org/"
+
+LICENSE = "BSD-2-Clause"
+LIC_FILES_CHKSUM = "file://LICENSE;md5=c1f21c4f72f372ef38a5a4aee55ec173"
+
+inherit deploy python3native
+require optee.inc
+
+CVE_PRODUCT = "linaro:op-tee op-tee:op-tee_os"
+
+DEPENDS = "python3-pyelftools-native python3-cryptography-native"
+
+DEPENDS:append:toolchain-clang = " compiler-rt"
+
+SRC_URI = "git://github.com/OP-TEE/optee_os.git;branch=master;protocol=https"
+
+SRC_URI:append = " \
+    file://0006-allow-setting-sysroot-for-libgcc-lookup.patch \
+    file://0007-allow-setting-sysroot-for-clang.patch \
+   "
+
+S = "${WORKDIR}/git"
+B = "${WORKDIR}/build"
+
+EXTRA_OEMAKE += " \
+    PLATFORM=${OPTEEMACHINE} \
+    CFG_${OPTEE_CORE}_core=y \
+    CROSS_COMPILE_core=${HOST_PREFIX} \
+    CROSS_COMPILE_ta_${OPTEE_ARCH}=${HOST_PREFIX} \
+    NOWERROR=1 \
+    ta-targets=ta_${OPTEE_ARCH} \
+    O=${B} \
+"
+
+CFLAGS[unexport] = "1"
+LDFLAGS[unexport] = "1"
+CPPFLAGS[unexport] = "1"
+AS[unexport] = "1"
+LD[unexport] = "1"
+
+do_configure[noexec] = "1"
+
+do_compile() {
+    oe_runmake -C ${S} all
+}
+do_compile[cleandirs] = "${B}"
+
+do_install() {
+    #install core in firmware
+    install -d ${D}${nonarch_base_libdir}/firmware/
+    install -m 644 ${B}/core/*.bin ${B}/core/tee.elf ${D}${nonarch_base_libdir}/firmware/
+}
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+do_deploy() {
+    install -d ${DEPLOYDIR}/${MLPREFIX}optee
+    install -m 644 ${D}${nonarch_base_libdir}/firmware/* ${DEPLOYDIR}/${MLPREFIX}optee
+}
+
+addtask deploy before do_build after do_install
+
+SYSROOT_DIRS += "${nonarch_base_libdir}/firmware"
+
+FILES:${PN} = "${nonarch_base_libdir}/firmware/"
+
+# note: "textrel" is not triggered on all archs
+INSANE_SKIP:${PN} = "textrel"
+# Build paths are currently embedded
+INSANE_SKIP:${PN} += "buildpaths"
+INSANE_SKIP:${PN}-dev = "staticdev"
+INHIBIT_PACKAGE_STRIP = "1"
+
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-os/0006-allow-setting-sysroot-for-libgcc-lookup.patch b/meta-arm/meta-arm/recipes-security/optee/optee-os/0006-allow-setting-sysroot-for-libgcc-lookup.patch
new file mode 100644
index 0000000..1700539
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-os/0006-allow-setting-sysroot-for-libgcc-lookup.patch
@@ -0,0 +1,34 @@
+From 0bab935695ebcf0c533b49896ab18ff33d4a47d1 Mon Sep 17 00:00:00 2001
+From: Ross Burton <ross.burton@arm.com>
+Date: Tue, 26 May 2020 14:38:02 -0500
+Subject: [PATCH] allow setting sysroot for libgcc lookup
+
+Explicitly pass the new variable LIBGCC_LOCATE_CFLAGS variable when searching
+for the compiler libraries as there's no easy way to reliably pass --sysroot
+otherwise.
+
+Upstream-Status: Pending [https://github.com/OP-TEE/optee_os/issues/4188]
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+---
+ mk/gcc.mk | 6 +++---
+ 1 file changed, 3 insertions(+), 3 deletions(-)
+
+diff --git a/mk/gcc.mk b/mk/gcc.mk
+index adc77a24..81bfa78a 100644
+--- a/mk/gcc.mk
++++ b/mk/gcc.mk
+@@ -13,11 +13,11 @@ nostdinc$(sm)	:= -nostdinc -isystem $(shell $(CC$(sm)) \
+ 			-print-file-name=include 2> /dev/null)
+ 
+ # Get location of libgcc from gcc
+-libgcc$(sm)  	:= $(shell $(CC$(sm)) $(CFLAGS$(arch-bits-$(sm))) \
++libgcc$(sm)  	:= $(shell $(CC$(sm)) $(LIBGCC_LOCATE_CFLAGS) $(CFLAGS$(arch-bits-$(sm))) \
+ 			-print-libgcc-file-name 2> /dev/null)
+-libstdc++$(sm)	:= $(shell $(CXX$(sm)) $(CXXFLAGS$(arch-bits-$(sm))) $(comp-cxxflags$(sm)) \
++libstdc++$(sm)	:= $(shell $(CXX$(sm)) $(LIBGCC_LOCATE_CFLAGS) $(CXXFLAGS$(arch-bits-$(sm))) $(comp-cxxflags$(sm)) \
+ 			-print-file-name=libstdc++.a 2> /dev/null)
+-libgcc_eh$(sm)	:= $(shell $(CXX$(sm)) $(CXXFLAGS$(arch-bits-$(sm))) $(comp-cxxflags$(sm)) \
++libgcc_eh$(sm)	:= $(shell $(CXX$(sm)) $(LIBGCC_LOCATE_CFLAGS) $(CXXFLAGS$(arch-bits-$(sm))) $(comp-cxxflags$(sm)) \
+ 			-print-file-name=libgcc_eh.a 2> /dev/null)
+ 
+ # Define these to something to discover accidental use
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-os/0007-allow-setting-sysroot-for-clang.patch b/meta-arm/meta-arm/recipes-security/optee/optee-os/0007-allow-setting-sysroot-for-clang.patch
new file mode 100644
index 0000000..5c0d0a5
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-os/0007-allow-setting-sysroot-for-clang.patch
@@ -0,0 +1,29 @@
+From 3167f2c0dba4db59d61b60a8fe66f969d20aafa9 Mon Sep 17 00:00:00 2001
+From: Brett Warren <brett.warren@arm.com>
+Date: Wed, 23 Sep 2020 09:27:34 +0100
+Subject: [PATCH] optee: enable clang support
+
+When compiling with clang, the LIBGCC_LOCATE_CFLAG variable used
+to provide a sysroot wasn't included, which results in not locating
+compiler-rt. This is mitigated by including the variable as ammended.
+
+Upstream-Status: Pending
+ChangeId: 8ba69a4b2eb8ebaa047cb266c9aa6c2c3da45701
+Signed-off-by: Brett Warren <brett.warren@arm.com>
+---
+ mk/clang.mk | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/mk/clang.mk b/mk/clang.mk
+index 0f48c836..47465523 100644
+--- a/mk/clang.mk
++++ b/mk/clang.mk
+@@ -27,7 +27,7 @@ comp-cflags-warns-clang := -Wno-language-extension-token \
+ 
+ # Note, use the compiler runtime library (libclang_rt.builtins.*.a) instead of
+ # libgcc for clang
+-libgcc$(sm)	:= $(shell $(CC$(sm)) $(CFLAGS$(arch-bits-$(sm))) \
++libgcc$(sm)	:= $(shell $(CC$(sm)) $(LIBGCC_LOCATE_CFLAGS) $(CFLAGS$(arch-bits-$(sm))) \
+ 			-rtlib=compiler-rt -print-libgcc-file-name 2> /dev/null)
+ 
+ # Core ASLR relies on the executable being ready to run from its preferred load
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-os_3.14.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-os_3.14.0.bb
new file mode 100644
index 0000000..83b89c4
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-os_3.14.0.bb
@@ -0,0 +1,5 @@
+require optee-os.inc
+
+SRCREV = "d21befa5e53eae9db469eba1685f5aa5c6f92c2f"
+
+DEPENDS = "python3-pycryptodome-native python3-pyelftools-native"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-os_3.17.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-os_3.17.0.bb
new file mode 100644
index 0000000..3e5e0a6
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-os_3.17.0.bb
@@ -0,0 +1,5 @@
+require optee-os.inc
+
+SRCREV = "f9e550142dd4b33ee1112f5dd64ffa94ba79cefa"
+
+DEPENDS += "dtc-native"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-os_3.18.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-os_3.18.0.bb
new file mode 100644
index 0000000..65d661f
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-os_3.18.0.bb
@@ -0,0 +1,5 @@
+require optee-os.inc
+
+DEPENDS += "dtc-native"
+
+SRCREV = "1ee647035939e073a2e8dddb727c0f019cc035f1"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-spdevkit_git.bb b/meta-arm/meta-arm/recipes-security/optee/optee-spdevkit_git.bb
new file mode 100644
index 0000000..7608cec
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-spdevkit_git.bb
@@ -0,0 +1,305 @@
+SUMMARY = "OP-TEE Secure Partion Development Kit"
+DESCRIPTION = "Open Portable Trusted Execution Environment - Development Kit to run secure partitions"
+HOMEPAGE = "https://www.op-tee.org/"
+
+LICENSE = "BSD-2-Clause"
+LIC_FILES_CHKSUM = "file://LICENSE;md5=c1f21c4f72f372ef38a5a4aee55ec173"
+
+inherit deploy python3native
+require optee.inc
+FILESEXTRAPATHS:prepend := "${THISDIR}/optee-os:"
+
+CVE_PRODUCT = "linaro:op-tee op-tee:op-tee_os"
+
+DEPENDS = "python3-pyelftools-native"
+
+DEPENDS:append:toolchain-clang = " compiler-rt"
+
+# spdevkit isn't yet merged to master
+SRC_URI = "git://git.trustedfirmware.org/OP-TEE/optee_os.git;protocol=https;branch=psa-development \
+    file://0006-allow-setting-sysroot-for-libgcc-lookup.patch \
+    file://0007-allow-setting-sysroot-for-clang.patch \
+"
+SRCREV = "f9de2c9520ed97b89760cc4c99424aae440b63f4"
+PV = "3.10+git${SRCPV}"
+
+S = "${WORKDIR}/git"
+B = "${WORKDIR}/build"
+
+EXTRA_OEMAKE += " \
+    PLATFORM=${OPTEEMACHINE} \
+    CFG_${OPTEE_CORE}_core=y \
+    CROSS_COMPILE_core=${HOST_PREFIX} \
+    CROSS_COMPILE_sp_${OPTEE_ARCH}=${HOST_PREFIX} \
+    CFG_CORE_FFA=y \
+    CFG_WITH_SP=y \
+    O=${B} \
+"
+
+CFLAGS[unexport] = "1"
+LDFLAGS[unexport] = "1"
+CPPFLAGS[unexport] = "1"
+AS[unexport] = "1"
+LD[unexport] = "1"
+
+do_configure[noexec] = "1"
+
+do_compile() {
+    oe_runmake -C ${S} sp_dev_kit
+}
+do_compile[cleandirs] = "${B}"
+
+do_install() {
+    #install SP devkit
+    install -d ${D}${includedir}/optee/export-user_sp/
+    for f in ${B}/export-sp_${OPTEE_ARCH}/* ; do
+        cp -aR $f ${D}${includedir}/optee/export-user_sp/
+    done
+    cat > ${D}${includedir}/optee/export-user_sp/include/stddef.h <<'EOF'
+#ifndef STDDEF_H
+#define STDDEF_H
+
+#include <stddef_.h>
+
+#ifndef _PTRDIFF_T
+typedef long ptrdiff_t;
+#define _PTRDIFF_T
+#endif
+
+#ifndef NULL
+#define NULL ((void *) 0)
+#endif
+
+#define offsetof(st, m) __builtin_offsetof(st, m)
+
+#endif /* STDDEF_H */
+EOF
+    cat > ${D}${includedir}/optee/export-user_sp/include/stddef_.h <<'EOF'
+#ifndef STDDEF__H
+#define STDDEF__H
+
+#ifndef SIZET_
+typedef unsigned long size_t;
+#define SIZET_
+#endif
+
+#endif /* STDDEF__H */
+EOF
+    cat > ${D}${includedir}/optee/export-user_sp/include/stdarg.h <<'EOF'
+#ifndef STDARG_H
+#define STDARG_H
+
+#define va_list __builtin_va_list
+#define va_start(ap, last) __builtin_va_start(ap, last)
+#define va_end(ap) __builtin_va_end(ap)
+#define va_copy(to, from) __builtin_va_copy(to, from)
+#define va_arg(to, type) __builtin_va_arg(to, type)
+
+#endif /* STDARG_H */
+EOF
+    cat > ${D}${includedir}/optee/export-user_sp/include/stdbool.h <<'EOF'
+#ifndef STDBOOL_H
+#define STDBOOL_H
+
+#define bool	_Bool
+
+#define true	1
+#define false	0
+
+#define __bool_true_false_are_defined	1
+
+#endif /* STDBOOL_H */
+EOF
+
+cat > ${D}${includedir}/optee/export-user_sp/include/features.h <<'EOF'
+    #ifndef _FEATURES_H
+    #define _FEATURES_H
+    #if defined(_ALL_SOURCE) && !defined(_GNU_SOURCE)
+    #define _GNU_SOURCE 1
+#endif
+    #if defined(_DEFAULT_SOURCE) && !defined(_BSD_SOURCE)
+    #define _BSD_SOURCE 1
+#endif
+    #if !defined(_POSIX_SOURCE) && !defined(_POSIX_C_SOURCE) \
+     && !defined(_XOPEN_SOURCE) && !defined(_GNU_SOURCE) \
+     && !defined(_BSD_SOURCE) && !defined(__STRICT_ANSI__)
+    #define _BSD_SOURCE 1
+    #define _XOPEN_SOURCE 700
+#endif
+    #if __STDC_VERSION__ >= 199901L
+    #define __restrict restrict
+    #elif !defined(__GNUC__)
+    #define __restrict
+#endif
+    #if __STDC_VERSION__ >= 199901L || defined(__cplusplus)
+    #define __inline inline
+    #elif !defined(__GNUC__)
+    #define __inline
+#endif
+    #if __STDC_VERSION__ >= 201112L
+    #elif defined(__GNUC__)
+    #define _Noreturn __attribute__((__noreturn__))
+#else
+    #define _Noreturn
+#endif
+    #define __REDIR(x,y) __typeof__(x) x __asm__(#y)
+#endif
+EOF
+cat > ${D}${includedir}/optee/export-user_sp/include/errno.h <<'EOF'
+    #ifndef _ERRNO_H
+    #define _ERRNO_H
+    #include <features.h>
+    #define EPERM            1
+    #define ENOENT           2
+    #define ESRCH            3
+    #define EINTR            4
+    #define EIO              5
+    #define ENXIO            6
+    #define E2BIG            7
+    #define ENOEXEC          8
+    #define EBADF            9
+    #define ECHILD          10
+    #define EAGAIN          11
+    #define ENOMEM          12
+    #define EACCES          13
+    #define EFAULT          14
+    #define ENOTBLK         15
+    #define EBUSY           16
+    #define EEXIST          17
+    #define EXDEV           18
+    #define ENODEV          19
+    #define ENOTDIR         20
+    #define EISDIR          21
+    #define EINVAL          22
+    #define ENFILE          23
+    #define EMFILE          24
+    #define ENOTTY          25
+    #define ETXTBSY         26
+    #define EFBIG           27
+    #define ENOSPC          28
+    #define ESPIPE          29
+    #define EROFS           30
+    #define EMLINK          31
+    #define EPIPE           32
+    #define EDOM            33
+    #define ERANGE          34
+    #define EDEADLK         35
+    #define ENAMETOOLONG    36
+    #define ENOLCK          37
+    #define ENOSYS          38
+    #define ENOTEMPTY       39
+    #define ELOOP           40
+    #define EWOULDBLOCK     EAGAIN
+    #define ENOMSG          42
+    #define EIDRM           43
+    #define ECHRNG          44
+    #define EL2NSYNC        45
+    #define EL3HLT          46
+    #define EL3RST          47
+    #define ELNRNG          48
+    #define EUNATCH         49
+    #define ENOCSI          50
+    #define EL2HLT          51
+    #define EBADE           52
+    #define EBADR           53
+    #define EXFULL          54
+    #define ENOANO          55
+    #define EBADRQC         56
+    #define EBADSLT         57
+    #define EDEADLOCK       EDEADLK
+    #define EBFONT          59
+    #define ENOSTR          60
+    #define ENODATA         61
+    #define ETIME           62
+    #define ENOSR           63
+    #define ENONET          64
+    #define ENOPKG          65
+    #define EREMOTE         66
+    #define ENOLINK         67
+    #define EADV            68
+    #define ESRMNT          69
+    #define ECOMM           70
+    #define EPROTO          71
+    #define EMULTIHOP       72
+    #define EDOTDOT         73
+    #define EBADMSG         74
+    #define EOVERFLOW       75
+    #define ENOTUNIQ        76
+    #define EBADFD          77
+    #define EREMCHG         78
+    #define ELIBACC         79
+    #define ELIBBAD         80
+    #define ELIBSCN         81
+    #define ELIBMAX         82
+    #define ELIBEXEC        83
+    #define EILSEQ          84
+    #define ERESTART        85
+    #define ESTRPIPE        86
+    #define EUSERS          87
+    #define ENOTSOCK        88
+    #define EDESTADDRREQ    89
+    #define EMSGSIZE        90
+    #define EPROTOTYPE      91
+    #define ENOPROTOOPT     92
+    #define EPROTONOSUPPORT 93
+    #define ESOCKTNOSUPPORT 94
+    #define EOPNOTSUPP      95
+    #define ENOTSUP         EOPNOTSUPP
+    #define EPFNOSUPPORT    96
+    #define EAFNOSUPPORT    97
+    #define EADDRINUSE      98
+    #define EADDRNOTAVAIL   99
+    #define ENETDOWN        100
+    #define ENETUNREACH     101
+    #define ENETRESET       102
+    #define ECONNABORTED    103
+    #define ECONNRESET      104
+    #define ENOBUFS         105
+    #define EISCONN         106
+    #define ENOTCONN        107
+    #define ESHUTDOWN       108
+    #define ETOOMANYREFS    109
+    #define ETIMEDOUT       110
+    #define ECONNREFUSED    111
+    #define EHOSTDOWN       112
+    #define EHOSTUNREACH    113
+    #define EALREADY        114
+    #define EINPROGRESS     115
+    #define ESTALE          116
+    #define EUCLEAN         117
+    #define ENOTNAM         118
+    #define ENAVAIL         119
+    #define EISNAM          120
+    #define EREMOTEIO       121
+    #define EDQUOT          122
+    #define ENOMEDIUM       123
+    #define EMEDIUMTYPE     124
+    #define ECANCELED       125
+    #define ENOKEY          126
+    #define EKEYEXPIRED     127
+    #define EKEYREVOKED     128
+    #define EKEYREJECTED    129
+    #define EOWNERDEAD      130
+    #define ENOTRECOVERABLE 131
+    #define ERFKILL         132
+    #define EHWPOISON       133
+    #ifdef __GNUC__
+    __attribute__((const))
+#endif
+    int *__errno_location(void);
+    #define errno (*__errno_location())
+    #ifdef _GNU_SOURCE
+    extern char *program_invocation_short_name, *program_invocation_name;
+#endif
+#endif
+EOF
+}
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+COMPATIBLE_HOST = "aarch64.*-linux"
+
+# optee-spdevkit static library is part of optee-os image. No need to package this library in a staticdev package
+INSANE_SKIP:${PN}-dev = "staticdev"
+# Build paths are currently embedded
+INSANE_SKIP:${PN}-dev += "buildpaths"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-test.inc b/meta-arm/meta-arm/recipes-security/optee/optee-test.inc
new file mode 100644
index 0000000..64b41a8
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-test.inc
@@ -0,0 +1,51 @@
+SUMMARY = "OP-TEE sanity testsuite"
+DESCRIPTION = "Open Portable Trusted Execution Environment - Test suite"
+HOMEPAGE = "https://www.op-tee.org/"
+
+LICENSE = "BSD-2-Clause & GPL-2.0-only"
+LIC_FILES_CHKSUM = "file://LICENSE.md;md5=daa2bcccc666345ab8940aab1315a4fa"
+
+inherit python3native ptest
+require optee.inc
+
+DEPENDS = "optee-client optee-os-tadevkit python3-cryptography-native"
+
+SRC_URI = "git://github.com/OP-TEE/optee_test.git;branch=master;protocol=https \
+           file://run-ptest \
+          "
+
+S = "${WORKDIR}/git"
+B = "${WORKDIR}/build"
+
+EXTRA_OEMAKE += "TA_DEV_KIT_DIR=${TA_DEV_KIT_DIR} \
+                 CROSS_COMPILE_HOST=${HOST_PREFIX} \
+                 CROSS_COMPILE_TA=${HOST_PREFIX} \
+                 O=${B} \
+               "
+
+do_compile() {
+    cd ${S}
+    # Top level makefile doesn't seem to handle parallel make gracefully
+    oe_runmake xtest
+    oe_runmake ta
+    oe_runmake test_plugin
+}
+do_compile[cleandirs] = "${B}"
+
+do_install () {
+    install -D -p -m0755 ${B}/xtest/xtest ${D}${bindir}/xtest
+
+    # install path should match the value set in optee-client/tee-supplicant
+    # default TEEC_LOAD_PATH is /lib
+    mkdir -p ${D}${nonarch_base_libdir}/optee_armtz/
+    install -D -p -m0444 ${B}/ta/*/*.ta ${D}${nonarch_base_libdir}/optee_armtz/
+    mkdir -p ${D}${libdir}/tee-supplicant/plugins
+    install -D -p -m0444 ${B}/supp_plugin/*.plugin ${D}${libdir}/tee-supplicant/plugins/
+}
+
+FILES:${PN} += "${nonarch_base_libdir}/optee_armtz/ \
+                ${libdir}/tee-supplicant/plugins/ \
+               "
+
+# Imports machine specific configs from staging to build
+PACKAGE_ARCH = "${MACHINE_ARCH}"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-test/musl-workaround.patch b/meta-arm/meta-arm/recipes-security/optee/optee-test/musl-workaround.patch
new file mode 100644
index 0000000..eed1bd4
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-test/musl-workaround.patch
@@ -0,0 +1,24 @@
+Hack to work around musl compile error:
+ In file included from optee-test/3.17.0-r0/recipe-sysroot/usr/include/sys/stat.h:23,
+                  from optee-test/3.17.0-r0/git/host/xtest/regression_1000.c:25:
+ optee-test/3.17.0-r0/recipe-sysroot/usr/include/bits/stat.h:17:26: error: expected identifier or '(' before '[' token
+    17 |         unsigned __unused[2];
+       |                          ^
+
+stat.h is not needed, since it is not being used in this file.  So removing it.
+
+Upstream-Status: Pending [Not submitted to upstream yet]
+Signed-off-by: Jon Mason <jon.mason@arm.com>
+
+diff --git a/host/xtest/regression_1000.c b/host/xtest/regression_1000.c
+index 4264884..7f1baca 100644
+--- a/host/xtest/regression_1000.c
++++ b/host/xtest/regression_1000.c
+@@ -22,7 +22,6 @@
+ #include <stdio.h>
+ #include <stdlib.h>
+ #include <string.h>
+-#include <sys/stat.h>
+ #include <sys/types.h>
+ #include <ta_arm_bti.h>
+ #include <ta_concurrent.h>
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-test/run-ptest b/meta-arm/meta-arm/recipes-security/optee/optee-test/run-ptest
new file mode 100755
index 0000000..ba88c14
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-test/run-ptest
@@ -0,0 +1,52 @@
+#!/bin/sh
+xtest | awk '
+
+    # Escapes the special characters in a string so that, when
+    # included in a regex, it represents a literal match
+    function regx_escape_literal(str,    ret) {
+        ret = str
+        gsub(/[\[\]\^\$\.\*\?\+\{\}\\\(\)\|]/ , "\\\\&", str)
+        return str
+    }
+
+    # Returns the simple test formatted name
+    function name(n,    ret) {
+        ret = n
+        gsub(/\./, " ", ret)
+        return ret
+    }
+
+    # Returns the simple test formatted result
+    function result(res) {
+        if(res ~ /OK/) {
+            return "PASS"
+        } else if(res ~ /FAILED/) {
+            return "FAIL"
+        }
+    }
+
+    function parse(name, description,     has_subtests, result_line) {
+        has_subtests = 0
+
+        # Consume every line up to the result line
+        result_line = "  " regx_escape_literal(name) " (OK|FAILED)"
+        do {
+            getline
+
+            # If this is a subtest (denoted by an "o" bullet) then subparse
+            if($0 ~ /^o /) {
+                parse($2, description " : " substr($0, index($0, $3)))
+                has_subtests = 1
+            }
+        } while ($0 !~ result_line)
+
+        # Only print the results for the deepest nested subtests
+        if(!has_subtests) {
+            print result($2) ": " name(name) " - " description
+        }
+    }
+
+    # Start parsing at the beginning of every test (denoted by a "*" bullet)
+    /^\* / { parse($2, substr($0, index($0, $3))) }
+
+'
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-test_3.14.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-test_3.14.0.bb
new file mode 100644
index 0000000..6367c27
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-test_3.14.0.bb
@@ -0,0 +1,3 @@
+require optee-test.inc
+
+SRCREV = "f2eb88affbb7f028561b4fd5cbd049d5d704f741"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee-test_3.17.0.bb b/meta-arm/meta-arm/recipes-security/optee/optee-test_3.17.0.bb
new file mode 100644
index 0000000..18870da
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee-test_3.17.0.bb
@@ -0,0 +1,10 @@
+require optee-test.inc
+
+SRC_URI:append = " \
+    file://musl-workaround.patch \
+   "
+SRCREV = "44a31d02379bd8e50762caa5e1592ad81e3339af"
+
+EXTRA_OEMAKE:append:libc-musl = " OPTEE_OPENSSL_EXPORT=${STAGING_INCDIR}"
+DEPENDS:append:libc-musl = " openssl"
+CFLAGS:append:libc-musl = " -Wno-error=deprecated-declarations"
diff --git a/meta-arm/meta-arm/recipes-security/optee/optee.inc b/meta-arm/meta-arm/recipes-security/optee/optee.inc
new file mode 100644
index 0000000..06c67cf
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/optee/optee.inc
@@ -0,0 +1,33 @@
+UPSTREAM_CHECK_GITTAGREGEX = "^(?P<pver>\d+(\.\d+)+)$"
+
+COMPATIBLE_MACHINE ?= "invalid"
+COMPATIBLE_MACHINE:qemuarm64 ?= "qemuarm64"
+COMPATIBLE_MACHINE:qemu-generic-arm64 ?= "qemu-generic-arm64"
+COMPATIBLE_MACHINE:qemuarm ?= "qemuarm"
+# Please add supported machines below or set it in .bbappend or .conf
+
+OPTEEMACHINE ?= "${MACHINE}"
+OPTEEMACHINE:aarch64:qemuall ?= "vexpress-qemu_armv8a"
+OPTEEMACHINE:arm:qemuall ?= "vexpress-qemu_virt"
+
+OPTEE_ARCH = "null"
+OPTEE_ARCH:arm = "arm32"
+OPTEE_ARCH:aarch64 = "arm64"
+OPTEE_CORE = "${@d.getVar('OPTEE_ARCH').upper()}"
+
+OPTEE_TOOLCHAIN = "${@d.getVar('TOOLCHAIN') or 'gcc'}"
+OPTEE_COMPILER = "${@bb.utils.contains("BBFILE_COLLECTIONS", "clang-layer", "${OPTEE_TOOLCHAIN}", "gcc", d)}"
+
+# Set here but not passed to EXTRA_OEMAKE by default as that breaks
+# the optee-os build
+TA_DEV_KIT_DIR = "${STAGING_INCDIR}/optee/export-user_ta"
+
+EXTRA_OEMAKE += "V=1 \
+                 LIBGCC_LOCATE_CFLAGS='${HOST_CC_ARCH}${TOOLCHAIN_OPTIONS}' \
+                 COMPILER=${OPTEE_COMPILER} \
+                 OPTEE_CLIENT_EXPORT=${STAGING_DIR_HOST}${prefix} \
+                 TEEC_EXPORT=${STAGING_DIR_HOST}${prefix} \
+                "
+# python3-cryptography needs the legacy provider, so set OPENSSL_MODULES to the
+# right path until this is relocated automatically.
+export OPENSSL_MODULES="${STAGING_LIBDIR_NATIVE}/ossl-modules"
diff --git a/meta-arm/meta-arm/recipes-security/trusted-services/secure-partitions.inc b/meta-arm/meta-arm/recipes-security/trusted-services/secure-partitions.inc
new file mode 100644
index 0000000..1df7409
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/trusted-services/secure-partitions.inc
@@ -0,0 +1,27 @@
+LICENSE = "Apache-2.0 & BSD-3-Clause & Zlib"
+LIC_FILES_CHKSUM = "file://license.rst;md5=ea160bac7f690a069c608516b17997f4"
+
+SRC_URI = "git://git.trustedfirmware.org/TS/trusted-services.git;protocol=https;branch=integration;name=ts;destsuffix=git/ts"
+
+SRCREV_ts ?= "a365a04f937b9b76ebb2e0eeade226f208cbc0d2"
+
+S = "${WORKDIR}/git/ts"
+B = "${WORKDIR}/build"
+
+export CROSS_COMPILE="${TARGET_PREFIX}"
+
+CFLAGS[unexport] = "1"
+CPPFLAGS[unexport] = "1"
+AS[unexport] = "1"
+LD[unexport] = "1"
+
+# setting the linker options used to build the secure partitions
+SECURITY_LDFLAGS = ""
+TARGET_LDFLAGS = "-Wl,--build-id=none -Wl,--hash-style=both"
+
+do_configure[cleandirs] = "${B}"
+
+# Currently trusted-services and psa-arch-tests use FetchContent to download
+# more sources during do_configure. Until this is resolved we need to allow
+# network operations.
+do_configure[network] = "1"
diff --git a/meta-arm/meta-arm/recipes-security/trusted-services/secure-partitions_git.bb b/meta-arm/meta-arm/recipes-security/trusted-services/secure-partitions_git.bb
new file mode 100644
index 0000000..fca6d9d
--- /dev/null
+++ b/meta-arm/meta-arm/recipes-security/trusted-services/secure-partitions_git.bb
@@ -0,0 +1,74 @@
+SUMMARY = "Trusted Services secure partitions"
+HOMEPAGE = "https://trusted-services.readthedocs.io/en/latest/index.html"
+
+COMPATIBLE_MACHINE ?= "invalid"
+
+PACKAGE_ARCH = "${MACHINE_ARCH}"
+
+require secure-partitions.inc
+
+SRCREV_FORMAT = "ts"
+PV = "0.0+git${SRCPV}"
+
+# Which environment to create the secure partions for (opteesp or shim)
+TS_ENVIRONMENT ?= "opteesp"
+
+inherit deploy python3native
+
+DEPENDS = "python3-pycryptodome-native python3-pycryptodomex-native \
+           python3-pyelftools-native python3-grpcio-tools-native \
+           python3-protobuf-native protobuf-native cmake-native \
+           "
+
+DEPENDS:append = " ${@bb.utils.contains('TS_ENVIRONMENT', 'opteesp', 'optee-spdevkit', '', d)}"
+
+export CROSS_COMPILE="${TARGET_PREFIX}"
+
+CFLAGS[unexport] = "1"
+CPPFLAGS[unexport] = "1"
+AS[unexport] = "1"
+LD[unexport] = "1"
+
+# only used if TS_ENVIRONMENT is opteesp
+SP_DEV_KIT_DIR = "${@bb.utils.contains('TS_ENVIRONMENT', 'opteesp', '${STAGING_INCDIR}/optee/export-user_sp', '', d)}"
+
+# SP images are embedded into optee os image
+SP_PACKAGING_METHOD ?= "embedded"
+
+do_configure() {
+    for TS_DEPLOYMENT in ${TS_DEPLOYMENTS}; do
+        cmake \
+          -DCMAKE_INSTALL_PREFIX=${D}/firmware/sp \
+          -DSP_DEV_KIT_DIR=${SP_DEV_KIT_DIR} \
+          -DSP_PACKAGING_METHOD=${SP_PACKAGING_METHOD} \
+	  -DTS_PLATFORM="${TS_PLATFORM}" \
+          -S ${S}/$TS_DEPLOYMENT -B "${B}/$TS_DEPLOYMENT"
+    done
+}
+
+do_compile() {
+    for TS_DEPLOYMENT in ${TS_DEPLOYMENTS}; do
+        cmake --build "${B}/$TS_DEPLOYMENT"
+    done
+}
+
+do_install () {
+    if [ "${TS_ENVIRONMENT}" = "opteesp" ]; then
+        for TS_DEPLOYMENT in ${TS_DEPLOYMENTS}; do
+            cmake --install "${B}/$TS_DEPLOYMENT"
+        done
+    fi
+}
+
+SYSROOT_DIRS = "/firmware"
+
+do_deploy() {
+    cp -rf ${D}/firmware/* ${DEPLOYDIR}/
+}
+addtask deploy after do_install
+
+FILES:${PN} = "/firmware/sp/opteesp*"
+
+# Build paths are currently embedded
+INSANE_SKIP:${PN} += "buildpaths"
+INSANE_SKIP:${PN}-dbg += "buildpaths"
diff --git a/meta-arm/meta-arm/wic/efi-disk.wks.in b/meta-arm/meta-arm/wic/efi-disk.wks.in
new file mode 100644
index 0000000..1f06830
--- /dev/null
+++ b/meta-arm/meta-arm/wic/efi-disk.wks.in
@@ -0,0 +1,11 @@
+# short-description: Create an EFI disk image
+# long-description: Creates a partitioned EFI disk image that the user
+# can directly dd to boot media.
+
+part /boot --source bootimg-efi --sourceparams="loader=${EFI_PROVIDER}" --label boot --active --align 1024 --use-uuid
+
+part / --source rootfs --fstype=ext4 --label root --align 1024 --use-uuid
+
+part swap --size 44 --label swap --fstype=swap --use-uuid
+
+bootloader --ptable gpt --timeout=5 --append="rootwait rootfstype=ext4"
diff --git a/meta-arm/meta-arm/wic/qemu-efi-disk.wks.in b/meta-arm/meta-arm/wic/qemu-efi-disk.wks.in
new file mode 100644
index 0000000..4f898ef
--- /dev/null
+++ b/meta-arm/meta-arm/wic/qemu-efi-disk.wks.in
@@ -0,0 +1,11 @@
+# short-description: Create an EFI disk image
+# long-description: Creates a partitioned EFI disk image that the user
+# can directly dd to boot media.
+
+part /boot --source bootimg-efi --sourceparams="loader=${EFI_PROVIDER}" --label boot --active --align 1024 --use-uuid
+
+part / --source rootfs --fstype=ext4 --label root --align 1024 --use-uuid
+
+part swap --size 44 --label swap --fstype=swap --use-uuid
+
+bootloader --ptable gpt --timeout=5 --append="rootfstype=ext4 ip=dhcp"
diff --git a/meta-arm/meta-arm/wic/qemuarm.cfg b/meta-arm/meta-arm/wic/qemuarm.cfg
new file mode 100644
index 0000000..79ce7b4
--- /dev/null
+++ b/meta-arm/meta-arm/wic/qemuarm.cfg
@@ -0,0 +1,3 @@
+default Yocto
+label Yocto
+    kernel /zImage
diff --git a/meta-arm/meta-arm/wic/qemuarm.wks b/meta-arm/meta-arm/wic/qemuarm.wks
new file mode 100644
index 0000000..ccd53c2
--- /dev/null
+++ b/meta-arm/meta-arm/wic/qemuarm.wks
@@ -0,0 +1,4 @@
+bootloader --ptable gpt --configfile="qemuarm.cfg"
+
+part /boot --ondisk=vda --align 64 --size=100M --active --source bootimg-partition --fstype=ext4 --label boot --sourceparams="loader=u-boot"
+part /     --ondisk=vda                                 --source rootfs            --fstype=ext4 --label root
diff --git a/meta-arm/meta-arm/wic/qemuarm64.cfg b/meta-arm/meta-arm/wic/qemuarm64.cfg
new file mode 100644
index 0000000..b9c9da6
--- /dev/null
+++ b/meta-arm/meta-arm/wic/qemuarm64.cfg
@@ -0,0 +1,3 @@
+default Yocto
+label Yocto
+    kernel /Image
diff --git a/meta-arm/meta-arm/wic/qemuarm64.wks b/meta-arm/meta-arm/wic/qemuarm64.wks
new file mode 100644
index 0000000..9df44c2
--- /dev/null
+++ b/meta-arm/meta-arm/wic/qemuarm64.wks
@@ -0,0 +1,4 @@
+bootloader --ptable gpt --configfile="qemuarm64.cfg"
+
+part /boot --ondisk=vda --align 64 --size=100M --active --source bootimg-partition --fstype=ext4 --label boot --sourceparams="loader=u-boot"
+part /     --ondisk=vda                                 --source rootfs            --fstype=ext4 --label root
diff --git a/meta-arm/meta-atp/README.md b/meta-arm/meta-atp/README.md
new file mode 100644
index 0000000..15d0e29
--- /dev/null
+++ b/meta-arm/meta-atp/README.md
@@ -0,0 +1,76 @@
+# meta-atp layer
+
+The meta-atp layer supports building environments with traffic generation capabilities based on [AMBA Adaptive Traffic Profiles (ATP)](https://developer.arm.com/documentation/ihi0082/latest).
+
+## Recipes
+
+The meta-atp layer supports building the following software components:
+
+- Arm's implementation of the AMBA ATP specification, namely the [AMBA ATP Engine](https://github.com/ARM-software/ATP-Engine).
+- Linux kernel modules and user API (UAPI) for programming ATP devices.
+- Integration test suite for verification of kernel modules and UAPI.
+
+It is also possible to build the AMBA ATP Engine as part of the final [gem5](https://www.gem5.org/) executable. For this, meta-atp extends the `gem5-aarch64-native` recipe to add the AMBA ATP engine code as extra sources.
+
+## Machines
+
+The `gem5-atp-arm64` machine extends the `gem5-arm64` machine to instantiate a simulated platform with support for programmable AMBA ATP traffic generation. The platform includes the following models:
+
+- `ProfileGen` model. This is the adapter layer between gem5 and the AMBA ATP Engine. It is the source of traffic into the gem5 host platform.
+- `ATPDevice` model. Software can program it using the Linux kernel modules and UAPI to control traffic generation.
+
+## Usage
+
+Users should add the meta-atp layer and layer dependencies to `conf/bblayers.conf`. See `conf/layer.conf` for dependencies.
+
+### Standalone Engine executable
+
+Users can build the AMBA ATP Engine as a standalone native executable as follows:
+
+```bash
+bitbake atp-native
+```
+
+Users can run the executable through standard build scripts:
+
+```bash
+oe-run-native atp-native atpeng [--help | args...]
+```
+
+## Integration of the Engine in gem5
+
+Users should select the `gem5-atp-arm64` platform in their `conf/local.conf` file.
+
+Users can build the target image of preference, for example:
+
+```bash
+bitbake core-image-minimal
+```
+
+The resulting gem5 native executable contains the AMBA ATP Engine. The resulting target image contains the kernel modules, UAPI and test suite.
+
+Users should run the environment as follows:
+
+```bash
+./tmp/deploy/tools/start-gem5-atp.sh
+```
+
+This script launches a fast simulation to fast-forward Linux boot. Once Linux boot is completed, the fast simulation switches into a detailed simulation for the final usable environment. Users can connect and interact with the environment as follows:
+
+```bash
+oe-run-native gem5-m5term-native m5term <PORT>
+```
+
+The connection PORT is announced by the deploy script as:
+
+```bash
+system.terminal: Listening for connections on port <PORT>
+```
+
+This is usually port 3456.
+
+Users can verify access to the ATP device by running the integration test suite from within the simulated environment as follows:
+
+```bash
+test_atp.out
+```
diff --git a/meta-arm/meta-atp/conf/layer.conf b/meta-arm/meta-atp/conf/layer.conf
new file mode 100644
index 0000000..3ec36e8
--- /dev/null
+++ b/meta-arm/meta-atp/conf/layer.conf
@@ -0,0 +1,9 @@
+BBPATH .= ":${LAYERDIR}"
+BBFILES += "${LAYERDIR}/recipes-*/*/*.bb \
+            ${LAYERDIR}/recipes-*/*/*.bbappend"
+
+BBFILE_COLLECTIONS += "meta-atp"
+BBFILE_PATTERN_meta-atp = "^${LAYERDIR}/"
+
+LAYERDEPENDS_meta-atp = "core openembedded-layer meta-gem5"
+LAYERSERIES_COMPAT_meta-atp = "kirkstone"
diff --git a/meta-arm/meta-atp/conf/machine/gem5-atp-arm64.conf b/meta-arm/meta-atp/conf/machine/gem5-atp-arm64.conf
new file mode 100644
index 0000000..d5fe22a
--- /dev/null
+++ b/meta-arm/meta-atp/conf/machine/gem5-atp-arm64.conf
@@ -0,0 +1,11 @@
+require conf/machine/gem5-arm64.conf
+MACHINEOVERRIDES =. "gem5-arm64:"
+
+# Use baremetal_atp.py as machine configuration
+GEM5_RUN_PROFILE = "configs/baremetal_atp.py"
+# Require m5term
+EXTRA_IMAGEDEPENDS += "gem5-m5term-native"
+# Require ATP kernel modules, user API and gem5 m5ops
+MACHINE_ESSENTIAL_EXTRA_RDEPENDS += "kernel-module-atp atp-uapi gem5-m5ops"
+# Optionally provide ATP kernel tests
+MACHINE_ESSENTIAL_EXTRA_RRECOMMENDS += "atp-test"
diff --git a/meta-arm/meta-atp/recipes-devtools/atp/atp-native_3.1.bb b/meta-arm/meta-atp/recipes-devtools/atp/atp-native_3.1.bb
new file mode 100644
index 0000000..31b58bf
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-devtools/atp/atp-native_3.1.bb
@@ -0,0 +1,18 @@
+require atp-source_3.1.inc
+inherit pkgconfig native
+
+SUMMARY = "AMBA ATP Engine: synthetic traffic interface modelling framework"
+
+S = "${WORKDIR}/git"
+SRC_URI = "${ATP_SRC} \
+           file://no-werror.patch"
+
+EXTRA_OEMAKE += "EXTRA_CXX_FLAGS='${CXXFLAGS}' EXTRA_LD_FLAGS='${LDFLAGS}'"
+
+do_install() {
+    oe_runmake install
+}
+
+DEPENDS = "protobuf-native cppunit-native"
+
+addtask addto_recipe_sysroot before do_build
diff --git a/meta-arm/meta-atp/recipes-devtools/atp/atp-source_3.1.inc b/meta-arm/meta-atp/recipes-devtools/atp/atp-source_3.1.inc
new file mode 100644
index 0000000..de24ef8
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-devtools/atp/atp-source_3.1.inc
@@ -0,0 +1,9 @@
+ATP_SRC = "git://github.com/ARM-software/ATP-Engine.git;protocol=https;branch=master"
+ATP_REV = "be1066029d6256626b37be004e2a663fbc29f37e"
+ATP_LIC = "BSD-3-Clause-Clear"
+ATP_LIC_MD5 = "e836b5992257064f488715d9a59752c3"
+
+HOMEPAGE ?= "https://github.com/ARM-software/ATP-Engine"
+SRCREV ?= "${ATP_REV}"
+LICENSE ?= "${ATP_LIC}"
+LIC_FILES_CHKSUM ?= "file://LICENSE;md5=${ATP_LIC_MD5}"
diff --git a/meta-arm/meta-atp/recipes-devtools/atp/files/no-werror.patch b/meta-arm/meta-atp/recipes-devtools/atp/files/no-werror.patch
new file mode 100644
index 0000000..4be143a
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-devtools/atp/files/no-werror.patch
@@ -0,0 +1,18 @@
+Don't pass -Werror, as new compilers introduce new warnings.
+
+Upstream-Status: Pending
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+diff --git a/Makefile b/Makefile
+index c01b120..1d3c30a 100644
+--- a/Makefile
++++ b/Makefile
+@@ -18,7 +18,7 @@ PROTOBUF_C_FLAGS:= $(shell pkg-config --cflags protobuf)
+ CPPUNIT_C_FLAGS := $(shell pkg-config --cflags cppunit)
+ PROTOBUF_L_FLAGS:= $(shell pkg-config --libs protobuf)
+ CPPUNIT_L_FLAGS	:= $(shell pkg-config --libs cppunit)
+-CXX_FLAGS       := $(PROTOBUF_C_FLAGS) -std=c++11 -Wall -Werror -Wextra -Wno-unused-parameter -Wno-unused-variable $(CPPUNIT_C_FLAGS) -fPIC $(EXTRA_CXX_FLAGS)
++CXX_FLAGS       := $(PROTOBUF_C_FLAGS) -std=c++11 -Wall -Wextra -Wno-unused-parameter -Wno-unused-variable $(CPPUNIT_C_FLAGS) -fPIC $(EXTRA_CXX_FLAGS)
+ LD_FLAGS        := $(PROTOBUF_L_FLAGS) $(CPPUNIT_L_FLAGS) $(EXTRA_LD_FLAGS)
+ PROTO_SRC_DIR   := ./proto/
+ PROTO_SRC       := $(wildcard $(PROTO_SRC_DIR)tp*.proto)
diff --git a/meta-arm/meta-atp/recipes-devtools/gem5/gem5-aarch64-dtb.bbappend b/meta-arm/meta-atp/recipes-devtools/gem5/gem5-aarch64-dtb.bbappend
new file mode 100644
index 0000000..2b55b89
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-devtools/gem5/gem5-aarch64-dtb.bbappend
@@ -0,0 +1,3 @@
+# Export datadir paths for baremetal_atp.py script
+export GEM5_DATADIR = "${STAGING_DATADIR_NATIVE}/gem5"
+export ATP_DATADIR = "${STAGING_DATADIR_NATIVE}/gem5"
diff --git a/meta-arm/meta-atp/recipes-devtools/gem5/gem5-aarch64-native/start-gem5-atp.sh b/meta-arm/meta-atp/recipes-devtools/gem5/gem5-aarch64-native/start-gem5-atp.sh
new file mode 100755
index 0000000..16dac47
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-devtools/gem5/gem5-aarch64-native/start-gem5-atp.sh
@@ -0,0 +1,13 @@
+#!/usr/bin/env bash
+
+source <(bitbake -e gem5-aarch64-native | grep \
+    -e "^STAGING_DATADIR_NATIVE=" -e "^DEPLOY_DIR_TOOLS=")
+
+# Used by baremetal_atp.py
+export GEM5_DATADIR=${STAGING_DATADIR_NATIVE}/gem5
+export ATP_DATADIR=${STAGING_DATADIR_NATIVE}/gem5
+
+# Fast-forward Linux boot and restore into timing simulation
+${DEPLOY_DIR_TOOLS}/start-gem5.sh --checkpoint $@
+${DEPLOY_DIR_TOOLS}/start-gem5.sh --restore-with-cpu TimingSimpleCPU \
+                                  --checkpoint-restore 1 $@
diff --git a/meta-arm/meta-atp/recipes-devtools/gem5/gem5-aarch64-native_20.bbappend b/meta-arm/meta-atp/recipes-devtools/gem5/gem5-aarch64-native_20.bbappend
new file mode 100644
index 0000000..6607f0f
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-devtools/gem5/gem5-aarch64-native_20.bbappend
@@ -0,0 +1,24 @@
+require recipes-devtools/atp/atp-source_3.1.inc
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/${BPN}:"
+
+SRC_URI += "${ATP_SRC};destsuffix=git/atp;name=atp \
+            file://start-gem5-atp.sh"
+SRCREV_FORMAT = "gem5_atp"
+SRCREV_atp = "${ATP_REV}"
+LICENSE += "& ${ATP_LIC}"
+LIC_FILES_CHKSUM += "file://atp/LICENSE;md5=${ATP_LIC_MD5}"
+
+EXTRA_OESCONS += "EXTRAS=${S}/atp"
+
+do_install:append() {
+    # baremetal_atp.py machine configuration and sample stream.atp file
+    install -m 644 ${B}/atp/gem5/baremetal_atp.py \
+                   ${B}/atp/configs/stream.atp \
+                   ${D}${datadir}/gem5/configs
+}
+
+do_deploy:append() {
+    # start-gem5-atp.sh launch script
+    install -m 755 ${WORKDIR}/start-gem5-atp.sh ${DEPLOYDIR}
+}
diff --git a/meta-arm/meta-atp/recipes-devtools/gem5/gem5-m5ops/m5-readfile.sh b/meta-arm/meta-atp/recipes-devtools/gem5/gem5-m5ops/m5-readfile.sh
new file mode 100755
index 0000000..44477e9
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-devtools/gem5/gem5-m5ops/m5-readfile.sh
@@ -0,0 +1,9 @@
+#!/bin/sh
+### BEGIN INIT INFO
+# Provides:         m5-readfile
+# Required-Start:   $all
+# Default-Start:    5
+# Description:      Enables reading any script at simulation launch time.
+### END INIT INFO
+
+m5 readfile | sh
diff --git a/meta-arm/meta-atp/recipes-devtools/gem5/gem5-m5ops_20.bbappend b/meta-arm/meta-atp/recipes-devtools/gem5/gem5-m5ops_20.bbappend
new file mode 100644
index 0000000..3ba0c3c
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-devtools/gem5/gem5-m5ops_20.bbappend
@@ -0,0 +1,14 @@
+inherit update-rc.d
+
+FILESEXTRAPATHS:prepend := "${THISDIR}/${BPN}:"
+
+# Add startup script calling m5 readfile for automatic checkpoint and restore
+SRC_URI += "file://m5-readfile.sh"
+
+INITSCRIPT_NAME = "m5-readfile.sh"
+INITSCRIPT_PARAMS = "defaults 99"
+
+do_install:append() {
+    install -d ${D}/${INIT_D_DIR}
+    install -m 755 ${WORKDIR}/m5-readfile.sh ${D}/${INIT_D_DIR}
+}
diff --git a/meta-arm/meta-atp/recipes-kernel/atp/atp-module_3.1.bb b/meta-arm/meta-atp/recipes-kernel/atp/atp-module_3.1.bb
new file mode 100644
index 0000000..0bf4949
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-kernel/atp/atp-module_3.1.bb
@@ -0,0 +1,24 @@
+require recipes-devtools/atp/atp-source_3.1.inc
+inherit module
+
+SUMMARY = "Kernel modules for interacting wih ATP Engine and devices"
+SECTION = "kernel/modules"
+
+S = "${WORKDIR}/git"
+SRC_URI = "${ATP_SRC}"
+
+ATP_MOD_DIR = "linux"
+
+EXTRA_OEMAKE += "-C ${ATP_MOD_DIR}"
+
+PROVIDES = "kernel-module-atp"
+RPROVIDES:${PN} = "kernel-module-atp"
+KERNEL_MODULE_AUTOLOAD += "atp_buffer_manager atp_device"
+MODULES_MODULE_SYMVERS_LOCATION = "${ATP_MOD_DIR}"
+
+do_install:append() {
+    install -d ${D}${includedir}/linux
+    install -m 644 ${ATP_MOD_DIR}/atp_buffer_manager_user.h \
+                   ${ATP_MOD_DIR}/atp_device_user.h \
+                   ${D}${includedir}/linux
+}
diff --git a/meta-arm/meta-atp/recipes-kernel/atp/atp-test_3.1.bb b/meta-arm/meta-atp/recipes-kernel/atp/atp-test_3.1.bb
new file mode 100644
index 0000000..5a3097e
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-kernel/atp/atp-test_3.1.bb
@@ -0,0 +1,21 @@
+require recipes-devtools/atp/atp-source_3.1.inc
+inherit package
+
+SUMMARY = "End-to-end tests evaluating ATP kernel modules service correctness"
+SECTION = "kernel/userland"
+
+S = "${WORKDIR}/git"
+SRC_URI = "${ATP_SRC}"
+
+EXTRA_OEMAKE += "-C linux/test"
+
+do_compile() {
+    oe_runmake
+}
+
+do_install() {
+    oe_runmake DESTDIR=${D} PREFIX=${prefix} install
+}
+
+DEPENDS = "atp-uapi cppunit"
+RDEPENDS:${PN} = "atp-uapi"
diff --git a/meta-arm/meta-atp/recipes-kernel/atp/atp-uapi_3.1.bb b/meta-arm/meta-atp/recipes-kernel/atp/atp-uapi_3.1.bb
new file mode 100644
index 0000000..8c793a3
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-kernel/atp/atp-uapi_3.1.bb
@@ -0,0 +1,24 @@
+require recipes-devtools/atp/atp-source_3.1.inc
+inherit package
+
+SUMMARY = "User API for accessing services from ATP kernel modules"
+SECTION = "kernel/userland"
+
+S = "${WORKDIR}/git"
+SRC_URI = "${ATP_SRC}"
+
+# Unversioned library
+SOLIBS = ".so"
+FILES_SOLIBSDEV = ""
+
+EXTRA_OEMAKE += "-C linux/uapi"
+
+do_compile() {
+    oe_runmake KERNEL_HDR_PATH=${STAGING_INCDIR}
+}
+
+do_install() {
+    oe_runmake DESTDIR=${D} PREFIX=${prefix} install
+}
+
+DEPENDS = "linux-libc-headers kernel-module-atp"
diff --git a/meta-arm/meta-atp/recipes-kernel/linux/files/no_ftrace.cfg b/meta-arm/meta-atp/recipes-kernel/linux/files/no_ftrace.cfg
new file mode 100644
index 0000000..870eeaf
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-kernel/linux/files/no_ftrace.cfg
@@ -0,0 +1 @@
+# CONFIG_FTRACE is not set
diff --git a/meta-arm/meta-atp/recipes-kernel/linux/files/smmuv3.cfg b/meta-arm/meta-atp/recipes-kernel/linux/files/smmuv3.cfg
new file mode 100644
index 0000000..2d0a87d
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-kernel/linux/files/smmuv3.cfg
@@ -0,0 +1,4 @@
+CONFIG_IOMMU_IO_PGTABLE=y
+CONFIG_IOMMU_IO_PGTABLE_LPAE=y
+# CONFIG_IOMMU_IO_PGTABLE_LPAE_SELFTEST is not set
+CONFIG_ARM_SMMU_V3=y
diff --git a/meta-arm/meta-atp/recipes-kernel/linux/linux-yocto_%.bbappend b/meta-arm/meta-atp/recipes-kernel/linux/linux-yocto_%.bbappend
new file mode 100644
index 0000000..f59f8d4
--- /dev/null
+++ b/meta-arm/meta-atp/recipes-kernel/linux/linux-yocto_%.bbappend
@@ -0,0 +1,2 @@
+FILESEXTRAPATHS:prepend := "${THISDIR}/files:"
+SRC_URI += "file://no_ftrace.cfg file://smmuv3.cfg"
diff --git a/meta-arm/meta-gem5/COPYING.MIT b/meta-arm/meta-gem5/COPYING.MIT
new file mode 100644
index 0000000..fb950dc
--- /dev/null
+++ b/meta-arm/meta-gem5/COPYING.MIT
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy 
+of this software and associated documentation files (the "Software"), to deal 
+in the Software without restriction, including without limitation the rights 
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
+copies of the Software, and to permit persons to whom the Software is 
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in 
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 
+THE SOFTWARE.
diff --git a/meta-arm/meta-gem5/README.md b/meta-arm/meta-gem5/README.md
new file mode 100644
index 0000000..490ddca
--- /dev/null
+++ b/meta-arm/meta-gem5/README.md
@@ -0,0 +1 @@
+See ../README.md
diff --git a/meta-arm/meta-gem5/conf/layer.conf b/meta-arm/meta-gem5/conf/layer.conf
new file mode 100644
index 0000000..d329bd0
--- /dev/null
+++ b/meta-arm/meta-gem5/conf/layer.conf
@@ -0,0 +1,19 @@
+# We have a conf and classes directory, add to BBPATH
+BBPATH .= ":${LAYERDIR}"
+
+# We have recipes-* directories, add to BBFILES
+BBFILES += "${LAYERDIR}/recipes-*/*/*.bb \
+            ${LAYERDIR}/recipes-*/*/*.bbappend"
+
+BBFILE_COLLECTIONS += "meta-gem5"
+BBFILE_PATTERN_meta-gem5 = "^${LAYERDIR}/"
+BBFILE_PRIORITY_meta-gem5 = "5"
+
+LAYERDEPENDS_meta-gem5 = "core openembedded-layer meta-arm"
+LAYERSERIES_COMPAT_meta-gem5 = "kirkstone"
+
+BBFILES_DYNAMIC += " \
+    virtualization-layer:${LAYERDIR}/dynamic-layers/meta-virtualization/*/*/*.bbappend \
+"
+
+DISTRO_FEATURES_NATIVE:append = " ${@bb.utils.filter('DISTRO_FEATURES', 'xen', d)}"
diff --git a/meta-arm/meta-gem5/conf/machine/gem5-arm64.conf b/meta-arm/meta-gem5/conf/machine/gem5-arm64.conf
new file mode 100644
index 0000000..fa931da
--- /dev/null
+++ b/meta-arm/meta-gem5/conf/machine/gem5-arm64.conf
@@ -0,0 +1,54 @@
+# Configuration for gem5 running on ARM64
+
+#@TYPE: Machine
+#@NAME: Gem5 arm64 machine
+#@DESCRIPTION: Machine configuration for Gem5 arm64
+
+TUNE_FEATURES = "aarch64"
+
+require conf/machine/include/arm/arch-armv8a.inc
+
+MACHINE_FEATURES = "optee pci"
+
+KERNEL_IMAGETYPES = "Image vmlinux"
+KERNEL_IMAGETYPE = "Image"
+
+IMAGE_FSTYPES += "tar.bz2 ext4"
+
+SERIAL_CONSOLES = "115200;ttyAMA0"
+
+EXTRA_IMAGEDEPENDS += "virtual/gem5-bootloader"
+
+PREFERRED_PROVIDER_virtual/kernel ?= "linux-yocto"
+PREFERRED_VERSION_linux-yocto ?= "5.4%"
+
+
+# Uncomment the following if you need to build gem5 provided bootloader, else
+# using standard bootloader by Linux aarch64
+# Use gem5 provided bootloader
+# PREFERRED_PROVIDER_virtual/gem5-bootloader = "gem5-aarch64-bootloader"
+
+# use the dtb stored in the kernel recipe
+# KERNEL_DEVICETREE ?= "gem5-arm64/armv8_gem5_v2_4cpu.dtb"
+
+# Use Linux aarch64 boot wrapper with FDT support and generated
+# dtb (gem5-aarch64-dtb.bb)
+PREFERRED_PROVIDER_virtual/gem5-bootloader = "boot-wrapper-aarch64"
+
+BOOT_WRAPPER_AARCH64_CMDLINE ?= "\
+    earlyprintk=pl011,0x1c090000 console=ttyAMA0 root=/dev/vda rw mem=1G \
+    "
+# Use baremetal profile and axf file so dtb is in axf file
+GEM5_RUN_PROFILE = "configs/example/arm/baremetal.py"
+GEM5_RUN_KERNEL = "linux-system.axf"
+GEM5_RUN_EXTRA = ""
+GEM5_RUN_DTB = ""
+GEM5_RUN_CMDLINE = ""
+
+EXTRA_IMAGEDEPENDS += "gem5-aarch64-native"
+
+# As this is a virtual target that will not be used in the real world there is
+# no need for real SSH keys.  Disable rng-tools (which takes too long to
+# initialise) and install the pre-generated keys.
+PACKAGECONFIG:remove:pn-openssh = "rng-tools"
+MACHINE_EXTRA_RRECOMMENDS += "ssh-pregen-hostkeys"
diff --git a/meta-arm/meta-gem5/documentation/gem5-arm64.md b/meta-arm/meta-gem5/documentation/gem5-arm64.md
new file mode 100644
index 0000000..137676b
--- /dev/null
+++ b/meta-arm/meta-gem5/documentation/gem5-arm64.md
@@ -0,0 +1,32 @@
+# Gem5 Arm64 Platform Support in meta-gem5
+
+## Howto Build and Run
+
+### Configuration:
+In the local.conf file, MACHINE should be set as follow:
+MACHINE ?= "gem5-arm64"
+
+And in the bblayers.conf the following layers need to be added:
+##OEROOT##/meta-arm/meta-arm-toolchain
+##OEROOT##/meta-arm/meta-arm
+
+### Build:
+```bash$ bitbake core-image-minimal```
+
+### Run:
+After compilation of an image, you can execute it using the compiled gem5
+with the followin command:
+```./tmp/deploy/tools/start-gem5.sh```
+
+You can modify the script to change the command line options of gem5.
+
+## Devices supported in the kernel
+- serial
+
+### Untested:
+- pci
+- sata
+- ide
+
+
+## Devices not supported or not functional
diff --git a/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-bsp/boot-wrapper-aaarch64/boot-wrapper-aarch64_%.bbappend b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-bsp/boot-wrapper-aaarch64/boot-wrapper-aarch64_%.bbappend
new file mode 100644
index 0000000..e4b97e9
--- /dev/null
+++ b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-bsp/boot-wrapper-aaarch64/boot-wrapper-aarch64_%.bbappend
@@ -0,0 +1,32 @@
+# Use OVERRIDES to minimize the usage of
+# ${@bb.utils.contains('DISTRO_FEATURES', 'xen', ...
+OVERRIDES:append = "${@bb.utils.contains('DISTRO_FEATURES', 'xen', ':xen', '', d)}"
+
+# Xen image to put in the image
+# This should point to a file in the deploy image directory
+BOOT_WRAPPER_AARCH64_XEN ??= "xen-${MACHINE}"
+
+# Xen command line for the image
+BOOT_WRAPPER_AARCH64_XEN_CMDLINE ??= "noreboot dom0_mem=256M"
+
+BOOT_WRAPPER_AARCH64_XEN_CMDLINE:gem5-arm64 = "noreboot dom0_mem=256M console=dtuart \
+    dtuart=/uart@1c090000 bootscrub=0"
+
+# Fix command line in the axf file for gem5-arm64 when Xen is present
+BOOT_WRAPPER_AARCH64_CMDLINE_xen:gem5-arm64 = "console=hvc0 root=/dev/vda rw"
+
+# Image generated by boot wrapper when Xen is present
+BOOT_WRAPPER_AARCH64_IMAGE:xen ?= "xen-system.axf"
+
+EXTRA_OECONF:append:xen = " \
+--with-xen=${WORKDIR}/kernel/arch/arm64/boot/Image \
+--with-xen-cmdline="" \
+"
+
+EXTRA_OEMAKE:append:xen = " \
+XEN_IMAGE=${DEPLOY_DIR_IMAGE}/${BOOT_WRAPPER_AARCH64_XEN} \
+XEN_CMDLINE="${BOOT_WRAPPER_AARCH64_XEN_CMDLINE}" \
+"
+
+# We need xen if it is activated
+do_deploy[depends] += "${@bb.utils.contains('DISTRO_FEATURES', 'xen', 'xen:do_deploy', '', d)}"
diff --git a/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-devtools/gem5/gem5-aarch64-native_20.bbappend b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-devtools/gem5/gem5-aarch64-native_20.bbappend
new file mode 100644
index 0000000..5c38bdd
--- /dev/null
+++ b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-devtools/gem5/gem5-aarch64-native_20.bbappend
@@ -0,0 +1,7 @@
+# When booting gem5-arm64 with Xen we need to set the cpu as Cortex A53 and
+# remove support for pointer authentification
+GEM5_RUN_EXTRA:append = " \
+${@bb.utils.contains('DISTRO_FEATURES_NATIVE', 'xen', \
+'--param=system.cpu_cluster[0].cpus[0].isa[0].midr=0x410fd030 \
+--param=system.cpu_cluster[0].cpus[0].isa[0].id_aa64isar1_el1=0x0', \
+'', d)}"
diff --git a/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-extended/xen/files/gem5-arm64/early-printk.cfg b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-extended/xen/files/gem5-arm64/early-printk.cfg
new file mode 100644
index 0000000..e89e546
--- /dev/null
+++ b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-extended/xen/files/gem5-arm64/early-printk.cfg
@@ -0,0 +1,2 @@
+CONFIG_DEBUG=y
+CONFIG_EARLY_PRINTK_VEXPRESS=y
diff --git a/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-extended/xen/xen_%.bbappend b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-extended/xen/xen_%.bbappend
new file mode 100644
index 0000000..6ee4dbd
--- /dev/null
+++ b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-extended/xen/xen_%.bbappend
@@ -0,0 +1,4 @@
+# gem5-arm64 support
+COMPATIBLE_MACHINE:gem5-arm64 = "gem5-arm64"
+FILESEXTRAPATHS:prepend:gem5-arm64 := "${THISDIR}/files:"
+SRC_URI:append:gem5-arm64 = " file://early-printk.cfg"
diff --git a/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-kernel/linux/linux-%.bbappend b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-kernel/linux/linux-%.bbappend
new file mode 100644
index 0000000..7ff3fe2
--- /dev/null
+++ b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-kernel/linux/linux-%.bbappend
@@ -0,0 +1,14 @@
+FILESEXTRAPATHS:prepend:gem5-arm64 := "${THISDIR}:"
+
+#
+# virtualization kmeta extra
+#
+SRC_URI:append:gem5-arm64 = " file://virtualization-kmeta-extra-gem5;type=kmeta;name=virtualization-kmeta-extra-gem5;destsuffix=virtualization-kmeta-extra-gem5"
+
+# We need to turn off SVE support in the Linux kernel otherwise Xen is stopping
+# Linux kernel with a coredump while trying to access XEN bit of CPACR1 core
+# register.
+LINUX_VIRTUALIZATION_DISABLE_ARM64_SVE:gem5-arm64 = "${@bb.utils.contains('DISTRO_FEATURES', \
+                                         'xen', ' features/disable-arm64-sve.scc','',d)}"
+
+KERNEL_FEATURES:append:gem5-arm64 = "${LINUX_VIRTUALIZATION_DISABLE_ARM64_SVE}"
diff --git a/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-kernel/linux/virtualization-kmeta-extra-gem5/features/disable-arm64-sve.cfg b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-kernel/linux/virtualization-kmeta-extra-gem5/features/disable-arm64-sve.cfg
new file mode 100644
index 0000000..7e87cab
--- /dev/null
+++ b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-kernel/linux/virtualization-kmeta-extra-gem5/features/disable-arm64-sve.cfg
@@ -0,0 +1,4 @@
+# We need to turn off SVE support in the Linux kernel otherwise Xen is stopping
+# Linux kernel with a coredump while trying to access ZEN bit of CPACR1 core
+# register.
+# CONFIG_ARM64_SVE is not set
diff --git a/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-kernel/linux/virtualization-kmeta-extra-gem5/features/disable-arm64-sve.scc b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-kernel/linux/virtualization-kmeta-extra-gem5/features/disable-arm64-sve.scc
new file mode 100644
index 0000000..6bc769c
--- /dev/null
+++ b/meta-arm/meta-gem5/dynamic-layers/meta-virtualization/recipes-kernel/linux/virtualization-kmeta-extra-gem5/features/disable-arm64-sve.scc
@@ -0,0 +1,3 @@
+define KFEATURE_DESCRIPTION "Disable SVE support"
+
+kconf non-hardware disable-arm64-sve.cfg
diff --git a/meta-arm/meta-gem5/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_%.bbappend b/meta-arm/meta-gem5/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_%.bbappend
new file mode 100644
index 0000000..3dd2cba
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-bsp/boot-wrapper-aarch64/boot-wrapper-aarch64_%.bbappend
@@ -0,0 +1,16 @@
+# Gem5 aarch64 support
+COMPATIBLE_MACHINE:gem5-arm64 = "gem5-arm64"
+
+PROVIDES:gem5-arm64 += "virtual/gem5-bootloader"
+
+# For gem5 we use the dtb generated by gem5 directly
+DEPENDS:append:gem5-arm64 = " gem5-aarch64-dtb"
+BOOT_WRAPPER_AARCH64_DEVICETREE:gem5-arm64 = "gem5-aarch64.dtb"
+
+# The dtb must be generated for us to generate the axf
+DEPLOY_DEPEND_LIST ?= ""
+DEPLOY_DEPEND_LIST:gem5-arm64 = " gem5-aarch64-dtb:do_deploy"
+do_deploy[depends] += "${DEPLOY_DEPEND_LIST}"
+
+# The base recipe has been upgraded, so hold back at known working revision
+SRCREV = "8d5a765251d9113c3c0f9fa14de42a9e7486fe8a"
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/files/mapping.patch b/meta-arm/meta-gem5/recipes-devtools/gem5/files/mapping.patch
new file mode 100644
index 0000000..3a7cb43
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/files/mapping.patch
@@ -0,0 +1,37 @@
+Upstream-Status: Backport
+Signed-off-by: Ross Burton <ross.burton@arm.com>
+
+From 89958f7f30ec722e30e1bcffdeab547c874fa475 Mon Sep 17 00:00:00 2001
+From: Adrian Herrera <adrian.herrera@arm.com>
+Date: Mon, 15 Mar 2021 13:14:44 +0000
+Subject: [PATCH] python: debug, fix Mapping import
+
+Change "collections.Mapping" to "collections.abc.Mapping".
+"collections.Mapping" was an alias, it is deprecated starting from Python 3.3, and it will be removed in Python 3.10.
+
+Change-Id: Ic257e3c5206eb3d48d4eed85a93fac48bd3b8dc4
+Signed-off-by: Adrian Herrera <adrian.herrera@arm.com>
+Reviewed-on: https://gem5-review.googlesource.com/c/public/gem5/+/43023
+Reviewed-by: Jason Lowe-Power <power.jg@gmail.com>
+Maintainer: Jason Lowe-Power <power.jg@gmail.com>
+Tested-by: kokoro <noreply+kokoro@google.com>
+---
+ src/python/m5/debug.py | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/src/python/m5/debug.py b/src/python/m5/debug.py
+index d808850cc..787a39ece 100644
+--- a/src/python/m5/debug.py
++++ b/src/python/m5/debug.py
+@@ -26,7 +26,7 @@
+
+ from __future__ import print_function
+
+-from collections import Mapping
++from collections.abc import Mapping
+
+ import _m5.debug
+ from _m5.debug import SimpleFlag, CompoundFlag
+-- 
+2.25.1
+
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/files/start-gem5.sh b/meta-arm/meta-gem5/recipes-devtools/gem5/files/start-gem5.sh
new file mode 100644
index 0000000..6335411
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/files/start-gem5.sh
@@ -0,0 +1,39 @@
+#!/bin/bash
+
+# Get parameters from bitbake configuration
+
+source <(bitbake -e gem5-aarch64-native | grep \
+    -e "^STAGING_.*_NATIVE=" \
+    -e "^DEPLOY_DIR.*=" \
+    -e "^GEM5_RUN.*=")
+
+export M5_PATH="${DEPLOY_DIR_IMAGE}"
+
+args=""
+
+if [ -n "${GEM5_RUN_KERNEL}" ]; then
+    kernfile=$(readlink -f ${DEPLOY_DIR_IMAGE}/${GEM5_RUN_KERNEL})
+    args="$args --kernel=$kernfile"
+fi
+
+if [ -n "${GEM5_RUN_DISK}" ]; then
+    diskfile=$(readlink -f ${DEPLOY_DIR_IMAGE}/${GEM5_RUN_DISK})
+    args="$args --disk-image=$diskfile"
+fi
+
+if [ -n "${GEM5_RUN_DTB}" ]; then
+    dtbfile=$(readlink -f ${DEPLOY_DIR_IMAGE}/${GEM5_RUN_DTB})
+    args="$args --dtb=$dtbfile"
+fi
+
+if [ -n "${GEM5_RUN_CMDLINE}" ]; then
+    args="$args --command-line='${GEM5_RUN_CMDLINE}'"
+fi
+
+if [ -n "${GEM5_RUN_EXTRA}" ]; then
+    args="$args ${GEM5_RUN_EXTRA}"
+fi
+
+oe-run-native gem5-aarch64-native ${GEM5_RUN_CONFIG} \
+    ${STAGING_DATADIR_NATIVE}/gem5/${GEM5_RUN_PROFILE} ${args} "$@"
+
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-bootloader.inc b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-bootloader.inc
new file mode 100644
index 0000000..c4fd3a1
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-bootloader.inc
@@ -0,0 +1,30 @@
+# Build instructions for gem5 custom bootloader
+
+SUMMARY = "Gem5 AARCH64 boot loader"
+LICENSE = "BSD-3-Clause"
+
+inherit deploy
+
+PROVIDES += "virtual/gem5-bootloader"
+
+COMPATIBLE_MACHINE = "gem5-arm64"
+
+BOOTLOADER_SRC_PATH ?= "${S}/system/arm/bootloader/arm64"
+
+# no configure step
+do_configure[noexec] = "1"
+
+# no install
+do_install[noexec] = "1"
+
+do_compile() {
+    oe_runmake -C ${BOOTLOADER_SRC_PATH} all CROSS_COMPILE=${TARGET_PREFIX}
+}
+
+do_deploy() {
+    oe_runmake -C ${BOOTLOADER_SRC_PATH} install \
+        CROSS_COMPILE=${TARGET_PREFIX} DESTDIR=${DEPLOYDIR}/binaries
+}
+
+addtask deploy before do_build after do_compile
+
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-bootloader_20.bb b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-bootloader_20.bb
new file mode 100644
index 0000000..67570f6
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-bootloader_20.bb
@@ -0,0 +1,7 @@
+# gem5 custom bootloader
+
+require gem5-source_20.inc
+
+BPN = "gem5-aarch64-bootloader"
+
+require gem5-aarch64-bootloader.inc
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-dtb.bb b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-dtb.bb
new file mode 100644
index 0000000..50ac030
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-dtb.bb
@@ -0,0 +1,30 @@
+# Use gem5 executable to produce a dtb
+
+LICENSE = "MIT"
+
+inherit deploy
+
+DEPENDS = "gem5-aarch64-native"
+
+do_configure[noexec] = "1"
+
+do_compile() {
+    # generate a dtb using gem5
+    gem5.opt \
+        ${STAGING_DATADIR_NATIVE}/gem5/${GEM5_RUN_PROFILE} \
+        --dtb-gen
+
+    if [ ! -f m5out/system.dtb ]; then
+        echo "No dtb generated !!!"
+        exit 1
+    fi
+}
+
+do_install[noexec] = "1"
+
+do_deploy() {
+    install --d ${DEPLOYDIR}
+    cp m5out/system.dtb ${DEPLOYDIR}/gem5-aarch64.dtb
+}
+addtask deploy before do_build after do_compile
+
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-native.inc b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-native.inc
new file mode 100644
index 0000000..2fd5206
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-native.inc
@@ -0,0 +1,40 @@
+# gem5 aarch64 specific configuration
+
+# Build arm64 gem5
+GEM5_BUILD_CONFIGS ?= "build/ARM/gem5.${GEM5_BUILD_VARIANT}"
+
+SRC_URI += "file://start-gem5.sh"
+
+inherit deploy
+
+# Parameters for the start script
+
+GEM5_RUN_CONFIG ?= "gem5.${GEM5_BUILD_VARIANT}"
+
+# Linux kernel file to boot
+GEM5_RUN_KERNEL ?= "vmlinux"
+
+# Disk Image to use
+GEM5_RUN_DISK ?= "*-${MACHINE}.ext4"
+
+# DTB to use
+GEM5_RUN_DTB ?= "${@os.path.basename(d.getVar('KERNEL_DEVICETREE'))}"
+
+# Linux command line to pass
+GEM5_RUN_CMDLINE ?= "earlyprintk=pl011,0x1c090000 console=ttyAMA0 rw mem=512MB root=/dev/sda rootwait"
+
+# Extra arguments to pass to gem5
+GEM5_RUN_EXTRA ?= "--mem-size=512MB -n 4 --machine-type=VExpress_GEM5_V2"
+
+#This is required so that our binaries are in the sysroot. We need this
+# to have both gem5 required libraries and gem5 in the same sysroot.
+addtask addto_recipe_sysroot after do_populate_sysroot before do_build
+
+do_deploy[sstate-outputdirs] = "${DEPLOY_DIR_TOOLS}"
+do_deploy() {
+    install -d ${DEPLOYDIR}
+
+    install -m 755 ${WORKDIR}/start-gem5.sh ${DEPLOYDIR}/.
+}
+addtask deploy before do_build after do_compile
+
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-native/0001-dev-arm-SMMUv3-enable-interrupt-interface.patch b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-native/0001-dev-arm-SMMUv3-enable-interrupt-interface.patch
new file mode 100644
index 0000000..6d00cc2
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-native/0001-dev-arm-SMMUv3-enable-interrupt-interface.patch
@@ -0,0 +1,97 @@
+From be710c5657b03bc9a9ce18ecf7ce1956265bae47 Mon Sep 17 00:00:00 2001
+From: Adrian Herrera <adrian.herrera@arm.com>
+Date: Thu, 10 Dec 2020 18:07:21 +0000
+Subject: [PATCH] dev-arm: SMMUv3, enable interrupt interface
+
+Users can set "irq_interface_enable" to allow software to program
+SMMU_IRQ_CTRL and SMMU_IRQ_CTRLACK. This is required to boot Linux v5.4+
+in a reasonable time. Notice the model does not implement architectural
+interrupt sources, so no assertions will happen.
+
+Change-Id: Ie138befdf5a204fe8fce961081c575c2166e22b9
+Signed-off-by: Adrian Herrera <adrian.herrera@arm.com>
+Reviewed-on: https://gem5-review.googlesource.com/c/public/gem5/+/38555
+Tested-by: kokoro <noreply+kokoro@google.com>
+Reviewed-by: Andreas Sandberg <andreas.sandberg@arm.com>
+Maintainer: Andreas Sandberg <andreas.sandberg@arm.com>
+
+Upstream-Status: Accepted [https://gem5-review.googlesource.com/c/public/gem5/+/38555]
+Expected version: v20.2
+---
+ src/dev/arm/SMMUv3.py  |  5 +++++
+ src/dev/arm/smmu_v3.cc | 10 +++++++++-
+ src/dev/arm/smmu_v3.hh |  4 +++-
+ 3 files changed, 17 insertions(+), 2 deletions(-)
+
+diff --git a/src/dev/arm/SMMUv3.py b/src/dev/arm/SMMUv3.py
+index 29c15682bf..f57be896f9 100644
+--- a/src/dev/arm/SMMUv3.py
++++ b/src/dev/arm/SMMUv3.py
+@@ -91,6 +91,11 @@ class SMMUv3(ClockedObject):
+     reg_map = Param.AddrRange('Address range for control registers')
+     system = Param.System(Parent.any, "System this device is part of")
+ 
++    irq_interface_enable = Param.Bool(False,
++            "This flag enables software to program SMMU_IRQ_CTRL and "
++            "SMMU_IRQ_CTRLACK as if the model implemented architectural "
++            "interrupt sources")
++
+     device_interfaces = VectorParam.SMMUv3DeviceInterface([],
+                                         "Responder interfaces")
+ 
+diff --git a/src/dev/arm/smmu_v3.cc b/src/dev/arm/smmu_v3.cc
+index f9bdc277c6..d73f270170 100644
+--- a/src/dev/arm/smmu_v3.cc
++++ b/src/dev/arm/smmu_v3.cc
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2013, 2018-2019 ARM Limited
++ * Copyright (c) 2013, 2018-2020 ARM Limited
+  * All rights reserved
+  *
+  * The license below extends only to copyright in the software and shall
+@@ -58,6 +58,7 @@ SMMUv3::SMMUv3(SMMUv3Params *params) :
+     requestPort(name() + ".request", *this),
+     tableWalkPort(name() + ".walker", *this),
+     controlPort(name() + ".control", *this, params->reg_map),
++    irqInterfaceEnable(params->irq_interface_enable),
+     tlb(params->tlb_entries, params->tlb_assoc, params->tlb_policy),
+     configCache(params->cfg_entries, params->cfg_assoc, params->cfg_policy),
+     ipaCache(params->ipa_entries, params->ipa_assoc, params->ipa_policy),
+@@ -626,6 +627,13 @@ SMMUv3::writeControl(PacketPtr pkt)
+             assert(pkt->getSize() == sizeof(uint32_t));
+             regs.cr0 = regs.cr0ack = pkt->getLE<uint32_t>();
+             break;
++        case offsetof(SMMURegs, irq_ctrl):
++            assert(pkt->getSize() == sizeof(uint32_t));
++            if (irqInterfaceEnable) {
++                warn("SMMUv3::%s No support for interrupt sources", __func__);
++                regs.irq_ctrl = regs.irq_ctrlack = pkt->getLE<uint32_t>();
++            }
++            break;
+ 
+         case offsetof(SMMURegs, cr1):
+         case offsetof(SMMURegs, cr2):
+diff --git a/src/dev/arm/smmu_v3.hh b/src/dev/arm/smmu_v3.hh
+index 6b3f3982b8..a001d71178 100644
+--- a/src/dev/arm/smmu_v3.hh
++++ b/src/dev/arm/smmu_v3.hh
+@@ -1,5 +1,5 @@
+ /*
+- * Copyright (c) 2013, 2018-2019 ARM Limited
++ * Copyright (c) 2013, 2018-2020 ARM Limited
+  * All rights reserved
+  *
+  * The license below extends only to copyright in the software and shall
+@@ -94,6 +94,8 @@ class SMMUv3 : public ClockedObject
+     SMMUTableWalkPort tableWalkPort;
+     SMMUControlPort   controlPort;
+ 
++    const bool irqInterfaceEnable;
++
+     ARMArchTLB  tlb;
+     ConfigCache configCache;
+     IPACache    ipaCache;
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-native_20.bb b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-native_20.bb
new file mode 100644
index 0000000..057aad8
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-aarch64-native_20.bb
@@ -0,0 +1,22 @@
+require gem5-source_20.inc
+
+SRC_URI += "file://0001-dev-arm-SMMUv3-enable-interrupt-interface.patch"
+
+BPN = "gem5-aarch64-native"
+
+require gem5-aarch64-native.inc
+require gem5-native.inc
+
+# Get rid of compiler errors when building protobuf
+GEM5_SCONS_ARGS:append = " CCFLAGS_EXTRA='-Wno-error=unused-variable' --verbose"
+
+# Get rid of linker errors and have a faster link process
+GEM5_SCONS_ARGS:append = " LDFLAGS_EXTRA='${BUILD_LDFLAGS}' \
+MARSHAL_LDFLAGS_EXTRA='${BUILD_LDFLAGS}' --force-lto "
+
+do_compile:prepend() {
+    # Gem5 expect to have python in the path (can be python2 or 3)
+    # Create a link named python to python3
+    real=$(which ${PYTHON})
+    ln -snf $real $(dirname $real)/python
+}
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5ops/0001-util-m5ops-optional-extra-build-flags.patch b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5ops/0001-util-m5ops-optional-extra-build-flags.patch
new file mode 100644
index 0000000..b446a2f
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5ops/0001-util-m5ops-optional-extra-build-flags.patch
@@ -0,0 +1,59 @@
+From 32e35a2f179d1b036d2eb699d77f869f7787f36b Mon Sep 17 00:00:00 2001
+From: Adrian Herrera <adrian.herrera@arm.com>
+Date: Tue, 8 Dec 2020 20:12:55 +0000
+Subject: [PATCH] util: m5ops, optional extra build flags
+
+This increases compilation control for users. Main use case is building
+m5ops as part of an image distribution. Specifying a different sysroot
+or dynamic linker may be required when the cross toolchain is built as
+part of the process.
+
+Change-Id: Icbd3faa92ea6e084fc4a9b2db83129bce73faf21
+Signed-off-by: Adrian Herrera <adrian.herrera@arm.com>
+Reviewed-on: https://gem5-review.googlesource.com/c/public/gem5/+/38416
+Reviewed-by: Jason Lowe-Power <power.jg@gmail.com>
+Reviewed-by: Gabe Black <gabe.black@gmail.com>
+Maintainer: Jason Lowe-Power <power.jg@gmail.com>
+Tested-by: kokoro <noreply+kokoro@google.com>
+
+Upstream-Status: Accepted [https://gem5-review.googlesource.com/c/public/gem5/+/38416]
+Expected version: v20.2
+---
+ util/m5/SConstruct | 14 +++++++++++---
+ 1 file changed, 11 insertions(+), 3 deletions(-)
+
+diff --git a/util/m5/SConstruct b/util/m5/SConstruct
+index bbae8d9bbf..a37573d763 100644
+--- a/util/m5/SConstruct
++++ b/util/m5/SConstruct
+@@ -136,16 +136,24 @@ for root, dirs, files in os.walk(abspath(src_dir)):
+         #
+         # This also considers scons command line settings which may look like
+         # environment variables, but are set after "scons" on the command line.
+-        def get_abi_opt(name, default):
++        def _extract_abi_opt_val(name, default):
+             var_name = env.subst('${ABI}.%s' % name)
+-            env[name] = os.environ.get(
+-                    var_name, ARGUMENTS.get(var_name, default))
++            return os.environ.get(var_name, ARGUMENTS.get(var_name, default))
++        def get_abi_opt(name, default):
++            env[name] = _extract_abi_opt_val(name, default)
++        def append_abi_opt(name):
++            env.Append(**{ name: _extract_abi_opt_val(name, '') })
+ 
+         # Process the ABI's settings in the SConsopts file, storing them
+         # in a copy of the primary environment.
+         env.SConscript(Dir(root).File('SConsopts'),
+                        exports=[ 'env', 'get_abi_opt' ])
+ 
++        # The user can pass extra build flags for each ABI
++        append_abi_opt('CCFLAGS')
++        append_abi_opt('CXXFLAGS')
++        append_abi_opt('LINKFLAGS')
++
+         # Once all the options have been configured, set up build targets for
+         # this abi.
+         abi_dir = build_dir.Dir(env.subst('${ABI}'))
+-- 
+2.17.1
+
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5ops_20.bb b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5ops_20.bb
new file mode 100644
index 0000000..8ff4826
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5ops_20.bb
@@ -0,0 +1,26 @@
+require gem5-source_20.inc
+inherit scons package
+
+HOMEPAGE = "https://www.gem5.org/documentation/general_docs/m5ops"
+SUMMARY = "m5ops provide pseudo-instructions to trigger gem5 functionality"
+LICENSE = "BSD-3-Clause"
+
+M5OPS_DIR = "util/m5"
+
+SRC_URI += "file://0001-util-m5ops-optional-extra-build-flags.patch"
+
+OUT_DIR = "build/${TARGET_ARCH}/out"
+
+EXTRA_OESCONS += "${TARGET_ARCH}.CROSS_COMPILE=${TARGET_PREFIX} \
+                  ${TARGET_ARCH}.CCFLAGS=--sysroot=${STAGING_DIR_TARGET} \
+                  ${TARGET_ARCH}.LINKFLAGS=--sysroot=${STAGING_DIR_TARGET} \
+                  -C ${S}/${M5OPS_DIR} ${OUT_DIR}/m5"
+
+# The SConstruct file for m5ops does not provide a "install" target
+# We do the install process within the recipe
+do_install() {
+    install -d ${D}${bindir} ${D}${libdir} ${D}${includedir}
+    install -m 755 ${B}/${M5OPS_DIR}/${OUT_DIR}/m5 ${D}${bindir}
+    install -m 644 ${B}/${M5OPS_DIR}/${OUT_DIR}/libm5.a ${D}${libdir}
+    install -m 644 ${B}/include/gem5/m5ops.h ${D}${includedir}
+}
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5term-native_20.bb b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5term-native_20.bb
new file mode 100644
index 0000000..b91dbbc
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5term-native_20.bb
@@ -0,0 +1,24 @@
+require gem5-source_20.inc
+
+SUMMARY = "m5term allows users to connect to gem5's simulated console"
+HOMEPAGE = "https://www.gem5.org/documentation/general_docs/fullsystem/m5term"
+LICENSE = "BSD-3-Clause"
+
+inherit native
+
+M5TERM_DIR = "util/term"
+
+SRC_URI += "file://0001-add-makefile-flags.patch"
+
+do_compile() {
+    oe_runmake -C ${S}/${M5TERM_DIR}
+}
+
+# The Makefile for m5term does not provide a "install" target
+# We do the install process within the recipe
+do_install() {
+    install -d ${D}${bindir}
+    install -m 755 ${B}/${M5TERM_DIR}/m5term ${D}${bindir}
+}
+
+addtask addto_recipe_sysroot before do_build
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5term/0001-add-makefile-flags.patch b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5term/0001-add-makefile-flags.patch
new file mode 100644
index 0000000..d13ef4c
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-m5term/0001-add-makefile-flags.patch
@@ -0,0 +1,34 @@
+From 688f7103e08318edbd48cf830c0ab3a048761bbb Mon Sep 17 00:00:00 2001
+From: Adrian Herrera <adrian.herrera@arm.com>
+Date: Thu, 03 Dec 2020 16:45:59 +0000
+Subject: [PATCH] util: m5term, fix LDFLAGS, standard make variables
+
+Enables build systems to provide necessary flags to build m5term.
+Useful specially if a different linker is intended to be used.
+
+Change-Id: If7f867cc0965d6ad4627b5421e00a99cc3d64989
+Signed-off-by: Adrian Herrera <adrian.herrera@arm.com>
+
+Upstream-Status: Accepted [https://gem5-review.googlesource.com/c/public/gem5/+/38256]
+Expected version: v20.2
+---
+
+diff --git a/util/term/Makefile b/util/term/Makefile
+index 658b961..4aa1c52 100644
+--- a/util/term/Makefile
++++ b/util/term/Makefile
+@@ -24,12 +24,12 @@
+ # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ 
+-CCFLAGS= -g -O0
++CFLAGS ?= -g -O0
+ 
+ default: m5term
+ 
+ m5term: term.c
+-	$(CC) $(LFLAGS) -o $@ $^
++	$(CC) $(CPPFLAGS) $(CFLAGS) $(LDFLAGS) -o $@ $^ $(LDLIBS)
+ 
+ install: m5term
+ 	$(SUDO) install -o root -m 555 m5term /usr/local/bin
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-native.inc b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-native.inc
new file mode 100644
index 0000000..91a554b
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-native.inc
@@ -0,0 +1,52 @@
+# gem5 platform independent build information
+
+SUMMARY = "A modular platform for computer-system architecture research"
+HOMEPAGE = "http://gem5.org"
+LICENSE = "BSD-3-Clause & MIT & LGPL-2.1-only"
+
+# Gem5 build and run parameter
+
+# See http://www.gem5.org/documentation/general_docs/building for the list of variants
+GEM5_BUILD_VARIANT ?= "opt"
+
+# What gem5 binary are we building
+GEM5_BUILD_CONFIGS ?= "build/X86/gem5.{GEM5_BUILD_VARIANT}"
+
+# Scons build arguments
+GEM5_SCONS_ARGS ?= "CC=${BUILD_CC} CXX=${BUILD_CXX} \
+    AS=${BUILD_AS} AR=${BUILD_AR} ${GEM5_BUILD_CONFIGS} \
+    PYTHON_CONFIG=python3-config"
+
+# Default profile to run
+GEM5_RUN_PROFILE ?= "configs/example/fs.py"
+
+# We are building a native package and we need to use scons
+inherit scons native
+
+# the build is using several tools:
+# python3: scons and six
+# google protobuf
+# pkgconfig
+# hdf5
+DEPENDS += "m4-native python3-six-native protobuf-native hdf5-native pkgconfig-native \
+    boost-native libpng-native"
+
+EXTRA_OESCONS = "${GEM5_SCONS_ARGS}"
+
+do_install() {
+
+    install -d ${D}${datadir}/gem5
+    cp -a --no-preserve=ownership -rf configs ${D}${datadir}/gem5/.
+
+    for f in ${GEM5_BUILD_CONFIGS}; do
+        destname=$(basename $f)
+        install -d ${D}${bindir}
+        install -m 755 $f ${D}${bindir}/$destname
+    done
+}
+
+FILES:${PN} = "${datadir}/gem5/* ${bindir}/*"
+INSANE_SKIP:${PN} += "already-stripped"
+RDEPENDS:${PN} += "python3-native hdf5-native protobuf-native libpng-native"
+
+addtask addto_recipe_sysroot before do_build
diff --git a/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-source_20.inc b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-source_20.inc
new file mode 100644
index 0000000..bf448fe
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-devtools/gem5/gem5-source_20.inc
@@ -0,0 +1,11 @@
+LIC_FILES_CHKSUM = "file://COPYING;md5=2d9514d69d8abf88b6e9125e759bf0ab \
+                    file://LICENSE;md5=a585e2893cee63d16a1d8bc16c6297ec"
+
+SRC_URI = "git://gem5.googlesource.com/public/gem5;protocol=https;nobranch=1 \
+           file://mapping.patch"
+RELEASE_TAG = "v20.1.0.5"
+SRCREV = "31cd81fdec46bae4b48d4f3788776936389dbdec"
+
+PV = "${RELEASE_TAG}"
+
+S = "${WORKDIR}/git"
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_1cpu.dts b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_1cpu.dts
new file mode 100644
index 0000000..0e59fdf
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_1cpu.dts
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2015-2016 ARM Limited
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met: redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer;
+ * redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution;
+ * neither the name of the copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Andreas Sandberg
+ */
+
+/dts-v1/;
+
+/memreserve/ 0x80000000 0x00010000;
+
+/include/ "vexpress_gem5_v2.dtsi"
+
+/ {
+	model = "V2P-AARCH64";
+	compatible = "arm,vexpress,v2p-aarch64", "arm,vexpress";
+
+	memory@80000000 {
+		device_type = "memory";
+		reg = <0 0x80000000 0x4 0x00000000>;
+	};
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+        cpu@0 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 0 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+	};
+
+	virt-encoder {
+		compatible = "drm,virtual-encoder";
+		port {
+			dp0_virt_input: endpoint@0 {
+				remote-endpoint = <&dp0_output>;
+			};
+		};
+
+		display-timings {
+			native-mode = <&timing0>;
+
+			timing0: timing_1080p60 {
+				/* 1920x1080-60 */
+				clock-frequency = <148500000>;
+				hactive = <1920>;
+				vactive = <1080>;
+				hfront-porch = <148>;
+				hback-porch = <88>;
+				hsync-len = <44>;
+				vfront-porch = <36>;
+				vback-porch = <4>;
+				vsync-len = <5>;
+			};
+		};
+	};
+};
+
+&dp0 {
+	status = "ok";
+
+	port {
+		dp0_output: endpoint@0 {
+			remote-endpoint = <&dp0_virt_input>;
+		};
+	};
+};
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_2cpu.dts b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_2cpu.dts
new file mode 100644
index 0000000..441d3df
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_2cpu.dts
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2015-2016 ARM Limited
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met: redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer;
+ * redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution;
+ * neither the name of the copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Andreas Sandberg
+ */
+
+/dts-v1/;
+
+/memreserve/ 0x80000000 0x00010000;
+
+/include/ "vexpress_gem5_v2.dtsi"
+
+/ {
+	model = "V2P-AARCH64";
+	compatible = "arm,vexpress,v2p-aarch64", "arm,vexpress";
+
+	memory@80000000 {
+		device_type = "memory";
+		reg = <0 0x80000000 0x4 0x00000000>;
+	};
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+        cpu@0 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 0 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@1 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 1 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+	};
+
+	virt-encoder {
+		compatible = "drm,virtual-encoder";
+		port {
+			dp0_virt_input: endpoint@0 {
+				remote-endpoint = <&dp0_output>;
+			};
+		};
+
+		display-timings {
+			native-mode = <&timing0>;
+
+			timing0: timing_1080p60 {
+				/* 1920x1080-60 */
+				clock-frequency = <148500000>;
+				hactive = <1920>;
+				vactive = <1080>;
+				hfront-porch = <148>;
+				hback-porch = <88>;
+				hsync-len = <44>;
+				vfront-porch = <36>;
+				vback-porch = <4>;
+				vsync-len = <5>;
+			};
+		};
+	};
+};
+
+&dp0 {
+	status = "ok";
+
+	port {
+		dp0_output: endpoint@0 {
+			remote-endpoint = <&dp0_virt_input>;
+		};
+	};
+};
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_4cpu.dts b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_4cpu.dts
new file mode 100644
index 0000000..2d0311a
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_4cpu.dts
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2015-2016 ARM Limited
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met: redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer;
+ * redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution;
+ * neither the name of the copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Andreas Sandberg
+ */
+
+/dts-v1/;
+
+/memreserve/ 0x80000000 0x00010000;
+
+/include/ "vexpress_gem5_v2.dtsi"
+
+/ {
+	model = "V2P-AARCH64";
+	compatible = "arm,vexpress,v2p-aarch64", "arm,vexpress";
+
+	memory@80000000 {
+		device_type = "memory";
+		reg = <0 0x80000000 0x4 0x00000000>;
+	};
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+        cpu@0 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 0 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@1 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 1 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@2 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 2 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@3 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 3 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+	};
+
+	virt-encoder {
+		compatible = "drm,virtual-encoder";
+		port {
+			dp0_virt_input: endpoint@0 {
+				remote-endpoint = <&dp0_output>;
+			};
+		};
+
+		display-timings {
+			native-mode = <&timing0>;
+
+			timing0: timing_1080p60 {
+				/* 1920x1080-60 */
+				clock-frequency = <148500000>;
+				hactive = <1920>;
+				vactive = <1080>;
+				hfront-porch = <148>;
+				hback-porch = <88>;
+				hsync-len = <44>;
+				vfront-porch = <36>;
+				vback-porch = <4>;
+				vsync-len = <5>;
+			};
+		};
+	};
+};
+
+&dp0 {
+	status = "ok";
+
+	port {
+		dp0_output: endpoint@0 {
+			remote-endpoint = <&dp0_virt_input>;
+		};
+	};
+};
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_8cpu.dts b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_8cpu.dts
new file mode 100644
index 0000000..ba94d07
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/armv8_gem5_v2_8cpu.dts
@@ -0,0 +1,151 @@
+/*
+ * Copyright (c) 2015-2016 ARM Limited
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met: redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer;
+ * redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution;
+ * neither the name of the copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Andreas Sandberg
+ */
+
+/dts-v1/;
+
+/memreserve/ 0x80000000 0x00010000;
+
+/include/ "vexpress_gem5_v2.dtsi"
+
+/ {
+	model = "V2P-AARCH64";
+	compatible = "arm,vexpress,v2p-aarch64", "arm,vexpress";
+
+	memory@80000000 {
+		device_type = "memory";
+		reg = <0 0x80000000 0x4 0x00000000>;
+	};
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+        cpu@0 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 0 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@1 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 1 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@2 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 2 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@3 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 3 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@4 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 4 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@5 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 5 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@6 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 6 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+        cpu@7 {
+            device_type = "cpu";
+            compatible = "gem5,armv8", "arm,armv8";
+            reg = < 7 >;
+            enable-method = "spin-table";
+            cpu-release-addr = <0 0x8000fff8>;
+        };
+
+	};
+
+	virt-encoder {
+		compatible = "drm,virtual-encoder";
+		port {
+			dp0_virt_input: endpoint@0 {
+				remote-endpoint = <&dp0_output>;
+			};
+		};
+
+		display-timings {
+			native-mode = <&timing0>;
+
+			timing0: timing_1080p60 {
+				/* 1920x1080-60 */
+				clock-frequency = <148500000>;
+				hactive = <1920>;
+				vactive = <1080>;
+				hfront-porch = <148>;
+				hback-porch = <88>;
+				hsync-len = <44>;
+				vfront-porch = <36>;
+				vback-porch = <4>;
+				vsync-len = <5>;
+			};
+		};
+	};
+};
+
+&dp0 {
+	status = "ok";
+
+	port {
+		dp0_output: endpoint@0 {
+			remote-endpoint = <&dp0_virt_input>;
+		};
+	};
+};
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/vexpress_gem5_v2.dtsi b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/vexpress_gem5_v2.dtsi
new file mode 100644
index 0000000..e53e6e8
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/vexpress_gem5_v2.dtsi
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2015-2018 ARM Limited
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met: redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer;
+ * redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution;
+ * neither the name of the copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Andreas Sandberg
+ */
+
+/include/ "vexpress_gem5_v2_base.dtsi"
+
+/ {
+	/* The display processor needs custom configuration to setup its
+	 * output ports. Disable it by default in the platform until the
+	 * DT bindings have stabilize.
+	 */
+	dp0: hdlcd@2b000000 {
+		compatible = "arm,hdlcd";
+		reg = <0x0 0x2b000000 0x0 0x1000>;
+		interrupts = <0 63 4>;
+		clocks = <&osc_pxl>;
+		clock-names = "pxlclk";
+		status = "disabled";
+	};
+};
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/vexpress_gem5_v2_base.dtsi b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/vexpress_gem5_v2_base.dtsi
new file mode 100644
index 0000000..eba0db2
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/files/dts/gem5-arm64/vexpress_gem5_v2_base.dtsi
@@ -0,0 +1,202 @@
+/*
+ * Copyright (c) 2015-2017, 2019 ARM Limited
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met: redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer;
+ * redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution;
+ * neither the name of the copyright holders nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Authors: Andreas Sandberg
+ */
+
+/ {
+	arm,hbi = <0x0>;
+	arm,vexpress,site = <0xf>;
+	interrupt-parent = <&gic>;
+	#address-cells = <2>;
+	#size-cells = <2>;
+
+	gic: interrupt-controller@2c000000 {
+		compatible = "arm,gic-v3";
+		#interrupt-cells = <0x3>;
+		#address-cells = <0x2>;
+		ranges;
+		interrupt-controller;
+		redistributor-stride = <0x0 0x40000>; // 256kB stride
+		reg = <0x0 0x2c000000 0x0 0x10000
+		       0x0 0x2c010000 0x0 0x2000000 // room for 128 redistributors using 128K each (256K strided...)
+		       0x0 0x0 0x0 0x0>;
+		interrupts = <1 9 0xf04>;
+		#size-cells = <0x2>;
+		linux,phandle = <0x1>;
+		phandle = <0x1>;
+
+		gic-its@2e010000 {
+			compatible = "arm,gic-v3-its";
+			msi-controller;
+			#msi-cells = <1>;
+			reg = <0x0 0x2e010000 0 0x20000>;
+		};
+	};
+
+	timer {
+		compatible = "arm,cortex-a15-timer",
+			     "arm,armv7-timer";
+		interrupts = <1 13 0xf08>,
+		             <1 14 0xf08>,
+			     <1 11 0xf08>,
+		             <1 10 0xf08>;
+		clocks = <&osc_sys>;
+		clock-names="apb_pclk";
+	};
+
+	pci {
+		compatible = "pci-host-ecam-generic";
+		device_type = "pci";
+		#address-cells = <0x3>;
+		#size-cells = <0x2>;
+		#interrupt-cells = <0x1>;
+
+		reg = <0x0 0x30000000 0x0 0x10000000>;
+
+		ranges = <0x01000000 0x0 0x00000000  0x0 0x2f000000  0x0 0x00010000>,
+		         <0x02000000 0x0 0x40000000  0x0 0x40000000  0x0 0x40000000>;
+
+	/*
+	  child unit address, #cells = #address-cells
+	  child interrupt specifier, #cells = #interrupt-cells (INTA = 1, INTB = 2, INTC = 3 and INTD = 4)
+	  interrupt-parent, phandle
+	  parent unit address, #cells = #address-cells@gic
+	  parent interrupt specifier, #cells = #interrupt-cells@gic
+	*/
+	interrupt-map = <0x0    0x0 0x0  0x1  &gic  0x0 0x0  0x0 0x44 0x1
+		         0x800  0x0 0x0  0x1  &gic  0x0 0x0  0x0 0x45 0x1
+		         0x1000 0x0 0x0  0x1  &gic  0x0 0x0  0x0 0x46 0x1
+		         0x1800 0x0 0x0  0x1  &gic  0x0 0x0  0x0 0x47 0x1>;
+
+	interrupt-map-mask = <0x001800 0x0 0x0 0x0>;
+	dma-coherent;
+	};
+
+	kmi@1c060000 {
+		compatible = "arm,pl050", "arm,primecell";
+		reg = <0x0 0x1c060000 0x0 0x1000>;
+		interrupts = <0 12 4>;
+		clocks = <&v2m_clk24mhz>, <&osc_smb>;
+		clock-names = "KMIREFCLK", "apb_pclk";
+	};
+
+	kmi@1c070000 {
+		compatible = "arm,pl050", "arm,primecell";
+		reg = <0x0 0x1c070000 0x0 0x1000>;
+		interrupts = <0 13 4>;
+		clocks = <&v2m_clk24mhz>, <&osc_smb>;
+		clock-names = "KMIREFCLK", "apb_pclk";
+	};
+
+	uart0: uart@1c090000 {
+		compatible = "arm,pl011", "arm,primecell";
+		reg = <0x0 0x1c090000 0x0 0x1000>;
+		interrupts = <0 5 4>;
+		clocks = <&osc_peripheral>, <&osc_smb>;
+		clock-names = "uartclk", "apb_pclk";
+	};
+
+	rtc@1c170000 {
+		compatible = "arm,pl031", "arm,primecell";
+		reg = <0x0 0x1c170000 0x0 0x1000>;
+		interrupts = <0 4 4>;
+		clocks = <&osc_smb>;
+		clock-names = "apb_pclk";
+	};
+
+	v2m_clk24mhz: clk24mhz {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <24000000>;
+		clock-output-names = "v2m:clk24mhz";
+	};
+
+
+	v2m_sysreg: sysreg@1c010000 {
+		compatible = "arm,vexpress-sysreg";
+		reg = <0 0x1c010000 0x0 0x1000>;
+		gpio-controller;
+		#gpio-cells = <2>;
+	};
+
+	vio@1c130000 {
+		compatible = "virtio,mmio";
+		reg = <0 0x1c130000 0x0 0x1000>;
+		interrupts = <0 42 4>;
+	};
+
+	vio@1c140000 {
+		compatible = "virtio,mmio";
+		reg = <0 0x1c140000 0x0 0x1000>;
+		interrupts = <0 43 4>;
+	};
+
+	dcc {
+		compatible = "arm,vexpress,config-bus";
+		arm,vexpress,config-bridge = <&v2m_sysreg>;
+
+		osc_pxl: osc@5 {
+			compatible = "arm,vexpress-osc";
+			arm,vexpress-sysreg,func = <1 5>;
+			freq-range = <23750000 1000000000>;
+			#clock-cells = <0>;
+			clock-output-names = "oscclk5";
+		};
+
+		osc_smb: osc@6 {
+			compatible = "arm,vexpress-osc";
+			arm,vexpress-sysreg,func = <1 6>;
+			freq-range = <20000000 50000000>;
+			#clock-cells = <0>;
+			clock-output-names = "oscclk6";
+		};
+
+		osc_sys: osc@7 {
+			compatible = "arm,vexpress-osc";
+			arm,vexpress-sysreg,func = <1 7>;
+			freq-range = <20000000 60000000>;
+			#clock-cells = <0>;
+			clock-output-names = "oscclk7";
+		};
+	};
+
+
+	mcc {
+		compatible = "arm,vexpress,config-bus";
+		arm,vexpress,config-bridge = <&v2m_sysreg>;
+		arm,vexpress,site = <0>;
+
+		osc_peripheral: osc@2 {
+			compatible = "arm,vexpress-osc";
+			arm,vexpress-sysreg,func = <1 2>;
+			freq-range = <24000000 24000000>;
+			#clock-cells = <0>;
+			clock-output-names = "v2m:oscclk2";
+		};
+	};
+};
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64-standard.scc b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64-standard.scc
new file mode 100644
index 0000000..0fb69e4
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64-standard.scc
@@ -0,0 +1,11 @@
+define KMACHINE gem5-arm64
+define KTYPE standard
+define KARCH arm64
+
+include ktypes/standard/standard.scc
+
+include gem5-arm64.scc
+
+# default policy for standard kernels
+#include features/latencytop/latencytop.scc
+#include features/profiling/profiling.scc
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64.scc b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64.scc
new file mode 100644
index 0000000..a24a3af
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64.scc
@@ -0,0 +1,14 @@
+include features/input/input.scc
+include features/net/net.scc
+include cfg/timer/no_hz.scc
+
+kconf hardware gem5-arm64/gem5-arm64-board.cfg
+kconf hardware gem5-arm64/gem5-arm64-drm.cfg
+kconf hardware gem5-arm64/gem5-arm64-net.cfg
+kconf hardware gem5-arm64/gem5-arm64-rtc.cfg
+kconf hardware gem5-arm64/gem5-arm64-serial.cfg
+kconf hardware gem5-arm64/gem5-arm64-virtio.cfg
+kconf hardware gem5-arm64/gem5-arm64-cfi.cfg
+kconf hardware gem5-arm64/gem5-arm64-virtio.cfg
+kconf hardware gem5-arm64/gem5-arm64-pci.cfg
+kconf hardware gem5-arm64/gem5-arm64-pata.cfg
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-board.cfg b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-board.cfg
new file mode 100644
index 0000000..56bb9e6
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-board.cfg
@@ -0,0 +1,23 @@
+CONFIG_ARM64=y
+CONFIG_ARCH_VEXPRESS=y
+CONFIG_SMP=y
+CONFIG_NR_CPUS=256
+CONFIG_HOTPLUG_CPU=y
+
+CONFIG_REGULATOR=y
+CONFIG_REGULATOR_FIXED_VOLTAGE=y
+
+CONFIG_CPU_IDLE=y
+CONFIG_ARM_CPUIDLE=y
+
+CONFIG_VEXPRESS_CONFIG=y
+
+# Keyboard over AMBA
+CONFIG_SERIO=y
+CONFIG_SERIO_AMBAKMI=y
+
+CONFIG_MFD_VEXPRESS_SYSREG=y
+
+# Turn off RAID to speed up boot
+CONFIG_MD=n
+CONFIG_BTRFS_FS=n
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-cfi.cfg b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-cfi.cfg
new file mode 100644
index 0000000..f28e0d9
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-cfi.cfg
@@ -0,0 +1,3 @@
+# CFI Flash
+CONFIG_MTD=y
+CONFIG_MTD_CFI=y
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-drm.cfg b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-drm.cfg
new file mode 100644
index 0000000..34b0141
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-drm.cfg
@@ -0,0 +1,4 @@
+CONFIG_DRM=y
+CONFIG_DRM_HDLCD=y
+CONFIG_FB=y
+CONFIG_FB_ARMCLCD=y
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-net.cfg b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-net.cfg
new file mode 100644
index 0000000..54e3686
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-net.cfg
@@ -0,0 +1,2 @@
+CONFIG_SMSC911X=y
+CONFIG_SMC91X=y
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-pata.cfg b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-pata.cfg
new file mode 100644
index 0000000..6272288
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-pata.cfg
@@ -0,0 +1,12 @@
+CONFIG_ATA=y
+CONFIG_ATA_GENERIC=y
+CONFIG_PATA_PLATFORM=y
+CONFIG_PATA_OF_PLATFORM=y
+CONFIG_SATA_AHCI=y
+CONFIG_SATA_AHCI_PLATFORM=y
+CONFIG_ATA_PIIX=y
+CONFIG_PATA_OLDPIIX=y
+CONFIG_SCSI=y
+CONFIG_SCSI_DMA=y
+CONFIG_BLK_DEV_SD=y
+CONFIG_CHR_DEV_SG=y
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-pci.cfg b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-pci.cfg
new file mode 100644
index 0000000..c8ae9d4
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-pci.cfg
@@ -0,0 +1,2 @@
+CONFIG_PCI=y
+CONFIG_PCI_HOST_GENERIC=y
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-rtc.cfg b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-rtc.cfg
new file mode 100644
index 0000000..5d377b3
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-rtc.cfg
@@ -0,0 +1,2 @@
+CONFIG_RTC_CLASS=y
+CONFIG_RTC_DRV_PL031=y
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-serial.cfg b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-serial.cfg
new file mode 100644
index 0000000..f58e3c2
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-serial.cfg
@@ -0,0 +1,3 @@
+CONFIG_SERIAL_AMBA_PL011=y
+CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
+CONFIG_SERIAL_EARLYCON_ARM_SEMIHOST=y
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-virtio.cfg b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-virtio.cfg
new file mode 100644
index 0000000..b4a53da
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/gem5-kmeta/bsp/gem5-platforms/gem5-arm64/gem5-arm64-virtio.cfg
@@ -0,0 +1,9 @@
+CONFIG_VIRTIO=y
+CONFIG_VIRTIO_MMIO=y
+CONFIG_BLOCK=y
+CONFIG_VIRTIO_BLK=y
+CONFIG_BLK_MQ_VIRTIO=y
+CONFIG_SCSI_VIRTIO=y
+CONFIG_VIRTIO_BLK_SCSI=y
+CONFIG_VIRTIO_PCI=y
+CONFIG_VIRTIO_PCI_LEGACY=y
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/linux-yocto_%.bbappend b/meta-arm/meta-gem5/recipes-kernel/linux/linux-yocto_%.bbappend
new file mode 100644
index 0000000..b36ea06
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/linux-yocto_%.bbappend
@@ -0,0 +1,11 @@
+FILESEXTRAPATHS:prepend:gem5-arm64 := "${THISDIR}:${THISDIR}/files:"
+
+COMPATIBLE_MACHINE:gem5-arm64 = "gem5-arm64"
+KMACHINE:gem5-arm64 = "gem5-arm64"
+SRC_URI:append:gem5-arm64 = " file://gem5-kmeta;type=kmeta;name=gem5-kmeta;destsuffix=gem5-kmeta \
+                              file://dts/gem5-arm64;subdir=add-files"
+
+do_patch:append:gem5-arm64() {
+    tar -C ${WORKDIR}/add-files/dts -cf - gem5-arm64 | \
+        tar -C arch/arm64/boot/dts -xf -
+}
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/linux-yocto_5.4.bb b/meta-arm/meta-gem5/recipes-kernel/linux/linux-yocto_5.4.bb
new file mode 100644
index 0000000..99a1669
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/linux-yocto_5.4.bb
@@ -0,0 +1,22 @@
+KBRANCH ?= "v5.4/standard/base"
+
+require recipes-kernel/linux/linux-yocto.inc
+
+SRCREV_machine ?= "e2020dbe2ccaef50d7e8f37a5bf08c68a006a064"
+SRCREV_meta ?= "e8c675c7e11fbd96cd812dfb9f4f6fb6f92b6abb"
+
+SRC_URI = "git://git.yoctoproject.org/linux-yocto.git;name=machine;branch=${KBRANCH}; \
+           git://git.yoctoproject.org/yocto-kernel-cache;type=kmeta;name=meta;branch=yocto-5.4;destsuffix=${KMETA}"
+
+LIC_FILES_CHKSUM = "file://COPYING;md5=bbea815ee2795b2f4230826c0c6b8814"
+LINUX_VERSION ?= "5.4.178"
+
+DEPENDS += "openssl-native util-linux-native"
+
+PV = "${LINUX_VERSION}+git${SRCPV}"
+
+KMETA = "kernel-meta"
+KCONF_BSP_AUDIT_LEVEL = "1"
+
+# Functionality flags
+KERNEL_FEATURES:append = " ${KERNEL_EXTRA_FEATURES}"
diff --git a/meta-arm/meta-gem5/recipes-kernel/linux/linux-yocto_5.4.bbappend b/meta-arm/meta-gem5/recipes-kernel/linux/linux-yocto_5.4.bbappend
new file mode 100644
index 0000000..078b684
--- /dev/null
+++ b/meta-arm/meta-gem5/recipes-kernel/linux/linux-yocto_5.4.bbappend
@@ -0,0 +1 @@
+require ${@bb.utils.contains('DISTRO_FEATURES', 'virtualization', 'recipes-kernel/linux/linux-yocto_virtualization.inc', '', d)}
diff --git a/meta-arm/scripts/layer-overview.py b/meta-arm/scripts/layer-overview.py
new file mode 100755
index 0000000..326470e
--- /dev/null
+++ b/meta-arm/scripts/layer-overview.py
@@ -0,0 +1,74 @@
+#! /usr/bin/env python3
+
+"""
+Print an overview of the layer to help writing release notes.
+
+Output includes sublayers, machines, recipes.
+"""
+
+import argparse
+import sys
+
+# TODO:
+# - More human-readable output
+# - Diff mode, give two revisions and list the changes
+
+def is_layer(path):
+    """
+    Determine if this path looks like a layer (is a directory and contains conf/layer.conf).
+    """
+    return path.is_dir() and (path / "conf" / "layer.conf").exists()
+
+
+def print_layer(layer):
+    """
+    Print a summary of the layer.
+    """
+    print(layer.name)
+
+    machines = sorted(p for p in layer.glob("conf/machine/*.conf"))
+    if machines:
+        print(" Machines")
+        for m in machines:
+            print(f"  {m.stem}")
+        print()
+
+    recipes = sorted((p for p in layer.glob("recipes-*/*/*.bb")), key=lambda p:p.name)
+    if recipes:
+        print(" Recipes")
+        for r in recipes:
+            if "_" in r.stem:
+                pn, pv = r.stem.rsplit("_", 1)
+                print(f"  {pn} {pv}")
+            else:
+                print(f"  {r.stem}")
+        print()
+
+
+parser = argparse.ArgumentParser()
+parser.add_argument("repository")
+parser.add_argument("revision", nargs="?")
+args = parser.parse_args()
+
+if args.revision:
+    import gitpathlib
+    base = gitpathlib.GitPath(args.repository, args.revision)
+else:
+    import pathlib
+    base = pathlib.Path(args.repository)
+
+if is_layer(base):
+    print_layer(base)
+else:
+    sublayers = sorted(p for p in base.glob("meta-*") if is_layer(p))
+    if sublayers:
+        print("Sub-Layers")
+        for l in sublayers:
+            print(f" {l.name}")
+        print()
+
+        for layer in sublayers:
+            print_layer(layer)
+    else:
+        print(f"No layers found in {base}", file=sys.stderr)
+        sys.exit(1)
diff --git a/meta-arm/scripts/machine-summary-overview.txt.jinja b/meta-arm/scripts/machine-summary-overview.txt.jinja
new file mode 100644
index 0000000..d585065
--- /dev/null
+++ b/meta-arm/scripts/machine-summary-overview.txt.jinja
@@ -0,0 +1,12 @@
+Machine Overview
+Generated at {{ timestamp }}.
+{% for machine, data in data|dictsort %}
+
+MACHINE: {{ machine }}
+{% for recipe in recipes|sort %}
+{% if recipe in data %}
+{% set details = data[recipe] %}
+{{ details.recipe }}: {{ details.version }}
+{% endif %}
+{% endfor %}
+{% endfor %}
diff --git a/meta-arm/scripts/machine-summary.py b/meta-arm/scripts/machine-summary.py
new file mode 100755
index 0000000..c2b43da
--- /dev/null
+++ b/meta-arm/scripts/machine-summary.py
@@ -0,0 +1,232 @@
+#! /usr/bin/env python3
+
+import argparse
+import datetime
+import os
+import pathlib
+import re
+import sys
+
+import jinja2
+
+def trim_pv(pv):
+    """
+    Strip anything after +git from the PV
+    """
+    return "".join(pv.partition("+git")[:2])
+
+def needs_update(version, upstream):
+    """
+    Do a dumb comparison to determine if the version needs to be updated.
+    """
+    if "+git" in version:
+        # strip +git and see if this is a post-release snapshot
+        version = version.replace("+git", "")
+    return version != upstream
+
+def safe_patches(patches):
+    for info in patches:
+        if info["status"] in ("Denied", "Pending", "Unknown"):
+            return False
+    return True
+
+def layer_path(layername: str, d) -> pathlib.Path:
+    """
+    Return the path to the specified layer, or None if the layer isn't present.
+    """
+    if not hasattr(layer_path, "cache"):
+        # Don't use functools.lru_cache as we don't want d changing to invalidate the cache
+        layer_path.cache = {}
+
+    if layername in layer_path.cache:
+        return layer_path.cache[layername]
+
+    bbpath = d.getVar("BBPATH").split(":")
+    pattern = d.getVar('BBFILE_PATTERN_' + layername)
+    for path in reversed(sorted(bbpath)):
+        if re.match(pattern, path + "/"):
+            layer_path.cache[layername] = pathlib.Path(path)
+            return layer_path.cache[layername]
+    return None
+
+def get_url_for_patch(layer: str, localpath: pathlib.Path, d) -> str:
+    relative = localpath.relative_to(layer_path(layer, d))
+
+    # TODO: use layerindexlib
+    # TODO: assumes default branch
+    if layer == "core":
+        return f"https://git.openembedded.org/openembedded-core/tree/meta/{relative}"
+    elif layer in ("meta-arm", "meta-arm-bsp", "arm-toolchain", "meta-atp", "meta-gem5"):
+        return f"https://git.yoctoproject.org/meta-arm/tree/{layer}/{relative}"
+    else:
+        print(f"WARNING: Don't know web URL for layer {layer}", file=sys.stderr)
+        return None
+
+def extract_patch_info(src_uri, d):
+    """
+    Parse the specified patch entry from a SRC_URI and return (base name, layer name, status) tuple
+    """
+    import bb.fetch, bb.utils
+
+    info = {}
+    localpath = pathlib.Path(bb.fetch.decodeurl(src_uri)[2])
+    info["name"] = localpath.name
+    info["layer"] = bb.utils.get_file_layer(str(localpath), d)
+    info["url"] = get_url_for_patch(info["layer"], localpath, d)
+
+    status = "Unknown"
+    with open(localpath, errors="ignore") as f:
+        m = re.search(r"^[\t ]*Upstream[-_ ]Status:?[\t ]*(\w*)", f.read(), re.IGNORECASE | re.MULTILINE)
+        if m:
+            # TODO: validate
+            status = m.group(1)
+    info["status"] = status
+    return info
+
+def harvest_data(machines, recipes):
+    import bb.tinfoil
+    with bb.tinfoil.Tinfoil() as tinfoil:
+        tinfoil.prepare(config_only=True)
+        corepath = layer_path("core", tinfoil.config_data)
+        sys.path.append(os.path.join(corepath, "lib"))
+    import oe.recipeutils
+    import oe.patch
+
+    # Queue of recipes that we're still looking for upstream releases for
+    to_check = list(recipes)
+
+    # Upstream releases
+    upstreams = {}
+    # Machines to recipes to versions
+    versions = {}
+
+    for machine in machines:
+        print(f"Gathering data for {machine}...")
+        os.environ["MACHINE"] = machine
+        with bb.tinfoil.Tinfoil() as tinfoil:
+            versions[machine] = {}
+
+            tinfoil.prepare(quiet=2)
+            for recipe in recipes:
+                try:
+                    d = tinfoil.parse_recipe(recipe)
+                except bb.providers.NoProvider:
+                    continue
+
+                if recipe in to_check:
+                    try:
+                        info = oe.recipeutils.get_recipe_upstream_version(d)
+                        upstreams[recipe] = info["version"]
+                        to_check.remove(recipe)
+                    except (bb.providers.NoProvider, KeyError):
+                        pass
+
+                details = versions[machine][recipe] = {}
+                details["recipe"] = d.getVar("PN")
+                details["version"] = trim_pv(d.getVar("PV"))
+                details["fullversion"] = d.getVar("PV")
+                details["patches"] = [extract_patch_info(p, d) for p in oe.patch.src_patches(d)]
+                details["patched"] = bool(details["patches"])
+                details["patches_safe"] = safe_patches(details["patches"])
+
+    # Now backfill the upstream versions
+    for machine in versions:
+        for recipe in versions[machine]:
+            data = versions[machine][recipe]
+            data["upstream"] = upstreams[recipe]
+            data["needs_update"] = needs_update(data["version"], data["upstream"])
+    return upstreams, versions
+
+# TODO can this be inferred from the list of recipes in the layer
+recipes = ("virtual/kernel",
+           "scp-firmware",
+           "trusted-firmware-a",
+           "trusted-firmware-m",
+           "edk2-firmware",
+           "u-boot",
+           "optee-os",
+           "gcc-aarch64-none-elf-native",
+           "gcc-arm-none-eabi-native")
+
+
+class Format:
+    """
+    The name of this format
+    """
+    name = None
+    """
+    Registry of names to classes
+    """
+    registry = {}
+
+    def __init_subclass__(cls, **kwargs):
+        super().__init_subclass__(**kwargs)
+        assert cls.name
+        cls.registry[cls.name] = cls
+
+    @classmethod
+    def get_format(cls, name):
+        return cls.registry[name]()
+
+    def render(self, context, output: pathlib.Path):
+        pass
+
+    def get_template(self, name):
+        template_dir = os.path.dirname(os.path.abspath(__file__))
+        env = jinja2.Environment(
+            loader=jinja2.FileSystemLoader(template_dir),
+            extensions=['jinja2.ext.i18n'],
+            autoescape=jinja2.select_autoescape(),
+            trim_blocks=True,
+            lstrip_blocks=True
+        )
+
+        # We only need i18n for plurals
+        env.install_null_translations()
+
+        return env.get_template(name)
+
+class TextOverview(Format):
+    name = "overview.txt"
+
+    def render(self, context, output: pathlib.Path):
+        with open(output, "wt") as f:
+            f.write(self.get_template(f"machine-summary-overview.txt.jinja").render(context))
+
+class HtmlUpdates(Format):
+    name = "report"
+
+    def render(self, context, output: pathlib.Path):
+        if output.exists() and not output.is_dir():
+            print(f"{output} is not a directory", file=sys.stderr)
+            sys.exit(1)
+
+        if not output.exists():
+            output.mkdir(parents=True)
+
+        with open(output / "index.html", "wt") as f:
+            f.write(self.get_template(f"report-index.html.jinja").render(context))
+
+        subcontext = context.copy()
+        del subcontext["data"]
+        for machine, subdata in context["data"].items():
+            subcontext["machine"] = machine
+            subcontext["data"] = subdata
+            with open(output / f"{machine}.html", "wt") as f:
+                f.write(self.get_template(f"report-details.html.jinja").render(subcontext))
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="machine-summary")
+    parser.add_argument("machines", nargs="+", help="machine names", metavar="MACHINE")
+    parser.add_argument("-t", "--type", required=True, choices=Format.registry.keys())
+    parser.add_argument("-o", "--output", type=pathlib.Path, required=True)
+    args = parser.parse_args()
+
+    context = {}
+    # TODO: include git describe for meta-arm
+    context["timestamp"] = str(datetime.datetime.now().strftime("%c"))
+    context["recipes"] = sorted(recipes)
+    context["releases"], context["data"] = harvest_data(args.machines, recipes)
+
+    formatter = Format.get_format(args.type)
+    formatter.render(context, args.output)
diff --git a/meta-arm/scripts/report-base.html.jinja b/meta-arm/scripts/report-base.html.jinja
new file mode 100644
index 0000000..be08125
--- /dev/null
+++ b/meta-arm/scripts/report-base.html.jinja
@@ -0,0 +1,35 @@
+<!DOCTYPE html>
+<html>
+  <head>
+    <title>{% block title %}{% endblock %}</title>
+    <meta name="viewport" content="width=device-width, initial-scale=1">
+    <link rel="stylesheet" href="https://cdn.jsdelivr.net/npm/bulma@0.9.1/css/bulma.min.css">
+  </head>
+  <body>
+    <section class="section">
+      {# TODO use position: sticky to glue this to the top #}
+      <nav class="breadcrumb is-large">
+        <ul>
+          <li class="{{ "is-active" if machine is undefined }}">
+            <a href="index.html">Recipe Report</a>
+          </li>
+          {% if machine is defined %}
+          <li class="is-active">
+            <a href="#">{{machine}}</a>
+          </li>
+          {% endif %}
+        </ul>
+      </nav>
+
+      <div class="content">
+        {% block content %}{% endblock %}
+      </div>
+    </section>
+
+    <footer class="footer">
+      <div class="content has-text-centered">
+        Generated by <code>machine-summary</code> at {{ timestamp }}.
+      </div>
+    </footer>
+  </body>
+</html>
diff --git a/meta-arm/scripts/report-details.html.jinja b/meta-arm/scripts/report-details.html.jinja
new file mode 100644
index 0000000..a656c26
--- /dev/null
+++ b/meta-arm/scripts/report-details.html.jinja
@@ -0,0 +1,64 @@
+{% extends "report-base.html.jinja" %}
+{% block title %}Recipe Report for {{ machine }}{% endblock %}
+
+{# Write a tag element using the Upstream-Status to determine the class. #}
+{% macro make_patch_tag(status) -%}
+  {% set status = status.split()[0] %}
+  {% if status in ("Unknown", "Pending") %}
+    {% set class = "is-danger" %}
+  {% elif status in ("Backport", "Accepted", "Inappropriate", "Denied") %}
+    {% set class = "is-success" %}
+  {% elif status in ("Submitted",) %}
+    {% set class = "is-info" %}
+  {% else %}
+    {% set class = "is-info" %}
+  {% endif %}
+  <span class="tag {{ class }}">{{ status }}</span>
+{%- endmacro %}
+
+{% block content %}
+  <!-- TODO table of contents -->
+
+  {% for name, data in data|dictsort if data.needs_update or data.patched %}
+  <h2 class="title is-4">
+    {{ data.recipe }} {{ data.fullversion }}
+    {% if name != data.recipe %}
+      (provides {{ name }})
+    {% endif %}
+    {% if data.needs_update %}<span class="tag is-danger">Upgrade Needed</span>{% endif %}
+    <a id="recipe-{{ data.recipe }}" class="has-text-grey-lighter">#</a>
+  </h2>
+
+  {% if data.needs_update %}
+  <p>
+    Recipe is version {{ data.fullversion }}, latest upstream release is <strong>{{ data.upstream }}</strong>.
+  </p>
+  {% endif%}
+
+  {% if data.patched %}
+  <table class="table is-striped is-bordered">
+    <thead>
+      <tr>
+        <th>Patch</th>
+        <th style="width: 20em">Layer</th>
+        <th style="width: 10em">Status</th>
+      </tr>
+    </thead>
+    <tbody>
+      {% for pinfo in data.patches %}
+      <tr>
+        <td>
+          {% if pinfo.url %}<a href="{{pinfo.url}}">{% endif %}
+          {{ pinfo.name }}
+          {% if pinfo.url %}</a>{% endif %}
+        </td>
+        <td>{{ pinfo.layer }}</td>
+        <!-- TODO: tooltip with full status? -->
+        <td class="has-text-centered">{{ make_patch_tag(pinfo.status)}}</td>
+      </tr>
+      {% endfor %}
+    </tbody>
+  </table>
+  {% endif %}
+  {% endfor %}
+{% endblock %}
diff --git a/meta-arm/scripts/report-index.html.jinja b/meta-arm/scripts/report-index.html.jinja
new file mode 100644
index 0000000..4453758
--- /dev/null
+++ b/meta-arm/scripts/report-index.html.jinja
@@ -0,0 +1,50 @@
+{% extends "report-base.html.jinja" %}
+{% block title %}Recipe Report{% endblock %}
+
+{% block content %}
+  <table class="table is-striped">
+    <thead>
+      <tr>
+        <th>Machine</th>
+        {% for recipe in recipes|sort %}
+        <th>{{ recipe }} ({{releases[recipe]|default("?")}})</th>
+        {% endfor %}
+      </tr>
+    </thead>
+    <tbody>
+      {% for machine, data in data|dictsort %}
+      <tr>
+        <th><a href="{{machine}}.html">{{ machine }}</a></th>
+        {% for recipe in recipes|sort %}
+          {% if recipe in data %}
+            {% set details = data[recipe] %}
+            <td style="text-align: center">
+            <a href="{{machine}}.html#recipe-{{details.recipe}}">
+              {{ details.recipe if details.recipe != recipe}}
+              {{ details.version }}
+            </a>
+            {% if details.patches or details.needs_update %}
+            <br>
+            {% if details.patches %}
+              <span class="tag {{ "is-info" if details.patches_safe else "is-danger" }}">
+                {% trans count=details.patches|length %}
+                  {{ count }} Patch
+                {% pluralize %}
+                  {{ count }} Patches
+                {% endtrans %}
+              </span>
+            {% endif %}
+            {% if details.needs_update %}
+              <span class="tag is-danger">Upgrade</span>
+            {% endif %}
+            {% endif %}
+            </td>
+          {% else %}
+            <td>-</td>
+          {% endif %}
+        {% endfor %}
+      </tr>
+      {% endfor %}
+    </tbody>
+  </table>
+{% endblock %}
diff --git a/meta-arm/scripts/runfvp b/meta-arm/scripts/runfvp
new file mode 100755
index 0000000..9fb77d3
--- /dev/null
+++ b/meta-arm/scripts/runfvp
@@ -0,0 +1,104 @@
+#! /usr/bin/env python3
+
+import asyncio
+import os
+import pathlib
+import signal
+import sys
+
+import logging
+logger = logging.getLogger("RunFVP")
+
+# Add meta-arm/lib/ to path
+libdir = pathlib.Path(__file__).parents[1] / "meta-arm" / "lib"
+sys.path.insert(0, str(libdir))
+
+from fvp import terminal, runner, conffile
+
+def parse_args(arguments):
+    import argparse
+    terminals = terminal.terminals
+
+    parser = argparse.ArgumentParser(description="Run images in a FVP")
+    parser.add_argument("config", nargs="?", help="Machine name or path to .fvpconf file")
+    group = parser.add_mutually_exclusive_group()
+    group.add_argument("-t", "--terminals", choices=terminals.all_terminals(), default=terminals.preferred_terminal(), help="Automatically start terminals (default: %(default)s)")
+    group.add_argument("-c", "--console", action="store_true", help="Attach the first uart to stdin/stdout")
+    parser.add_argument("--verbose", action="store_true", help="Output verbose logging")
+    parser.usage = f"{parser.format_usage().strip()} -- [ arguments passed to FVP ]"
+    # TODO option for telnet vs netcat
+
+    # If the arguments contains -- then everything after it should be passed to the FVP binary directly.
+    if "--" in arguments:
+        i = arguments.index("--")
+        fvp_args = arguments[i+1:]
+        arguments = arguments[:i]
+    else:
+        fvp_args = []
+
+    args = parser.parse_args(args=arguments)
+    logging.basicConfig(level=args.verbose and logging.DEBUG or logging.WARNING)
+
+    # If we're hooking up the console, don't start any terminals
+    if args.console:
+        args.terminals = "none"
+
+    logger.debug(f"Parsed arguments: {vars(args)}")
+    logger.debug(f"FVP arguments: {fvp_args}")
+    return args, fvp_args
+
+
+async def start_fvp(args, config, extra_args):
+    fvp = runner.FVPRunner(logger)
+    try:
+        await fvp.start(config, extra_args, args.terminals)
+
+        if args.console:
+            fvp.add_line_callback(lambda line: logger.debug(f"FVP output: {line}"))
+            expected_terminal = config["consoles"]["default"]
+            if not expected_terminal:
+                logger.error("--console used but FVP_CONSOLE not set in machine configuration")
+                return 1
+            telnet = await fvp.create_telnet(expected_terminal)
+            await telnet.wait()
+            logger.debug(f"Telnet quit, cancelling tasks")
+        else:
+            fvp.add_line_callback(lambda line: print(line))
+
+        await fvp.run()
+    finally:
+        await fvp.stop()
+
+def runfvp(cli_args):
+    args, extra_args = parse_args(cli_args)
+    if args.config and pathlib.Path(args.config).exists():
+        config_file = args.config
+    else:
+        config_file = conffile.find(args.config)
+    logger.debug(f"Loading {config_file}")
+    config = conffile.load(config_file)
+
+    try:
+        # When we can assume Py3.7+, this can simply be asyncio.run()
+        loop = asyncio.get_event_loop()
+        return loop.run_until_complete(start_fvp(args, config, extra_args))
+    except asyncio.CancelledError:
+        # This means telnet exited, which isn't an error
+        return 0
+
+if __name__ == "__main__":
+    try:
+        # Set the process group so that it's possible to kill runfvp and
+        # everything it spawns easily.
+        # Ignore permission errors happening when spawned from an other process
+        # for example run from except
+        try:
+            os.setpgid(0, 0)
+        except PermissionError:
+            pass
+        if sys.stdin.isatty():
+            signal.signal(signal.SIGTTOU, signal.SIG_IGN)
+            os.tcsetpgrp(sys.stdin.fileno(), os.getpgrp())
+        sys.exit(runfvp(sys.argv[1:]))
+    except KeyboardInterrupt:
+        pass
diff --git a/meta-phosphor/scripts/run-repotest b/meta-phosphor/scripts/run-repotest
index 0e6b06d..3669e36 100755
--- a/meta-phosphor/scripts/run-repotest
+++ b/meta-phosphor/scripts/run-repotest
@@ -18,6 +18,7 @@
 git ls-files -- \
   '*.patch' \
   ':!:poky/**' \
+  ':!:meta-arm/**' \
   ':!:meta-security/**' \
   ':!:meta-raspberrypi/**' \
   ':!:meta-openembedded/**' \
@@ -82,6 +83,7 @@
 git ls-files -- \
   '*.dts' \
   ':!:poky/**' \
+  ':!:meta-arm/**' \
   ':!:meta-security/**' \
   ':!:meta-raspberrypi/**' \
   ':!:meta-openembedded/**' \
@@ -203,6 +205,7 @@
 
 non_bbfiles=$(git ls-files -- \
   ':!:poky/**' \
+  ':!:meta-arm/**' \
   ':!:meta-security/**' \
   ':!:meta-raspberrypi/**' \
   ':!:meta-openembedded/**' \