Merge branch '2022-04-01-arm-semihosting-cleanups-and-new-features' into next

This brings in two related series.  The first from Andre:

This series is the continuation of last year's effort to support the new
Armv8-R64 application profile. This led to a significant rework of the
existing fastmodel (FVP) support, to both upgrade it to newest U-Boot
standards (OF_CONTROL and distro_boot support), but also to generalise
the code, so that plugging in the v8-R64 support in the last patch gets
much easier. This is because apart from the twisted memory map between
the two profiles there is actually little difference, when it comes to
U-Boot relevant parts of the hardware.

I kept the legacy semihosting support (which picks up magic files from
the current directory), but if that fails, we go and try virtio-blk
(.iso installer images work), then virtio-net.

Please have a look, and give it a try, if possible. Both the v8-R and
v8-A FVP models are available for free on the Arm website[1].

Patch 01/11 fixes a regression introduced in December, it should be
applied now. The rest of the patches are for the next merge window.

[1]
https://developer.arm.com/tools-and-software/simulation-models/fast-models

And the second from Sean (where we exclude 27, 28 and 29 for now):
This cleans up the semihosting code and adds the following new features:

- hostfs support (like sandbox)
- support for being used as a SPL boot device
- serial device support
- falling back to normal drivers if semihosting is disabled

The main device affected by these changes is vexpress64, so I'd
appreciate
if Andre (or anyone else) could try booting.

These changes are motivated by bringup for ls1046a. When forcing JTAG
boot, this device disables most communication peripherals, including
serial and ethernet devices. This appears to be fixed in later
generation devices, but we are stuck with it for now. Semihosting
provides an easy way to run a few console commands.

The patches in this series are organized as follows:

0-4: rST conversions and other documentation updates
5-9: Semihosting cleanups
10-14: Filesystem support (including SPL boot device)
15-16: Serial support
16: Documentation update
17: JTAG boot support for LS1046A
19-25: Semihosting fallback
26-29: DM puts support

The last two groups of patches are "bonus;" the first 17 patches stand
on their own. The last two groups could be broken out as separate
series, but I have kept them in this one to help with my sanity (and not
have to deal with too many outstanding series).

Patch 14 depends on [1] to apply cleanly. Patch 17 depends on [2] for
correctness. This series should be applied to u-boot/next (in
particular, the EROFS series must have been applied).

[1]
https://lore.kernel.org/u-boot/CACRpkdZ+9fmNjC_mvrbPa9-iuTQVd8UkJ7Zpe7cL0c5vZygsVw@mail.gmail.com/T/
[2]
https://lore.kernel.org/u-boot/20220222183840.1355337-2-sean.anderson@seco.com/

16 files changed, 1203 insertions, 175 deletions

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index b5ca14f041..f277929c99 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -403,11 +403,50 @@ config ARM_SMCCC
 	  firmware (for example, PSCI) according to SMCCC.
 
 config SEMIHOSTING
-	bool "support boot from semihosting"
+	bool "Support ARM semihosting"
 	help
-	  In emulated environments, semihosting is a way for
-	  the hosted environment to call out to the emulator to
-	  retrieve files from the host machine.
+	  Semihosting is a method for a target to communicate with a host
+	  debugger. It uses special instructions which the debugger will trap
+	  on and interpret. This allows U-Boot to read/write files, print to
+	  the console, and execute arbitrary commands on the host system.
+
+	  Enabling this option will add support for reading and writing files
+	  on the host system. If you don't have a debugger attached then trying
+	  to do this will likely cause U-Boot to hang. Say 'n' if you are unsure.
+
+config SEMIHOSTING_FALLBACK
+	bool "Recover gracefully when semihosting fails"
+	depends on SEMIHOSTING && ARM64
+	default y
+	help
+	  Normally, if U-Boot makes a semihosting call and no debugger is
+	  attached, then it will panic due to a synchronous abort
+	  exception. This config adds an exception handler which will allow
+	  U-Boot to recover. Say 'y' if unsure.
+
+config SPL_SEMIHOSTING
+	bool "Support ARM semihosting in SPL"
+	depends on SPL
+	help
+	  Semihosting is a method for a target to communicate with a host
+	  debugger. It uses special instructions which the debugger will trap
+	  on and interpret. This allows U-Boot to read/write files, print to
+	  the console, and execute arbitrary commands on the host system.
+
+	  Enabling this option will add support for reading and writing files
+	  on the host system. If you don't have a debugger attached then trying
+	  to do this will likely cause U-Boot to hang. Say 'n' if you are unsure.
+
+config SPL_SEMIHOSTING_FALLBACK
+	bool "Recover gracefully when semihosting fails in SPL"
+	depends on SPL_SEMIHOSTING && ARM64
+	select ARMV8_SPL_EXCEPTION_VECTORS
+	default y
+	help
+	  Normally, if U-Boot makes a semihosting call and no debugger is
+	  attached, then it will panic due to a synchronous abort
+	  exception. This config adds an exception handler which will allow
+	  U-Boot to recover. Say 'y' if unsure.
 
 config SYS_THUMB_BUILD
 	bool "Build U-Boot using the Thumb instruction set"
@@ -1250,34 +1289,19 @@ config ARCH_TEGRA
 	imply DISTRO_DEFAULTS
 	imply FAT_WRITE
 
-config TARGET_VEXPRESS64_AEMV8A
-	bool "Support vexpress_aemv8a"
-	select ARM64
-	select GPIO_EXTRA_HEADER
-	select PL01X_SERIAL
-
-config TARGET_VEXPRESS64_BASE_FVP
-	bool "Support Versatile Express ARMv8a FVP BASE model"
+config ARCH_VEXPRESS64
+	bool "Support ARMv8 Arm Ltd. VExpress based boards and models"
 	select ARM64
-	select GPIO_EXTRA_HEADER
-	select PL01X_SERIAL
-	select SEMIHOSTING
-
-config TARGET_VEXPRESS64_JUNO
-	bool "Support Versatile Express Juno Development Platform"
-	select ARM64
-	select GPIO_EXTRA_HEADER
-	select PL01X_SERIAL
 	select DM
+	select DM_SERIAL
+	select PL01X_SERIAL
 	select OF_CONTROL
 	select CLK
-	select DM_SERIAL
-	select ARM_PSCI_FW
-	select PSCI_RESET
-	select DM_ETH
 	select BLK
-	select USB
-	imply OF_HAS_PRIOR_STAGE
+	select MTD_NOR_FLASH if MTD
+	select FLASH_CFI_DRIVER if MTD
+	select ENV_IS_IN_FLASH if MTD
+	imply DISTRO_DEFAULTS
 
 config TARGET_TOTAL_COMPUTE
 	bool "Support Total Compute Platform"
diff --git a/arch/arm/cpu/armv8/exceptions.S b/arch/arm/cpu/armv8/exceptions.S
index a15af72e02..504d566721 100644
--- a/arch/arm/cpu/armv8/exceptions.S
+++ b/arch/arm/cpu/armv8/exceptions.S
@@ -77,14 +77,18 @@ _save_el_regs:
 	switch_el x11, 3f, 2f, 1f
 3:	mrs	x1, esr_el3
 	mrs	x2, elr_el3
+	mrs	x3, spsr_el3
 	b	0f
 2:	mrs	x1, esr_el2
 	mrs	x2, elr_el2
+	mrs	x3, spsr_el2
 	b	0f
 1:	mrs	x1, esr_el1
 	mrs	x2, elr_el1
+	mrs	x3, spsr_el1
 0:
-	stp	x2, x0, [sp, #-16]!
+	stp	x1, x0, [sp, #-16]!
+	stp	x3, x2, [sp, #-16]!
 	mov	x0, sp
 	ret
 
@@ -98,7 +102,7 @@ _save_el_regs:
  * This is the first part of the shared routine called into from all entries.
  */
 exception_exit:
-	ldp	x2, x0, [sp],#16
+	ldp	xzr, x2, [sp],#16
 	switch_el x11, 3f, 2f, 1f
 3:	msr	elr_el3, x2
 	b	_restore_regs
@@ -118,6 +122,7 @@ exception_exit:
  * This is the second part of the shared routine called into from all entries.
  */
 _restore_regs:
+	ldp	xzr, x0, [sp],#16
 	ldp	x1, x2, [sp],#16
 	ldp	x3, x4, [sp],#16
 	ldp	x5, x6, [sp],#16
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/spl.c b/arch/arm/cpu/armv8/fsl-layerscape/spl.c
index 564cc27c8b..5f09ef0a4a 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/spl.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/spl.c
@@ -12,6 +12,7 @@
 #include <image.h>
 #include <init.h>
 #include <log.h>
+#include <semihosting.h>
 #include <spl.h>
 #include <asm/cache.h>
 #include <asm/global_data.h>
@@ -27,6 +28,8 @@ DECLARE_GLOBAL_DATA_PTR;
 
 u32 spl_boot_device(void)
 {
+	if (semihosting_enabled())
+		return BOOT_DEVICE_SMH;
 #ifdef CONFIG_SPL_MMC
 	return BOOT_DEVICE_MMC1;
 #endif
diff --git a/arch/arm/dts/Makefile b/arch/arm/dts/Makefile
index beaaf15131..99dc7bc777 100644
--- a/arch/arm/dts/Makefile
+++ b/arch/arm/dts/Makefile
@@ -1185,6 +1185,8 @@ dtb-$(CONFIG_TARGET_MX53PPD) += imx53-ppd.dtb
 # TODO(Linus Walleij <linus.walleij@linaro.org>): Should us a single vexpress
 # Kconfig option to build all of these. See examples above.
 dtb-$(CONFIG_TARGET_VEXPRESS_CA9X4) += vexpress-v2p-ca9.dtb
+dtb-$(CONFIG_TARGET_VEXPRESS64_BASE_FVP) += fvp-base-revc.dtb
+dtb-$(CONFIG_TARGET_VEXPRESS64_BASER_FVP) += arm_fvp.dtb
 dtb-$(CONFIG_TARGET_VEXPRESS64_JUNO) += juno-r2.dtb
 
 dtb-$(CONFIG_TARGET_TOTAL_COMPUTE) += total_compute.dtb
diff --git a/arch/arm/dts/arm_fvp.dts b/arch/arm/dts/arm_fvp.dts
new file mode 100644
index 0000000000..3a4ad5d180
--- /dev/null
+++ b/arch/arm/dts/arm_fvp.dts
@@ -0,0 +1,11 @@
+// SPDX-License-Identifier: GPL-2.0+ OR MIT
+/*
+ * Empty device tree for the Arm Ltd FVP platform model
+
+ * Copyright 2022 Arm Ltd.
+ */
+
+/dts-v1/;
+
+/ {
+};
diff --git a/arch/arm/dts/fvp-base-revc.dts b/arch/arm/dts/fvp-base-revc.dts
new file mode 100644
index 0000000000..269b649934
--- /dev/null
+++ b/arch/arm/dts/fvp-base-revc.dts
@@ -0,0 +1,246 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * ARM Ltd. Fast Models
+ *
+ * Architecture Envelope Model (AEM) ARMv8-A
+ * ARMAEMv8AMPCT
+ *
+ * FVP Base RevC
+ */
+
+/dts-v1/;
+
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+
+/memreserve/ 0x80000000 0x00010000;
+
+#include "rtsm_ve-motherboard.dtsi"
+#include "rtsm_ve-motherboard-rs2.dtsi"
+
+/ {
+	model = "FVP Base RevC";
+	compatible = "arm,fvp-base-revc", "arm,vexpress";
+	interrupt-parent = <&gic>;
+	#address-cells = <2>;
+	#size-cells = <2>;
+
+	chosen { };
+
+	aliases {
+		serial0 = &v2m_serial0;
+		serial1 = &v2m_serial1;
+		serial2 = &v2m_serial2;
+		serial3 = &v2m_serial3;
+	};
+
+	psci {
+		compatible = "arm,psci-0.2";
+		method = "smc";
+	};
+
+	cpus {
+		#address-cells = <2>;
+		#size-cells = <0>;
+
+		cpu0: cpu@0 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x000>;
+			enable-method = "psci";
+		};
+		cpu1: cpu@100 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x100>;
+			enable-method = "psci";
+		};
+		cpu2: cpu@200 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x200>;
+			enable-method = "psci";
+		};
+		cpu3: cpu@300 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x300>;
+			enable-method = "psci";
+		};
+		cpu4: cpu@10000 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x10000>;
+			enable-method = "psci";
+		};
+		cpu5: cpu@10100 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x10100>;
+			enable-method = "psci";
+		};
+		cpu6: cpu@10200 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x10200>;
+			enable-method = "psci";
+		};
+		cpu7: cpu@10300 {
+			device_type = "cpu";
+			compatible = "arm,armv8";
+			reg = <0x0 0x10300>;
+			enable-method = "psci";
+		};
+	};
+
+	memory@80000000 {
+		device_type = "memory";
+		reg = <0x00000000 0x80000000 0 0x80000000>,
+		      <0x00000008 0x80000000 0 0x80000000>;
+	};
+
+	reserved-memory {
+		#address-cells = <2>;
+		#size-cells = <2>;
+		ranges;
+
+		/* Chipselect 2,00000000 is physically at 0x18000000 */
+		vram: vram@18000000 {
+			/* 8 MB of designated video RAM */
+			compatible = "shared-dma-pool";
+			reg = <0x00000000 0x18000000 0 0x00800000>;
+			no-map;
+		};
+	};
+
+	gic: interrupt-controller@2f000000 {
+		compatible = "arm,gic-v3";
+		#interrupt-cells = <3>;
+		#address-cells = <2>;
+		#size-cells = <2>;
+		ranges;
+		interrupt-controller;
+		reg = <0x0 0x2f000000 0 0x10000>,	// GICD
+		      <0x0 0x2f100000 0 0x200000>,	// GICR
+		      <0x0 0x2c000000 0 0x2000>,	// GICC
+		      <0x0 0x2c010000 0 0x2000>,	// GICH
+		      <0x0 0x2c02f000 0 0x2000>;	// GICV
+		interrupts = <GIC_PPI 9 IRQ_TYPE_LEVEL_HIGH>;
+
+		its: msi-controller@2f020000 {
+			#msi-cells = <1>;
+			compatible = "arm,gic-v3-its";
+			reg = <0x0 0x2f020000 0x0 0x20000>; // GITS
+			msi-controller;
+		};
+	};
+
+	timer {
+		compatible = "arm,armv8-timer";
+		interrupts = <GIC_PPI 13 IRQ_TYPE_LEVEL_LOW>,
+			     <GIC_PPI 14 IRQ_TYPE_LEVEL_LOW>,
+			     <GIC_PPI 11 IRQ_TYPE_LEVEL_LOW>,
+			     <GIC_PPI 10 IRQ_TYPE_LEVEL_LOW>;
+	};
+
+	pmu {
+		compatible = "arm,armv8-pmuv3";
+		interrupts = <GIC_PPI 7 IRQ_TYPE_LEVEL_HIGH>;
+	};
+
+	spe-pmu {
+		compatible = "arm,statistical-profiling-extension-v1";
+		interrupts = <GIC_PPI 5 IRQ_TYPE_LEVEL_HIGH>;
+	};
+
+	pci: pci@40000000 {
+		#address-cells = <0x3>;
+		#size-cells = <0x2>;
+		#interrupt-cells = <0x1>;
+		compatible = "pci-host-ecam-generic";
+		device_type = "pci";
+		bus-range = <0x0 0x1>;
+		reg = <0x0 0x40000000 0x0 0x10000000>;
+		ranges = <0x2000000 0x0 0x50000000 0x0 0x50000000 0x0 0x10000000>;
+		interrupt-map = <0 0 0 1 &gic 0 0 GIC_SPI 168 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 0 2 &gic 0 0 GIC_SPI 169 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 0 3 &gic 0 0 GIC_SPI 170 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 0 4 &gic 0 0 GIC_SPI 171 IRQ_TYPE_LEVEL_HIGH>;
+		interrupt-map-mask = <0x0 0x0 0x0 0x7>;
+		msi-map = <0x0 &its 0x0 0x10000>;
+		iommu-map = <0x0 &smmu 0x0 0x10000>;
+
+		dma-coherent;
+	};
+
+	smmu: iommu@2b400000 {
+		compatible = "arm,smmu-v3";
+		reg = <0x0 0x2b400000 0x0 0x100000>;
+		interrupts = <GIC_SPI 74 IRQ_TYPE_EDGE_RISING>,
+			     <GIC_SPI 79 IRQ_TYPE_EDGE_RISING>,
+			     <GIC_SPI 75 IRQ_TYPE_EDGE_RISING>,
+			     <GIC_SPI 77 IRQ_TYPE_EDGE_RISING>;
+		interrupt-names = "eventq", "gerror", "priq", "cmdq-sync";
+		dma-coherent;
+		#iommu-cells = <1>;
+		msi-parent = <&its 0x10000>;
+	};
+
+	panel {
+		compatible = "arm,rtsm-display", "panel-dpi";
+		port {
+			panel_in: endpoint {
+				remote-endpoint = <&clcd_pads>;
+			};
+		};
+	};
+
+	bus@8000000 {
+		#interrupt-cells = <1>;
+		interrupt-map-mask = <0 0 63>;
+		interrupt-map = <0 0  0 &gic 0 0 GIC_SPI  0 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0  1 &gic 0 0 GIC_SPI  1 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0  2 &gic 0 0 GIC_SPI  2 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0  3 &gic 0 0 GIC_SPI  3 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0  4 &gic 0 0 GIC_SPI  4 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0  5 &gic 0 0 GIC_SPI  5 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0  6 &gic 0 0 GIC_SPI  6 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0  7 &gic 0 0 GIC_SPI  7 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0  8 &gic 0 0 GIC_SPI  8 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0  9 &gic 0 0 GIC_SPI  9 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 10 &gic 0 0 GIC_SPI 10 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 11 &gic 0 0 GIC_SPI 11 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 12 &gic 0 0 GIC_SPI 12 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 13 &gic 0 0 GIC_SPI 13 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 14 &gic 0 0 GIC_SPI 14 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 15 &gic 0 0 GIC_SPI 15 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 16 &gic 0 0 GIC_SPI 16 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 17 &gic 0 0 GIC_SPI 17 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 18 &gic 0 0 GIC_SPI 18 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 19 &gic 0 0 GIC_SPI 19 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 20 &gic 0 0 GIC_SPI 20 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 21 &gic 0 0 GIC_SPI 21 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 22 &gic 0 0 GIC_SPI 22 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 23 &gic 0 0 GIC_SPI 23 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 24 &gic 0 0 GIC_SPI 24 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 25 &gic 0 0 GIC_SPI 25 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 26 &gic 0 0 GIC_SPI 26 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 27 &gic 0 0 GIC_SPI 27 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 28 &gic 0 0 GIC_SPI 28 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 29 &gic 0 0 GIC_SPI 29 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 30 &gic 0 0 GIC_SPI 30 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 31 &gic 0 0 GIC_SPI 31 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 32 &gic 0 0 GIC_SPI 32 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 33 &gic 0 0 GIC_SPI 33 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 34 &gic 0 0 GIC_SPI 34 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 35 &gic 0 0 GIC_SPI 35 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 36 &gic 0 0 GIC_SPI 36 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 37 &gic 0 0 GIC_SPI 37 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 38 &gic 0 0 GIC_SPI 38 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 39 &gic 0 0 GIC_SPI 39 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 40 &gic 0 0 GIC_SPI 40 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 41 &gic 0 0 GIC_SPI 41 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 42 &gic 0 0 GIC_SPI 42 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 43 &gic 0 0 GIC_SPI 43 IRQ_TYPE_LEVEL_HIGH>,
+				<0 0 44 &gic 0 0 GIC_SPI 44 IRQ_TYPE_LEVEL_HIGH>;
+	};
+};
diff --git a/arch/arm/dts/rtsm_ve-motherboard-rs2.dtsi b/arch/arm/dts/rtsm_ve-motherboard-rs2.dtsi
new file mode 100644
index 0000000000..33182d9e58
--- /dev/null
+++ b/arch/arm/dts/rtsm_ve-motherboard-rs2.dtsi
@@ -0,0 +1,27 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * ARM Ltd. Fast Models
+ *
+ * "rs2" extension for the v2m motherboard
+ */
+/ {
+	bus@8000000 {
+		motherboard-bus@8000000 {
+			arm,v2m-memory-map = "rs2";
+
+			iofpga-bus@300000000 {
+				virtio-p9@140000 {
+					compatible = "virtio,mmio";
+					reg = <0x140000 0x200>;
+					interrupts = <43>;
+				};
+
+				virtio-net@150000 {
+					compatible = "virtio,mmio";
+					reg = <0x150000 0x200>;
+					interrupts = <44>;
+				};
+			};
+		};
+	};
+};
diff --git a/arch/arm/dts/rtsm_ve-motherboard.dtsi b/arch/arm/dts/rtsm_ve-motherboard.dtsi
new file mode 100644
index 0000000000..5f6cab668a
--- /dev/null
+++ b/arch/arm/dts/rtsm_ve-motherboard.dtsi
@@ -0,0 +1,258 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * ARM Ltd. Fast Models
+ *
+ * Versatile Express (VE) system model
+ * Motherboard component
+ *
+ * VEMotherBoard.lisa
+ */
+/ {
+	v2m_clk24mhz: clk24mhz {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <24000000>;
+		clock-output-names = "v2m:clk24mhz";
+	};
+
+	v2m_refclk1mhz: refclk1mhz {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <1000000>;
+		clock-output-names = "v2m:refclk1mhz";
+	};
+
+	v2m_refclk32khz: refclk32khz {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <32768>;
+		clock-output-names = "v2m:refclk32khz";
+	};
+
+	v2m_fixed_3v3: v2m-3v3 {
+		compatible = "regulator-fixed";
+		regulator-name = "3V3";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+		regulator-always-on;
+	};
+
+	mcc {
+		compatible = "arm,vexpress,config-bus";
+		arm,vexpress,config-bridge = <&v2m_sysreg>;
+
+		v2m_oscclk1: oscclk1 {
+			/* CLCD clock */
+			compatible = "arm,vexpress-osc";
+			arm,vexpress-sysreg,func = <1 1>;
+			freq-range = <23750000 63500000>;
+			#clock-cells = <0>;
+			clock-output-names = "v2m:oscclk1";
+		};
+
+		reset {
+			compatible = "arm,vexpress-reset";
+			arm,vexpress-sysreg,func = <5 0>;
+		};
+
+		muxfpga {
+			compatible = "arm,vexpress-muxfpga";
+			arm,vexpress-sysreg,func = <7 0>;
+		};
+
+		shutdown {
+			compatible = "arm,vexpress-shutdown";
+			arm,vexpress-sysreg,func = <8 0>;
+		};
+
+		reboot {
+			compatible = "arm,vexpress-reboot";
+			arm,vexpress-sysreg,func = <9 0>;
+		};
+
+		dvimode {
+			compatible = "arm,vexpress-dvimode";
+			arm,vexpress-sysreg,func = <11 0>;
+		};
+	};
+
+	bus@8000000 {
+		compatible = "simple-bus";
+		#address-cells = <2>;
+		#size-cells = <1>;
+		ranges = <0 0x8000000 0 0x8000000 0x18000000>;
+
+		motherboard-bus@8000000 {
+			compatible = "arm,vexpress,v2m-p1", "simple-bus";
+			#address-cells = <2>; /* SMB chipselect number and offset */
+			#size-cells = <1>;
+			ranges = <0 0 0 0x08000000 0x04000000>,
+				 <1 0 0 0x14000000 0x04000000>,
+				 <2 0 0 0x18000000 0x04000000>,
+				 <3 0 0 0x1c000000 0x04000000>,
+				 <4 0 0 0x0c000000 0x04000000>,
+				 <5 0 0 0x10000000 0x04000000>;
+
+			flash@0 {
+				compatible = "arm,vexpress-flash", "cfi-flash";
+				reg = <0 0x00000000 0x04000000>,
+				      <4 0x00000000 0x04000000>;
+				bank-width = <4>;
+			};
+
+			ethernet@202000000 {
+				compatible = "smsc,lan91c111";
+				reg = <2 0x02000000 0x10000>;
+				interrupts = <15>;
+			};
+
+			iofpga-bus@300000000 {
+				compatible = "simple-bus";
+				#address-cells = <1>;
+				#size-cells = <1>;
+				ranges = <0 3 0 0x200000>;
+
+				v2m_sysreg: sysreg@10000 {
+					compatible = "arm,vexpress-sysreg";
+					reg = <0x010000 0x1000>;
+					gpio-controller;
+					#gpio-cells = <2>;
+				};
+
+				v2m_sysctl: sysctl@20000 {
+					compatible = "arm,sp810", "arm,primecell";
+					reg = <0x020000 0x1000>;
+					clocks = <&v2m_refclk32khz>, <&v2m_refclk1mhz>, <&v2m_clk24mhz>;
+					clock-names = "refclk", "timclk", "apb_pclk";
+					#clock-cells = <1>;
+					clock-output-names = "timerclken0", "timerclken1", "timerclken2", "timerclken3";
+					assigned-clocks = <&v2m_sysctl 0>, <&v2m_sysctl 1>, <&v2m_sysctl 3>, <&v2m_sysctl 3>;
+					assigned-clock-parents = <&v2m_refclk1mhz>, <&v2m_refclk1mhz>, <&v2m_refclk1mhz>, <&v2m_refclk1mhz>;
+				};
+
+				aaci@40000 {
+					compatible = "arm,pl041", "arm,primecell";
+					reg = <0x040000 0x1000>;
+					interrupts = <11>;
+					clocks = <&v2m_clk24mhz>;
+					clock-names = "apb_pclk";
+				};
+
+				mmc@50000 {
+					compatible = "arm,pl180", "arm,primecell";
+					reg = <0x050000 0x1000>;
+					interrupts = <9>, <10>;
+					cd-gpios = <&v2m_sysreg 0 0>;
+					wp-gpios = <&v2m_sysreg 1 0>;
+					max-frequency = <12000000>;
+					vmmc-supply = <&v2m_fixed_3v3>;
+					clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+					clock-names = "mclk", "apb_pclk";
+				};
+
+				kmi@60000 {
+					compatible = "arm,pl050", "arm,primecell";
+					reg = <0x060000 0x1000>;
+					interrupts = <12>;
+					clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+					clock-names = "KMIREFCLK", "apb_pclk";
+				};
+
+				kmi@70000 {
+					compatible = "arm,pl050", "arm,primecell";
+					reg = <0x070000 0x1000>;
+					interrupts = <13>;
+					clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+					clock-names = "KMIREFCLK", "apb_pclk";
+				};
+
+				v2m_serial0: serial@90000 {
+					compatible = "arm,pl011", "arm,primecell";
+					reg = <0x090000 0x1000>;
+					interrupts = <5>;
+					clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+					clock-names = "uartclk", "apb_pclk";
+				};
+
+				v2m_serial1: serial@a0000 {
+					compatible = "arm,pl011", "arm,primecell";
+					reg = <0x0a0000 0x1000>;
+					interrupts = <6>;
+					clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+					clock-names = "uartclk", "apb_pclk";
+				};
+
+				v2m_serial2: serial@b0000 {
+					compatible = "arm,pl011", "arm,primecell";
+					reg = <0x0b0000 0x1000>;
+					interrupts = <7>;
+					clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+					clock-names = "uartclk", "apb_pclk";
+				};
+
+				v2m_serial3: serial@c0000 {
+					compatible = "arm,pl011", "arm,primecell";
+					reg = <0x0c0000 0x1000>;
+					interrupts = <8>;
+					clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+					clock-names = "uartclk", "apb_pclk";
+				};
+
+				watchdog@f0000 {
+					compatible = "arm,sp805", "arm,primecell";
+					reg = <0x0f0000 0x1000>;
+					interrupts = <0>;
+					clocks = <&v2m_refclk32khz>, <&v2m_clk24mhz>;
+					clock-names = "wdog_clk", "apb_pclk";
+				};
+
+				v2m_timer01: timer@110000 {
+					compatible = "arm,sp804", "arm,primecell";
+					reg = <0x110000 0x1000>;
+					interrupts = <2>;
+					clocks = <&v2m_sysctl 0>, <&v2m_sysctl 1>, <&v2m_clk24mhz>;
+					clock-names = "timclken1", "timclken2", "apb_pclk";
+				};
+
+				v2m_timer23: timer@120000 {
+					compatible = "arm,sp804", "arm,primecell";
+					reg = <0x120000 0x1000>;
+					interrupts = <3>;
+					clocks = <&v2m_sysctl 2>, <&v2m_sysctl 3>, <&v2m_clk24mhz>;
+					clock-names = "timclken1", "timclken2", "apb_pclk";
+				};
+
+				virtio-block@130000 {
+					compatible = "virtio,mmio";
+					reg = <0x130000 0x200>;
+					interrupts = <42>;
+				};
+
+				rtc@170000 {
+					compatible = "arm,pl031", "arm,primecell";
+					reg = <0x170000 0x1000>;
+					interrupts = <4>;
+					clocks = <&v2m_clk24mhz>;
+					clock-names = "apb_pclk";
+				};
+
+				clcd@1f0000 {
+					compatible = "arm,pl111", "arm,primecell";
+					reg = <0x1f0000 0x1000>;
+					interrupt-names = "combined";
+					interrupts = <14>;
+					clocks = <&v2m_oscclk1>, <&v2m_clk24mhz>;
+					clock-names = "clcdclk", "apb_pclk";
+					memory-region = <&vram>;
+
+					port {
+						clcd_pads: endpoint {
+							remote-endpoint = <&panel_in>;
+							arm,pl11x,tft-r0g0b0-pads = <0 8 16>;
+						};
+					};
+				};
+			};
+		};
+	};
+};
diff --git a/arch/arm/include/asm/esr.h b/arch/arm/include/asm/esr.h
new file mode 100644
index 0000000000..f19e4e726a
--- /dev/null
+++ b/arch/arm/include/asm/esr.h
@@ -0,0 +1,343 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2013 - ARM Ltd
+ * Author: Marc Zyngier <marc.zyngier@arm.com>
+ */
+
+#ifndef __ASM_ESR_H
+#define __ASM_ESR_H
+
+#include <asm/memory.h>
+#include <linux/const.h>
+
+#define ESR_ELx_EC_UNKNOWN	(0x00)
+#define ESR_ELx_EC_WFx		(0x01)
+/* Unallocated EC: 0x02 */
+#define ESR_ELx_EC_CP15_32	(0x03)
+#define ESR_ELx_EC_CP15_64	(0x04)
+#define ESR_ELx_EC_CP14_MR	(0x05)
+#define ESR_ELx_EC_CP14_LS	(0x06)
+#define ESR_ELx_EC_FP_ASIMD	(0x07)
+#define ESR_ELx_EC_CP10_ID	(0x08)	/* EL2 only */
+#define ESR_ELx_EC_PAC		(0x09)	/* EL2 and above */
+/* Unallocated EC: 0x0A - 0x0B */
+#define ESR_ELx_EC_CP14_64	(0x0C)
+#define ESR_ELx_EC_BTI		(0x0D)
+#define ESR_ELx_EC_ILL		(0x0E)
+/* Unallocated EC: 0x0F - 0x10 */
+#define ESR_ELx_EC_SVC32	(0x11)
+#define ESR_ELx_EC_HVC32	(0x12)	/* EL2 only */
+#define ESR_ELx_EC_SMC32	(0x13)	/* EL2 and above */
+/* Unallocated EC: 0x14 */
+#define ESR_ELx_EC_SVC64	(0x15)
+#define ESR_ELx_EC_HVC64	(0x16)	/* EL2 and above */
+#define ESR_ELx_EC_SMC64	(0x17)	/* EL2 and above */
+#define ESR_ELx_EC_SYS64	(0x18)
+#define ESR_ELx_EC_SVE		(0x19)
+#define ESR_ELx_EC_ERET		(0x1a)	/* EL2 only */
+/* Unallocated EC: 0x1B */
+#define ESR_ELx_EC_FPAC		(0x1C)	/* EL1 and above */
+/* Unallocated EC: 0x1D - 0x1E */
+#define ESR_ELx_EC_IMP_DEF	(0x1f)	/* EL3 only */
+#define ESR_ELx_EC_IABT_LOW	(0x20)
+#define ESR_ELx_EC_IABT_CUR	(0x21)
+#define ESR_ELx_EC_PC_ALIGN	(0x22)
+/* Unallocated EC: 0x23 */
+#define ESR_ELx_EC_DABT_LOW	(0x24)
+#define ESR_ELx_EC_DABT_CUR	(0x25)
+#define ESR_ELx_EC_SP_ALIGN	(0x26)
+/* Unallocated EC: 0x27 */
+#define ESR_ELx_EC_FP_EXC32	(0x28)
+/* Unallocated EC: 0x29 - 0x2B */
+#define ESR_ELx_EC_FP_EXC64	(0x2C)
+/* Unallocated EC: 0x2D - 0x2E */
+#define ESR_ELx_EC_SERROR	(0x2F)
+#define ESR_ELx_EC_BREAKPT_LOW	(0x30)
+#define ESR_ELx_EC_BREAKPT_CUR	(0x31)
+#define ESR_ELx_EC_SOFTSTP_LOW	(0x32)
+#define ESR_ELx_EC_SOFTSTP_CUR	(0x33)
+#define ESR_ELx_EC_WATCHPT_LOW	(0x34)
+#define ESR_ELx_EC_WATCHPT_CUR	(0x35)
+/* Unallocated EC: 0x36 - 0x37 */
+#define ESR_ELx_EC_BKPT32	(0x38)
+/* Unallocated EC: 0x39 */
+#define ESR_ELx_EC_VECTOR32	(0x3A)	/* EL2 only */
+/* Unallocated EC: 0x3B */
+#define ESR_ELx_EC_BRK64	(0x3C)
+/* Unallocated EC: 0x3D - 0x3F */
+#define ESR_ELx_EC_MAX		(0x3F)
+
+#define ESR_ELx_EC_SHIFT	(26)
+#define ESR_ELx_EC_WIDTH	(6)
+#define ESR_ELx_EC_MASK		(UL(0x3F) << ESR_ELx_EC_SHIFT)
+#define ESR_ELx_EC(esr)		(((esr) & ESR_ELx_EC_MASK) >> ESR_ELx_EC_SHIFT)
+
+#define ESR_ELx_IL_SHIFT	(25)
+#define ESR_ELx_IL		(UL(1) << ESR_ELx_IL_SHIFT)
+#define ESR_ELx_ISS_MASK	(ESR_ELx_IL - 1)
+
+/* ISS field definitions shared by different classes */
+#define ESR_ELx_WNR_SHIFT	(6)
+#define ESR_ELx_WNR		(UL(1) << ESR_ELx_WNR_SHIFT)
+
+/* Asynchronous Error Type */
+#define ESR_ELx_IDS_SHIFT	(24)
+#define ESR_ELx_IDS		(UL(1) << ESR_ELx_IDS_SHIFT)
+#define ESR_ELx_AET_SHIFT	(10)
+#define ESR_ELx_AET		(UL(0x7) << ESR_ELx_AET_SHIFT)
+
+#define ESR_ELx_AET_UC		(UL(0) << ESR_ELx_AET_SHIFT)
+#define ESR_ELx_AET_UEU		(UL(1) << ESR_ELx_AET_SHIFT)
+#define ESR_ELx_AET_UEO		(UL(2) << ESR_ELx_AET_SHIFT)
+#define ESR_ELx_AET_UER		(UL(3) << ESR_ELx_AET_SHIFT)
+#define ESR_ELx_AET_CE		(UL(6) << ESR_ELx_AET_SHIFT)
+
+/* Shared ISS field definitions for Data/Instruction aborts */
+#define ESR_ELx_SET_SHIFT	(11)
+#define ESR_ELx_SET_MASK	(UL(3) << ESR_ELx_SET_SHIFT)
+#define ESR_ELx_FnV_SHIFT	(10)
+#define ESR_ELx_FnV		(UL(1) << ESR_ELx_FnV_SHIFT)
+#define ESR_ELx_EA_SHIFT	(9)
+#define ESR_ELx_EA		(UL(1) << ESR_ELx_EA_SHIFT)
+#define ESR_ELx_S1PTW_SHIFT	(7)
+#define ESR_ELx_S1PTW		(UL(1) << ESR_ELx_S1PTW_SHIFT)
+
+/* Shared ISS fault status code(IFSC/DFSC) for Data/Instruction aborts */
+#define ESR_ELx_FSC		(0x3F)
+#define ESR_ELx_FSC_TYPE	(0x3C)
+#define ESR_ELx_FSC_LEVEL	(0x03)
+#define ESR_ELx_FSC_EXTABT	(0x10)
+#define ESR_ELx_FSC_MTE		(0x11)
+#define ESR_ELx_FSC_SERROR	(0x11)
+#define ESR_ELx_FSC_ACCESS	(0x08)
+#define ESR_ELx_FSC_FAULT	(0x04)
+#define ESR_ELx_FSC_PERM	(0x0C)
+
+/* ISS field definitions for Data Aborts */
+#define ESR_ELx_ISV_SHIFT	(24)
+#define ESR_ELx_ISV		(UL(1) << ESR_ELx_ISV_SHIFT)
+#define ESR_ELx_SAS_SHIFT	(22)
+#define ESR_ELx_SAS		(UL(3) << ESR_ELx_SAS_SHIFT)
+#define ESR_ELx_SSE_SHIFT	(21)
+#define ESR_ELx_SSE		(UL(1) << ESR_ELx_SSE_SHIFT)
+#define ESR_ELx_SRT_SHIFT	(16)
+#define ESR_ELx_SRT_MASK	(UL(0x1F) << ESR_ELx_SRT_SHIFT)
+#define ESR_ELx_SF_SHIFT	(15)
+#define ESR_ELx_SF		(UL(1) << ESR_ELx_SF_SHIFT)
+#define ESR_ELx_AR_SHIFT	(14)
+#define ESR_ELx_AR		(UL(1) << ESR_ELx_AR_SHIFT)
+#define ESR_ELx_CM_SHIFT	(8)
+#define ESR_ELx_CM		(UL(1) << ESR_ELx_CM_SHIFT)
+
+/* ISS field definitions for exceptions taken in to Hyp */
+#define ESR_ELx_CV		(UL(1) << 24)
+#define ESR_ELx_COND_SHIFT	(20)
+#define ESR_ELx_COND_MASK	(UL(0xF) << ESR_ELx_COND_SHIFT)
+#define ESR_ELx_WFx_ISS_TI	(UL(1) << 0)
+#define ESR_ELx_WFx_ISS_WFI	(UL(0) << 0)
+#define ESR_ELx_WFx_ISS_WFE	(UL(1) << 0)
+#define ESR_ELx_xVC_IMM_MASK	((1UL << 16) - 1)
+
+#define DISR_EL1_IDS		(UL(1) << 24)
+/*
+ * DISR_EL1 and ESR_ELx share the bottom 13 bits, but the RES0 bits may mean
+ * different things in the future...
+ */
+#define DISR_EL1_ESR_MASK	(ESR_ELx_AET | ESR_ELx_EA | ESR_ELx_FSC)
+
+/* ESR value templates for specific events */
+#define ESR_ELx_WFx_MASK	(ESR_ELx_EC_MASK | ESR_ELx_WFx_ISS_TI)
+#define ESR_ELx_WFx_WFI_VAL	((ESR_ELx_EC_WFx << ESR_ELx_EC_SHIFT) |	\
+				 ESR_ELx_WFx_ISS_WFI)
+
+/* BRK instruction trap from AArch64 state */
+#define ESR_ELx_BRK64_ISS_COMMENT_MASK	0xffff
+
+/* ISS field definitions for System instruction traps */
+#define ESR_ELx_SYS64_ISS_RES0_SHIFT	22
+#define ESR_ELx_SYS64_ISS_RES0_MASK	(UL(0x7) << ESR_ELx_SYS64_ISS_RES0_SHIFT)
+#define ESR_ELx_SYS64_ISS_DIR_MASK	0x1
+#define ESR_ELx_SYS64_ISS_DIR_READ	0x1
+#define ESR_ELx_SYS64_ISS_DIR_WRITE	0x0
+
+#define ESR_ELx_SYS64_ISS_RT_SHIFT	5
+#define ESR_ELx_SYS64_ISS_RT_MASK	(UL(0x1f) << ESR_ELx_SYS64_ISS_RT_SHIFT)
+#define ESR_ELx_SYS64_ISS_CRM_SHIFT	1
+#define ESR_ELx_SYS64_ISS_CRM_MASK	(UL(0xf) << ESR_ELx_SYS64_ISS_CRM_SHIFT)
+#define ESR_ELx_SYS64_ISS_CRN_SHIFT	10
+#define ESR_ELx_SYS64_ISS_CRN_MASK	(UL(0xf) << ESR_ELx_SYS64_ISS_CRN_SHIFT)
+#define ESR_ELx_SYS64_ISS_OP1_SHIFT	14
+#define ESR_ELx_SYS64_ISS_OP1_MASK	(UL(0x7) << ESR_ELx_SYS64_ISS_OP1_SHIFT)
+#define ESR_ELx_SYS64_ISS_OP2_SHIFT	17
+#define ESR_ELx_SYS64_ISS_OP2_MASK	(UL(0x7) << ESR_ELx_SYS64_ISS_OP2_SHIFT)
+#define ESR_ELx_SYS64_ISS_OP0_SHIFT	20
+#define ESR_ELx_SYS64_ISS_OP0_MASK	(UL(0x3) << ESR_ELx_SYS64_ISS_OP0_SHIFT)
+#define ESR_ELx_SYS64_ISS_SYS_MASK	(ESR_ELx_SYS64_ISS_OP0_MASK | \
+					 ESR_ELx_SYS64_ISS_OP1_MASK | \
+					 ESR_ELx_SYS64_ISS_OP2_MASK | \
+					 ESR_ELx_SYS64_ISS_CRN_MASK | \
+					 ESR_ELx_SYS64_ISS_CRM_MASK)
+#define ESR_ELx_SYS64_ISS_SYS_VAL(op0, op1, op2, crn, crm) \
+					(((op0) << ESR_ELx_SYS64_ISS_OP0_SHIFT) | \
+					 ((op1) << ESR_ELx_SYS64_ISS_OP1_SHIFT) | \
+					 ((op2) << ESR_ELx_SYS64_ISS_OP2_SHIFT) | \
+					 ((crn) << ESR_ELx_SYS64_ISS_CRN_SHIFT) | \
+					 ((crm) << ESR_ELx_SYS64_ISS_CRM_SHIFT))
+
+#define ESR_ELx_SYS64_ISS_SYS_OP_MASK	(ESR_ELx_SYS64_ISS_SYS_MASK | \
+					 ESR_ELx_SYS64_ISS_DIR_MASK)
+#define ESR_ELx_SYS64_ISS_RT(esr) \
+	(((esr) & ESR_ELx_SYS64_ISS_RT_MASK) >> ESR_ELx_SYS64_ISS_RT_SHIFT)
+/*
+ * User space cache operations have the following sysreg encoding
+ * in System instructions.
+ * op0=1, op1=3, op2=1, crn=7, crm={ 5, 10, 11, 12, 13, 14 }, WRITE (L=0)
+ */
+#define ESR_ELx_SYS64_ISS_CRM_DC_CIVAC	14
+#define ESR_ELx_SYS64_ISS_CRM_DC_CVADP	13
+#define ESR_ELx_SYS64_ISS_CRM_DC_CVAP	12
+#define ESR_ELx_SYS64_ISS_CRM_DC_CVAU	11
+#define ESR_ELx_SYS64_ISS_CRM_DC_CVAC	10
+#define ESR_ELx_SYS64_ISS_CRM_IC_IVAU	5
+
+#define ESR_ELx_SYS64_ISS_EL0_CACHE_OP_MASK	(ESR_ELx_SYS64_ISS_OP0_MASK | \
+						 ESR_ELx_SYS64_ISS_OP1_MASK | \
+						 ESR_ELx_SYS64_ISS_OP2_MASK | \
+						 ESR_ELx_SYS64_ISS_CRN_MASK | \
+						 ESR_ELx_SYS64_ISS_DIR_MASK)
+#define ESR_ELx_SYS64_ISS_EL0_CACHE_OP_VAL \
+				(ESR_ELx_SYS64_ISS_SYS_VAL(1, 3, 1, 7, 0) | \
+				 ESR_ELx_SYS64_ISS_DIR_WRITE)
+/*
+ * User space MRS operations which are supported for emulation
+ * have the following sysreg encoding in System instructions.
+ * op0 = 3, op1= 0, crn = 0, {crm = 0, 4-7}, READ (L = 1)
+ */
+#define ESR_ELx_SYS64_ISS_SYS_MRS_OP_MASK	(ESR_ELx_SYS64_ISS_OP0_MASK | \
+						 ESR_ELx_SYS64_ISS_OP1_MASK | \
+						 ESR_ELx_SYS64_ISS_CRN_MASK | \
+						 ESR_ELx_SYS64_ISS_DIR_MASK)
+#define ESR_ELx_SYS64_ISS_SYS_MRS_OP_VAL \
+				(ESR_ELx_SYS64_ISS_SYS_VAL(3, 0, 0, 0, 0) | \
+				 ESR_ELx_SYS64_ISS_DIR_READ)
+
+#define ESR_ELx_SYS64_ISS_SYS_CTR	ESR_ELx_SYS64_ISS_SYS_VAL(3, 3, 1, 0, 0)
+#define ESR_ELx_SYS64_ISS_SYS_CTR_READ	(ESR_ELx_SYS64_ISS_SYS_CTR | \
+					 ESR_ELx_SYS64_ISS_DIR_READ)
+
+#define ESR_ELx_SYS64_ISS_SYS_CNTVCT	(ESR_ELx_SYS64_ISS_SYS_VAL(3, 3, 2, 14, 0) | \
+					 ESR_ELx_SYS64_ISS_DIR_READ)
+
+#define ESR_ELx_SYS64_ISS_SYS_CNTVCTSS	(ESR_ELx_SYS64_ISS_SYS_VAL(3, 3, 6, 14, 0) | \
+					 ESR_ELx_SYS64_ISS_DIR_READ)
+
+#define ESR_ELx_SYS64_ISS_SYS_CNTFRQ	(ESR_ELx_SYS64_ISS_SYS_VAL(3, 3, 0, 14, 0) | \
+					 ESR_ELx_SYS64_ISS_DIR_READ)
+
+#define esr_sys64_to_sysreg(e)					\
+	sys_reg((((e) & ESR_ELx_SYS64_ISS_OP0_MASK) >>		\
+		 ESR_ELx_SYS64_ISS_OP0_SHIFT),			\
+		(((e) & ESR_ELx_SYS64_ISS_OP1_MASK) >>		\
+		 ESR_ELx_SYS64_ISS_OP1_SHIFT),			\
+		(((e) & ESR_ELx_SYS64_ISS_CRN_MASK) >>		\
+		 ESR_ELx_SYS64_ISS_CRN_SHIFT),			\
+		(((e) & ESR_ELx_SYS64_ISS_CRM_MASK) >>		\
+		 ESR_ELx_SYS64_ISS_CRM_SHIFT),			\
+		(((e) & ESR_ELx_SYS64_ISS_OP2_MASK) >>		\
+		 ESR_ELx_SYS64_ISS_OP2_SHIFT))
+
+#define esr_cp15_to_sysreg(e)					\
+	sys_reg(3,						\
+		(((e) & ESR_ELx_SYS64_ISS_OP1_MASK) >>		\
+		 ESR_ELx_SYS64_ISS_OP1_SHIFT),			\
+		(((e) & ESR_ELx_SYS64_ISS_CRN_MASK) >>		\
+		 ESR_ELx_SYS64_ISS_CRN_SHIFT),			\
+		(((e) & ESR_ELx_SYS64_ISS_CRM_MASK) >>		\
+		 ESR_ELx_SYS64_ISS_CRM_SHIFT),			\
+		(((e) & ESR_ELx_SYS64_ISS_OP2_MASK) >>		\
+		 ESR_ELx_SYS64_ISS_OP2_SHIFT))
+
+/*
+ * ISS field definitions for floating-point exception traps
+ * (FP_EXC_32/FP_EXC_64).
+ *
+ * (The FPEXC_* constants are used instead for common bits.)
+ */
+
+#define ESR_ELx_FP_EXC_TFV	(UL(1) << 23)
+
+/*
+ * ISS field definitions for CP15 accesses
+ */
+#define ESR_ELx_CP15_32_ISS_DIR_MASK	0x1
+#define ESR_ELx_CP15_32_ISS_DIR_READ	0x1
+#define ESR_ELx_CP15_32_ISS_DIR_WRITE	0x0
+
+#define ESR_ELx_CP15_32_ISS_RT_SHIFT	5
+#define ESR_ELx_CP15_32_ISS_RT_MASK	(UL(0x1f) << ESR_ELx_CP15_32_ISS_RT_SHIFT)
+#define ESR_ELx_CP15_32_ISS_CRM_SHIFT	1
+#define ESR_ELx_CP15_32_ISS_CRM_MASK	(UL(0xf) << ESR_ELx_CP15_32_ISS_CRM_SHIFT)
+#define ESR_ELx_CP15_32_ISS_CRN_SHIFT	10
+#define ESR_ELx_CP15_32_ISS_CRN_MASK	(UL(0xf) << ESR_ELx_CP15_32_ISS_CRN_SHIFT)
+#define ESR_ELx_CP15_32_ISS_OP1_SHIFT	14
+#define ESR_ELx_CP15_32_ISS_OP1_MASK	(UL(0x7) << ESR_ELx_CP15_32_ISS_OP1_SHIFT)
+#define ESR_ELx_CP15_32_ISS_OP2_SHIFT	17
+#define ESR_ELx_CP15_32_ISS_OP2_MASK	(UL(0x7) << ESR_ELx_CP15_32_ISS_OP2_SHIFT)
+
+#define ESR_ELx_CP15_32_ISS_SYS_MASK	(ESR_ELx_CP15_32_ISS_OP1_MASK | \
+					 ESR_ELx_CP15_32_ISS_OP2_MASK | \
+					 ESR_ELx_CP15_32_ISS_CRN_MASK | \
+					 ESR_ELx_CP15_32_ISS_CRM_MASK | \
+					 ESR_ELx_CP15_32_ISS_DIR_MASK)
+#define ESR_ELx_CP15_32_ISS_SYS_VAL(op1, op2, crn, crm) \
+					(((op1) << ESR_ELx_CP15_32_ISS_OP1_SHIFT) | \
+					 ((op2) << ESR_ELx_CP15_32_ISS_OP2_SHIFT) | \
+					 ((crn) << ESR_ELx_CP15_32_ISS_CRN_SHIFT) | \
+					 ((crm) << ESR_ELx_CP15_32_ISS_CRM_SHIFT))
+
+#define ESR_ELx_CP15_64_ISS_DIR_MASK	0x1
+#define ESR_ELx_CP15_64_ISS_DIR_READ	0x1
+#define ESR_ELx_CP15_64_ISS_DIR_WRITE	0x0
+
+#define ESR_ELx_CP15_64_ISS_RT_SHIFT	5
+#define ESR_ELx_CP15_64_ISS_RT_MASK	(UL(0x1f) << ESR_ELx_CP15_64_ISS_RT_SHIFT)
+
+#define ESR_ELx_CP15_64_ISS_RT2_SHIFT	10
+#define ESR_ELx_CP15_64_ISS_RT2_MASK	(UL(0x1f) << ESR_ELx_CP15_64_ISS_RT2_SHIFT)
+
+#define ESR_ELx_CP15_64_ISS_OP1_SHIFT	16
+#define ESR_ELx_CP15_64_ISS_OP1_MASK	(UL(0xf) << ESR_ELx_CP15_64_ISS_OP1_SHIFT)
+#define ESR_ELx_CP15_64_ISS_CRM_SHIFT	1
+#define ESR_ELx_CP15_64_ISS_CRM_MASK	(UL(0xf) << ESR_ELx_CP15_64_ISS_CRM_SHIFT)
+
+#define ESR_ELx_CP15_64_ISS_SYS_VAL(op1, crm) \
+					(((op1) << ESR_ELx_CP15_64_ISS_OP1_SHIFT) | \
+					 ((crm) << ESR_ELx_CP15_64_ISS_CRM_SHIFT))
+
+#define ESR_ELx_CP15_64_ISS_SYS_MASK	(ESR_ELx_CP15_64_ISS_OP1_MASK |	\
+					 ESR_ELx_CP15_64_ISS_CRM_MASK | \
+					 ESR_ELx_CP15_64_ISS_DIR_MASK)
+
+#define ESR_ELx_CP15_64_ISS_SYS_CNTVCT	(ESR_ELx_CP15_64_ISS_SYS_VAL(1, 14) | \
+					 ESR_ELx_CP15_64_ISS_DIR_READ)
+
+#define ESR_ELx_CP15_64_ISS_SYS_CNTVCTSS (ESR_ELx_CP15_64_ISS_SYS_VAL(9, 14) | \
+					 ESR_ELx_CP15_64_ISS_DIR_READ)
+
+#define ESR_ELx_CP15_32_ISS_SYS_CNTFRQ	(ESR_ELx_CP15_32_ISS_SYS_VAL(0, 0, 14, 0) |\
+					 ESR_ELx_CP15_32_ISS_DIR_READ)
+
+#ifndef __ASSEMBLY__
+#include <asm/types.h>
+
+static inline bool esr_is_data_abort(u32 esr)
+{
+	const u32 ec = ESR_ELx_EC(esr);
+
+	return ec == ESR_ELx_EC_DABT_LOW || ec == ESR_ELx_EC_DABT_CUR;
+}
+
+const char *esr_get_class_string(u32 esr);
+#endif /* __ASSEMBLY */
+
+#endif /* __ASM_ESR_H */
diff --git a/arch/arm/include/asm/proc-armv/ptrace.h b/arch/arm/include/asm/proc-armv/ptrace.h
index e37ad8fd1f..2db60d552d 100644
--- a/arch/arm/include/asm/proc-armv/ptrace.h
+++ b/arch/arm/include/asm/proc-armv/ptrace.h
@@ -14,6 +14,79 @@
 
 #define PCMASK		0
 
+/*
+ * PSR bits
+ */
+#define PSR_MODE_EL0t	0x00000000
+#define PSR_MODE_EL1t	0x00000004
+#define PSR_MODE_EL1h	0x00000005
+#define PSR_MODE_EL2t	0x00000008
+#define PSR_MODE_EL2h	0x00000009
+#define PSR_MODE_EL3t	0x0000000c
+#define PSR_MODE_EL3h	0x0000000d
+#define PSR_MODE_MASK	0x0000000f
+
+/* AArch32 CPSR bits */
+#define PSR_MODE32_BIT		0x00000010
+
+/* AArch64 SPSR bits */
+#define PSR_F_BIT	0x00000040
+#define PSR_I_BIT	0x00000080
+#define PSR_A_BIT	0x00000100
+#define PSR_D_BIT	0x00000200
+#define PSR_BTYPE_MASK	0x00000c00
+#define PSR_SSBS_BIT	0x00001000
+#define PSR_PAN_BIT	0x00400000
+#define PSR_UAO_BIT	0x00800000
+#define PSR_DIT_BIT	0x01000000
+#define PSR_TCO_BIT	0x02000000
+#define PSR_V_BIT	0x10000000
+#define PSR_C_BIT	0x20000000
+#define PSR_Z_BIT	0x40000000
+#define PSR_N_BIT	0x80000000
+
+#define PSR_BTYPE_SHIFT		10
+
+/*
+ * Groups of PSR bits
+ */
+#define PSR_f		0xff000000	/* Flags		*/
+#define PSR_s		0x00ff0000	/* Status		*/
+#define PSR_x		0x0000ff00	/* Extension		*/
+#define PSR_c		0x000000ff	/* Control		*/
+
+/* Convenience names for the values of PSTATE.BTYPE */
+#define PSR_BTYPE_NONE		(0b00 << PSR_BTYPE_SHIFT)
+#define PSR_BTYPE_JC		(0b01 << PSR_BTYPE_SHIFT)
+#define PSR_BTYPE_C		(0b10 << PSR_BTYPE_SHIFT)
+#define PSR_BTYPE_J		(0b11 << PSR_BTYPE_SHIFT)
+
+/* SPSR_ELx bits for exceptions taken from AArch32 */
+#define PSR_AA32_MODE_MASK	0x0000001f
+#define PSR_AA32_MODE_USR	0x00000010
+#define PSR_AA32_MODE_FIQ	0x00000011
+#define PSR_AA32_MODE_IRQ	0x00000012
+#define PSR_AA32_MODE_SVC	0x00000013
+#define PSR_AA32_MODE_ABT	0x00000017
+#define PSR_AA32_MODE_HYP	0x0000001a
+#define PSR_AA32_MODE_UND	0x0000001b
+#define PSR_AA32_MODE_SYS	0x0000001f
+#define PSR_AA32_T_BIT		0x00000020
+#define PSR_AA32_F_BIT		0x00000040
+#define PSR_AA32_I_BIT		0x00000080
+#define PSR_AA32_A_BIT		0x00000100
+#define PSR_AA32_E_BIT		0x00000200
+#define PSR_AA32_PAN_BIT	0x00400000
+#define PSR_AA32_SSBS_BIT	0x00800000
+#define PSR_AA32_DIT_BIT	0x01000000
+#define PSR_AA32_Q_BIT		0x08000000
+#define PSR_AA32_V_BIT		0x10000000
+#define PSR_AA32_C_BIT		0x20000000
+#define PSR_AA32_Z_BIT		0x40000000
+#define PSR_AA32_N_BIT		0x80000000
+#define PSR_AA32_IT_MASK	0x0600fc00	/* If-Then execution state mask */
+#define PSR_AA32_GE_MASK	0x000f0000
+
 #ifndef __ASSEMBLY__
 
 /*
@@ -21,7 +94,9 @@
  * on the stack during an exception.
  */
 struct pt_regs {
+	unsigned long spsr;
 	unsigned long elr;
+	unsigned long esr;
 	unsigned long regs[31];
 };
 
diff --git a/arch/arm/include/asm/spl.h b/arch/arm/include/asm/spl.h
index e568af2561..b5790bd0bc 100644
--- a/arch/arm/include/asm/spl.h
+++ b/arch/arm/include/asm/spl.h
@@ -30,6 +30,7 @@ enum {
 	BOOT_DEVICE_DFU,
 	BOOT_DEVICE_XIP,
 	BOOT_DEVICE_BOOTROM,
+	BOOT_DEVICE_SMH,
 	BOOT_DEVICE_NONE
 };
 #endif
diff --git a/arch/arm/include/asm/u-boot-arm.h b/arch/arm/include/asm/u-boot-arm.h
index 0b93cc48c5..aef048708d 100644
--- a/arch/arm/include/asm/u-boot-arm.h
+++ b/arch/arm/include/asm/u-boot-arm.h
@@ -46,13 +46,8 @@ void do_software_interrupt(struct pt_regs *pt_regs);
 void do_prefetch_abort(struct pt_regs *pt_regs);
 void do_data_abort(struct pt_regs *pt_regs);
 void do_not_used(struct pt_regs *pt_regs);
-#ifdef CONFIG_ARM64
-void do_fiq(struct pt_regs *pt_regs, unsigned int esr);
-void do_irq(struct pt_regs *pt_regs, unsigned int esr);
-#else
 void do_fiq(struct pt_regs *pt_regs);
-void do_irq(struct pt_regs *pt_regswq);
-#endif
+void do_irq(struct pt_regs *pt_regs);
 
 void reset_misc(void);
 
diff --git a/arch/arm/lib/Makefile b/arch/arm/lib/Makefile
index c48e1f622d..594fc1228a 100644
--- a/arch/arm/lib/Makefile
+++ b/arch/arm/lib/Makefile
@@ -46,7 +46,7 @@ else
 obj-$(CONFIG_$(SPL_TPL_)USE_ARCH_MEMSET) += memset.o
 obj-$(CONFIG_$(SPL_TPL_)USE_ARCH_MEMCPY) += memcpy.o
 endif
-obj-$(CONFIG_SEMIHOSTING) += semihosting.o
+obj-$(CONFIG_$(SPL_TPL_)SEMIHOSTING) += semihosting.o
 
 obj-y	+= bdinfo.o
 obj-y	+= sections.o
diff --git a/arch/arm/lib/interrupts_64.c b/arch/arm/lib/interrupts_64.c
index c653e67db2..2e091415a4 100644
--- a/arch/arm/lib/interrupts_64.c
+++ b/arch/arm/lib/interrupts_64.c
@@ -5,11 +5,13 @@
  */
 
 #include <common.h>
+#include <asm/esr.h>
 #include <asm/global_data.h>
 #include <asm/ptrace.h>
 #include <irq_func.h>
 #include <linux/compiler.h>
 #include <efi_loader.h>
+#include <semihosting.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -64,12 +66,55 @@ void show_regs(struct pt_regs *regs)
 }
 
 /*
+ * Try to "emulate" a semihosting call in the event that we don't have a
+ * debugger attached.
+ */
+static bool smh_emulate_trap(struct pt_regs *regs)
+{
+	int size;
+
+	if (ESR_ELx_EC(regs->esr) != ESR_ELx_EC_UNKNOWN)
+		return false;
+
+	if (regs->spsr & PSR_MODE32_BIT) {
+		if (regs->spsr & PSR_AA32_T_BIT) {
+			u16 *insn = (u16 *)ALIGN_DOWN(regs->elr, 2);
+
+			if (*insn != SMH_T32_SVC && *insn != SMH_T32_HLT)
+				return false;
+			size = 2;
+		} else {
+			u32 *insn = (u32 *)ALIGN_DOWN(regs->elr, 4);
+
+			if (*insn != SMH_A32_SVC && *insn != SMH_A32_HLT)
+				return false;
+			size = 4;
+		}
+	} else {
+		u32 *insn = (u32 *)ALIGN_DOWN(regs->elr, 4);
+
+		if (*insn != SMH_A64_HLT)
+			return false;
+		size = 4;
+	}
+
+	/* Avoid future semihosting calls */
+	disable_semihosting();
+
+	/* Just pretend the call failed */
+	regs->regs[0] = -1;
+	regs->elr += size;
+	return true;
+}
+
+/*
  * do_bad_sync handles the impossible case in the Synchronous Abort vector.
  */
-void do_bad_sync(struct pt_regs *pt_regs, unsigned int esr)
+void do_bad_sync(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("Bad mode in \"Synchronous Abort\" handler, esr 0x%08x\n", esr);
+	printf("Bad mode in \"Synchronous Abort\" handler, esr 0x%08lx\n",
+	       pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -78,10 +123,10 @@ void do_bad_sync(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_bad_irq handles the impossible case in the Irq vector.
  */
-void do_bad_irq(struct pt_regs *pt_regs, unsigned int esr)
+void do_bad_irq(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("Bad mode in \"Irq\" handler, esr 0x%08x\n", esr);
+	printf("Bad mode in \"Irq\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -90,10 +135,10 @@ void do_bad_irq(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_bad_fiq handles the impossible case in the Fiq vector.
  */
-void do_bad_fiq(struct pt_regs *pt_regs, unsigned int esr)
+void do_bad_fiq(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("Bad mode in \"Fiq\" handler, esr 0x%08x\n", esr);
+	printf("Bad mode in \"Fiq\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -102,10 +147,10 @@ void do_bad_fiq(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_bad_error handles the impossible case in the Error vector.
  */
-void do_bad_error(struct pt_regs *pt_regs, unsigned int esr)
+void do_bad_error(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("Bad mode in \"Error\" handler, esr 0x%08x\n", esr);
+	printf("Bad mode in \"Error\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -114,10 +159,13 @@ void do_bad_error(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_sync handles the Synchronous Abort exception.
  */
-void do_sync(struct pt_regs *pt_regs, unsigned int esr)
+void do_sync(struct pt_regs *pt_regs)
 {
+	if (CONFIG_IS_ENABLED(SEMIHOSTING_FALLBACK) &&
+	    smh_emulate_trap(pt_regs))
+		return;
 	efi_restore_gd();
-	printf("\"Synchronous Abort\" handler, esr 0x%08x\n", esr);
+	printf("\"Synchronous Abort\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -126,10 +174,10 @@ void do_sync(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_irq handles the Irq exception.
  */
-void do_irq(struct pt_regs *pt_regs, unsigned int esr)
+void do_irq(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("\"Irq\" handler, esr 0x%08x\n", esr);
+	printf("\"Irq\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -138,10 +186,10 @@ void do_irq(struct pt_regs *pt_regs, unsigned int esr)
 /*
  * do_fiq handles the Fiq exception.
  */
-void do_fiq(struct pt_regs *pt_regs, unsigned int esr)
+void do_fiq(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("\"Fiq\" handler, esr 0x%08x\n", esr);
+	printf("\"Fiq\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
@@ -153,10 +201,10 @@ void do_fiq(struct pt_regs *pt_regs, unsigned int esr)
  * it is defined with weak attribute and can be redefined
  * in processor specific code.
  */
-void __weak do_error(struct pt_regs *pt_regs, unsigned int esr)
+void __weak do_error(struct pt_regs *pt_regs)
 {
 	efi_restore_gd();
-	printf("\"Error\" handler, esr 0x%08x\n", esr);
+	printf("\"Error\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	show_efi_loaded_images(pt_regs);
 	panic("Resetting CPU ...\n");
diff --git a/arch/arm/lib/semihosting.c b/arch/arm/lib/semihosting.c
index 9fd82459b2..dbea2b06fb 100644
--- a/arch/arm/lib/semihosting.c
+++ b/arch/arm/lib/semihosting.c
@@ -1,28 +1,29 @@
 // SPDX-License-Identifier: GPL-2.0+
 /*
+ * Copyright (C) 2022 Sean Anderson <sean.anderson@seco.com>
  * Copyright 2014 Broadcom Corporation
  */
 
 /*
- * Minimal semihosting implementation for reading files into memory. If more
- * features like writing files or console output are required they can be
- * added later. This code has been tested on arm64/aarch64 fastmodel only.
- * An untested placeholder exists for armv7 architectures, but since they
- * are commonly available in silicon now, fastmodel usage makes less sense
- * for them.
+ * This code has been tested on arm64/aarch64 fastmodel only.  An untested
+ * placeholder exists for armv7 architectures, but since they are commonly
+ * available in silicon now, fastmodel usage makes less sense for them.
  */
 #include <common.h>
-#include <command.h>
-#include <env.h>
 #include <log.h>
+#include <semihosting.h>
 
 #define SYSOPEN		0x01
 #define SYSCLOSE	0x02
+#define SYSWRITEC	0x03
+#define SYSWRITE0	0x04
+#define SYSWRITE	0x05
 #define SYSREAD		0x06
+#define SYSREADC	0x07
+#define SYSISERROR	0x08
+#define SYSSEEK		0x0A
 #define SYSFLEN		0x0C
-
-#define MODE_READ	0x0
-#define MODE_READBIN	0x1
+#define SYSERRNO	0x13
 
 /*
  * Call the handler
@@ -41,32 +42,54 @@ static noinline long smh_trap(unsigned int sysnum, void *addr)
 	return result;
 }
 
-/*
- * Open a file on the host. Mode is "r" or "rb" currently. Returns a file
- * descriptor or -1 on error.
+#if CONFIG_IS_ENABLED(SEMIHOSTING_FALLBACK)
+static bool _semihosting_enabled = true;
+static bool try_semihosting = true;
+
+bool semihosting_enabled(void)
+{
+	if (try_semihosting) {
+		smh_trap(SYSERRNO, NULL);
+		try_semihosting = false;
+	}
+
+	return _semihosting_enabled;
+}
+
+void disable_semihosting(void)
+{
+	_semihosting_enabled = false;
+}
+#endif
+
+/**
+ * smh_errno() - Read the host's errno
+ *
+ * This gets the value of the host's errno and negates it. The host's errno may
+ * or may not be set, so only call this function if a previous semihosting call
+ * has failed.
+ *
+ * Return: a negative error value
  */
-static long smh_open(const char *fname, char *modestr)
+static int smh_errno(void)
+{
+	long ret = smh_trap(SYSERRNO, NULL);
+
+	if (ret > 0 && ret < INT_MAX)
+		return -ret;
+	return -EIO;
+}
+
+long smh_open(const char *fname, enum smh_open_mode mode)
 {
 	long fd;
-	unsigned long mode;
 	struct smh_open_s {
 		const char *fname;
 		unsigned long mode;
 		size_t len;
 	} open;
 
-	debug("%s: file \'%s\', mode \'%s\'\n", __func__, fname, modestr);
-
-	/* Check the file mode */
-	if (!(strcmp(modestr, "r"))) {
-		mode = MODE_READ;
-	} else if (!(strcmp(modestr, "rb"))) {
-		mode = MODE_READBIN;
-	} else {
-		printf("%s: ERROR mode \'%s\' not supported\n", __func__,
-		       modestr);
-		return -1;
-	}
+	debug("%s: file \'%s\', mode \'%u\'\n", __func__, fname, mode);
 
 	open.fname = fname;
 	open.len = strlen(fname);
@@ -75,23 +98,26 @@ static long smh_open(const char *fname, char *modestr)
 	/* Open the file on the host */
 	fd = smh_trap(SYSOPEN, &open);
 	if (fd == -1)
-		printf("%s: ERROR fd %ld for file \'%s\'\n", __func__, fd,
-		       fname);
-
+		return smh_errno();
 	return fd;
 }
 
-/*
- * Read 'len' bytes of file into 'memp'. Returns 0 on success, else failure
+/**
+ * struct smg_rdwr_s - Arguments for read and write
+ * @fd: A file descriptor returned from smh_open()
+ * @memp: Pointer to a buffer of memory of at least @len bytes
+ * @len: The number of bytes to read or write
  */
-static long smh_read(long fd, void *memp, size_t len)
+struct smh_rdwr_s {
+	long fd;
+	void *memp;
+	size_t len;
+};
+
+long smh_read(long fd, void *memp, size_t len)
 {
 	long ret;
-	struct smh_read_s {
-		long fd;
-		void *memp;
-		size_t len;
-	} read;
+	struct smh_rdwr_s read;
 
 	debug("%s: fd %ld, memp %p, len %zu\n", __func__, fd, memp, len);
 
@@ -100,25 +126,30 @@ static long smh_read(long fd, void *memp, size_t len)
 	read.len = len;
 
 	ret = smh_trap(SYSREAD, &read);
-	if (ret < 0) {
-		/*
-		 * The ARM handler allows for returning partial lengths,
-		 * but in practice this never happens so rather than create
-		 * hard to maintain partial read loops and such, just fail
-		 * with an error message.
-		 */
-		printf("%s: ERROR ret %ld, fd %ld, len %zu memp %p\n",
-		       __func__, ret, fd, len, memp);
-		return -1;
-	}
+	if (ret < 0)
+		return smh_errno();
+	return len - ret;
+}
+
+long smh_write(long fd, const void *memp, size_t len, ulong *written)
+{
+	long ret;
+	struct smh_rdwr_s write;
+
+	debug("%s: fd %ld, memp %p, len %zu\n", __func__, fd, memp, len);
 
+	write.fd = fd;
+	write.memp = (void *)memp;
+	write.len = len;
+
+	ret = smh_trap(SYSWRITE, &write);
+	*written = len - ret;
+	if (ret)
+		return smh_errno();
 	return 0;
 }
 
-/*
- * Close the file using the file descriptor
- */
-static long smh_close(long fd)
+long smh_close(long fd)
 {
 	long ret;
 
@@ -126,15 +157,11 @@ static long smh_close(long fd)
 
 	ret = smh_trap(SYSCLOSE, &fd);
 	if (ret == -1)
-		printf("%s: ERROR fd %ld\n", __func__, fd);
-
-	return ret;
+		return smh_errno();
+	return 0;
 }
 
-/*
- * Get the file length from the file descriptor
- */
-static long smh_len_fd(long fd)
+long smh_flen(long fd)
 {
 	long ret;
 
@@ -142,77 +169,40 @@ static long smh_len_fd(long fd)
 
 	ret = smh_trap(SYSFLEN, &fd);
 	if (ret == -1)
-		printf("%s: ERROR ret %ld, fd %ld\n", __func__, ret, fd);
-
+		return smh_errno();
 	return ret;
 }
 
-static int smh_load_file(const char * const name, ulong load_addr,
-			 ulong *end_addr)
+long smh_seek(long fd, long pos)
 {
-	long fd;
-	long len;
 	long ret;
+	struct smh_seek_s {
+		long fd;
+		long pos;
+	} seek;
 
-	fd = smh_open(name, "rb");
-	if (fd == -1)
-		return -1;
+	debug("%s: fd %ld pos %ld\n", __func__, fd, pos);
 
-	len = smh_len_fd(fd);
-	if (len < 0) {
-		smh_close(fd);
-		return -1;
-	}
-
-	ret = smh_read(fd, (void *)load_addr, len);
-	smh_close(fd);
-
-	if (ret == 0) {
-		*end_addr = load_addr + len - 1;
-		printf("loaded file %s from %08lX to %08lX, %08lX bytes\n",
-		       name,
-		       load_addr,
-		       *end_addr,
-		       len);
-	} else {
-		printf("read failed\n");
-		return 0;
-	}
+	seek.fd = fd;
+	seek.pos = pos;
 
+	ret = smh_trap(SYSSEEK, &seek);
+	if (ret)
+		return smh_errno();
 	return 0;
 }
 
-static int do_smhload(struct cmd_tbl *cmdtp, int flag, int argc,
-		      char *const argv[])
+int smh_getc(void)
 {
-	if (argc == 3 || argc == 4) {
-		ulong load_addr;
-		ulong end_addr = 0;
-		int ret;
-		char end_str[64];
-
-		load_addr = hextoul(argv[2], NULL);
-		if (!load_addr)
-			return -1;
-
-		ret = smh_load_file(argv[1], load_addr, &end_addr);
-		if (ret < 0)
-			return CMD_RET_FAILURE;
-
-		/* Optionally save returned end to the environment */
-		if (argc == 4) {
-			sprintf(end_str, "0x%08lx", end_addr);
-			env_set(argv[3], end_str);
-		}
-	} else {
-		return CMD_RET_USAGE;
-	}
-	return 0;
+	return smh_trap(SYSREADC, NULL);
+}
+
+void smh_putc(char ch)
+{
+	smh_trap(SYSWRITEC, &ch);
 }
 
-U_BOOT_CMD(smhload, 4, 0, do_smhload, "load a file using semihosting",
-	   "<file> 0x<address> [end var]\n"
-	   "    - load a semihosted file to the address specified\n"
-	   "      if the optional [end var] is specified, the end\n"
-	   "      address of the file will be stored in this environment\n"
-	   "      variable.\n");
+void smh_puts(const char *s)
+{
+	smh_trap(SYSWRITE0, (char *)s);
+}
diff --git a/arch/arm/mach-imx/imx8m/soc.c b/arch/arm/mach-imx/imx8m/soc.c
index 838f0a3749..7397b99a1e 100644
--- a/arch/arm/mach-imx/imx8m/soc.c
+++ b/arch/arm/mach-imx/imx8m/soc.c
@@ -1295,7 +1295,7 @@ void imx_tmu_arch_init(void *reg_base)
 #if defined(CONFIG_IMX8MQ) || defined(CONFIG_IMX8MM) || defined(CONFIG_IMX8MN)
 bool serror_need_skip = true;
 
-void do_error(struct pt_regs *pt_regs, unsigned int esr)
+void do_error(struct pt_regs *pt_regs)
 {
 	/*
 	 * If stack is still in ROM reserved OCRAM not switch to SPL,
@@ -1320,7 +1320,7 @@ void do_error(struct pt_regs *pt_regs, unsigned int esr)
 	}
 
 	efi_restore_gd();
-	printf("\"Error\" handler, esr 0x%08x\n", esr);
+	printf("\"Error\" handler, esr 0x%08lx\n", pt_regs->esr);
 	show_regs(pt_regs);
 	panic("Resetting CPU ...\n");
 }