Merge tag 'pull-target-arm-20230606' of https://git.linaro.org/people/pmaydell/qemu-arm into staging

target-arm queue:
 * Support gdbstub (guest debug) in HVF
 * xnlx-versal: Support CANFD controller
 * bpim2u: New board model: Banana Pi BPI-M2 Ultra
 * Emulate FEAT_LSE2
 * allow DC CVA[D]P in user mode emulation
 * trap DCC access in user mode emulation

# -----BEGIN PGP SIGNATURE-----
#
# iQJNBAABCAA3FiEE4aXFk81BneKOgxXPPCUl7RQ2DN4FAmR/AKUZHHBldGVyLm1h
# eWRlbGxAbGluYXJvLm9yZwAKCRA8JSXtFDYM3jzIEACNepQGY44yPhrEG+wD4WAB
# fH670KI33HcsFd2rGsC369gcssQbRIW/29reOzNhRMuol+kHI6OFaONpuKSdO0Rz
# TLVIsnT2Uq8KwbYfLtDQt5knj027amPy75d4re8wIK1eZB4dOIHysqAvQrJYeync
# 9obKku8xXGLwZh/mYHoVgHcZU0cPJO9nri39n1tV3JUBsgmqEURjzbZrMcF+yMX7
# bUzOYQvC1Iedmo+aWfx43u82AlNQFz1lsqmnQj7Z5rvv0HT+BRF5WzVMP0qRh5+Z
# njkqmBH9xb9kkgeHmeMvHpWox+J+obeSmVg/4gDNlJpThmpuU0Vr7EXUN3MBQlV9
# lhyy6zrTwC/BToiQqdT2dnpao9FzXy5exfnqi/py5IuqfjAzSO+p61LlPPZ4cJri
# pCK4yq2gzQXYfrlZkUJipvRMH8Xa4IdQx+w7lXrQoJdduF4/+6aJW/GAWSu0e7eC
# zgBwaJjI7ENce8ixJnuEFUxUnaBo8dl72a0PGA1UU8PL+cJNOIpyhPk4goWQprdn
# iFF4ZnjhBRZ2gk/4HGD9u5Vo2lNqP93YS5QhkGkF+HJsBmcOZgidIUpfHhPQvvHO
# Np196T2cAETCWGV1xG4CaTpxN2ndRReq3C0/mzfhIbwhXEACtvAiSlO4KB8t6pJj
# MzinCABXHcovJbGbxZ9j6w==
# =8SdN
# -----END PGP SIGNATURE-----
# gpg: Signature made Tue 06 Jun 2023 02:47:17 AM PDT
# gpg:                using RSA key E1A5C593CD419DE28E8315CF3C2525ED14360CDE
# gpg:                issuer "peter.maydell@linaro.org"
# gpg: Good signature from "Peter Maydell <peter.maydell@linaro.org>" [full]
# gpg:                 aka "Peter Maydell <pmaydell@gmail.com>" [full]
# gpg:                 aka "Peter Maydell <pmaydell@chiark.greenend.org.uk>" [full]
# gpg:                 aka "Peter Maydell <peter@archaic.org.uk>" [unknown]

* tag 'pull-target-arm-20230606' of https://git.linaro.org/people/pmaydell/qemu-arm: (42 commits)
  target/arm: trap DCC access in user mode emulation
  tests/tcg/aarch64: add DC CVA[D]P tests
  target/arm: allow DC CVA[D]P in user mode emulation
  target/arm: Enable FEAT_LSE2 for -cpu max
  tests/tcg/multiarch: Adjust sigbus.c
  tests/tcg/aarch64: Use stz2g in mte-7.c
  target/arm: Move mte check for store-exclusive
  target/arm: Relax ordered/atomic alignment checks for LSE2
  target/arm: Add SCTLR.nAA to TBFLAG_A64
  target/arm: Check alignment in helper_mte_check
  target/arm: Pass single_memop to gen_mte_checkN
  target/arm: Pass memop to gen_mte_check1*
  target/arm: Hoist finalize_memop out of do_fp_{ld, st}
  target/arm: Hoist finalize_memop out of do_gpr_{ld, st}
  target/arm: Load/store integer pair with one tcg operation
  target/arm: Sink gen_mte_check1 into load/store_exclusive
  target/arm: Use tcg_gen_qemu_{ld, st}_i128 in gen_sve_{ld, st}r
  target/arm: Use tcg_gen_qemu_st_i128 for STZG, STZ2G
  target/arm: Use tcg_gen_qemu_{st, ld}_i128 for do_fp_{st, ld}
  target/arm: Use tcg_gen_qemu_ld_i128 for LDXP
  ...

Signed-off-by: Richard Henderson <richard.henderson@linaro.org>

63 files changed, 7386 insertions, 733 deletions

diff --git a/MAINTAINERS b/MAINTAINERS
index 62ab61c..436b3f0 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -1819,7 +1819,7 @@ M: Francisco Iglesias <francisco.iglesias@amd.com>
 S: Maintained
 F: hw/net/can/xlnx-*
 F: include/hw/net/xlnx-*
-F: tests/qtest/xlnx-can-test*
+F: tests/qtest/xlnx-can*-test*
 
 EDU
 M: Jiri Slaby <jslaby@suse.cz>
diff --git a/accel/hvf/hvf-accel-ops.c b/accel/hvf/hvf-accel-ops.c
index 24913ca..9c3da03 100644
--- a/accel/hvf/hvf-accel-ops.c
+++ b/accel/hvf/hvf-accel-ops.c
@@ -52,6 +52,7 @@
 #include "qemu/main-loop.h"
 #include "exec/address-spaces.h"
 #include "exec/exec-all.h"
+#include "exec/gdbstub.h"
 #include "sysemu/cpus.h"
 #include "sysemu/hvf.h"
 #include "sysemu/hvf_int.h"
@@ -334,18 +335,26 @@ static int hvf_accel_init(MachineState *ms)
         s->slots[x].slot_id = x;
     }
 
+    QTAILQ_INIT(&s->hvf_sw_breakpoints);
+
     hvf_state = s;
     memory_listener_register(&hvf_memory_listener, &address_space_memory);
 
     return hvf_arch_init();
 }
 
+static inline int hvf_gdbstub_sstep_flags(void)
+{
+    return SSTEP_ENABLE | SSTEP_NOIRQ;
+}
+
 static void hvf_accel_class_init(ObjectClass *oc, void *data)
 {
     AccelClass *ac = ACCEL_CLASS(oc);
     ac->name = "HVF";
     ac->init_machine = hvf_accel_init;
     ac->allowed = &hvf_allowed;
+    ac->gdbstub_supported_sstep_flags = hvf_gdbstub_sstep_flags;
 }
 
 static const TypeInfo hvf_accel_type = {
@@ -395,6 +404,8 @@ static int hvf_init_vcpu(CPUState *cpu)
     cpu->vcpu_dirty = 1;
     assert_hvf_ok(r);
 
+    cpu->hvf->guest_debug_enabled = false;
+
     return hvf_arch_init_vcpu(cpu);
 }
 
@@ -462,6 +473,108 @@ static void hvf_start_vcpu_thread(CPUState *cpu)
                        cpu, QEMU_THREAD_JOINABLE);
 }
 
+static int hvf_insert_breakpoint(CPUState *cpu, int type, hwaddr addr, hwaddr len)
+{
+    struct hvf_sw_breakpoint *bp;
+    int err;
+
+    if (type == GDB_BREAKPOINT_SW) {
+        bp = hvf_find_sw_breakpoint(cpu, addr);
+        if (bp) {
+            bp->use_count++;
+            return 0;
+        }
+
+        bp = g_new(struct hvf_sw_breakpoint, 1);
+        bp->pc = addr;
+        bp->use_count = 1;
+        err = hvf_arch_insert_sw_breakpoint(cpu, bp);
+        if (err) {
+            g_free(bp);
+            return err;
+        }
+
+        QTAILQ_INSERT_HEAD(&hvf_state->hvf_sw_breakpoints, bp, entry);
+    } else {
+        err = hvf_arch_insert_hw_breakpoint(addr, len, type);
+        if (err) {
+            return err;
+        }
+    }
+
+    CPU_FOREACH(cpu) {
+        err = hvf_update_guest_debug(cpu);
+        if (err) {
+            return err;
+        }
+    }
+    return 0;
+}
+
+static int hvf_remove_breakpoint(CPUState *cpu, int type, hwaddr addr, hwaddr len)
+{
+    struct hvf_sw_breakpoint *bp;
+    int err;
+
+    if (type == GDB_BREAKPOINT_SW) {
+        bp = hvf_find_sw_breakpoint(cpu, addr);
+        if (!bp) {
+            return -ENOENT;
+        }
+
+        if (bp->use_count > 1) {
+            bp->use_count--;
+            return 0;
+        }
+
+        err = hvf_arch_remove_sw_breakpoint(cpu, bp);
+        if (err) {
+            return err;
+        }
+
+        QTAILQ_REMOVE(&hvf_state->hvf_sw_breakpoints, bp, entry);
+        g_free(bp);
+    } else {
+        err = hvf_arch_remove_hw_breakpoint(addr, len, type);
+        if (err) {
+            return err;
+        }
+    }
+
+    CPU_FOREACH(cpu) {
+        err = hvf_update_guest_debug(cpu);
+        if (err) {
+            return err;
+        }
+    }
+    return 0;
+}
+
+static void hvf_remove_all_breakpoints(CPUState *cpu)
+{
+    struct hvf_sw_breakpoint *bp, *next;
+    CPUState *tmpcpu;
+
+    QTAILQ_FOREACH_SAFE(bp, &hvf_state->hvf_sw_breakpoints, entry, next) {
+        if (hvf_arch_remove_sw_breakpoint(cpu, bp) != 0) {
+            /* Try harder to find a CPU that currently sees the breakpoint. */
+            CPU_FOREACH(tmpcpu)
+            {
+                if (hvf_arch_remove_sw_breakpoint(tmpcpu, bp) == 0) {
+                    break;
+                }
+            }
+        }
+        QTAILQ_REMOVE(&hvf_state->hvf_sw_breakpoints, bp, entry);
+        g_free(bp);
+    }
+    hvf_arch_remove_all_hw_breakpoints();
+
+    CPU_FOREACH(cpu) {
+        hvf_update_guest_debug(cpu);
+    }
+}
+
 static void hvf_accel_ops_class_init(ObjectClass *oc, void *data)
 {
     AccelOpsClass *ops = ACCEL_OPS_CLASS(oc);
@@ -473,6 +586,12 @@ static void hvf_accel_ops_class_init(ObjectClass *oc, void *data)
     ops->synchronize_post_init = hvf_cpu_synchronize_post_init;
     ops->synchronize_state = hvf_cpu_synchronize_state;
     ops->synchronize_pre_loadvm = hvf_cpu_synchronize_pre_loadvm;
+
+    ops->insert_breakpoint = hvf_insert_breakpoint;
+    ops->remove_breakpoint = hvf_remove_breakpoint;
+    ops->remove_all_breakpoints = hvf_remove_all_breakpoints;
+    ops->update_guest_debug = hvf_update_guest_debug;
+    ops->supports_guest_debug = hvf_arch_supports_guest_debug;
 };
 static const TypeInfo hvf_accel_ops_type = {
     .name = ACCEL_OPS_NAME("hvf"),
diff --git a/accel/hvf/hvf-all.c b/accel/hvf/hvf-all.c
index 0043f4d..754707d 100644
--- a/accel/hvf/hvf-all.c
+++ b/accel/hvf/hvf-all.c
@@ -44,3 +44,26 @@ void assert_hvf_ok(hv_return_t ret)
 
     abort();
 }
+
+struct hvf_sw_breakpoint *hvf_find_sw_breakpoint(CPUState *cpu, target_ulong pc)
+{
+    struct hvf_sw_breakpoint *bp;
+
+    QTAILQ_FOREACH(bp, &hvf_state->hvf_sw_breakpoints, entry) {
+        if (bp->pc == pc) {
+            return bp;
+        }
+    }
+    return NULL;
+}
+
+int hvf_sw_breakpoints_active(CPUState *cpu)
+{
+    return !QTAILQ_EMPTY(&hvf_state->hvf_sw_breakpoints);
+}
+
+int hvf_update_guest_debug(CPUState *cpu)
+{
+    hvf_arch_update_guest_debug(cpu);
+    return 0;
+}
diff --git a/docs/system/arm/bananapi_m2u.rst b/docs/system/arm/bananapi_m2u.rst
new file mode 100644
index 0000000..b09ba5c
--- /dev/null
+++ b/docs/system/arm/bananapi_m2u.rst
@@ -0,0 +1,139 @@
+Banana Pi BPI-M2U (``bpim2u``)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Banana Pi BPI-M2 Ultra is a quad-core mini single board computer built with
+Allwinner A40i/R40/V40 SoC. It features 2GB of RAM and 8GB eMMC. It also
+has onboard WiFi and BT. On the ports side, the BPI-M2 Ultra has 2 USB A
+2.0 ports, 1 USB OTG port, 1 HDMI port, 1 audio jack, a DC power port,
+and last but not least, a SATA port.
+
+Supported devices
+"""""""""""""""""
+
+The Banana Pi M2U machine supports the following devices:
+
+ * SMP (Quad Core Cortex-A7)
+ * Generic Interrupt Controller configuration
+ * SRAM mappings
+ * SDRAM controller
+ * Timer device (re-used from Allwinner A10)
+ * UART
+ * SD/MMC storage controller
+ * EMAC ethernet
+ * GMAC ethernet
+ * Clock Control Unit
+ * TWI (I2C)
+
+Limitations
+"""""""""""
+
+Currently, Banana Pi M2U does *not* support the following features:
+
+- Graphical output via HDMI, GPU and/or the Display Engine
+- Audio output
+- Hardware Watchdog
+- Real Time Clock
+- USB 2.0 interfaces
+
+Also see the 'unimplemented' array in the Allwinner R40 SoC module
+for a complete list of unimplemented I/O devices: ``./hw/arm/allwinner-r40.c``
+
+Boot options
+""""""""""""
+
+The Banana Pi M2U machine can start using the standard -kernel functionality
+for loading a Linux kernel or ELF executable. Additionally, the Banana Pi M2U
+machine can also emulate the BootROM which is present on an actual Allwinner R40
+based SoC, which loads the bootloader from a SD card, specified via the -sd
+argument to qemu-system-arm.
+
+Running mainline Linux
+""""""""""""""""""""""
+
+To build a Linux mainline kernel that can be booted by the Banana Pi M2U machine,
+simply configure the kernel using the sunxi_defconfig configuration:
+
+.. code-block:: bash
+
+  $ ARCH=arm CROSS_COMPILE=arm-linux-gnueabi- make mrproper
+  $ ARCH=arm CROSS_COMPILE=arm-linux-gnueabi- make sunxi_defconfig
+
+To boot the newly build linux kernel in QEMU with the Banana Pi M2U machine, use:
+
+.. code-block:: bash
+
+  $ qemu-system-arm -M bpim2u -nographic \
+      -kernel /path/to/linux/arch/arm/boot/zImage \
+      -append 'console=ttyS0,115200' \
+      -dtb /path/to/linux/arch/arm/boot/dts/sun8i-r40-bananapi-m2-ultra.dtb
+
+Banana Pi M2U images
+""""""""""""""""""""
+
+Note that the mainline kernel does not have a root filesystem. You can choose
+to build you own image with buildroot using the bananapi_m2_ultra_defconfig.
+Also see https://buildroot.org for more information.
+
+Another possibility is to run an OpenWrt image for Banana Pi M2U which
+can be downloaded from:
+
+   https://downloads.openwrt.org/releases/22.03.3/targets/sunxi/cortexa7/
+
+When using an image as an SD card, it must be resized to a power of two. This can be
+done with the ``qemu-img`` command. It is recommended to only increase the image size
+instead of shrinking it to a power of two, to avoid loss of data. For example,
+to prepare a downloaded Armbian image, first extract it and then increase
+its size to one gigabyte as follows:
+
+.. code-block:: bash
+
+  $ qemu-img resize \
+    openwrt-22.03.3-sunxi-cortexa7-sinovoip_bananapi-m2-ultra-ext4-sdcard.img \
+    1G
+
+Instead of providing a custom Linux kernel via the -kernel command you may also
+choose to let the Banana Pi M2U machine load the bootloader from SD card, just like
+a real board would do using the BootROM. Simply pass the selected image via the -sd
+argument and remove the -kernel, -append, -dbt and -initrd arguments:
+
+.. code-block:: bash
+
+  $ qemu-system-arm -M bpim2u -nic user -nographic \
+    -sd openwrt-22.03.3-sunxi-cortexa7-sinovoip_bananapi-m2-ultra-ext4-sdcard.img
+
+Running U-Boot
+""""""""""""""
+
+U-Boot mainline can be build and configured using the Bananapi_M2_Ultra_defconfig
+using similar commands as describe above for Linux. Note that it is recommended
+for development/testing to select the following configuration setting in U-Boot:
+
+  Device Tree Control > Provider for DTB for DT Control > Embedded DTB
+
+The BootROM of allwinner R40 loading u-boot from the 8KiB offset of sdcard.
+Let's create an bootable disk image:
+
+.. code-block:: bash
+
+  $ dd if=/dev/zero of=sd.img bs=32M count=1
+  $ dd if=u-boot-sunxi-with-spl.bin of=sd.img bs=1k seek=8 conv=notrunc
+
+And then boot it.
+
+.. code-block:: bash
+
+  $ qemu-system-arm -M bpim2u -nographic -sd sd.img
+
+Banana Pi M2U integration tests
+"""""""""""""""""""""""""""""""
+
+The Banana Pi M2U machine has several integration tests included.
+To run the whole set of tests, build QEMU from source and simply
+provide the following command:
+
+.. code-block:: bash
+
+  $ cd qemu-build-dir
+  $ AVOCADO_ALLOW_LARGE_STORAGE=yes tests/venv/bin/avocado \
+    --verbose --show=app,console run -t machine:bpim2u \
+    ../tests/avocado/boot_linux_console.py
diff --git a/docs/system/arm/emulation.rst b/docs/system/arm/emulation.rst
index 7338987..ecbbd63 100644
--- a/docs/system/arm/emulation.rst
+++ b/docs/system/arm/emulation.rst
@@ -50,6 +50,7 @@ the following architecture extensions:
 - FEAT_LRCPC (Load-acquire RCpc instructions)
 - FEAT_LRCPC2 (Load-acquire RCpc instructions v2)
 - FEAT_LSE (Large System Extensions)
+- FEAT_LSE2 (Large System Extensions v2)
 - FEAT_LVA (Large Virtual Address space)
 - FEAT_MTE (Memory Tagging Extension)
 - FEAT_MTE2 (Memory Tagging Extension)
diff --git a/docs/system/arm/xlnx-versal-virt.rst b/docs/system/arm/xlnx-versal-virt.rst
index 92ad10d..d2d1b26 100644
--- a/docs/system/arm/xlnx-versal-virt.rst
+++ b/docs/system/arm/xlnx-versal-virt.rst
@@ -34,6 +34,7 @@ Implemented devices:
 - DDR memory
 - BBRAM (36 bytes of Battery-backed RAM)
 - eFUSE (3072 bytes of one-time field-programmable bit array)
+- 2 CANFDs
 
 QEMU does not yet model any other devices, including the PL and the AI Engine.
 
@@ -224,3 +225,33 @@ To use a different index value, N, from default of 1, add:
 
   Better yet, do not use actual product data when running guest image
   on this Xilinx Versal Virt board.
+
+Using CANFDs for Versal Virt
+""""""""""""""""""""""""""""
+Versal CANFD controller is developed based on SocketCAN and QEMU CAN bus
+implementation. Bus connection and socketCAN connection for each CAN module
+can be set through command lines.
+
+To connect both CANFD0 and CANFD1 on the same bus:
+
+.. code-block:: bash
+
+    -object can-bus,id=canbus -machine canbus0=canbus -machine canbus1=canbus
+
+To connect CANFD0 and CANFD1 to separate buses:
+
+.. code-block:: bash
+
+    -object can-bus,id=canbus0 -object can-bus,id=canbus1 \
+    -machine canbus0=canbus0 -machine canbus1=canbus1
+
+The SocketCAN interface can connect to a Physical or a Virtual CAN interfaces on
+the host machine. Please check this document to learn about CAN interface on
+Linux: docs/system/devices/can.rst
+
+To connect CANFD0 and CANFD1 to host machine's CAN interface can0:
+
+.. code-block:: bash
+
+    -object can-bus,id=canbus -machine canbus0=canbus -machine canbus1=canbus
+    -object can-host-socketcan,id=canhost0,if=can0,canbus=canbus
diff --git a/docs/system/target-arm.rst b/docs/system/target-arm.rst
index 91ebc26..a12b6bc 100644
--- a/docs/system/target-arm.rst
+++ b/docs/system/target-arm.rst
@@ -83,6 +83,7 @@ undocumented; you can get a complete list by running
    arm/versatile
    arm/vexpress
    arm/aspeed
+   arm/bananapi_m2u.rst
    arm/sabrelite
    arm/digic
    arm/cubieboard
diff --git a/hw/arm/Kconfig b/hw/arm/Kconfig
index acc4371..2159de3 100644
--- a/hw/arm/Kconfig
+++ b/hw/arm/Kconfig
@@ -383,7 +383,7 @@ config ALLWINNER_A10
     select ALLWINNER_WDT
     select ALLWINNER_EMAC
     select ALLWINNER_I2C
-    select AXP209_PMU
+    select AXP2XX_PMU
     select SERIAL
     select UNIMP
 
@@ -403,6 +403,18 @@ config ALLWINNER_H3
     select USB_EHCI_SYSBUS
     select SD
 
+config ALLWINNER_R40
+    bool
+    default y if TCG && ARM
+    select ALLWINNER_SRAMC
+    select ALLWINNER_A10_PIT
+    select AXP2XX_PMU
+    select SERIAL
+    select ARM_TIMER
+    select ARM_GIC
+    select UNIMP
+    select SD
+
 config RASPI
     bool
     default y
diff --git a/hw/arm/allwinner-r40.c b/hw/arm/allwinner-r40.c
new file mode 100644
index 0000000..7d29eb2
--- /dev/null
+++ b/hw/arm/allwinner-r40.c
@@ -0,0 +1,526 @@
+/*
+ * Allwinner R40/A40i/T3 System on Chip emulation
+ *
+ * Copyright (C) 2023 qianfan Zhao <qianfanguijin@163.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "qemu/osdep.h"
+#include "qapi/error.h"
+#include "qemu/error-report.h"
+#include "qemu/bswap.h"
+#include "qemu/module.h"
+#include "qemu/units.h"
+#include "hw/qdev-core.h"
+#include "hw/sysbus.h"
+#include "hw/char/serial.h"
+#include "hw/misc/unimp.h"
+#include "hw/usb/hcd-ehci.h"
+#include "hw/loader.h"
+#include "sysemu/sysemu.h"
+#include "hw/arm/allwinner-r40.h"
+#include "hw/misc/allwinner-r40-dramc.h"
+
+/* Memory map */
+const hwaddr allwinner_r40_memmap[] = {
+    [AW_R40_DEV_SRAM_A1]    = 0x00000000,
+    [AW_R40_DEV_SRAM_A2]    = 0x00004000,
+    [AW_R40_DEV_SRAM_A3]    = 0x00008000,
+    [AW_R40_DEV_SRAM_A4]    = 0x0000b400,
+    [AW_R40_DEV_SRAMC]      = 0x01c00000,
+    [AW_R40_DEV_EMAC]       = 0x01c0b000,
+    [AW_R40_DEV_MMC0]       = 0x01c0f000,
+    [AW_R40_DEV_MMC1]       = 0x01c10000,
+    [AW_R40_DEV_MMC2]       = 0x01c11000,
+    [AW_R40_DEV_MMC3]       = 0x01c12000,
+    [AW_R40_DEV_CCU]        = 0x01c20000,
+    [AW_R40_DEV_PIT]        = 0x01c20c00,
+    [AW_R40_DEV_UART0]      = 0x01c28000,
+    [AW_R40_DEV_UART1]      = 0x01c28400,
+    [AW_R40_DEV_UART2]      = 0x01c28800,
+    [AW_R40_DEV_UART3]      = 0x01c28c00,
+    [AW_R40_DEV_UART4]      = 0x01c29000,
+    [AW_R40_DEV_UART5]      = 0x01c29400,
+    [AW_R40_DEV_UART6]      = 0x01c29800,
+    [AW_R40_DEV_UART7]      = 0x01c29c00,
+    [AW_R40_DEV_TWI0]       = 0x01c2ac00,
+    [AW_R40_DEV_GMAC]       = 0x01c50000,
+    [AW_R40_DEV_DRAMCOM]    = 0x01c62000,
+    [AW_R40_DEV_DRAMCTL]    = 0x01c63000,
+    [AW_R40_DEV_DRAMPHY]    = 0x01c65000,
+    [AW_R40_DEV_GIC_DIST]   = 0x01c81000,
+    [AW_R40_DEV_GIC_CPU]    = 0x01c82000,
+    [AW_R40_DEV_GIC_HYP]    = 0x01c84000,
+    [AW_R40_DEV_GIC_VCPU]   = 0x01c86000,
+    [AW_R40_DEV_SDRAM]      = 0x40000000
+};
+
+/* List of unimplemented devices */
+struct AwR40Unimplemented {
+    const char *device_name;
+    hwaddr base;
+    hwaddr size;
+};
+
+static struct AwR40Unimplemented r40_unimplemented[] = {
+    { "d-engine",   0x01000000, 4 * MiB },
+    { "d-inter",    0x01400000, 128 * KiB },
+    { "dma",        0x01c02000, 4 * KiB },
+    { "nfdc",       0x01c03000, 4 * KiB },
+    { "ts",         0x01c04000, 4 * KiB },
+    { "spi0",       0x01c05000, 4 * KiB },
+    { "spi1",       0x01c06000, 4 * KiB },
+    { "cs0",        0x01c09000, 4 * KiB },
+    { "keymem",     0x01c0a000, 4 * KiB },
+    { "usb0-otg",   0x01c13000, 4 * KiB },
+    { "usb0-host",  0x01c14000, 4 * KiB },
+    { "crypto",     0x01c15000, 4 * KiB },
+    { "spi2",       0x01c17000, 4 * KiB },
+    { "sata",       0x01c18000, 4 * KiB },
+    { "usb1-host",  0x01c19000, 4 * KiB },
+    { "sid",        0x01c1b000, 4 * KiB },
+    { "usb2-host",  0x01c1c000, 4 * KiB },
+    { "cs1",        0x01c1d000, 4 * KiB },
+    { "spi3",       0x01c1f000, 4 * KiB },
+    { "rtc",        0x01c20400, 1 * KiB },
+    { "pio",        0x01c20800, 1 * KiB },
+    { "owa",        0x01c21000, 1 * KiB },
+    { "ac97",       0x01c21400, 1 * KiB },
+    { "cir0",       0x01c21800, 1 * KiB },
+    { "cir1",       0x01c21c00, 1 * KiB },
+    { "pcm0",       0x01c22000, 1 * KiB },
+    { "pcm1",       0x01c22400, 1 * KiB },
+    { "pcm2",       0x01c22800, 1 * KiB },
+    { "audio",      0x01c22c00, 1 * KiB },
+    { "keypad",     0x01c23000, 1 * KiB },
+    { "pwm",        0x01c23400, 1 * KiB },
+    { "keyadc",     0x01c24400, 1 * KiB },
+    { "ths",        0x01c24c00, 1 * KiB },
+    { "rtp",        0x01c25000, 1 * KiB },
+    { "pmu",        0x01c25400, 1 * KiB },
+    { "cpu-cfg",    0x01c25c00, 1 * KiB },
+    { "uart0",      0x01c28000, 1 * KiB },
+    { "uart1",      0x01c28400, 1 * KiB },
+    { "uart2",      0x01c28800, 1 * KiB },
+    { "uart3",      0x01c28c00, 1 * KiB },
+    { "uart4",      0x01c29000, 1 * KiB },
+    { "uart5",      0x01c29400, 1 * KiB },
+    { "uart6",      0x01c29800, 1 * KiB },
+    { "uart7",      0x01c29c00, 1 * KiB },
+    { "ps20",       0x01c2a000, 1 * KiB },
+    { "ps21",       0x01c2a400, 1 * KiB },
+    { "twi1",       0x01c2b000, 1 * KiB },
+    { "twi2",       0x01c2b400, 1 * KiB },
+    { "twi3",       0x01c2b800, 1 * KiB },
+    { "twi4",       0x01c2c000, 1 * KiB },
+    { "scr",        0x01c2c400, 1 * KiB },
+    { "tvd-top",    0x01c30000, 4 * KiB },
+    { "tvd0",       0x01c31000, 4 * KiB },
+    { "tvd1",       0x01c32000, 4 * KiB },
+    { "tvd2",       0x01c33000, 4 * KiB },
+    { "tvd3",       0x01c34000, 4 * KiB },
+    { "gpu",        0x01c40000, 64 * KiB },
+    { "hstmr",      0x01c60000, 4 * KiB },
+    { "tcon-top",   0x01c70000, 4 * KiB },
+    { "lcd0",       0x01c71000, 4 * KiB },
+    { "lcd1",       0x01c72000, 4 * KiB },
+    { "tv0",        0x01c73000, 4 * KiB },
+    { "tv1",        0x01c74000, 4 * KiB },
+    { "tve-top",    0x01c90000, 16 * KiB },
+    { "tve0",       0x01c94000, 16 * KiB },
+    { "tve1",       0x01c98000, 16 * KiB },
+    { "mipi_dsi",   0x01ca0000, 4 * KiB },
+    { "mipi_dphy",  0x01ca1000, 4 * KiB },
+    { "ve",         0x01d00000, 1024 * KiB },
+    { "mp",         0x01e80000, 128 * KiB },
+    { "hdmi",       0x01ee0000, 128 * KiB },
+    { "prcm",       0x01f01400, 1 * KiB },
+    { "debug",      0x3f500000, 64 * KiB },
+    { "cpubist",    0x3f501000, 4 * KiB },
+    { "dcu",        0x3fff0000, 64 * KiB },
+    { "hstmr",      0x01c60000, 4 * KiB },
+    { "brom",       0xffff0000, 36 * KiB }
+};
+
+/* Per Processor Interrupts */
+enum {
+    AW_R40_GIC_PPI_MAINT     =  9,
+    AW_R40_GIC_PPI_HYPTIMER  = 10,
+    AW_R40_GIC_PPI_VIRTTIMER = 11,
+    AW_R40_GIC_PPI_SECTIMER  = 13,
+    AW_R40_GIC_PPI_PHYSTIMER = 14
+};
+
+/* Shared Processor Interrupts */
+enum {
+    AW_R40_GIC_SPI_UART0     =  1,
+    AW_R40_GIC_SPI_UART1     =  2,
+    AW_R40_GIC_SPI_UART2     =  3,
+    AW_R40_GIC_SPI_UART3     =  4,
+    AW_R40_GIC_SPI_TWI0      =  7,
+    AW_R40_GIC_SPI_UART4     = 17,
+    AW_R40_GIC_SPI_UART5     = 18,
+    AW_R40_GIC_SPI_UART6     = 19,
+    AW_R40_GIC_SPI_UART7     = 20,
+    AW_R40_GIC_SPI_TIMER0    = 22,
+    AW_R40_GIC_SPI_TIMER1    = 23,
+    AW_R40_GIC_SPI_MMC0      = 32,
+    AW_R40_GIC_SPI_MMC1      = 33,
+    AW_R40_GIC_SPI_MMC2      = 34,
+    AW_R40_GIC_SPI_MMC3      = 35,
+    AW_R40_GIC_SPI_EMAC      = 55,
+    AW_R40_GIC_SPI_GMAC      = 85,
+};
+
+/* Allwinner R40 general constants */
+enum {
+    AW_R40_GIC_NUM_SPI       = 128
+};
+
+#define BOOT0_MAGIC             "eGON.BT0"
+
+/* The low 8-bits of the 'boot_media' field in the SPL header */
+#define SUNXI_BOOTED_FROM_MMC0  0
+#define SUNXI_BOOTED_FROM_NAND  1
+#define SUNXI_BOOTED_FROM_MMC2  2
+#define SUNXI_BOOTED_FROM_SPI   3
+
+struct boot_file_head {
+    uint32_t            b_instruction;
+    uint8_t             magic[8];
+    uint32_t            check_sum;
+    uint32_t            length;
+    uint32_t            pub_head_size;
+    uint32_t            fel_script_address;
+    uint32_t            fel_uEnv_length;
+    uint32_t            dt_name_offset;
+    uint32_t            dram_size;
+    uint32_t            boot_media;
+    uint32_t            string_pool[13];
+};
+
+bool allwinner_r40_bootrom_setup(AwR40State *s, BlockBackend *blk, int unit)
+{
+    const int64_t rom_size = 32 * KiB;
+    g_autofree uint8_t *buffer = g_new0(uint8_t, rom_size);
+    struct boot_file_head *head = (struct boot_file_head *)buffer;
+
+    if (blk_pread(blk, 8 * KiB, rom_size, buffer, 0) < 0) {
+        error_setg(&error_fatal, "%s: failed to read BlockBackend data",
+                   __func__);
+        return false;
+    }
+
+    /* we only check the magic string here. */
+    if (memcmp(head->magic, BOOT0_MAGIC, sizeof(head->magic))) {
+        return false;
+    }
+
+    /*
+     * Simulate the behavior of the bootROM, it will change the boot_media
+     * flag to indicate where the chip is booting from. R40 can boot from
+     * mmc0 or mmc2, the default value of boot_media is zero
+     * (SUNXI_BOOTED_FROM_MMC0), let's fix this flag when it is booting from
+     * the others.
+     */
+    if (unit == 2) {
+        head->boot_media = cpu_to_le32(SUNXI_BOOTED_FROM_MMC2);
+    } else {
+        head->boot_media = cpu_to_le32(SUNXI_BOOTED_FROM_MMC0);
+    }
+
+    rom_add_blob("allwinner-r40.bootrom", buffer, rom_size,
+                  rom_size, s->memmap[AW_R40_DEV_SRAM_A1],
+                  NULL, NULL, NULL, NULL, false);
+    return true;
+}
+
+static void allwinner_r40_init(Object *obj)
+{
+    static const char *mmc_names[AW_R40_NUM_MMCS] = {
+        "mmc0", "mmc1", "mmc2", "mmc3"
+    };
+    AwR40State *s = AW_R40(obj);
+
+    s->memmap = allwinner_r40_memmap;
+
+    for (int i = 0; i < AW_R40_NUM_CPUS; i++) {
+        object_initialize_child(obj, "cpu[*]", &s->cpus[i],
+                                ARM_CPU_TYPE_NAME("cortex-a7"));
+    }
+
+    object_initialize_child(obj, "gic", &s->gic, TYPE_ARM_GIC);
+
+    object_initialize_child(obj, "timer", &s->timer, TYPE_AW_A10_PIT);
+    object_property_add_alias(obj, "clk0-freq", OBJECT(&s->timer),
+                              "clk0-freq");
+    object_property_add_alias(obj, "clk1-freq", OBJECT(&s->timer),
+                              "clk1-freq");
+
+    object_initialize_child(obj, "ccu", &s->ccu, TYPE_AW_R40_CCU);
+
+    for (int i = 0; i < AW_R40_NUM_MMCS; i++) {
+        object_initialize_child(obj, mmc_names[i], &s->mmc[i],
+                                TYPE_AW_SDHOST_SUN50I_A64);
+    }
+
+    object_initialize_child(obj, "twi0", &s->i2c0, TYPE_AW_I2C_SUN6I);
+
+    object_initialize_child(obj, "emac", &s->emac, TYPE_AW_EMAC);
+    object_initialize_child(obj, "gmac", &s->gmac, TYPE_AW_SUN8I_EMAC);
+    object_property_add_alias(obj, "gmac-phy-addr",
+                              OBJECT(&s->gmac), "phy-addr");
+
+    object_initialize_child(obj, "dramc", &s->dramc, TYPE_AW_R40_DRAMC);
+    object_property_add_alias(obj, "ram-addr", OBJECT(&s->dramc),
+                             "ram-addr");
+    object_property_add_alias(obj, "ram-size", OBJECT(&s->dramc),
+                              "ram-size");
+
+    object_initialize_child(obj, "sramc", &s->sramc, TYPE_AW_SRAMC_SUN8I_R40);
+}
+
+static void allwinner_r40_realize(DeviceState *dev, Error **errp)
+{
+    const char *r40_nic_models[] = { "gmac", "emac", NULL };
+    AwR40State *s = AW_R40(dev);
+    unsigned i;
+
+    /* CPUs */
+    for (i = 0; i < AW_R40_NUM_CPUS; i++) {
+
+        /*
+         * Disable secondary CPUs. Guest EL3 firmware will start
+         * them via CPU reset control registers.
+         */
+        qdev_prop_set_bit(DEVICE(&s->cpus[i]), "start-powered-off",
+                          i > 0);
+
+        /* All exception levels required */
+        qdev_prop_set_bit(DEVICE(&s->cpus[i]), "has_el3", true);
+        qdev_prop_set_bit(DEVICE(&s->cpus[i]), "has_el2", true);
+
+        /* Mark realized */
+        qdev_realize(DEVICE(&s->cpus[i]), NULL, &error_fatal);
+    }
+
+    /* Generic Interrupt Controller */
+    qdev_prop_set_uint32(DEVICE(&s->gic), "num-irq", AW_R40_GIC_NUM_SPI +
+                                                     GIC_INTERNAL);
+    qdev_prop_set_uint32(DEVICE(&s->gic), "revision", 2);
+    qdev_prop_set_uint32(DEVICE(&s->gic), "num-cpu", AW_R40_NUM_CPUS);
+    qdev_prop_set_bit(DEVICE(&s->gic), "has-security-extensions", false);
+    qdev_prop_set_bit(DEVICE(&s->gic), "has-virtualization-extensions", true);
+    sysbus_realize(SYS_BUS_DEVICE(&s->gic), &error_fatal);
+
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->gic), 0, s->memmap[AW_R40_DEV_GIC_DIST]);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->gic), 1, s->memmap[AW_R40_DEV_GIC_CPU]);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->gic), 2, s->memmap[AW_R40_DEV_GIC_HYP]);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->gic), 3, s->memmap[AW_R40_DEV_GIC_VCPU]);
+
+    /*
+     * Wire the outputs from each CPU's generic timer and the GICv2
+     * maintenance interrupt signal to the appropriate GIC PPI inputs,
+     * and the GIC's IRQ/FIQ/VIRQ/VFIQ interrupt outputs to the CPU's inputs.
+     */
+    for (i = 0; i < AW_R40_NUM_CPUS; i++) {
+        DeviceState *cpudev = DEVICE(&s->cpus[i]);
+        int ppibase = AW_R40_GIC_NUM_SPI + i * GIC_INTERNAL + GIC_NR_SGIS;
+        int irq;
+        /*
+         * Mapping from the output timer irq lines from the CPU to the
+         * GIC PPI inputs used for this board.
+         */
+        const int timer_irq[] = {
+            [GTIMER_PHYS] = AW_R40_GIC_PPI_PHYSTIMER,
+            [GTIMER_VIRT] = AW_R40_GIC_PPI_VIRTTIMER,
+            [GTIMER_HYP]  = AW_R40_GIC_PPI_HYPTIMER,
+            [GTIMER_SEC]  = AW_R40_GIC_PPI_SECTIMER,
+        };
+
+        /* Connect CPU timer outputs to GIC PPI inputs */
+        for (irq = 0; irq < ARRAY_SIZE(timer_irq); irq++) {
+            qdev_connect_gpio_out(cpudev, irq,
+                                  qdev_get_gpio_in(DEVICE(&s->gic),
+                                                   ppibase + timer_irq[irq]));
+        }
+
+        /* Connect GIC outputs to CPU interrupt inputs */
+        sysbus_connect_irq(SYS_BUS_DEVICE(&s->gic), i,
+                           qdev_get_gpio_in(cpudev, ARM_CPU_IRQ));
+        sysbus_connect_irq(SYS_BUS_DEVICE(&s->gic), i + AW_R40_NUM_CPUS,
+                           qdev_get_gpio_in(cpudev, ARM_CPU_FIQ));
+        sysbus_connect_irq(SYS_BUS_DEVICE(&s->gic), i + (2 * AW_R40_NUM_CPUS),
+                           qdev_get_gpio_in(cpudev, ARM_CPU_VIRQ));
+        sysbus_connect_irq(SYS_BUS_DEVICE(&s->gic), i + (3 * AW_R40_NUM_CPUS),
+                           qdev_get_gpio_in(cpudev, ARM_CPU_VFIQ));
+
+        /* GIC maintenance signal */
+        sysbus_connect_irq(SYS_BUS_DEVICE(&s->gic), i + (4 * AW_R40_NUM_CPUS),
+                           qdev_get_gpio_in(DEVICE(&s->gic),
+                                            ppibase + AW_R40_GIC_PPI_MAINT));
+    }
+
+    /* Timer */
+    sysbus_realize(SYS_BUS_DEVICE(&s->timer), &error_fatal);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->timer), 0, s->memmap[AW_R40_DEV_PIT]);
+    sysbus_connect_irq(SYS_BUS_DEVICE(&s->timer), 0,
+                       qdev_get_gpio_in(DEVICE(&s->gic),
+                       AW_R40_GIC_SPI_TIMER0));
+    sysbus_connect_irq(SYS_BUS_DEVICE(&s->timer), 1,
+                       qdev_get_gpio_in(DEVICE(&s->gic),
+                       AW_R40_GIC_SPI_TIMER1));
+
+    /* SRAM */
+    sysbus_realize(SYS_BUS_DEVICE(&s->sramc), &error_fatal);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->sramc), 0, s->memmap[AW_R40_DEV_SRAMC]);
+
+    memory_region_init_ram(&s->sram_a1, OBJECT(dev), "sram A1",
+                            16 * KiB, &error_abort);
+    memory_region_init_ram(&s->sram_a2, OBJECT(dev), "sram A2",
+                            16 * KiB, &error_abort);
+    memory_region_init_ram(&s->sram_a3, OBJECT(dev), "sram A3",
+                            13 * KiB, &error_abort);
+    memory_region_init_ram(&s->sram_a4, OBJECT(dev), "sram A4",
+                            3 * KiB, &error_abort);
+    memory_region_add_subregion(get_system_memory(),
+                                s->memmap[AW_R40_DEV_SRAM_A1], &s->sram_a1);
+    memory_region_add_subregion(get_system_memory(),
+                                s->memmap[AW_R40_DEV_SRAM_A2], &s->sram_a2);
+    memory_region_add_subregion(get_system_memory(),
+                                s->memmap[AW_R40_DEV_SRAM_A3], &s->sram_a3);
+    memory_region_add_subregion(get_system_memory(),
+                                s->memmap[AW_R40_DEV_SRAM_A4], &s->sram_a4);
+
+    /* Clock Control Unit */
+    sysbus_realize(SYS_BUS_DEVICE(&s->ccu), &error_fatal);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->ccu), 0, s->memmap[AW_R40_DEV_CCU]);
+
+    /* SD/MMC */
+    for (int i = 0; i < AW_R40_NUM_MMCS; i++) {
+        qemu_irq irq = qdev_get_gpio_in(DEVICE(&s->gic),
+                                        AW_R40_GIC_SPI_MMC0 + i);
+        const hwaddr addr = s->memmap[AW_R40_DEV_MMC0 + i];
+
+        object_property_set_link(OBJECT(&s->mmc[i]), "dma-memory",
+                                 OBJECT(get_system_memory()), &error_fatal);
+        sysbus_realize(SYS_BUS_DEVICE(&s->mmc[i]), &error_fatal);
+        sysbus_mmio_map(SYS_BUS_DEVICE(&s->mmc[i]), 0, addr);
+        sysbus_connect_irq(SYS_BUS_DEVICE(&s->mmc[i]), 0, irq);
+    }
+
+    /* UART0. For future clocktree API: All UARTS are connected to APB2_CLK. */
+    for (int i = 0; i < AW_R40_NUM_UARTS; i++) {
+        static const int uart_irqs[AW_R40_NUM_UARTS] = {
+            AW_R40_GIC_SPI_UART0,
+            AW_R40_GIC_SPI_UART1,
+            AW_R40_GIC_SPI_UART2,
+            AW_R40_GIC_SPI_UART3,
+            AW_R40_GIC_SPI_UART4,
+            AW_R40_GIC_SPI_UART5,
+            AW_R40_GIC_SPI_UART6,
+            AW_R40_GIC_SPI_UART7,
+        };
+        const hwaddr addr = s->memmap[AW_R40_DEV_UART0 + i];
+
+        serial_mm_init(get_system_memory(), addr, 2,
+                       qdev_get_gpio_in(DEVICE(&s->gic), uart_irqs[i]),
+                       115200, serial_hd(i), DEVICE_NATIVE_ENDIAN);
+    }
+
+    /* I2C */
+    sysbus_realize(SYS_BUS_DEVICE(&s->i2c0), &error_fatal);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->i2c0), 0, s->memmap[AW_R40_DEV_TWI0]);
+    sysbus_connect_irq(SYS_BUS_DEVICE(&s->i2c0), 0,
+                       qdev_get_gpio_in(DEVICE(&s->gic), AW_R40_GIC_SPI_TWI0));
+
+    /* DRAMC */
+    sysbus_realize(SYS_BUS_DEVICE(&s->dramc), &error_fatal);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->dramc), 0,
+                    s->memmap[AW_R40_DEV_DRAMCOM]);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->dramc), 1,
+                    s->memmap[AW_R40_DEV_DRAMCTL]);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->dramc), 2,
+                    s->memmap[AW_R40_DEV_DRAMPHY]);
+
+    /* nic support gmac and emac */
+    for (int i = 0; i < ARRAY_SIZE(r40_nic_models) - 1; i++) {
+        NICInfo *nic = &nd_table[i];
+
+        if (!nic->used) {
+            continue;
+        }
+        if (qemu_show_nic_models(nic->model, r40_nic_models)) {
+            exit(0);
+        }
+
+        switch (qemu_find_nic_model(nic, r40_nic_models, r40_nic_models[0])) {
+        case 0: /* gmac */
+            qdev_set_nic_properties(DEVICE(&s->gmac), nic);
+            break;
+        case 1: /* emac */
+            qdev_set_nic_properties(DEVICE(&s->emac), nic);
+            break;
+        default:
+            exit(1);
+            break;
+        }
+    }
+
+    /* GMAC */
+    object_property_set_link(OBJECT(&s->gmac), "dma-memory",
+                                     OBJECT(get_system_memory()), &error_fatal);
+    sysbus_realize(SYS_BUS_DEVICE(&s->gmac), &error_fatal);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->gmac), 0, s->memmap[AW_R40_DEV_GMAC]);
+    sysbus_connect_irq(SYS_BUS_DEVICE(&s->gmac), 0,
+                       qdev_get_gpio_in(DEVICE(&s->gic), AW_R40_GIC_SPI_GMAC));
+
+    /* EMAC */
+    sysbus_realize(SYS_BUS_DEVICE(&s->emac), &error_fatal);
+    sysbus_mmio_map(SYS_BUS_DEVICE(&s->emac), 0, s->memmap[AW_R40_DEV_EMAC]);
+    sysbus_connect_irq(SYS_BUS_DEVICE(&s->emac), 0,
+                       qdev_get_gpio_in(DEVICE(&s->gic), AW_R40_GIC_SPI_EMAC));
+
+    /* Unimplemented devices */
+    for (i = 0; i < ARRAY_SIZE(r40_unimplemented); i++) {
+        create_unimplemented_device(r40_unimplemented[i].device_name,
+                                    r40_unimplemented[i].base,
+                                    r40_unimplemented[i].size);
+    }
+}
+
+static void allwinner_r40_class_init(ObjectClass *oc, void *data)
+{
+    DeviceClass *dc = DEVICE_CLASS(oc);
+
+    dc->realize = allwinner_r40_realize;
+    /* Reason: uses serial_hd() in realize function */
+    dc->user_creatable = false;
+}
+
+static const TypeInfo allwinner_r40_type_info = {
+    .name = TYPE_AW_R40,
+    .parent = TYPE_DEVICE,
+    .instance_size = sizeof(AwR40State),
+    .instance_init = allwinner_r40_init,
+    .class_init = allwinner_r40_class_init,
+};
+
+static void allwinner_r40_register_types(void)
+{
+    type_register_static(&allwinner_r40_type_info);
+}
+
+type_init(allwinner_r40_register_types)
diff --git a/hw/arm/bananapi_m2u.c b/hw/arm/bananapi_m2u.c
new file mode 100644
index 0000000..74121d8
--- /dev/null
+++ b/hw/arm/bananapi_m2u.c
@@ -0,0 +1,145 @@
+/*
+ * Bananapi M2U emulation
+ *
+ * Copyright (C) 2023 qianfan Zhao <qianfanguijin@163.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "qemu/osdep.h"
+#include "qemu/units.h"
+#include "exec/address-spaces.h"
+#include "qapi/error.h"
+#include "qemu/error-report.h"
+#include "hw/boards.h"
+#include "hw/i2c/i2c.h"
+#include "hw/qdev-properties.h"
+#include "hw/arm/allwinner-r40.h"
+
+static struct arm_boot_info bpim2u_binfo;
+
+/*
+ * R40 can boot from mmc0 and mmc2, and bpim2u has two mmc interface, one is
+ * connected to sdcard and another mount an emmc media.
+ * Attach the mmc driver and try loading bootloader.
+ */
+static void mmc_attach_drive(AwR40State *s, AwSdHostState *mmc, int unit,
+                             bool load_bootroom, bool *bootroom_loaded)
+{
+    DriveInfo *di = drive_get(IF_SD, 0, unit);
+    BlockBackend *blk = di ? blk_by_legacy_dinfo(di) : NULL;
+    BusState *bus;
+    DeviceState *carddev;
+
+    bus = qdev_get_child_bus(DEVICE(mmc), "sd-bus");
+    if (bus == NULL) {
+        error_report("No SD bus found in SOC object");
+        exit(1);
+    }
+
+    carddev = qdev_new(TYPE_SD_CARD);
+    qdev_prop_set_drive_err(carddev, "drive", blk, &error_fatal);
+    qdev_realize_and_unref(carddev, bus, &error_fatal);
+
+    if (load_bootroom && blk && blk_is_available(blk)) {
+        /* Use Boot ROM to copy data from SD card to SRAM */
+        *bootroom_loaded = allwinner_r40_bootrom_setup(s, blk, unit);
+    }
+}
+
+static void bpim2u_init(MachineState *machine)
+{
+    bool bootroom_loaded = false;
+    AwR40State *r40;
+    I2CBus *i2c;
+
+    /* BIOS is not supported by this board */
+    if (machine->firmware) {
+        error_report("BIOS not supported for this machine");
+        exit(1);
+    }
+
+    /* Only allow Cortex-A7 for this board */
+    if (strcmp(machine->cpu_type, ARM_CPU_TYPE_NAME("cortex-a7")) != 0) {
+        error_report("This board can only be used with cortex-a7 CPU");
+        exit(1);
+    }
+
+    r40 = AW_R40(object_new(TYPE_AW_R40));
+    object_property_add_child(OBJECT(machine), "soc", OBJECT(r40));
+    object_unref(OBJECT(r40));
+
+    /* Setup timer properties */
+    object_property_set_int(OBJECT(r40), "clk0-freq", 32768, &error_abort);
+    object_property_set_int(OBJECT(r40), "clk1-freq", 24 * 1000 * 1000,
+                            &error_abort);
+
+    /* DRAMC */
+    r40->ram_size = machine->ram_size / MiB;
+    object_property_set_uint(OBJECT(r40), "ram-addr",
+                             r40->memmap[AW_R40_DEV_SDRAM], &error_abort);
+    object_property_set_int(OBJECT(r40), "ram-size",
+                            r40->ram_size, &error_abort);
+
+    /* GMAC PHY */
+    object_property_set_uint(OBJECT(r40), "gmac-phy-addr", 1, &error_abort);
+
+    /* Mark R40 object realized */
+    qdev_realize(DEVICE(r40), NULL, &error_abort);
+
+    /*
+     * Plug in SD card and try load bootrom, R40 has 4 mmc controllers but can
+     * only booting from mmc0 and mmc2.
+     */
+    for (int i = 0; i < AW_R40_NUM_MMCS; i++) {
+        switch (i) {
+        case 0:
+        case 2:
+            mmc_attach_drive(r40, &r40->mmc[i], i,
+                             !machine->kernel_filename && !bootroom_loaded,
+                             &bootroom_loaded);
+            break;
+        default:
+            mmc_attach_drive(r40, &r40->mmc[i], i, false, NULL);
+            break;
+        }
+    }
+
+    /* Connect AXP221 */
+    i2c = I2C_BUS(qdev_get_child_bus(DEVICE(&r40->i2c0), "i2c"));
+    i2c_slave_create_simple(i2c, "axp221_pmu", 0x34);
+
+    /* SDRAM */
+    memory_region_add_subregion(get_system_memory(),
+                                r40->memmap[AW_R40_DEV_SDRAM], machine->ram);
+
+    bpim2u_binfo.loader_start = r40->memmap[AW_R40_DEV_SDRAM];
+    bpim2u_binfo.ram_size = machine->ram_size;
+    bpim2u_binfo.psci_conduit = QEMU_PSCI_CONDUIT_SMC;
+    arm_load_kernel(ARM_CPU(first_cpu), machine, &bpim2u_binfo);
+}
+
+static void bpim2u_machine_init(MachineClass *mc)
+{
+    mc->desc = "Bananapi M2U (Cortex-A7)";
+    mc->init = bpim2u_init;
+    mc->min_cpus = AW_R40_NUM_CPUS;
+    mc->max_cpus = AW_R40_NUM_CPUS;
+    mc->default_cpus = AW_R40_NUM_CPUS;
+    mc->default_cpu_type = ARM_CPU_TYPE_NAME("cortex-a7");
+    mc->default_ram_size = 1 * GiB;
+    mc->default_ram_id = "bpim2u.ram";
+}
+
+DEFINE_MACHINE("bpim2u", bpim2u_machine_init)
diff --git a/hw/arm/meson.build b/hw/arm/meson.build
index b545ba0..870ec67 100644
--- a/hw/arm/meson.build
+++ b/hw/arm/meson.build
@@ -37,6 +37,7 @@ arm_ss.add(when: 'CONFIG_OMAP', if_true: files('omap1.c', 'omap2.c'))
 arm_ss.add(when: 'CONFIG_STRONGARM', if_true: files('strongarm.c'))
 arm_ss.add(when: 'CONFIG_ALLWINNER_A10', if_true: files('allwinner-a10.c', 'cubieboard.c'))
 arm_ss.add(when: 'CONFIG_ALLWINNER_H3', if_true: files('allwinner-h3.c', 'orangepi.c'))
+arm_ss.add(when: 'CONFIG_ALLWINNER_R40', if_true: files('allwinner-r40.c', 'bananapi_m2u.c'))
 arm_ss.add(when: 'CONFIG_RASPI', if_true: files('bcm2836.c', 'raspi.c'))
 arm_ss.add(when: 'CONFIG_STM32F100_SOC', if_true: files('stm32f100_soc.c'))
 arm_ss.add(when: 'CONFIG_STM32F205_SOC', if_true: files('stm32f205_soc.c'))
diff --git a/hw/arm/xlnx-versal-virt.c b/hw/arm/xlnx-versal-virt.c
index 668a9d6..1ee2b86 100644
--- a/hw/arm/xlnx-versal-virt.c
+++ b/hw/arm/xlnx-versal-virt.c
@@ -40,9 +40,11 @@ struct VersalVirt {
         uint32_t clk_25Mhz;
         uint32_t usb;
         uint32_t dwc;
+        uint32_t canfd[2];
     } phandle;
     struct arm_boot_info binfo;
 
+    CanBusState *canbus[XLNX_VERSAL_NR_CANFD];
     struct {
         bool secure;
     } cfg;
@@ -235,6 +237,38 @@ static void fdt_add_uart_nodes(VersalVirt *s)
     }
 }
 
+static void fdt_add_canfd_nodes(VersalVirt *s)
+{
+    uint64_t addrs[] = { MM_CANFD1, MM_CANFD0 };
+    uint32_t size[] = { MM_CANFD1_SIZE, MM_CANFD0_SIZE };
+    unsigned int irqs[] = { VERSAL_CANFD1_IRQ_0, VERSAL_CANFD0_IRQ_0 };
+    const char clocknames[] = "can_clk\0s_axi_aclk";
+    int i;
+
+    /* Create and connect CANFD0 and CANFD1 nodes to canbus0. */
+    for (i = 0; i < ARRAY_SIZE(addrs); i++) {
+        char *name = g_strdup_printf("/canfd@%" PRIx64, addrs[i]);
+        qemu_fdt_add_subnode(s->fdt, name);
+
+        qemu_fdt_setprop_cell(s->fdt, name, "rx-fifo-depth", 0x40);
+        qemu_fdt_setprop_cell(s->fdt, name, "tx-mailbox-count", 0x20);
+
+        qemu_fdt_setprop_cells(s->fdt, name, "clocks",
+                               s->phandle.clk_25Mhz, s->phandle.clk_25Mhz);
+        qemu_fdt_setprop(s->fdt, name, "clock-names",
+                         clocknames, sizeof(clocknames));
+        qemu_fdt_setprop_cells(s->fdt, name, "interrupts",
+                               GIC_FDT_IRQ_TYPE_SPI, irqs[i],
+                               GIC_FDT_IRQ_FLAGS_LEVEL_HI);
+        qemu_fdt_setprop_sized_cells(s->fdt, name, "reg",
+                                     2, addrs[i], 2, size[i]);
+        qemu_fdt_setprop_string(s->fdt, name, "compatible",
+                                "xlnx,canfd-2.0");
+
+        g_free(name);
+    }
+}
+
 static void fdt_add_fixed_link_nodes(VersalVirt *s, char *gemname,
                                      uint32_t phandle)
 {
@@ -639,12 +673,17 @@ static void versal_virt_init(MachineState *machine)
                             TYPE_XLNX_VERSAL);
     object_property_set_link(OBJECT(&s->soc), "ddr", OBJECT(machine->ram),
                              &error_abort);
+    object_property_set_link(OBJECT(&s->soc), "canbus0", OBJECT(s->canbus[0]),
+                             &error_abort);
+    object_property_set_link(OBJECT(&s->soc), "canbus1", OBJECT(s->canbus[1]),
+                             &error_abort);
     sysbus_realize(SYS_BUS_DEVICE(&s->soc), &error_fatal);
 
     fdt_create(s);
     create_virtio_regions(s);
     fdt_add_gem_nodes(s);
     fdt_add_uart_nodes(s);
+    fdt_add_canfd_nodes(s);
     fdt_add_gic_nodes(s);
     fdt_add_timer_nodes(s);
     fdt_add_zdma_nodes(s);
@@ -712,6 +751,20 @@ static void versal_virt_init(MachineState *machine)
 
 static void versal_virt_machine_instance_init(Object *obj)
 {
+    VersalVirt *s = XLNX_VERSAL_VIRT_MACHINE(obj);
+
+    /*
+     * User can set canbus0 and canbus1 properties to can-bus object and connect
+     * to socketcan(optional) interface via command line.
+     */
+    object_property_add_link(obj, "canbus0", TYPE_CAN_BUS,
+                             (Object **)&s->canbus[0],
+                             object_property_allow_set_link,
+                             0);
+    object_property_add_link(obj, "canbus1", TYPE_CAN_BUS,
+                             (Object **)&s->canbus[1],
+                             object_property_allow_set_link,
+                             0);
 }
 
 static void versal_virt_machine_class_init(ObjectClass *oc, void *data)
diff --git a/hw/arm/xlnx-versal.c b/hw/arm/xlnx-versal.c
index db1e0de..60bf5fe 100644
--- a/hw/arm/xlnx-versal.c
+++ b/hw/arm/xlnx-versal.c
@@ -184,6 +184,38 @@ static void versal_create_uarts(Versal *s, qemu_irq *pic)
     }
 }
 
+static void versal_create_canfds(Versal *s, qemu_irq *pic)
+{
+    int i;
+    uint32_t irqs[] = { VERSAL_CANFD0_IRQ_0, VERSAL_CANFD1_IRQ_0};
+    uint64_t addrs[] = { MM_CANFD0, MM_CANFD1 };
+
+    for (i = 0; i < ARRAY_SIZE(s->lpd.iou.canfd); i++) {
+        char *name = g_strdup_printf("canfd%d", i);
+        SysBusDevice *sbd;
+        MemoryRegion *mr;
+
+        object_initialize_child(OBJECT(s), name, &s->lpd.iou.canfd[i],
+                                TYPE_XILINX_CANFD);
+        sbd = SYS_BUS_DEVICE(&s->lpd.iou.canfd[i]);
+
+        object_property_set_int(OBJECT(&s->lpd.iou.canfd[i]), "ext_clk_freq",
+                                XLNX_VERSAL_CANFD_REF_CLK , &error_abort);
+
+        object_property_set_link(OBJECT(&s->lpd.iou.canfd[i]), "canfdbus",
+                                 OBJECT(s->lpd.iou.canbus[i]),
+                                 &error_abort);
+
+        sysbus_realize(sbd, &error_fatal);
+
+        mr = sysbus_mmio_get_region(sbd, 0);
+        memory_region_add_subregion(&s->mr_ps, addrs[i], mr);
+
+        sysbus_connect_irq(sbd, 0, pic[irqs[i]]);
+        g_free(name);
+    }
+}
+
 static void versal_create_usbs(Versal *s, qemu_irq *pic)
 {
     DeviceState *dev;
@@ -718,6 +750,7 @@ static void versal_realize(DeviceState *dev, Error **errp)
     versal_create_apu_gic(s, pic);
     versal_create_rpu_cpus(s);
     versal_create_uarts(s, pic);
+    versal_create_canfds(s, pic);
     versal_create_usbs(s, pic);
     versal_create_gems(s, pic);
     versal_create_admas(s, pic);
@@ -757,6 +790,10 @@ static void versal_init(Object *obj)
 static Property versal_properties[] = {
     DEFINE_PROP_LINK("ddr", Versal, cfg.mr_ddr, TYPE_MEMORY_REGION,
                      MemoryRegion *),
+    DEFINE_PROP_LINK("canbus0", Versal, lpd.iou.canbus[0],
+                      TYPE_CAN_BUS, CanBusState *),
+    DEFINE_PROP_LINK("canbus1", Versal, lpd.iou.canbus[1],
+                      TYPE_CAN_BUS, CanBusState *),
     DEFINE_PROP_END_OF_LIST()
 };
 
diff --git a/hw/misc/Kconfig b/hw/misc/Kconfig
index 2ef5781..e4c2149 100644
--- a/hw/misc/Kconfig
+++ b/hw/misc/Kconfig
@@ -170,13 +170,16 @@ config VIRT_CTRL
 config LASI
     bool
 
+config ALLWINNER_SRAMC
+    bool
+
 config ALLWINNER_A10_CCM
     bool
 
 config ALLWINNER_A10_DRAMC
     bool
 
-config AXP209_PMU
+config AXP2XX_PMU
     bool
     depends on I2C
 
diff --git a/hw/misc/allwinner-r40-ccu.c b/hw/misc/allwinner-r40-ccu.c
new file mode 100644
index 0000000..d82fee1
--- /dev/null
+++ b/hw/misc/allwinner-r40-ccu.c
@@ -0,0 +1,209 @@
+/*
+ * Allwinner R40 Clock Control Unit emulation
+ *
+ * Copyright (C) 2023 qianfan Zhao <qianfanguijin@163.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "qemu/osdep.h"
+#include "qemu/units.h"
+#include "hw/sysbus.h"
+#include "migration/vmstate.h"
+#include "qemu/log.h"
+#include "qemu/module.h"
+#include "hw/misc/allwinner-r40-ccu.h"
+
+/* CCU register offsets */
+enum {
+    REG_PLL_CPUX_CTRL           = 0x0000,
+    REG_PLL_AUDIO_CTRL          = 0x0008,
+    REG_PLL_VIDEO0_CTRL         = 0x0010,
+    REG_PLL_VE_CTRL             = 0x0018,
+    REG_PLL_DDR0_CTRL           = 0x0020,
+    REG_PLL_PERIPH0_CTRL        = 0x0028,
+    REG_PLL_PERIPH1_CTRL        = 0x002c,
+    REG_PLL_VIDEO1_CTRL         = 0x0030,
+    REG_PLL_SATA_CTRL           = 0x0034,
+    REG_PLL_GPU_CTRL            = 0x0038,
+    REG_PLL_MIPI_CTRL           = 0x0040,
+    REG_PLL_DE_CTRL             = 0x0048,
+    REG_PLL_DDR1_CTRL           = 0x004c,
+    REG_AHB1_APB1_CFG           = 0x0054,
+    REG_APB2_CFG                = 0x0058,
+    REG_MMC0_CLK                = 0x0088,
+    REG_MMC1_CLK                = 0x008c,
+    REG_MMC2_CLK                = 0x0090,
+    REG_MMC3_CLK                = 0x0094,
+    REG_USBPHY_CFG              = 0x00cc,
+    REG_PLL_DDR_AUX             = 0x00f0,
+    REG_DRAM_CFG                = 0x00f4,
+    REG_PLL_DDR1_CFG            = 0x00f8,
+    REG_DRAM_CLK_GATING         = 0x0100,
+    REG_GMAC_CLK                = 0x0164,
+    REG_SYS_32K_CLK             = 0x0310,
+    REG_PLL_LOCK_CTRL           = 0x0320,
+};
+
+#define REG_INDEX(offset)    (offset / sizeof(uint32_t))
+
+/* CCU register flags */
+enum {
+    REG_PLL_ENABLE           = (1 << 31),
+    REG_PLL_LOCK             = (1 << 28),
+};
+
+static uint64_t allwinner_r40_ccu_read(void *opaque, hwaddr offset,
+                                       unsigned size)
+{
+    const AwR40ClockCtlState *s = AW_R40_CCU(opaque);
+    const uint32_t idx = REG_INDEX(offset);
+
+    switch (offset) {
+    case 0x324 ... AW_R40_CCU_IOSIZE:
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        return 0;
+    }
+
+    return s->regs[idx];
+}
+
+static void allwinner_r40_ccu_write(void *opaque, hwaddr offset,
+                                    uint64_t val, unsigned size)
+{
+    AwR40ClockCtlState *s = AW_R40_CCU(opaque);
+
+    switch (offset) {
+    case REG_DRAM_CFG:    /* DRAM Configuration(for DDR0) */
+        /* bit16: SDRCLK_UPD (SDRCLK configuration 0 update) */
+        val &= ~(1 << 16);
+        break;
+    case REG_PLL_DDR1_CTRL: /* DDR1 Control register */
+        /* bit30: SDRPLL_UPD */
+        val &= ~(1 << 30);
+        if (val & REG_PLL_ENABLE) {
+            val |= REG_PLL_LOCK;
+        }
+        break;
+    case REG_PLL_CPUX_CTRL:
+    case REG_PLL_AUDIO_CTRL:
+    case REG_PLL_VE_CTRL:
+    case REG_PLL_VIDEO0_CTRL:
+    case REG_PLL_DDR0_CTRL:
+    case REG_PLL_PERIPH0_CTRL:
+    case REG_PLL_PERIPH1_CTRL:
+    case REG_PLL_VIDEO1_CTRL:
+    case REG_PLL_SATA_CTRL:
+    case REG_PLL_GPU_CTRL:
+    case REG_PLL_MIPI_CTRL:
+    case REG_PLL_DE_CTRL:
+        if (val & REG_PLL_ENABLE) {
+            val |= REG_PLL_LOCK;
+        }
+        break;
+    case 0x324 ... AW_R40_CCU_IOSIZE:
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        break;
+    default:
+        qemu_log_mask(LOG_UNIMP, "%s: unimplemented write offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        break;
+    }
+
+    s->regs[REG_INDEX(offset)] = (uint32_t) val;
+}
+
+static const MemoryRegionOps allwinner_r40_ccu_ops = {
+    .read = allwinner_r40_ccu_read,
+    .write = allwinner_r40_ccu_write,
+    .endianness = DEVICE_NATIVE_ENDIAN,
+    .valid = {
+        .min_access_size = 4,
+        .max_access_size = 4,
+    },
+    .impl.min_access_size = 4,
+};
+
+static void allwinner_r40_ccu_reset(DeviceState *dev)
+{
+    AwR40ClockCtlState *s = AW_R40_CCU(dev);
+
+    memset(s->regs, 0, sizeof(s->regs));
+
+    /* Set default values for registers */
+    s->regs[REG_INDEX(REG_PLL_CPUX_CTRL)]       = 0x00001000;
+    s->regs[REG_INDEX(REG_PLL_AUDIO_CTRL)]      = 0x00035514;
+    s->regs[REG_INDEX(REG_PLL_VIDEO0_CTRL)]     = 0x03006207;
+    s->regs[REG_INDEX(REG_PLL_VE_CTRL)]         = 0x03006207;
+    s->regs[REG_INDEX(REG_PLL_DDR0_CTRL)]       = 0x00001000,
+    s->regs[REG_INDEX(REG_PLL_PERIPH0_CTRL)]    = 0x00041811;
+    s->regs[REG_INDEX(REG_PLL_PERIPH1_CTRL)]    = 0x00041811;
+    s->regs[REG_INDEX(REG_PLL_VIDEO1_CTRL)]     = 0x03006207;
+    s->regs[REG_INDEX(REG_PLL_SATA_CTRL)]       = 0x00001811;
+    s->regs[REG_INDEX(REG_PLL_GPU_CTRL)]        = 0x03006207;
+    s->regs[REG_INDEX(REG_PLL_MIPI_CTRL)]       = 0x00000515;
+    s->regs[REG_INDEX(REG_PLL_DE_CTRL)]         = 0x03006207;
+    s->regs[REG_INDEX(REG_PLL_DDR1_CTRL)]       = 0x00001800;
+    s->regs[REG_INDEX(REG_AHB1_APB1_CFG)]       = 0x00001010;
+    s->regs[REG_INDEX(REG_APB2_CFG)]            = 0x01000000;
+    s->regs[REG_INDEX(REG_PLL_DDR_AUX)]         = 0x00000001;
+    s->regs[REG_INDEX(REG_PLL_DDR1_CFG)]        = 0x0ccca000;
+    s->regs[REG_INDEX(REG_SYS_32K_CLK)]         = 0x0000000f;
+}
+
+static void allwinner_r40_ccu_init(Object *obj)
+{
+    SysBusDevice *sbd = SYS_BUS_DEVICE(obj);
+    AwR40ClockCtlState *s = AW_R40_CCU(obj);
+
+    /* Memory mapping */
+    memory_region_init_io(&s->iomem, OBJECT(s), &allwinner_r40_ccu_ops, s,
+                          TYPE_AW_R40_CCU, AW_R40_CCU_IOSIZE);
+    sysbus_init_mmio(sbd, &s->iomem);
+}
+
+static const VMStateDescription allwinner_r40_ccu_vmstate = {
+    .name = "allwinner-r40-ccu",
+    .version_id = 1,
+    .minimum_version_id = 1,
+    .fields = (VMStateField[]) {
+        VMSTATE_UINT32_ARRAY(regs, AwR40ClockCtlState, AW_R40_CCU_REGS_NUM),
+        VMSTATE_END_OF_LIST()
+    }
+};
+
+static void allwinner_r40_ccu_class_init(ObjectClass *klass, void *data)
+{
+    DeviceClass *dc = DEVICE_CLASS(klass);
+
+    dc->reset = allwinner_r40_ccu_reset;
+    dc->vmsd = &allwinner_r40_ccu_vmstate;
+}
+
+static const TypeInfo allwinner_r40_ccu_info = {
+    .name          = TYPE_AW_R40_CCU,
+    .parent        = TYPE_SYS_BUS_DEVICE,
+    .instance_init = allwinner_r40_ccu_init,
+    .instance_size = sizeof(AwR40ClockCtlState),
+    .class_init    = allwinner_r40_ccu_class_init,
+};
+
+static void allwinner_r40_ccu_register(void)
+{
+    type_register_static(&allwinner_r40_ccu_info);
+}
+
+type_init(allwinner_r40_ccu_register)
diff --git a/hw/misc/allwinner-r40-dramc.c b/hw/misc/allwinner-r40-dramc.c
new file mode 100644
index 0000000..ea61247
--- /dev/null
+++ b/hw/misc/allwinner-r40-dramc.c
@@ -0,0 +1,513 @@
+/*
+ * Allwinner R40 SDRAM Controller emulation
+ *
+ * CCopyright (C) 2023 qianfan Zhao <qianfanguijin@163.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "qemu/osdep.h"
+#include "qemu/units.h"
+#include "qemu/error-report.h"
+#include "hw/sysbus.h"
+#include "migration/vmstate.h"
+#include "qemu/log.h"
+#include "qemu/module.h"
+#include "exec/address-spaces.h"
+#include "hw/qdev-properties.h"
+#include "qapi/error.h"
+#include "qemu/bitops.h"
+#include "hw/misc/allwinner-r40-dramc.h"
+#include "trace.h"
+
+#define REG_INDEX(offset)    (offset / sizeof(uint32_t))
+
+/* DRAMCOM register offsets */
+enum {
+    REG_DRAMCOM_CR    = 0x0000, /* Control Register */
+};
+
+/* DRAMCOMM register flags */
+enum {
+    REG_DRAMCOM_CR_DUAL_RANK = (1 << 0),
+};
+
+/* DRAMCTL register offsets */
+enum {
+    REG_DRAMCTL_PIR   = 0x0000, /* PHY Initialization Register */
+    REG_DRAMCTL_PGSR  = 0x0010, /* PHY General Status Register */
+    REG_DRAMCTL_STATR = 0x0018, /* Status Register */
+    REG_DRAMCTL_PGCR  = 0x0100, /* PHY general configuration registers */
+};
+
+/* DRAMCTL register flags */
+enum {
+    REG_DRAMCTL_PGSR_INITDONE = (1 << 0),
+    REG_DRAMCTL_PGSR_READ_TIMEOUT = (1 << 13),
+    REG_DRAMCTL_PGCR_ENABLE_READ_TIMEOUT = (1 << 25),
+};
+
+enum {
+    REG_DRAMCTL_STATR_ACTIVE  = (1 << 0),
+};
+
+#define DRAM_MAX_ROW_BITS       16
+#define DRAM_MAX_COL_BITS       13  /* 8192 */
+#define DRAM_MAX_BANK            3
+
+static uint64_t dram_autodetect_cells[DRAM_MAX_ROW_BITS]
+                                     [DRAM_MAX_BANK]
+                                     [DRAM_MAX_COL_BITS];
+struct VirtualDDRChip {
+    uint32_t    ram_size;
+    uint8_t     bank_bits;
+    uint8_t     row_bits;
+    uint8_t     col_bits;
+};
+
+/*
+ * Only power of 2 RAM sizes from 256MiB up to 2048MiB are supported,
+ * 2GiB memory is not supported due to dual rank feature.
+ */
+static const struct VirtualDDRChip dummy_ddr_chips[] = {
+    {
+        .ram_size   = 256,
+        .bank_bits  = 3,
+        .row_bits   = 12,
+        .col_bits   = 13,
+    }, {
+        .ram_size   = 512,
+        .bank_bits  = 3,
+        .row_bits   = 13,
+        .col_bits   = 13,
+    }, {
+        .ram_size   = 1024,
+        .bank_bits  = 3,
+        .row_bits   = 14,
+        .col_bits   = 13,
+    }, {
+        0
+    }
+};
+
+static const struct VirtualDDRChip *get_match_ddr(uint32_t ram_size)
+{
+    const struct VirtualDDRChip *ddr;
+
+    for (ddr = &dummy_ddr_chips[0]; ddr->ram_size; ddr++) {
+        if (ddr->ram_size == ram_size) {
+            return ddr;
+        }
+    }
+
+    return NULL;
+}
+
+static uint64_t *address_to_autodetect_cells(AwR40DramCtlState *s,
+                                             const struct VirtualDDRChip *ddr,
+                                             uint32_t offset)
+{
+    int row_index = 0, bank_index = 0, col_index = 0;
+    uint32_t row_addr, bank_addr, col_addr;
+
+    row_addr = extract32(offset, s->set_col_bits + s->set_bank_bits,
+                         s->set_row_bits);
+    bank_addr = extract32(offset, s->set_col_bits, s->set_bank_bits);
+    col_addr = extract32(offset, 0, s->set_col_bits);
+
+    for (int i = 0; i < ddr->row_bits; i++) {
+        if (row_addr & BIT(i)) {
+            row_index = i;
+        }
+    }
+
+    for (int i = 0; i < ddr->bank_bits; i++) {
+        if (bank_addr & BIT(i)) {
+            bank_index = i;
+        }
+    }
+
+    for (int i = 0; i < ddr->col_bits; i++) {
+        if (col_addr & BIT(i)) {
+            col_index = i;
+        }
+    }
+
+    trace_allwinner_r40_dramc_offset_to_cell(offset, row_index, bank_index,
+                                             col_index);
+    return &dram_autodetect_cells[row_index][bank_index][col_index];
+}
+
+static void allwinner_r40_dramc_map_rows(AwR40DramCtlState *s, uint8_t row_bits,
+                                         uint8_t bank_bits, uint8_t col_bits)
+{
+    const struct VirtualDDRChip *ddr = get_match_ddr(s->ram_size);
+    bool enable_detect_cells;
+
+    trace_allwinner_r40_dramc_map_rows(row_bits, bank_bits, col_bits);
+
+    if (!ddr) {
+        return;
+    }
+
+    s->set_row_bits = row_bits;
+    s->set_bank_bits = bank_bits;
+    s->set_col_bits = col_bits;
+
+    enable_detect_cells = ddr->bank_bits != bank_bits
+                        || ddr->row_bits != row_bits
+                        || ddr->col_bits != col_bits;
+
+    if (enable_detect_cells) {
+        trace_allwinner_r40_dramc_detect_cells_enable();
+    } else {
+        trace_allwinner_r40_dramc_detect_cells_disable();
+    }
+
+    memory_region_set_enabled(&s->detect_cells, enable_detect_cells);
+}
+
+static uint64_t allwinner_r40_dramcom_read(void *opaque, hwaddr offset,
+                                           unsigned size)
+{
+    const AwR40DramCtlState *s = AW_R40_DRAMC(opaque);
+    const uint32_t idx = REG_INDEX(offset);
+
+    if (idx >= AW_R40_DRAMCOM_REGS_NUM) {
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        return 0;
+    }
+
+    trace_allwinner_r40_dramcom_read(offset, s->dramcom[idx], size);
+    return s->dramcom[idx];
+}
+
+static void allwinner_r40_dramcom_write(void *opaque, hwaddr offset,
+                                        uint64_t val, unsigned size)
+{
+    AwR40DramCtlState *s = AW_R40_DRAMC(opaque);
+    const uint32_t idx = REG_INDEX(offset);
+
+    trace_allwinner_r40_dramcom_write(offset, val, size);
+
+    if (idx >= AW_R40_DRAMCOM_REGS_NUM) {
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        return;
+    }
+
+    switch (offset) {
+    case REG_DRAMCOM_CR:   /* Control Register */
+        if (!(val & REG_DRAMCOM_CR_DUAL_RANK)) {
+            allwinner_r40_dramc_map_rows(s, ((val >> 4) & 0xf) + 1,
+                                         ((val >> 2) & 0x1) + 2,
+                                         (((val >> 8) & 0xf) + 3));
+        }
+        break;
+    };
+
+    s->dramcom[idx] = (uint32_t) val;
+}
+
+static uint64_t allwinner_r40_dramctl_read(void *opaque, hwaddr offset,
+                                           unsigned size)
+{
+    const AwR40DramCtlState *s = AW_R40_DRAMC(opaque);
+    const uint32_t idx = REG_INDEX(offset);
+
+    if (idx >= AW_R40_DRAMCTL_REGS_NUM) {
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        return 0;
+    }
+
+    trace_allwinner_r40_dramctl_read(offset, s->dramctl[idx], size);
+    return s->dramctl[idx];
+}
+
+static void allwinner_r40_dramctl_write(void *opaque, hwaddr offset,
+                                        uint64_t val, unsigned size)
+{
+    AwR40DramCtlState *s = AW_R40_DRAMC(opaque);
+    const uint32_t idx = REG_INDEX(offset);
+
+    trace_allwinner_r40_dramctl_write(offset, val, size);
+
+    if (idx >= AW_R40_DRAMCTL_REGS_NUM) {
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        return;
+    }
+
+    switch (offset) {
+    case REG_DRAMCTL_PIR:    /* PHY Initialization Register */
+        s->dramctl[REG_INDEX(REG_DRAMCTL_PGSR)] |= REG_DRAMCTL_PGSR_INITDONE;
+        s->dramctl[REG_INDEX(REG_DRAMCTL_STATR)] |= REG_DRAMCTL_STATR_ACTIVE;
+        break;
+    }
+
+    s->dramctl[idx] = (uint32_t) val;
+}
+
+static uint64_t allwinner_r40_dramphy_read(void *opaque, hwaddr offset,
+                                           unsigned size)
+{
+    const AwR40DramCtlState *s = AW_R40_DRAMC(opaque);
+    const uint32_t idx = REG_INDEX(offset);
+
+    if (idx >= AW_R40_DRAMPHY_REGS_NUM) {
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        return 0;
+    }
+
+    trace_allwinner_r40_dramphy_read(offset, s->dramphy[idx], size);
+    return s->dramphy[idx];
+}
+
+static void allwinner_r40_dramphy_write(void *opaque, hwaddr offset,
+                                        uint64_t val, unsigned size)
+{
+    AwR40DramCtlState *s = AW_R40_DRAMC(opaque);
+    const uint32_t idx = REG_INDEX(offset);
+
+    trace_allwinner_r40_dramphy_write(offset, val, size);
+
+    if (idx >= AW_R40_DRAMPHY_REGS_NUM) {
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        return;
+    }
+
+    s->dramphy[idx] = (uint32_t) val;
+}
+
+static const MemoryRegionOps allwinner_r40_dramcom_ops = {
+    .read = allwinner_r40_dramcom_read,
+    .write = allwinner_r40_dramcom_write,
+    .endianness = DEVICE_NATIVE_ENDIAN,
+    .valid = {
+        .min_access_size = 4,
+        .max_access_size = 4,
+    },
+    .impl.min_access_size = 4,
+};
+
+static const MemoryRegionOps allwinner_r40_dramctl_ops = {
+    .read = allwinner_r40_dramctl_read,
+    .write = allwinner_r40_dramctl_write,
+    .endianness = DEVICE_NATIVE_ENDIAN,
+    .valid = {
+        .min_access_size = 4,
+        .max_access_size = 4,
+    },
+    .impl.min_access_size = 4,
+};
+
+static const MemoryRegionOps allwinner_r40_dramphy_ops = {
+    .read = allwinner_r40_dramphy_read,
+    .write = allwinner_r40_dramphy_write,
+    .endianness = DEVICE_NATIVE_ENDIAN,
+    .valid = {
+        .min_access_size = 4,
+        .max_access_size = 4,
+    },
+    .impl.min_access_size = 4,
+};
+
+static uint64_t allwinner_r40_detect_read(void *opaque, hwaddr offset,
+                                          unsigned size)
+{
+    AwR40DramCtlState *s = AW_R40_DRAMC(opaque);
+    const struct VirtualDDRChip *ddr = get_match_ddr(s->ram_size);
+    uint64_t data = 0;
+
+    if (ddr) {
+        data = *address_to_autodetect_cells(s, ddr, (uint32_t)offset);
+    }
+
+    trace_allwinner_r40_dramc_detect_cell_read(offset, data);
+    return data;
+}
+
+static void allwinner_r40_detect_write(void *opaque, hwaddr offset,
+                                       uint64_t data, unsigned size)
+{
+    AwR40DramCtlState *s = AW_R40_DRAMC(opaque);
+    const struct VirtualDDRChip *ddr = get_match_ddr(s->ram_size);
+
+    if (ddr) {
+        uint64_t *cell = address_to_autodetect_cells(s, ddr, (uint32_t)offset);
+        trace_allwinner_r40_dramc_detect_cell_write(offset, data);
+        *cell = data;
+    }
+}
+
+static const MemoryRegionOps allwinner_r40_detect_ops = {
+    .read = allwinner_r40_detect_read,
+    .write = allwinner_r40_detect_write,
+    .endianness = DEVICE_NATIVE_ENDIAN,
+    .valid = {
+        .min_access_size = 4,
+        .max_access_size = 4,
+    },
+    .impl.min_access_size = 4,
+};
+
+/*
+ * mctl_r40_detect_rank_count in u-boot will write the high 1G of DDR
+ * to detect wether the board support dual_rank or not. Create a virtual memory
+ * if the board's ram_size less or equal than 1G, and set read time out flag of
+ * REG_DRAMCTL_PGSR when the user touch this high dram.
+ */
+static uint64_t allwinner_r40_dualrank_detect_read(void *opaque, hwaddr offset,
+                                                   unsigned size)
+{
+    AwR40DramCtlState *s = AW_R40_DRAMC(opaque);
+    uint32_t reg;
+
+    reg = s->dramctl[REG_INDEX(REG_DRAMCTL_PGCR)];
+    if (reg & REG_DRAMCTL_PGCR_ENABLE_READ_TIMEOUT) { /* Enable read time out */
+        /*
+         * this driver only support one rank, mark READ_TIMEOUT when try
+         * read the second rank.
+         */
+        s->dramctl[REG_INDEX(REG_DRAMCTL_PGSR)]
+                                |= REG_DRAMCTL_PGSR_READ_TIMEOUT;
+    }
+
+    return 0;
+}
+
+static const MemoryRegionOps allwinner_r40_dualrank_detect_ops = {
+    .read = allwinner_r40_dualrank_detect_read,
+    .endianness = DEVICE_NATIVE_ENDIAN,
+    .valid = {
+        .min_access_size = 4,
+        .max_access_size = 4,
+    },
+    .impl.min_access_size = 4,
+};
+
+static void allwinner_r40_dramc_reset(DeviceState *dev)
+{
+    AwR40DramCtlState *s = AW_R40_DRAMC(dev);
+
+    /* Set default values for registers */
+    memset(&s->dramcom, 0, sizeof(s->dramcom));
+    memset(&s->dramctl, 0, sizeof(s->dramctl));
+    memset(&s->dramphy, 0, sizeof(s->dramphy));
+}
+
+static void allwinner_r40_dramc_realize(DeviceState *dev, Error **errp)
+{
+    AwR40DramCtlState *s = AW_R40_DRAMC(dev);
+
+    if (!get_match_ddr(s->ram_size)) {
+        error_report("%s: ram-size %u MiB is not supported",
+                        __func__, s->ram_size);
+        exit(1);
+    }
+
+    /* detect_cells */
+    sysbus_mmio_map_overlap(SYS_BUS_DEVICE(s), 3, s->ram_addr, 10);
+    memory_region_set_enabled(&s->detect_cells, false);
+
+    /*
+     * We only support DRAM size up to 1G now, so prepare a high memory page
+     * after 1G for dualrank detect. index = 4
+     */
+    memory_region_init_io(&s->dram_high, OBJECT(s),
+                            &allwinner_r40_dualrank_detect_ops, s,
+                            "DRAMHIGH", KiB);
+    sysbus_init_mmio(SYS_BUS_DEVICE(s), &s->dram_high);
+    sysbus_mmio_map(SYS_BUS_DEVICE(s), 4, s->ram_addr + GiB);
+}
+
+static void allwinner_r40_dramc_init(Object *obj)
+{
+    SysBusDevice *sbd = SYS_BUS_DEVICE(obj);
+    AwR40DramCtlState *s = AW_R40_DRAMC(obj);
+
+    /* DRAMCOM registers, index 0 */
+    memory_region_init_io(&s->dramcom_iomem, OBJECT(s),
+                          &allwinner_r40_dramcom_ops, s,
+                          "DRAMCOM", 4 * KiB);
+    sysbus_init_mmio(sbd, &s->dramcom_iomem);
+
+    /* DRAMCTL registers, index 1 */
+    memory_region_init_io(&s->dramctl_iomem, OBJECT(s),
+                          &allwinner_r40_dramctl_ops, s,
+                          "DRAMCTL", 4 * KiB);
+    sysbus_init_mmio(sbd, &s->dramctl_iomem);
+
+    /* DRAMPHY registers. index 2 */
+    memory_region_init_io(&s->dramphy_iomem, OBJECT(s),
+                          &allwinner_r40_dramphy_ops, s,
+                          "DRAMPHY", 4 * KiB);
+    sysbus_init_mmio(sbd, &s->dramphy_iomem);
+
+    /* R40 support max 2G memory but we only support up to 1G now. index 3 */
+    memory_region_init_io(&s->detect_cells, OBJECT(s),
+                          &allwinner_r40_detect_ops, s,
+                          "DRAMCELLS", 1 * GiB);
+    sysbus_init_mmio(sbd, &s->detect_cells);
+}
+
+static Property allwinner_r40_dramc_properties[] = {
+    DEFINE_PROP_UINT64("ram-addr", AwR40DramCtlState, ram_addr, 0x0),
+    DEFINE_PROP_UINT32("ram-size", AwR40DramCtlState, ram_size, 256), /* MiB */
+    DEFINE_PROP_END_OF_LIST()
+};
+
+static const VMStateDescription allwinner_r40_dramc_vmstate = {
+    .name = "allwinner-r40-dramc",
+    .version_id = 1,
+    .minimum_version_id = 1,
+    .fields = (VMStateField[]) {
+        VMSTATE_UINT32_ARRAY(dramcom, AwR40DramCtlState,
+                             AW_R40_DRAMCOM_REGS_NUM),
+        VMSTATE_UINT32_ARRAY(dramctl, AwR40DramCtlState,
+                             AW_R40_DRAMCTL_REGS_NUM),
+        VMSTATE_UINT32_ARRAY(dramphy, AwR40DramCtlState,
+                             AW_R40_DRAMPHY_REGS_NUM),
+        VMSTATE_END_OF_LIST()
+    }
+};
+
+static void allwinner_r40_dramc_class_init(ObjectClass *klass, void *data)
+{
+    DeviceClass *dc = DEVICE_CLASS(klass);
+
+    dc->reset = allwinner_r40_dramc_reset;
+    dc->vmsd = &allwinner_r40_dramc_vmstate;
+    dc->realize = allwinner_r40_dramc_realize;
+    device_class_set_props(dc, allwinner_r40_dramc_properties);
+}
+
+static const TypeInfo allwinner_r40_dramc_info = {
+    .name          = TYPE_AW_R40_DRAMC,
+    .parent        = TYPE_SYS_BUS_DEVICE,
+    .instance_init = allwinner_r40_dramc_init,
+    .instance_size = sizeof(AwR40DramCtlState),
+    .class_init    = allwinner_r40_dramc_class_init,
+};
+
+static void allwinner_r40_dramc_register(void)
+{
+    type_register_static(&allwinner_r40_dramc_info);
+}
+
+type_init(allwinner_r40_dramc_register)
diff --git a/hw/misc/allwinner-sramc.c b/hw/misc/allwinner-sramc.c
new file mode 100644
index 0000000..a8b731f
--- /dev/null
+++ b/hw/misc/allwinner-sramc.c
@@ -0,0 +1,184 @@
+/*
+ * Allwinner R40 SRAM controller emulation
+ *
+ * Copyright (C) 2023 qianfan Zhao <qianfanguijin@163.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "qemu/osdep.h"
+#include "qemu/units.h"
+#include "hw/sysbus.h"
+#include "migration/vmstate.h"
+#include "qemu/log.h"
+#include "qemu/module.h"
+#include "qapi/error.h"
+#include "hw/qdev-properties.h"
+#include "hw/qdev-properties-system.h"
+#include "hw/misc/allwinner-sramc.h"
+#include "trace.h"
+
+/*
+ * register offsets
+ * https://linux-sunxi.org/SRAM_Controller_Register_Guide
+ */
+enum {
+    REG_SRAM_CTL1_CFG               = 0x04, /* SRAM Control register 1 */
+    REG_SRAM_VER                    = 0x24, /* SRAM Version register */
+    REG_SRAM_R40_SOFT_ENTRY_REG0    = 0xbc,
+};
+
+/* REG_SRAMC_VERSION bit defines */
+#define SRAM_VER_READ_ENABLE            (1 << 15)
+#define SRAM_VER_VERSION_SHIFT          16
+#define SRAM_VERSION_SUN8I_R40          0x1701
+
+static uint64_t allwinner_sramc_read(void *opaque, hwaddr offset,
+                                     unsigned size)
+{
+    AwSRAMCState *s = AW_SRAMC(opaque);
+    AwSRAMCClass *sc = AW_SRAMC_GET_CLASS(s);
+    uint64_t val = 0;
+
+    switch (offset) {
+    case REG_SRAM_CTL1_CFG:
+        val = s->sram_ctl1;
+        break;
+    case REG_SRAM_VER:
+        /* bit15: lock bit, set this bit before reading this register */
+        if (s->sram_ver & SRAM_VER_READ_ENABLE) {
+            val = SRAM_VER_READ_ENABLE |
+                    (sc->sram_version_code << SRAM_VER_VERSION_SHIFT);
+        }
+        break;
+    case REG_SRAM_R40_SOFT_ENTRY_REG0:
+        val = s->sram_soft_entry_reg0;
+        break;
+    default:
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        return 0;
+    }
+
+    trace_allwinner_sramc_read(offset, val);
+
+    return val;
+}
+
+static void allwinner_sramc_write(void *opaque, hwaddr offset,
+                                  uint64_t val, unsigned size)
+{
+    AwSRAMCState *s = AW_SRAMC(opaque);
+
+    trace_allwinner_sramc_write(offset, val);
+
+    switch (offset) {
+    case REG_SRAM_CTL1_CFG:
+        s->sram_ctl1 = val;
+        break;
+    case REG_SRAM_VER:
+        /* Only the READ_ENABLE bit is writeable */
+        s->sram_ver = val & SRAM_VER_READ_ENABLE;
+        break;
+    case REG_SRAM_R40_SOFT_ENTRY_REG0:
+        s->sram_soft_entry_reg0 = val;
+        break;
+    default:
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset 0x%04x\n",
+                      __func__, (uint32_t)offset);
+        break;
+    }
+}
+
+static const MemoryRegionOps allwinner_sramc_ops = {
+    .read = allwinner_sramc_read,
+    .write = allwinner_sramc_write,
+    .endianness = DEVICE_NATIVE_ENDIAN,
+    .valid = {
+        .min_access_size = 4,
+        .max_access_size = 4,
+    },
+    .impl.min_access_size = 4,
+};
+
+static const VMStateDescription allwinner_sramc_vmstate = {
+    .name = "allwinner-sramc",
+    .version_id = 1,
+    .minimum_version_id = 1,
+    .fields = (VMStateField[]) {
+        VMSTATE_UINT32(sram_ver, AwSRAMCState),
+        VMSTATE_UINT32(sram_soft_entry_reg0, AwSRAMCState),
+        VMSTATE_END_OF_LIST()
+    }
+};
+
+static void allwinner_sramc_reset(DeviceState *dev)
+{
+    AwSRAMCState *s = AW_SRAMC(dev);
+    AwSRAMCClass *sc = AW_SRAMC_GET_CLASS(s);
+
+    switch (sc->sram_version_code) {
+    case SRAM_VERSION_SUN8I_R40:
+        s->sram_ctl1 = 0x1300;
+        break;
+    }
+}
+
+static void allwinner_sramc_class_init(ObjectClass *klass, void *data)
+{
+    DeviceClass *dc = DEVICE_CLASS(klass);
+
+    dc->reset = allwinner_sramc_reset;
+    dc->vmsd = &allwinner_sramc_vmstate;
+}
+
+static void allwinner_sramc_init(Object *obj)
+{
+    SysBusDevice *sbd = SYS_BUS_DEVICE(obj);
+    AwSRAMCState *s = AW_SRAMC(obj);
+
+    /* Memory mapping */
+    memory_region_init_io(&s->iomem, OBJECT(s), &allwinner_sramc_ops, s,
+                           TYPE_AW_SRAMC, 1 * KiB);
+    sysbus_init_mmio(sbd, &s->iomem);
+}
+
+static const TypeInfo allwinner_sramc_info = {
+    .name          = TYPE_AW_SRAMC,
+    .parent        = TYPE_SYS_BUS_DEVICE,
+    .instance_init = allwinner_sramc_init,
+    .instance_size = sizeof(AwSRAMCState),
+    .class_init    = allwinner_sramc_class_init,
+};
+
+static void allwinner_r40_sramc_class_init(ObjectClass *klass, void *data)
+{
+    AwSRAMCClass *sc = AW_SRAMC_CLASS(klass);
+
+    sc->sram_version_code = SRAM_VERSION_SUN8I_R40;
+}
+
+static const TypeInfo allwinner_r40_sramc_info = {
+    .name          = TYPE_AW_SRAMC_SUN8I_R40,
+    .parent        = TYPE_AW_SRAMC,
+    .class_init    = allwinner_r40_sramc_class_init,
+};
+
+static void allwinner_sramc_register(void)
+{
+    type_register_static(&allwinner_sramc_info);
+    type_register_static(&allwinner_r40_sramc_info);
+}
+
+type_init(allwinner_sramc_register)
diff --git a/hw/misc/axp209.c b/hw/misc/axp209.c
deleted file mode 100644
index 2908ed9..0000000
--- a/hw/misc/axp209.c
+++ /dev/null
@@ -1,238 +0,0 @@
-/*
- * AXP-209 PMU Emulation
- *
- * Copyright (C) 2022 Strahinja Jankovic <strahinja.p.jankovic@gmail.com>
- *
- * 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.
- *
- * SPDX-License-Identifier: MIT
- */
-
-#include "qemu/osdep.h"
-#include "qemu/log.h"
-#include "trace.h"
-#include "hw/i2c/i2c.h"
-#include "migration/vmstate.h"
-
-#define TYPE_AXP209_PMU "axp209_pmu"
-
-#define AXP209(obj) \
-    OBJECT_CHECK(AXP209I2CState, (obj), TYPE_AXP209_PMU)
-
-/* registers */
-enum {
-    REG_POWER_STATUS = 0x0u,
-    REG_OPERATING_MODE,
-    REG_OTG_VBUS_STATUS,
-    REG_CHIP_VERSION,
-    REG_DATA_CACHE_0,
-    REG_DATA_CACHE_1,
-    REG_DATA_CACHE_2,
-    REG_DATA_CACHE_3,
-    REG_DATA_CACHE_4,
-    REG_DATA_CACHE_5,
-    REG_DATA_CACHE_6,
-    REG_DATA_CACHE_7,
-    REG_DATA_CACHE_8,
-    REG_DATA_CACHE_9,
-    REG_DATA_CACHE_A,
-    REG_DATA_CACHE_B,
-    REG_POWER_OUTPUT_CTRL = 0x12u,
-    REG_DC_DC2_OUT_V_CTRL = 0x23u,
-    REG_DC_DC2_DVS_CTRL = 0x25u,
-    REG_DC_DC3_OUT_V_CTRL = 0x27u,
-    REG_LDO2_4_OUT_V_CTRL,
-    REG_LDO3_OUT_V_CTRL,
-    REG_VBUS_CH_MGMT = 0x30u,
-    REG_SHUTDOWN_V_CTRL,
-    REG_SHUTDOWN_CTRL,
-    REG_CHARGE_CTRL_1,
-    REG_CHARGE_CTRL_2,
-    REG_SPARE_CHARGE_CTRL,
-    REG_PEK_KEY_CTRL,
-    REG_DC_DC_FREQ_SET,
-    REG_CHR_TEMP_TH_SET,
-    REG_CHR_HIGH_TEMP_TH_CTRL,
-    REG_IPSOUT_WARN_L1,
-    REG_IPSOUT_WARN_L2,
-    REG_DISCHR_TEMP_TH_SET,
-    REG_DISCHR_HIGH_TEMP_TH_CTRL,
-    REG_IRQ_BANK_1_CTRL = 0x40u,
-    REG_IRQ_BANK_2_CTRL,
-    REG_IRQ_BANK_3_CTRL,
-    REG_IRQ_BANK_4_CTRL,
-    REG_IRQ_BANK_5_CTRL,
-    REG_IRQ_BANK_1_STAT = 0x48u,
-    REG_IRQ_BANK_2_STAT,
-    REG_IRQ_BANK_3_STAT,
-    REG_IRQ_BANK_4_STAT,
-    REG_IRQ_BANK_5_STAT,
-    REG_ADC_ACIN_V_H = 0x56u,
-    REG_ADC_ACIN_V_L,
-    REG_ADC_ACIN_CURR_H,
-    REG_ADC_ACIN_CURR_L,
-    REG_ADC_VBUS_V_H,
-    REG_ADC_VBUS_V_L,
-    REG_ADC_VBUS_CURR_H,
-    REG_ADC_VBUS_CURR_L,
-    REG_ADC_INT_TEMP_H,
-    REG_ADC_INT_TEMP_L,
-    REG_ADC_TEMP_SENS_V_H = 0x62u,
-    REG_ADC_TEMP_SENS_V_L,
-    REG_ADC_BAT_V_H = 0x78u,
-    REG_ADC_BAT_V_L,
-    REG_ADC_BAT_DISCHR_CURR_H,
-    REG_ADC_BAT_DISCHR_CURR_L,
-    REG_ADC_BAT_CHR_CURR_H,
-    REG_ADC_BAT_CHR_CURR_L,
-    REG_ADC_IPSOUT_V_H,
-    REG_ADC_IPSOUT_V_L,
-    REG_DC_DC_MOD_SEL = 0x80u,
-    REG_ADC_EN_1,
-    REG_ADC_EN_2,
-    REG_ADC_SR_CTRL,
-    REG_ADC_IN_RANGE,
-    REG_GPIO1_ADC_IRQ_RISING_TH,
-    REG_GPIO1_ADC_IRQ_FALLING_TH,
-    REG_TIMER_CTRL = 0x8au,
-    REG_VBUS_CTRL_MON_SRP,
-    REG_OVER_TEMP_SHUTDOWN = 0x8fu,
-    REG_GPIO0_FEAT_SET,
-    REG_GPIO_OUT_HIGH_SET,
-    REG_GPIO1_FEAT_SET,
-    REG_GPIO2_FEAT_SET,
-    REG_GPIO_SIG_STATE_SET_MON,
-    REG_GPIO3_SET,
-    REG_COULOMB_CNTR_CTRL = 0xb8u,
-    REG_POWER_MEAS_RES,
-    NR_REGS
-};
-
-#define AXP209_CHIP_VERSION_ID             (0x01)
-#define AXP209_DC_DC2_OUT_V_CTRL_RESET     (0x16)
-#define AXP209_IRQ_BANK_1_CTRL_RESET       (0xd8)
-
-/* A simple I2C slave which returns values of ID or CNT register. */
-typedef struct AXP209I2CState {
-    /*< private >*/
-    I2CSlave i2c;
-    /*< public >*/
-    uint8_t regs[NR_REGS];  /* peripheral registers */
-    uint8_t ptr;            /* current register index */
-    uint8_t count;          /* counter used for tx/rx */
-} AXP209I2CState;
-
-/* Reset all counters and load ID register */
-static void axp209_reset_enter(Object *obj, ResetType type)
-{
-    AXP209I2CState *s = AXP209(obj);
-
-    memset(s->regs, 0, NR_REGS);
-    s->ptr = 0;
-    s->count = 0;
-    s->regs[REG_CHIP_VERSION] = AXP209_CHIP_VERSION_ID;
-    s->regs[REG_DC_DC2_OUT_V_CTRL] = AXP209_DC_DC2_OUT_V_CTRL_RESET;
-    s->regs[REG_IRQ_BANK_1_CTRL] = AXP209_IRQ_BANK_1_CTRL_RESET;
-}
-
-/* Handle events from master. */
-static int axp209_event(I2CSlave *i2c, enum i2c_event event)
-{
-    AXP209I2CState *s = AXP209(i2c);
-
-    s->count = 0;
-
-    return 0;
-}
-
-/* Called when master requests read */
-static uint8_t axp209_rx(I2CSlave *i2c)
-{
-    AXP209I2CState *s = AXP209(i2c);
-    uint8_t ret = 0xff;
-
-    if (s->ptr < NR_REGS) {
-        ret = s->regs[s->ptr++];
-    }
-
-    trace_axp209_rx(s->ptr - 1, ret);
-
-    return ret;
-}
-
-/*
- * Called when master sends write.
- * Update ptr with byte 0, then perform write with second byte.
- */
-static int axp209_tx(I2CSlave *i2c, uint8_t data)
-{
-    AXP209I2CState *s = AXP209(i2c);
-
-    if (s->count == 0) {
-        /* Store register address */
-        s->ptr = data;
-        s->count++;
-        trace_axp209_select(data);
-    } else {
-        trace_axp209_tx(s->ptr, data);
-        if (s->ptr == REG_DC_DC2_OUT_V_CTRL) {
-            s->regs[s->ptr++] = data;
-        }
-    }
-
-    return 0;
-}
-
-static const VMStateDescription vmstate_axp209 = {
-    .name = TYPE_AXP209_PMU,
-    .version_id = 1,
-    .fields = (VMStateField[]) {
-        VMSTATE_UINT8_ARRAY(regs, AXP209I2CState, NR_REGS),
-        VMSTATE_UINT8(count, AXP209I2CState),
-        VMSTATE_UINT8(ptr, AXP209I2CState),
-        VMSTATE_END_OF_LIST()
-    }
-};
-
-static void axp209_class_init(ObjectClass *oc, void *data)
-{
-    DeviceClass *dc = DEVICE_CLASS(oc);
-    I2CSlaveClass *isc = I2C_SLAVE_CLASS(oc);
-    ResettableClass *rc = RESETTABLE_CLASS(oc);
-
-    rc->phases.enter = axp209_reset_enter;
-    dc->vmsd = &vmstate_axp209;
-    isc->event = axp209_event;
-    isc->recv = axp209_rx;
-    isc->send = axp209_tx;
-}
-
-static const TypeInfo axp209_info = {
-    .name = TYPE_AXP209_PMU,
-    .parent = TYPE_I2C_SLAVE,
-    .instance_size = sizeof(AXP209I2CState),
-    .class_init = axp209_class_init
-};
-
-static void axp209_register_devices(void)
-{
-    type_register_static(&axp209_info);
-}
-
-type_init(axp209_register_devices);
diff --git a/hw/misc/axp2xx.c b/hw/misc/axp2xx.c
new file mode 100644
index 0000000..41538c1
--- /dev/null
+++ b/hw/misc/axp2xx.c
@@ -0,0 +1,283 @@
+/*
+ * AXP-2XX PMU Emulation, supported lists:
+ *   AXP209
+ *   AXP221
+ *
+ * Copyright (C) 2022 Strahinja Jankovic <strahinja.p.jankovic@gmail.com>
+ * Copyright (C) 2023 qianfan Zhao <qianfanguijin@163.com>
+ *
+ * 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.
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#include "qemu/osdep.h"
+#include "qemu/log.h"
+#include "qom/object.h"
+#include "trace.h"
+#include "hw/i2c/i2c.h"
+#include "migration/vmstate.h"
+
+#define TYPE_AXP2XX     "axp2xx_pmu"
+#define TYPE_AXP209_PMU "axp209_pmu"
+#define TYPE_AXP221_PMU "axp221_pmu"
+
+OBJECT_DECLARE_TYPE(AXP2xxI2CState, AXP2xxClass, AXP2XX)
+
+#define NR_REGS                            (0xff)
+
+/* A simple I2C slave which returns values of ID or CNT register. */
+typedef struct AXP2xxI2CState {
+    /*< private >*/
+    I2CSlave i2c;
+    /*< public >*/
+    uint8_t regs[NR_REGS];  /* peripheral registers */
+    uint8_t ptr;            /* current register index */
+    uint8_t count;          /* counter used for tx/rx */
+} AXP2xxI2CState;
+
+typedef struct AXP2xxClass {
+    /*< private >*/
+    I2CSlaveClass parent_class;
+    /*< public >*/
+    void (*reset_enter)(AXP2xxI2CState *s, ResetType type);
+} AXP2xxClass;
+
+#define AXP209_CHIP_VERSION_ID             (0x01)
+#define AXP209_DC_DC2_OUT_V_CTRL_RESET     (0x16)
+
+/* Reset all counters and load ID register */
+static void axp209_reset_enter(AXP2xxI2CState *s, ResetType type)
+{
+    memset(s->regs, 0, NR_REGS);
+    s->ptr = 0;
+    s->count = 0;
+
+    s->regs[0x03] = AXP209_CHIP_VERSION_ID;
+    s->regs[0x23] = AXP209_DC_DC2_OUT_V_CTRL_RESET;
+
+    s->regs[0x30] = 0x60;
+    s->regs[0x32] = 0x46;
+    s->regs[0x34] = 0x41;
+    s->regs[0x35] = 0x22;
+    s->regs[0x36] = 0x5d;
+    s->regs[0x37] = 0x08;
+    s->regs[0x38] = 0xa5;
+    s->regs[0x39] = 0x1f;
+    s->regs[0x3a] = 0x68;
+    s->regs[0x3b] = 0x5f;
+    s->regs[0x3c] = 0xfc;
+    s->regs[0x3d] = 0x16;
+    s->regs[0x40] = 0xd8;
+    s->regs[0x42] = 0xff;
+    s->regs[0x43] = 0x3b;
+    s->regs[0x80] = 0xe0;
+    s->regs[0x82] = 0x83;
+    s->regs[0x83] = 0x80;
+    s->regs[0x84] = 0x32;
+    s->regs[0x86] = 0xff;
+    s->regs[0x90] = 0x07;
+    s->regs[0x91] = 0xa0;
+    s->regs[0x92] = 0x07;
+    s->regs[0x93] = 0x07;
+}
+
+#define AXP221_PWR_STATUS_ACIN_PRESENT          BIT(7)
+#define AXP221_PWR_STATUS_ACIN_AVAIL            BIT(6)
+#define AXP221_PWR_STATUS_VBUS_PRESENT          BIT(5)
+#define AXP221_PWR_STATUS_VBUS_USED             BIT(4)
+#define AXP221_PWR_STATUS_BAT_CHARGING          BIT(2)
+#define AXP221_PWR_STATUS_ACIN_VBUS_POWERED     BIT(1)
+
+/* Reset all counters and load ID register */
+static void axp221_reset_enter(AXP2xxI2CState *s, ResetType type)
+{
+    memset(s->regs, 0, NR_REGS);
+    s->ptr = 0;
+    s->count = 0;
+
+    /* input power status register */
+    s->regs[0x00] = AXP221_PWR_STATUS_ACIN_PRESENT
+                    | AXP221_PWR_STATUS_ACIN_AVAIL
+                    | AXP221_PWR_STATUS_ACIN_VBUS_POWERED;
+
+    s->regs[0x01] = 0x00; /* no battery is connected */
+
+    /*
+     * CHIPID register, no documented on datasheet, but it is checked in
+     * u-boot spl. I had read it from AXP221s and got 0x06 value.
+     * So leave 06h here.
+     */
+    s->regs[0x03] = 0x06;
+
+    s->regs[0x10] = 0xbf;
+    s->regs[0x13] = 0x01;
+    s->regs[0x30] = 0x60;
+    s->regs[0x31] = 0x03;
+    s->regs[0x32] = 0x43;
+    s->regs[0x33] = 0xc6;
+    s->regs[0x34] = 0x45;
+    s->regs[0x35] = 0x0e;
+    s->regs[0x36] = 0x5d;
+    s->regs[0x37] = 0x08;
+    s->regs[0x38] = 0xa5;
+    s->regs[0x39] = 0x1f;
+    s->regs[0x3c] = 0xfc;
+    s->regs[0x3d] = 0x16;
+    s->regs[0x80] = 0x80;
+    s->regs[0x82] = 0xe0;
+    s->regs[0x84] = 0x32;
+    s->regs[0x8f] = 0x01;
+
+    s->regs[0x90] = 0x07;
+    s->regs[0x91] = 0x1f;
+    s->regs[0x92] = 0x07;
+    s->regs[0x93] = 0x1f;
+
+    s->regs[0x40] = 0xd8;
+    s->regs[0x41] = 0xff;
+    s->regs[0x42] = 0x03;
+    s->regs[0x43] = 0x03;
+
+    s->regs[0xb8] = 0xc0;
+    s->regs[0xb9] = 0x64;
+    s->regs[0xe6] = 0xa0;
+}
+
+static void axp2xx_reset_enter(Object *obj, ResetType type)
+{
+    AXP2xxI2CState *s = AXP2XX(obj);
+    AXP2xxClass *sc = AXP2XX_GET_CLASS(s);
+
+    sc->reset_enter(s, type);
+}
+
+/* Handle events from master. */
+static int axp2xx_event(I2CSlave *i2c, enum i2c_event event)
+{
+    AXP2xxI2CState *s = AXP2XX(i2c);
+
+    s->count = 0;
+
+    return 0;
+}
+
+/* Called when master requests read */
+static uint8_t axp2xx_rx(I2CSlave *i2c)
+{
+    AXP2xxI2CState *s = AXP2XX(i2c);
+    uint8_t ret = 0xff;
+
+    if (s->ptr < NR_REGS) {
+        ret = s->regs[s->ptr++];
+    }
+
+    trace_axp2xx_rx(s->ptr - 1, ret);
+
+    return ret;
+}
+
+/*
+ * Called when master sends write.
+ * Update ptr with byte 0, then perform write with second byte.
+ */
+static int axp2xx_tx(I2CSlave *i2c, uint8_t data)
+{
+    AXP2xxI2CState *s = AXP2XX(i2c);
+
+    if (s->count == 0) {
+        /* Store register address */
+        s->ptr = data;
+        s->count++;
+        trace_axp2xx_select(data);
+    } else {
+        trace_axp2xx_tx(s->ptr, data);
+        s->regs[s->ptr++] = data;
+    }
+
+    return 0;
+}
+
+static const VMStateDescription vmstate_axp2xx = {
+    .name = TYPE_AXP2XX,
+    .version_id = 1,
+    .fields = (VMStateField[]) {
+        VMSTATE_UINT8_ARRAY(regs, AXP2xxI2CState, NR_REGS),
+        VMSTATE_UINT8(ptr, AXP2xxI2CState),
+        VMSTATE_UINT8(count, AXP2xxI2CState),
+        VMSTATE_END_OF_LIST()
+    }
+};
+
+static void axp2xx_class_init(ObjectClass *oc, void *data)
+{
+    DeviceClass *dc = DEVICE_CLASS(oc);
+    I2CSlaveClass *isc = I2C_SLAVE_CLASS(oc);
+    ResettableClass *rc = RESETTABLE_CLASS(oc);
+
+    rc->phases.enter = axp2xx_reset_enter;
+    dc->vmsd = &vmstate_axp2xx;
+    isc->event = axp2xx_event;
+    isc->recv = axp2xx_rx;
+    isc->send = axp2xx_tx;
+}
+
+static const TypeInfo axp2xx_info = {
+    .name = TYPE_AXP2XX,
+    .parent = TYPE_I2C_SLAVE,
+    .instance_size = sizeof(AXP2xxI2CState),
+    .class_size = sizeof(AXP2xxClass),
+    .class_init = axp2xx_class_init,
+    .abstract = true,
+};
+
+static void axp209_class_init(ObjectClass *oc, void *data)
+{
+    AXP2xxClass *sc = AXP2XX_CLASS(oc);
+
+    sc->reset_enter = axp209_reset_enter;
+}
+
+static const TypeInfo axp209_info = {
+    .name = TYPE_AXP209_PMU,
+    .parent = TYPE_AXP2XX,
+    .class_init = axp209_class_init
+};
+
+static void axp221_class_init(ObjectClass *oc, void *data)
+{
+    AXP2xxClass *sc = AXP2XX_CLASS(oc);
+
+    sc->reset_enter = axp221_reset_enter;
+}
+
+static const TypeInfo axp221_info = {
+    .name = TYPE_AXP221_PMU,
+    .parent = TYPE_AXP2XX,
+    .class_init = axp221_class_init,
+};
+
+static void axp2xx_register_devices(void)
+{
+    type_register_static(&axp2xx_info);
+    type_register_static(&axp209_info);
+    type_register_static(&axp221_info);
+}
+
+type_init(axp2xx_register_devices);
diff --git a/hw/misc/meson.build b/hw/misc/meson.build
index a40245a..78ca857 100644
--- a/hw/misc/meson.build
+++ b/hw/misc/meson.build
@@ -37,6 +37,7 @@ subdir('macio')
 
 softmmu_ss.add(when: 'CONFIG_IVSHMEM_DEVICE', if_true: files('ivshmem.c'))
 
+softmmu_ss.add(when: 'CONFIG_ALLWINNER_SRAMC', if_true: files('allwinner-sramc.c'))
 softmmu_ss.add(when: 'CONFIG_ALLWINNER_A10_CCM', if_true: files('allwinner-a10-ccm.c'))
 softmmu_ss.add(when: 'CONFIG_ALLWINNER_A10_DRAMC', if_true: files('allwinner-a10-dramc.c'))
 softmmu_ss.add(when: 'CONFIG_ALLWINNER_H3', if_true: files('allwinner-h3-ccu.c'))
@@ -44,7 +45,9 @@ specific_ss.add(when: 'CONFIG_ALLWINNER_H3', if_true: files('allwinner-cpucfg.c'
 softmmu_ss.add(when: 'CONFIG_ALLWINNER_H3', if_true: files('allwinner-h3-dramc.c'))
 softmmu_ss.add(when: 'CONFIG_ALLWINNER_H3', if_true: files('allwinner-h3-sysctrl.c'))
 softmmu_ss.add(when: 'CONFIG_ALLWINNER_H3', if_true: files('allwinner-sid.c'))
-softmmu_ss.add(when: 'CONFIG_AXP209_PMU', if_true: files('axp209.c'))
+softmmu_ss.add(when: 'CONFIG_ALLWINNER_R40', if_true: files('allwinner-r40-ccu.c'))
+softmmu_ss.add(when: 'CONFIG_ALLWINNER_R40', if_true: files('allwinner-r40-dramc.c'))
+softmmu_ss.add(when: 'CONFIG_AXP2XX_PMU', if_true: files('axp2xx.c'))
 softmmu_ss.add(when: 'CONFIG_REALVIEW', if_true: files('arm_sysctl.c'))
 softmmu_ss.add(when: 'CONFIG_NSERIES', if_true: files('cbus.c'))
 softmmu_ss.add(when: 'CONFIG_ECCMEMCTL', if_true: files('eccmemctl.c'))
diff --git a/hw/misc/trace-events b/hw/misc/trace-events
index c47876a..4d1a0e1 100644
--- a/hw/misc/trace-events
+++ b/hw/misc/trace-events
@@ -15,18 +15,36 @@ allwinner_h3_dramctl_write(uint64_t offset, uint64_t data, unsigned size) "Write
 allwinner_h3_dramphy_read(uint64_t offset, uint64_t data, unsigned size) "Read: offset 0x%" PRIx64 " data 0x%" PRIx64 " size %" PRIu32
 allwinner_h3_dramphy_write(uint64_t offset, uint64_t data, unsigned size) "write: offset 0x%" PRIx64 " data 0x%" PRIx64 " size %" PRIu32
 
+# allwinner-r40-dramc.c
+allwinner_r40_dramc_detect_cells_disable(void) "Disable detect cells"
+allwinner_r40_dramc_detect_cells_enable(void) "Enable detect cells"
+allwinner_r40_dramc_map_rows(uint8_t row_bits, uint8_t bank_bits, uint8_t col_bits) "DRAM layout: row_bits %d, bank_bits %d, col_bits %d"
+allwinner_r40_dramc_offset_to_cell(uint64_t offset, int row, int bank, int col) "offset 0x%" PRIx64 " row %d bank %d col %d"
+allwinner_r40_dramc_detect_cell_write(uint64_t offset, uint64_t data) "offset 0x%" PRIx64 " data 0x%" PRIx64 ""
+allwinner_r40_dramc_detect_cell_read(uint64_t offset, uint64_t data) "offset 0x%" PRIx64 " data 0x%" PRIx64 ""
+allwinner_r40_dramcom_read(uint64_t offset, uint64_t data, unsigned size) "Read: offset 0x%" PRIx64 " data 0x%" PRIx64 " size %" PRIu32
+allwinner_r40_dramcom_write(uint64_t offset, uint64_t data, unsigned size) "Write: offset 0x%" PRIx64 " data 0x%" PRIx64 " size %" PRIu32
+allwinner_r40_dramctl_read(uint64_t offset, uint64_t data, unsigned size) "Read: offset 0x%" PRIx64 " data 0x%" PRIx64 " size %" PRIu32
+allwinner_r40_dramctl_write(uint64_t offset, uint64_t data, unsigned size) "Write: offset 0x%" PRIx64 " data 0x%" PRIx64 " size %" PRIu32
+allwinner_r40_dramphy_read(uint64_t offset, uint64_t data, unsigned size) "Read: offset 0x%" PRIx64 " data 0x%" PRIx64 " size %" PRIu32
+allwinner_r40_dramphy_write(uint64_t offset, uint64_t data, unsigned size) "write: offset 0x%" PRIx64 " data 0x%" PRIx64 " size %" PRIu32
+
 # allwinner-sid.c
 allwinner_sid_read(uint64_t offset, uint64_t data, unsigned size) "offset 0x%" PRIx64 " data 0x%" PRIx64 " size %" PRIu32
 allwinner_sid_write(uint64_t offset, uint64_t data, unsigned size) "offset 0x%" PRIx64 " data 0x%" PRIx64 " size %" PRIu32
 
+# allwinner-sramc.c
+allwinner_sramc_read(uint64_t offset, uint64_t data) "offset 0x%" PRIx64 " data 0x%" PRIx64
+allwinner_sramc_write(uint64_t offset, uint64_t data) "offset 0x%" PRIx64 " data 0x%" PRIx64
+
 # avr_power.c
 avr_power_read(uint8_t value) "power_reduc read value:%u"
 avr_power_write(uint8_t value) "power_reduc write value:%u"
 
-# axp209.c
-axp209_rx(uint8_t reg, uint8_t data) "Read reg 0x%" PRIx8 " : 0x%" PRIx8
-axp209_select(uint8_t reg) "Accessing reg 0x%" PRIx8
-axp209_tx(uint8_t reg, uint8_t data) "Write reg 0x%" PRIx8 " : 0x%" PRIx8
+# axp2xx
+axp2xx_rx(uint8_t reg, uint8_t data) "Read reg 0x%" PRIx8 " : 0x%" PRIx8
+axp2xx_select(uint8_t reg) "Accessing reg 0x%" PRIx8
+axp2xx_tx(uint8_t reg, uint8_t data) "Write reg 0x%" PRIx8 " : 0x%" PRIx8
 
 # eccmemctl.c
 ecc_mem_writel_mer(uint32_t val) "Write memory enable 0x%08x"
diff --git a/hw/net/can/meson.build b/hw/net/can/meson.build
index 8fabbd9..8d85201 100644
--- a/hw/net/can/meson.build
+++ b/hw/net/can/meson.build
@@ -5,3 +5,4 @@ softmmu_ss.add(when: 'CONFIG_CAN_PCI', if_true: files('can_mioe3680_pci.c'))
 softmmu_ss.add(when: 'CONFIG_CAN_CTUCANFD', if_true: files('ctucan_core.c'))
 softmmu_ss.add(when: 'CONFIG_CAN_CTUCANFD_PCI', if_true: files('ctucan_pci.c'))
 softmmu_ss.add(when: 'CONFIG_XLNX_ZYNQMP', if_true: files('xlnx-zynqmp-can.c'))
+softmmu_ss.add(when: 'CONFIG_XLNX_VERSAL', if_true: files('xlnx-versal-canfd.c'))
diff --git a/hw/net/can/trace-events b/hw/net/can/trace-events
index 8346a98..de64ac1 100644
--- a/hw/net/can/trace-events
+++ b/hw/net/can/trace-events
@@ -7,3 +7,10 @@ xlnx_can_filter_mask_pre_write(uint8_t filter_num, uint32_t value) "Filter%d MAS
 xlnx_can_tx_data(uint32_t id, uint8_t dlc, uint8_t db0, uint8_t db1, uint8_t db2, uint8_t db3, uint8_t db4, uint8_t db5, uint8_t db6, uint8_t db7) "Frame: ID: 0x%08x DLC: 0x%02x DATA: 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x"
 xlnx_can_rx_data(uint32_t id, uint32_t dlc, uint8_t db0, uint8_t db1, uint8_t db2, uint8_t db3, uint8_t db4, uint8_t db5, uint8_t db6, uint8_t db7) "Frame: ID: 0x%08x DLC: 0x%02x DATA: 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x"
 xlnx_can_rx_discard(uint32_t status) "Controller is not enabled for bus communication. Status Register: 0x%08x"
+
+# xlnx-versal-canfd.c
+xlnx_canfd_update_irq(char *path, uint32_t isr, uint32_t ier, uint32_t irq) "%s: ISR: 0x%08x IER: 0x%08x IRQ: 0x%08x"
+xlnx_canfd_rx_fifo_filter_reject(char *path, uint32_t id, uint8_t dlc) "%s: Frame: ID: 0x%08x DLC: 0x%02x"
+xlnx_canfd_rx_data(char *path, uint32_t id, uint8_t dlc, uint8_t flags) "%s: Frame: ID: 0x%08x DLC: 0x%02x CANFD Flag: 0x%02x"
+xlnx_canfd_tx_data(char *path, uint32_t id, uint8_t dlc, uint8_t flgas) "%s: Frame: ID: 0x%08x DLC: 0x%02x CANFD Flag: 0x%02x"
+xlnx_canfd_reset(char *path, uint32_t val) "%s: Resetting controller with value = 0x%08x"
diff --git a/hw/net/can/xlnx-versal-canfd.c b/hw/net/can/xlnx-versal-canfd.c
new file mode 100644
index 0000000..5b8ce0a
--- /dev/null
+++ b/hw/net/can/xlnx-versal-canfd.c
@@ -0,0 +1,2107 @@
+/*
+ * QEMU model of the Xilinx Versal CANFD device.
+ *
+ * This implementation is based on the following datasheet:
+ * https://docs.xilinx.com/v/u/2.0-English/pg223-canfd
+ *
+ * Copyright (c) 2023 Advanced Micro Devices, Inc.
+ *
+ * Written-by: Vikram Garhwal <vikram.garhwal@amd.com>
+ *
+ * Based on QEMU CANFD Device emulation implemented by Jin Yang, Deniz Eren and
+ * Pavel Pisa
+ *
+ * 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.
+ */
+
+#include "qemu/osdep.h"
+#include "hw/sysbus.h"
+#include "hw/irq.h"
+#include "hw/register.h"
+#include "qapi/error.h"
+#include "qemu/bitops.h"
+#include "qemu/log.h"
+#include "qemu/cutils.h"
+#include "qemu/event_notifier.h"
+#include "hw/qdev-properties.h"
+#include "qom/object_interfaces.h"
+#include "migration/vmstate.h"
+#include "hw/net/xlnx-versal-canfd.h"
+#include "trace.h"
+
+REG32(SOFTWARE_RESET_REGISTER, 0x0)
+    FIELD(SOFTWARE_RESET_REGISTER, CEN, 1, 1)
+    FIELD(SOFTWARE_RESET_REGISTER, SRST, 0, 1)
+REG32(MODE_SELECT_REGISTER, 0x4)
+    FIELD(MODE_SELECT_REGISTER, ITO, 8, 8)
+    FIELD(MODE_SELECT_REGISTER, ABR, 7, 1)
+    FIELD(MODE_SELECT_REGISTER, SBR, 6, 1)
+    FIELD(MODE_SELECT_REGISTER, DPEE, 5, 1)
+    FIELD(MODE_SELECT_REGISTER, DAR, 4, 1)
+    FIELD(MODE_SELECT_REGISTER, BRSD, 3, 1)
+    FIELD(MODE_SELECT_REGISTER, SNOOP, 2, 1)
+    FIELD(MODE_SELECT_REGISTER, LBACK, 1, 1)
+    FIELD(MODE_SELECT_REGISTER, SLEEP, 0, 1)
+REG32(ARBITRATION_PHASE_BAUD_RATE_PRESCALER_REGISTER, 0x8)
+    FIELD(ARBITRATION_PHASE_BAUD_RATE_PRESCALER_REGISTER, BRP, 0, 8)
+REG32(ARBITRATION_PHASE_BIT_TIMING_REGISTER, 0xc)
+    FIELD(ARBITRATION_PHASE_BIT_TIMING_REGISTER, SJW, 16, 7)
+    FIELD(ARBITRATION_PHASE_BIT_TIMING_REGISTER, TS2, 8, 7)
+    FIELD(ARBITRATION_PHASE_BIT_TIMING_REGISTER, TS1, 0, 8)
+REG32(ERROR_COUNTER_REGISTER, 0x10)
+    FIELD(ERROR_COUNTER_REGISTER, REC, 8, 8)
+    FIELD(ERROR_COUNTER_REGISTER, TEC, 0, 8)
+REG32(ERROR_STATUS_REGISTER, 0x14)
+    FIELD(ERROR_STATUS_REGISTER, F_BERR, 11, 1)
+    FIELD(ERROR_STATUS_REGISTER, F_STER, 10, 1)
+    FIELD(ERROR_STATUS_REGISTER, F_FMER, 9, 1)
+    FIELD(ERROR_STATUS_REGISTER, F_CRCER, 8, 1)
+    FIELD(ERROR_STATUS_REGISTER, ACKER, 4, 1)
+    FIELD(ERROR_STATUS_REGISTER, BERR, 3, 1)
+    FIELD(ERROR_STATUS_REGISTER, STER, 2, 1)
+    FIELD(ERROR_STATUS_REGISTER, FMER, 1, 1)
+    FIELD(ERROR_STATUS_REGISTER, CRCER, 0, 1)
+REG32(STATUS_REGISTER, 0x18)
+    FIELD(STATUS_REGISTER, TDCV, 16, 7)
+    FIELD(STATUS_REGISTER, SNOOP, 12, 1)
+    FIELD(STATUS_REGISTER, BSFR_CONFIG, 10, 1)
+    FIELD(STATUS_REGISTER, PEE_CONFIG, 9, 1)
+    FIELD(STATUS_REGISTER, ESTAT, 7, 2)
+    FIELD(STATUS_REGISTER, ERRWRN, 6, 1)
+    FIELD(STATUS_REGISTER, BBSY, 5, 1)
+    FIELD(STATUS_REGISTER, BIDLE, 4, 1)
+    FIELD(STATUS_REGISTER, NORMAL, 3, 1)
+    FIELD(STATUS_REGISTER, SLEEP, 2, 1)
+    FIELD(STATUS_REGISTER, LBACK, 1, 1)
+    FIELD(STATUS_REGISTER, CONFIG, 0, 1)
+REG32(INTERRUPT_STATUS_REGISTER, 0x1c)
+    FIELD(INTERRUPT_STATUS_REGISTER, TXEWMFLL, 31, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, TXEOFLW, 30, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, RXBOFLW_BI, 24, 6)
+    FIELD(INTERRUPT_STATUS_REGISTER, RXLRM_BI, 18, 6)
+    FIELD(INTERRUPT_STATUS_REGISTER, RXMNF, 17, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, RXFWMFLL_1, 16, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, RXFOFLW_1, 15, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, TXCRS, 14, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, TXRRS, 13, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, RXFWMFLL, 12, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, WKUP, 11, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, SLP, 10, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, BSOFF, 9, 1)
+    /*
+     * In the original HW description below bit is named as ERROR but an ERROR
+     * field name collides with a macro in Windows build. To avoid Windows build
+     * failures, the bit is renamed to ERROR_BIT.
+     */
+    FIELD(INTERRUPT_STATUS_REGISTER, ERROR_BIT, 8, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, RXFOFLW, 6, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, TSCNT_OFLW, 5, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, RXOK, 4, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, BSFRD, 3, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, PEE, 2, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, TXOK, 1, 1)
+    FIELD(INTERRUPT_STATUS_REGISTER, ARBLST, 0, 1)
+REG32(INTERRUPT_ENABLE_REGISTER, 0x20)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ETXEWMFLL, 31, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ETXEOFLW, 30, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ERXMNF, 17, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ERXFWMFLL_1, 16, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ERXFOFLW_1, 15, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ETXCRS, 14, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ETXRRS, 13, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ERXFWMFLL, 12, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, EWKUP, 11, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ESLP, 10, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, EBSOFF, 9, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, EERROR, 8, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ERFXOFLW, 6, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ETSCNT_OFLW, 5, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ERXOK, 4, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, EBSFRD, 3, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, EPEE, 2, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, ETXOK, 1, 1)
+    FIELD(INTERRUPT_ENABLE_REGISTER, EARBLOST, 0, 1)
+REG32(INTERRUPT_CLEAR_REGISTER, 0x24)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CTXEWMFLL, 31, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CTXEOFLW, 30, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CRXMNF, 17, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CRXFWMFLL_1, 16, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CRXFOFLW_1, 15, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CTXCRS, 14, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CTXRRS, 13, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CRXFWMFLL, 12, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CWKUP, 11, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CSLP, 10, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CBSOFF, 9, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CERROR, 8, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CRFXOFLW, 6, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CTSCNT_OFLW, 5, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CRXOK, 4, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CBSFRD, 3, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CPEE, 2, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CTXOK, 1, 1)
+    FIELD(INTERRUPT_CLEAR_REGISTER, CARBLOST, 0, 1)
+REG32(TIMESTAMP_REGISTER, 0x28)
+    FIELD(TIMESTAMP_REGISTER, TIMESTAMP_CNT, 16, 16)
+    FIELD(TIMESTAMP_REGISTER, CTS, 0, 1)
+REG32(DATA_PHASE_BAUD_RATE_PRESCALER_REGISTER, 0x88)
+    FIELD(DATA_PHASE_BAUD_RATE_PRESCALER_REGISTER, TDC, 16, 1)
+    FIELD(DATA_PHASE_BAUD_RATE_PRESCALER_REGISTER, TDCOFF, 8, 6)
+    FIELD(DATA_PHASE_BAUD_RATE_PRESCALER_REGISTER, DP_BRP, 0, 8)
+REG32(DATA_PHASE_BIT_TIMING_REGISTER, 0x8c)
+    FIELD(DATA_PHASE_BIT_TIMING_REGISTER, DP_SJW, 16, 4)
+    FIELD(DATA_PHASE_BIT_TIMING_REGISTER, DP_TS2, 8, 4)
+    FIELD(DATA_PHASE_BIT_TIMING_REGISTER, DP_TS1, 0, 5)
+REG32(TX_BUFFER_READY_REQUEST_REGISTER, 0x90)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR31, 31, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR30, 30, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR29, 29, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR28, 28, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR27, 27, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR26, 26, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR25, 25, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR24, 24, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR23, 23, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR22, 22, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR21, 21, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR20, 20, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR19, 19, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR18, 18, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR17, 17, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR16, 16, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR15, 15, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR14, 14, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR13, 13, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR12, 12, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR11, 11, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR10, 10, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR9, 9, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR8, 8, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR7, 7, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR6, 6, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR5, 5, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR4, 4, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR3, 3, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR2, 2, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR1, 1, 1)
+    FIELD(TX_BUFFER_READY_REQUEST_REGISTER, RR0, 0, 1)
+REG32(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, 0x94)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS31, 31, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS30, 30, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS29, 29, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS28, 28, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS27, 27, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS26, 26, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS25, 25, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS24, 24, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS23, 23, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS22, 22, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS21, 21, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS20, 20, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS19, 19, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS18, 18, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS17, 17, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS16, 16, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS15, 15, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS14, 14, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS13, 13, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS12, 12, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS11, 11, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS10, 10, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS9, 9, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS8, 8, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS7, 7, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS6, 6, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS5, 5, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS4, 4, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS3, 3, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS2, 2, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS1, 1, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER, ERRS0, 0, 1)
+REG32(TX_BUFFER_CANCEL_REQUEST_REGISTER, 0x98)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR31, 31, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR30, 30, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR29, 29, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR28, 28, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR27, 27, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR26, 26, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR25, 25, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR24, 24, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR23, 23, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR22, 22, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR21, 21, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR20, 20, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR19, 19, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR18, 18, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR17, 17, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR16, 16, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR15, 15, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR14, 14, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR13, 13, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR12, 12, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR11, 11, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR10, 10, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR9, 9, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR8, 8, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR7, 7, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR6, 6, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR5, 5, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR4, 4, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR3, 3, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR2, 2, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR1, 1, 1)
+    FIELD(TX_BUFFER_CANCEL_REQUEST_REGISTER, CR0, 0, 1)
+REG32(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, 0x9c)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS31, 31,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS30, 30,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS29, 29,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS28, 28,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS27, 27,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS26, 26,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS25, 25,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS24, 24,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS23, 23,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS22, 22,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS21, 21,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS20, 20,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS19, 19,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS18, 18,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS17, 17,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS16, 16,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS15, 15,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS14, 14,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS13, 13,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS12, 12,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS11, 11,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS10, 10,
+            1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS9, 9, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS8, 8, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS7, 7, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS6, 6, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS5, 5, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS4, 4, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS3, 3, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS2, 2, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS1, 1, 1)
+    FIELD(INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER, ECRS0, 0, 1)
+REG32(TX_EVENT_FIFO_STATUS_REGISTER, 0xa0)
+    FIELD(TX_EVENT_FIFO_STATUS_REGISTER, TXE_FL, 8, 6)
+    FIELD(TX_EVENT_FIFO_STATUS_REGISTER, TXE_IRI, 7, 1)
+    FIELD(TX_EVENT_FIFO_STATUS_REGISTER, TXE_RI, 0, 5)
+REG32(TX_EVENT_FIFO_WATERMARK_REGISTER, 0xa4)
+    FIELD(TX_EVENT_FIFO_WATERMARK_REGISTER, TXE_FWM, 0, 5)
+REG32(ACCEPTANCE_FILTER_CONTROL_REGISTER, 0xe0)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF31, 31, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF30, 30, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF29, 29, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF28, 28, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF27, 27, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF26, 26, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF25, 25, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF24, 24, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF23, 23, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF22, 22, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF21, 21, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF20, 20, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF19, 19, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF18, 18, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF17, 17, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF16, 16, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF15, 15, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF14, 14, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF13, 13, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF12, 12, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF11, 11, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF10, 10, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF9, 9, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF8, 8, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF7, 7, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF6, 6, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF5, 5, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF4, 4, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF3, 3, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF2, 2, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF1, 1, 1)
+    FIELD(ACCEPTANCE_FILTER_CONTROL_REGISTER, UAF0, 0, 1)
+REG32(RX_FIFO_STATUS_REGISTER, 0xe8)
+    FIELD(RX_FIFO_STATUS_REGISTER, FL_1, 24, 7)
+    FIELD(RX_FIFO_STATUS_REGISTER, IRI_1, 23, 1)
+    FIELD(RX_FIFO_STATUS_REGISTER, RI_1, 16, 6)
+    FIELD(RX_FIFO_STATUS_REGISTER, FL, 8, 7)
+    FIELD(RX_FIFO_STATUS_REGISTER, IRI, 7, 1)
+    FIELD(RX_FIFO_STATUS_REGISTER, RI, 0, 6)
+REG32(RX_FIFO_WATERMARK_REGISTER, 0xec)
+    FIELD(RX_FIFO_WATERMARK_REGISTER, RXFP, 16, 5)
+    FIELD(RX_FIFO_WATERMARK_REGISTER, RXFWM_1, 8, 6)
+    FIELD(RX_FIFO_WATERMARK_REGISTER, RXFWM, 0, 6)
+REG32(TB_ID_REGISTER, 0x100)
+    FIELD(TB_ID_REGISTER, ID, 21, 11)
+    FIELD(TB_ID_REGISTER, SRR_RTR_RRS, 20, 1)
+    FIELD(TB_ID_REGISTER, IDE, 19, 1)
+    FIELD(TB_ID_REGISTER, ID_EXT, 1, 18)
+    FIELD(TB_ID_REGISTER, RTR_RRS, 0, 1)
+REG32(TB0_DLC_REGISTER, 0x104)
+    FIELD(TB0_DLC_REGISTER, DLC, 28, 4)
+    FIELD(TB0_DLC_REGISTER, FDF, 27, 1)
+    FIELD(TB0_DLC_REGISTER, BRS, 26, 1)
+    FIELD(TB0_DLC_REGISTER, RSVD2, 25, 1)
+    FIELD(TB0_DLC_REGISTER, EFC, 24, 1)
+    FIELD(TB0_DLC_REGISTER, MM, 16, 8)
+    FIELD(TB0_DLC_REGISTER, RSVD1, 0, 16)
+REG32(TB_DW0_REGISTER, 0x108)
+    FIELD(TB_DW0_REGISTER, DATA_BYTES0, 24, 8)
+    FIELD(TB_DW0_REGISTER, DATA_BYTES1, 16, 8)
+    FIELD(TB_DW0_REGISTER, DATA_BYTES2, 8, 8)
+    FIELD(TB_DW0_REGISTER, DATA_BYTES3, 0, 8)
+REG32(TB_DW1_REGISTER, 0x10c)
+    FIELD(TB_DW1_REGISTER, DATA_BYTES4, 24, 8)
+    FIELD(TB_DW1_REGISTER, DATA_BYTES5, 16, 8)
+    FIELD(TB_DW1_REGISTER, DATA_BYTES6, 8, 8)
+    FIELD(TB_DW1_REGISTER, DATA_BYTES7, 0, 8)
+REG32(TB_DW2_REGISTER, 0x110)
+    FIELD(TB_DW2_REGISTER, DATA_BYTES8, 24, 8)
+    FIELD(TB_DW2_REGISTER, DATA_BYTES9, 16, 8)
+    FIELD(TB_DW2_REGISTER, DATA_BYTES10, 8, 8)
+    FIELD(TB_DW2_REGISTER, DATA_BYTES11, 0, 8)
+REG32(TB_DW3_REGISTER, 0x114)
+    FIELD(TB_DW3_REGISTER, DATA_BYTES12, 24, 8)
+    FIELD(TB_DW3_REGISTER, DATA_BYTES13, 16, 8)
+    FIELD(TB_DW3_REGISTER, DATA_BYTES14, 8, 8)
+    FIELD(TB_DW3_REGISTER, DATA_BYTES15, 0, 8)
+REG32(TB_DW4_REGISTER, 0x118)
+    FIELD(TB_DW4_REGISTER, DATA_BYTES16, 24, 8)
+    FIELD(TB_DW4_REGISTER, DATA_BYTES17, 16, 8)
+    FIELD(TB_DW4_REGISTER, DATA_BYTES18, 8, 8)
+    FIELD(TB_DW4_REGISTER, DATA_BYTES19, 0, 8)
+REG32(TB_DW5_REGISTER, 0x11c)
+    FIELD(TB_DW5_REGISTER, DATA_BYTES20, 24, 8)
+    FIELD(TB_DW5_REGISTER, DATA_BYTES21, 16, 8)
+    FIELD(TB_DW5_REGISTER, DATA_BYTES22, 8, 8)
+    FIELD(TB_DW5_REGISTER, DATA_BYTES23, 0, 8)
+REG32(TB_DW6_REGISTER, 0x120)
+    FIELD(TB_DW6_REGISTER, DATA_BYTES24, 24, 8)
+    FIELD(TB_DW6_REGISTER, DATA_BYTES25, 16, 8)
+    FIELD(TB_DW6_REGISTER, DATA_BYTES26, 8, 8)
+    FIELD(TB_DW6_REGISTER, DATA_BYTES27, 0, 8)
+REG32(TB_DW7_REGISTER, 0x124)
+    FIELD(TB_DW7_REGISTER, DATA_BYTES28, 24, 8)
+    FIELD(TB_DW7_REGISTER, DATA_BYTES29, 16, 8)
+    FIELD(TB_DW7_REGISTER, DATA_BYTES30, 8, 8)
+    FIELD(TB_DW7_REGISTER, DATA_BYTES31, 0, 8)
+REG32(TB_DW8_REGISTER, 0x128)
+    FIELD(TB_DW8_REGISTER, DATA_BYTES32, 24, 8)
+    FIELD(TB_DW8_REGISTER, DATA_BYTES33, 16, 8)
+    FIELD(TB_DW8_REGISTER, DATA_BYTES34, 8, 8)
+    FIELD(TB_DW8_REGISTER, DATA_BYTES35, 0, 8)
+REG32(TB_DW9_REGISTER, 0x12c)
+    FIELD(TB_DW9_REGISTER, DATA_BYTES36, 24, 8)
+    FIELD(TB_DW9_REGISTER, DATA_BYTES37, 16, 8)
+    FIELD(TB_DW9_REGISTER, DATA_BYTES38, 8, 8)
+    FIELD(TB_DW9_REGISTER, DATA_BYTES39, 0, 8)
+REG32(TB_DW10_REGISTER, 0x130)
+    FIELD(TB_DW10_REGISTER, DATA_BYTES40, 24, 8)
+    FIELD(TB_DW10_REGISTER, DATA_BYTES41, 16, 8)
+    FIELD(TB_DW10_REGISTER, DATA_BYTES42, 8, 8)
+    FIELD(TB_DW10_REGISTER, DATA_BYTES43, 0, 8)
+REG32(TB_DW11_REGISTER, 0x134)
+    FIELD(TB_DW11_REGISTER, DATA_BYTES44, 24, 8)
+    FIELD(TB_DW11_REGISTER, DATA_BYTES45, 16, 8)
+    FIELD(TB_DW11_REGISTER, DATA_BYTES46, 8, 8)
+    FIELD(TB_DW11_REGISTER, DATA_BYTES47, 0, 8)
+REG32(TB_DW12_REGISTER, 0x138)
+    FIELD(TB_DW12_REGISTER, DATA_BYTES48, 24, 8)
+    FIELD(TB_DW12_REGISTER, DATA_BYTES49, 16, 8)
+    FIELD(TB_DW12_REGISTER, DATA_BYTES50, 8, 8)
+    FIELD(TB_DW12_REGISTER, DATA_BYTES51, 0, 8)
+REG32(TB_DW13_REGISTER, 0x13c)
+    FIELD(TB_DW13_REGISTER, DATA_BYTES52, 24, 8)
+    FIELD(TB_DW13_REGISTER, DATA_BYTES53, 16, 8)
+    FIELD(TB_DW13_REGISTER, DATA_BYTES54, 8, 8)
+    FIELD(TB_DW13_REGISTER, DATA_BYTES55, 0, 8)
+REG32(TB_DW14_REGISTER, 0x140)
+    FIELD(TB_DW14_REGISTER, DATA_BYTES56, 24, 8)
+    FIELD(TB_DW14_REGISTER, DATA_BYTES57, 16, 8)
+    FIELD(TB_DW14_REGISTER, DATA_BYTES58, 8, 8)
+    FIELD(TB_DW14_REGISTER, DATA_BYTES59, 0, 8)
+REG32(TB_DW15_REGISTER, 0x144)
+    FIELD(TB_DW15_REGISTER, DATA_BYTES60, 24, 8)
+    FIELD(TB_DW15_REGISTER, DATA_BYTES61, 16, 8)
+    FIELD(TB_DW15_REGISTER, DATA_BYTES62, 8, 8)
+    FIELD(TB_DW15_REGISTER, DATA_BYTES63, 0, 8)
+REG32(AFMR_REGISTER, 0xa00)
+    FIELD(AFMR_REGISTER, AMID, 21, 11)
+    FIELD(AFMR_REGISTER, AMSRR, 20, 1)
+    FIELD(AFMR_REGISTER, AMIDE, 19, 1)
+    FIELD(AFMR_REGISTER, AMID_EXT, 1, 18)
+    FIELD(AFMR_REGISTER, AMRTR, 0, 1)
+REG32(AFIR_REGISTER, 0xa04)
+    FIELD(AFIR_REGISTER, AIID, 21, 11)
+    FIELD(AFIR_REGISTER, AISRR, 20, 1)
+    FIELD(AFIR_REGISTER, AIIDE, 19, 1)
+    FIELD(AFIR_REGISTER, AIID_EXT, 1, 18)
+    FIELD(AFIR_REGISTER, AIRTR, 0, 1)
+REG32(TXE_FIFO_TB_ID_REGISTER, 0x2000)
+    FIELD(TXE_FIFO_TB_ID_REGISTER, ID, 21, 11)
+    FIELD(TXE_FIFO_TB_ID_REGISTER, SRR_RTR_RRS, 20, 1)
+    FIELD(TXE_FIFO_TB_ID_REGISTER, IDE, 19, 1)
+    FIELD(TXE_FIFO_TB_ID_REGISTER, ID_EXT, 1, 18)
+    FIELD(TXE_FIFO_TB_ID_REGISTER, RTR_RRS, 0, 1)
+REG32(TXE_FIFO_TB_DLC_REGISTER, 0x2004)
+    FIELD(TXE_FIFO_TB_DLC_REGISTER, DLC, 28, 4)
+    FIELD(TXE_FIFO_TB_DLC_REGISTER, FDF, 27, 1)
+    FIELD(TXE_FIFO_TB_DLC_REGISTER, BRS, 26, 1)
+    FIELD(TXE_FIFO_TB_DLC_REGISTER, ET, 24, 2)
+    FIELD(TXE_FIFO_TB_DLC_REGISTER, MM, 16, 8)
+    FIELD(TXE_FIFO_TB_DLC_REGISTER, TIMESTAMP, 0, 16)
+REG32(RB_ID_REGISTER, 0x2100)
+    FIELD(RB_ID_REGISTER, ID, 21, 11)
+    FIELD(RB_ID_REGISTER, SRR_RTR_RRS, 20, 1)
+    FIELD(RB_ID_REGISTER, IDE, 19, 1)
+    FIELD(RB_ID_REGISTER, ID_EXT, 1, 18)
+    FIELD(RB_ID_REGISTER, RTR_RRS, 0, 1)
+REG32(RB_DLC_REGISTER, 0x2104)
+    FIELD(RB_DLC_REGISTER, DLC, 28, 4)
+    FIELD(RB_DLC_REGISTER, FDF, 27, 1)
+    FIELD(RB_DLC_REGISTER, BRS, 26, 1)
+    FIELD(RB_DLC_REGISTER, ESI, 25, 1)
+    FIELD(RB_DLC_REGISTER, MATCHED_FILTER_INDEX, 16, 5)
+    FIELD(RB_DLC_REGISTER, TIMESTAMP, 0, 16)
+REG32(RB_DW0_REGISTER, 0x2108)
+    FIELD(RB_DW0_REGISTER, DATA_BYTES0, 24, 8)
+    FIELD(RB_DW0_REGISTER, DATA_BYTES1, 16, 8)
+    FIELD(RB_DW0_REGISTER, DATA_BYTES2, 8, 8)
+    FIELD(RB_DW0_REGISTER, DATA_BYTES3, 0, 8)
+REG32(RB_DW1_REGISTER, 0x210c)
+    FIELD(RB_DW1_REGISTER, DATA_BYTES4, 24, 8)
+    FIELD(RB_DW1_REGISTER, DATA_BYTES5, 16, 8)
+    FIELD(RB_DW1_REGISTER, DATA_BYTES6, 8, 8)
+    FIELD(RB_DW1_REGISTER, DATA_BYTES7, 0, 8)
+REG32(RB_DW2_REGISTER, 0x2110)
+    FIELD(RB_DW2_REGISTER, DATA_BYTES8, 24, 8)
+    FIELD(RB_DW2_REGISTER, DATA_BYTES9, 16, 8)
+    FIELD(RB_DW2_REGISTER, DATA_BYTES10, 8, 8)
+    FIELD(RB_DW2_REGISTER, DATA_BYTES11, 0, 8)
+REG32(RB_DW3_REGISTER, 0x2114)
+    FIELD(RB_DW3_REGISTER, DATA_BYTES12, 24, 8)
+    FIELD(RB_DW3_REGISTER, DATA_BYTES13, 16, 8)
+    FIELD(RB_DW3_REGISTER, DATA_BYTES14, 8, 8)
+    FIELD(RB_DW3_REGISTER, DATA_BYTES15, 0, 8)
+REG32(RB_DW4_REGISTER, 0x2118)
+    FIELD(RB_DW4_REGISTER, DATA_BYTES16, 24, 8)
+    FIELD(RB_DW4_REGISTER, DATA_BYTES17, 16, 8)
+    FIELD(RB_DW4_REGISTER, DATA_BYTES18, 8, 8)
+    FIELD(RB_DW4_REGISTER, DATA_BYTES19, 0, 8)
+REG32(RB_DW5_REGISTER, 0x211c)
+    FIELD(RB_DW5_REGISTER, DATA_BYTES20, 24, 8)
+    FIELD(RB_DW5_REGISTER, DATA_BYTES21, 16, 8)
+    FIELD(RB_DW5_REGISTER, DATA_BYTES22, 8, 8)
+    FIELD(RB_DW5_REGISTER, DATA_BYTES23, 0, 8)
+REG32(RB_DW6_REGISTER, 0x2120)
+    FIELD(RB_DW6_REGISTER, DATA_BYTES24, 24, 8)
+    FIELD(RB_DW6_REGISTER, DATA_BYTES25, 16, 8)
+    FIELD(RB_DW6_REGISTER, DATA_BYTES26, 8, 8)
+    FIELD(RB_DW6_REGISTER, DATA_BYTES27, 0, 8)
+REG32(RB_DW7_REGISTER, 0x2124)
+    FIELD(RB_DW7_REGISTER, DATA_BYTES28, 24, 8)
+    FIELD(RB_DW7_REGISTER, DATA_BYTES29, 16, 8)
+    FIELD(RB_DW7_REGISTER, DATA_BYTES30, 8, 8)
+    FIELD(RB_DW7_REGISTER, DATA_BYTES31, 0, 8)
+REG32(RB_DW8_REGISTER, 0x2128)
+    FIELD(RB_DW8_REGISTER, DATA_BYTES32, 24, 8)
+    FIELD(RB_DW8_REGISTER, DATA_BYTES33, 16, 8)
+    FIELD(RB_DW8_REGISTER, DATA_BYTES34, 8, 8)
+    FIELD(RB_DW8_REGISTER, DATA_BYTES35, 0, 8)
+REG32(RB_DW9_REGISTER, 0x212c)
+    FIELD(RB_DW9_REGISTER, DATA_BYTES36, 24, 8)
+    FIELD(RB_DW9_REGISTER, DATA_BYTES37, 16, 8)
+    FIELD(RB_DW9_REGISTER, DATA_BYTES38, 8, 8)
+    FIELD(RB_DW9_REGISTER, DATA_BYTES39, 0, 8)
+REG32(RB_DW10_REGISTER, 0x2130)
+    FIELD(RB_DW10_REGISTER, DATA_BYTES40, 24, 8)
+    FIELD(RB_DW10_REGISTER, DATA_BYTES41, 16, 8)
+    FIELD(RB_DW10_REGISTER, DATA_BYTES42, 8, 8)
+    FIELD(RB_DW10_REGISTER, DATA_BYTES43, 0, 8)
+REG32(RB_DW11_REGISTER, 0x2134)
+    FIELD(RB_DW11_REGISTER, DATA_BYTES44, 24, 8)
+    FIELD(RB_DW11_REGISTER, DATA_BYTES45, 16, 8)
+    FIELD(RB_DW11_REGISTER, DATA_BYTES46, 8, 8)
+    FIELD(RB_DW11_REGISTER, DATA_BYTES47, 0, 8)
+REG32(RB_DW12_REGISTER, 0x2138)
+    FIELD(RB_DW12_REGISTER, DATA_BYTES48, 24, 8)
+    FIELD(RB_DW12_REGISTER, DATA_BYTES49, 16, 8)
+    FIELD(RB_DW12_REGISTER, DATA_BYTES50, 8, 8)
+    FIELD(RB_DW12_REGISTER, DATA_BYTES51, 0, 8)
+REG32(RB_DW13_REGISTER, 0x213c)
+    FIELD(RB_DW13_REGISTER, DATA_BYTES52, 24, 8)
+    FIELD(RB_DW13_REGISTER, DATA_BYTES53, 16, 8)
+    FIELD(RB_DW13_REGISTER, DATA_BYTES54, 8, 8)
+    FIELD(RB_DW13_REGISTER, DATA_BYTES55, 0, 8)
+REG32(RB_DW14_REGISTER, 0x2140)
+    FIELD(RB_DW14_REGISTER, DATA_BYTES56, 24, 8)
+    FIELD(RB_DW14_REGISTER, DATA_BYTES57, 16, 8)
+    FIELD(RB_DW14_REGISTER, DATA_BYTES58, 8, 8)
+    FIELD(RB_DW14_REGISTER, DATA_BYTES59, 0, 8)
+REG32(RB_DW15_REGISTER, 0x2144)
+    FIELD(RB_DW15_REGISTER, DATA_BYTES60, 24, 8)
+    FIELD(RB_DW15_REGISTER, DATA_BYTES61, 16, 8)
+    FIELD(RB_DW15_REGISTER, DATA_BYTES62, 8, 8)
+    FIELD(RB_DW15_REGISTER, DATA_BYTES63, 0, 8)
+REG32(RB_ID_REGISTER_1, 0x4100)
+    FIELD(RB_ID_REGISTER_1, ID, 21, 11)
+    FIELD(RB_ID_REGISTER_1, SRR_RTR_RRS, 20, 1)
+    FIELD(RB_ID_REGISTER_1, IDE, 19, 1)
+    FIELD(RB_ID_REGISTER_1, ID_EXT, 1, 18)
+    FIELD(RB_ID_REGISTER_1, RTR_RRS, 0, 1)
+REG32(RB_DLC_REGISTER_1, 0x4104)
+    FIELD(RB_DLC_REGISTER_1, DLC, 28, 4)
+    FIELD(RB_DLC_REGISTER_1, FDF, 27, 1)
+    FIELD(RB_DLC_REGISTER_1, BRS, 26, 1)
+    FIELD(RB_DLC_REGISTER_1, ESI, 25, 1)
+    FIELD(RB_DLC_REGISTER_1, MATCHED_FILTER_INDEX, 16, 5)
+    FIELD(RB_DLC_REGISTER_1, TIMESTAMP, 0, 16)
+REG32(RB0_DW0_REGISTER_1, 0x4108)
+    FIELD(RB0_DW0_REGISTER_1, DATA_BYTES0, 24, 8)
+    FIELD(RB0_DW0_REGISTER_1, DATA_BYTES1, 16, 8)
+    FIELD(RB0_DW0_REGISTER_1, DATA_BYTES2, 8, 8)
+    FIELD(RB0_DW0_REGISTER_1, DATA_BYTES3, 0, 8)
+REG32(RB_DW1_REGISTER_1, 0x410c)
+    FIELD(RB_DW1_REGISTER_1, DATA_BYTES4, 24, 8)
+    FIELD(RB_DW1_REGISTER_1, DATA_BYTES5, 16, 8)
+    FIELD(RB_DW1_REGISTER_1, DATA_BYTES6, 8, 8)
+    FIELD(RB_DW1_REGISTER_1, DATA_BYTES7, 0, 8)
+REG32(RB_DW2_REGISTER_1, 0x4110)
+    FIELD(RB_DW2_REGISTER_1, DATA_BYTES8, 24, 8)
+    FIELD(RB_DW2_REGISTER_1, DATA_BYTES9, 16, 8)
+    FIELD(RB_DW2_REGISTER_1, DATA_BYTES10, 8, 8)
+    FIELD(RB_DW2_REGISTER_1, DATA_BYTES11, 0, 8)
+REG32(RB_DW3_REGISTER_1, 0x4114)
+    FIELD(RB_DW3_REGISTER_1, DATA_BYTES12, 24, 8)
+    FIELD(RB_DW3_REGISTER_1, DATA_BYTES13, 16, 8)
+    FIELD(RB_DW3_REGISTER_1, DATA_BYTES14, 8, 8)
+    FIELD(RB_DW3_REGISTER_1, DATA_BYTES15, 0, 8)
+REG32(RB_DW4_REGISTER_1, 0x4118)
+    FIELD(RB_DW4_REGISTER_1, DATA_BYTES16, 24, 8)
+    FIELD(RB_DW4_REGISTER_1, DATA_BYTES17, 16, 8)
+    FIELD(RB_DW4_REGISTER_1, DATA_BYTES18, 8, 8)
+    FIELD(RB_DW4_REGISTER_1, DATA_BYTES19, 0, 8)
+REG32(RB_DW5_REGISTER_1, 0x411c)
+    FIELD(RB_DW5_REGISTER_1, DATA_BYTES20, 24, 8)
+    FIELD(RB_DW5_REGISTER_1, DATA_BYTES21, 16, 8)
+    FIELD(RB_DW5_REGISTER_1, DATA_BYTES22, 8, 8)
+    FIELD(RB_DW5_REGISTER_1, DATA_BYTES23, 0, 8)
+REG32(RB_DW6_REGISTER_1, 0x4120)
+    FIELD(RB_DW6_REGISTER_1, DATA_BYTES24, 24, 8)
+    FIELD(RB_DW6_REGISTER_1, DATA_BYTES25, 16, 8)
+    FIELD(RB_DW6_REGISTER_1, DATA_BYTES26, 8, 8)
+    FIELD(RB_DW6_REGISTER_1, DATA_BYTES27, 0, 8)
+REG32(RB_DW7_REGISTER_1, 0x4124)
+    FIELD(RB_DW7_REGISTER_1, DATA_BYTES28, 24, 8)
+    FIELD(RB_DW7_REGISTER_1, DATA_BYTES29, 16, 8)
+    FIELD(RB_DW7_REGISTER_1, DATA_BYTES30, 8, 8)
+    FIELD(RB_DW7_REGISTER_1, DATA_BYTES31, 0, 8)
+REG32(RB_DW8_REGISTER_1, 0x4128)
+    FIELD(RB_DW8_REGISTER_1, DATA_BYTES32, 24, 8)
+    FIELD(RB_DW8_REGISTER_1, DATA_BYTES33, 16, 8)
+    FIELD(RB_DW8_REGISTER_1, DATA_BYTES34, 8, 8)
+    FIELD(RB_DW8_REGISTER_1, DATA_BYTES35, 0, 8)
+REG32(RB_DW9_REGISTER_1, 0x412c)
+    FIELD(RB_DW9_REGISTER_1, DATA_BYTES36, 24, 8)
+    FIELD(RB_DW9_REGISTER_1, DATA_BYTES37, 16, 8)
+    FIELD(RB_DW9_REGISTER_1, DATA_BYTES38, 8, 8)
+    FIELD(RB_DW9_REGISTER_1, DATA_BYTES39, 0, 8)
+REG32(RB_DW10_REGISTER_1, 0x4130)
+    FIELD(RB_DW10_REGISTER_1, DATA_BYTES40, 24, 8)
+    FIELD(RB_DW10_REGISTER_1, DATA_BYTES41, 16, 8)
+    FIELD(RB_DW10_REGISTER_1, DATA_BYTES42, 8, 8)
+    FIELD(RB_DW10_REGISTER_1, DATA_BYTES43, 0, 8)
+REG32(RB_DW11_REGISTER_1, 0x4134)
+    FIELD(RB_DW11_REGISTER_1, DATA_BYTES44, 24, 8)
+    FIELD(RB_DW11_REGISTER_1, DATA_BYTES45, 16, 8)
+    FIELD(RB_DW11_REGISTER_1, DATA_BYTES46, 8, 8)
+    FIELD(RB_DW11_REGISTER_1, DATA_BYTES47, 0, 8)
+REG32(RB_DW12_REGISTER_1, 0x4138)
+    FIELD(RB_DW12_REGISTER_1, DATA_BYTES48, 24, 8)
+    FIELD(RB_DW12_REGISTER_1, DATA_BYTES49, 16, 8)
+    FIELD(RB_DW12_REGISTER_1, DATA_BYTES50, 8, 8)
+    FIELD(RB_DW12_REGISTER_1, DATA_BYTES51, 0, 8)
+REG32(RB_DW13_REGISTER_1, 0x413c)
+    FIELD(RB_DW13_REGISTER_1, DATA_BYTES52, 24, 8)
+    FIELD(RB_DW13_REGISTER_1, DATA_BYTES53, 16, 8)
+    FIELD(RB_DW13_REGISTER_1, DATA_BYTES54, 8, 8)
+    FIELD(RB_DW13_REGISTER_1, DATA_BYTES55, 0, 8)
+REG32(RB_DW14_REGISTER_1, 0x4140)
+    FIELD(RB_DW14_REGISTER_1, DATA_BYTES56, 24, 8)
+    FIELD(RB_DW14_REGISTER_1, DATA_BYTES57, 16, 8)
+    FIELD(RB_DW14_REGISTER_1, DATA_BYTES58, 8, 8)
+    FIELD(RB_DW14_REGISTER_1, DATA_BYTES59, 0, 8)
+REG32(RB_DW15_REGISTER_1, 0x4144)
+    FIELD(RB_DW15_REGISTER_1, DATA_BYTES60, 24, 8)
+    FIELD(RB_DW15_REGISTER_1, DATA_BYTES61, 16, 8)
+    FIELD(RB_DW15_REGISTER_1, DATA_BYTES62, 8, 8)
+    FIELD(RB_DW15_REGISTER_1, DATA_BYTES63, 0, 8)
+
+static uint8_t canfd_dlc_array[8] = {8, 12, 16, 20, 24, 32, 48, 64};
+
+static void canfd_update_irq(XlnxVersalCANFDState *s)
+{
+    unsigned int irq = s->regs[R_INTERRUPT_STATUS_REGISTER] &
+                        s->regs[R_INTERRUPT_ENABLE_REGISTER];
+    g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+    /* RX watermark interrupts. */
+    if (ARRAY_FIELD_EX32(s->regs, RX_FIFO_STATUS_REGISTER, FL) >
+        ARRAY_FIELD_EX32(s->regs, RX_FIFO_WATERMARK_REGISTER, RXFWM)) {
+        ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXFWMFLL, 1);
+    }
+
+    if (ARRAY_FIELD_EX32(s->regs, RX_FIFO_STATUS_REGISTER, FL_1) >
+        ARRAY_FIELD_EX32(s->regs, RX_FIFO_WATERMARK_REGISTER, RXFWM_1)) {
+        ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXFWMFLL_1, 1);
+    }
+
+    /* TX watermark interrupt. */
+    if (ARRAY_FIELD_EX32(s->regs, TX_EVENT_FIFO_STATUS_REGISTER, TXE_FL) >
+        ARRAY_FIELD_EX32(s->regs, TX_EVENT_FIFO_WATERMARK_REGISTER, TXE_FWM)) {
+        ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, TXEWMFLL, 1);
+    }
+
+    trace_xlnx_canfd_update_irq(path, s->regs[R_INTERRUPT_STATUS_REGISTER],
+                                s->regs[R_INTERRUPT_ENABLE_REGISTER], irq);
+
+    qemu_set_irq(s->irq_canfd_int, irq);
+}
+
+static void canfd_ier_post_write(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+
+    canfd_update_irq(s);
+}
+
+static uint64_t canfd_icr_pre_write(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+    uint32_t val = val64;
+
+    s->regs[R_INTERRUPT_STATUS_REGISTER] &= ~val;
+
+    /*
+     * RXBOFLW_BI field is automatically cleared to default if RXBOFLW bit is
+     * cleared in ISR.
+     */
+    if (ARRAY_FIELD_EX32(s->regs, INTERRUPT_STATUS_REGISTER, RXFWMFLL_1)) {
+        ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXBOFLW_BI, 0);
+    }
+
+    canfd_update_irq(s);
+
+    return 0;
+}
+
+static void canfd_config_reset(XlnxVersalCANFDState *s)
+{
+
+    unsigned int i;
+
+    /* Reset all the configuration registers. */
+    for (i = 0; i < R_RX_FIFO_WATERMARK_REGISTER; ++i) {
+        register_reset(&s->reg_info[i]);
+    }
+
+    canfd_update_irq(s);
+}
+
+static void canfd_config_mode(XlnxVersalCANFDState *s)
+{
+    register_reset(&s->reg_info[R_ERROR_COUNTER_REGISTER]);
+    register_reset(&s->reg_info[R_ERROR_STATUS_REGISTER]);
+    register_reset(&s->reg_info[R_STATUS_REGISTER]);
+
+    /* Put XlnxVersalCANFDState in configuration mode. */
+    ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, CONFIG, 1);
+    ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, WKUP, 0);
+    ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, SLP, 0);
+    ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, BSOFF, 0);
+    ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, ERROR_BIT, 0);
+    ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXFOFLW, 0);
+    ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXFOFLW_1, 0);
+    ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXOK, 0);
+    ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, TXOK, 0);
+    ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, ARBLST, 0);
+
+    /* Clear the time stamp. */
+    ptimer_transaction_begin(s->canfd_timer);
+    ptimer_set_count(s->canfd_timer, 0);
+    ptimer_transaction_commit(s->canfd_timer);
+
+    canfd_update_irq(s);
+}
+
+static void update_status_register_mode_bits(XlnxVersalCANFDState *s)
+{
+    bool sleep_status = ARRAY_FIELD_EX32(s->regs, STATUS_REGISTER, SLEEP);
+    bool sleep_mode = ARRAY_FIELD_EX32(s->regs, MODE_SELECT_REGISTER, SLEEP);
+    /* Wake up interrupt bit. */
+    bool wakeup_irq_val = !sleep_mode && sleep_status;
+    /* Sleep interrupt bit. */
+    bool sleep_irq_val = sleep_mode && !sleep_status;
+
+    /* Clear previous core mode status bits. */
+    ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, LBACK, 0);
+    ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, SLEEP, 0);
+    ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, SNOOP, 0);
+    ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, NORMAL, 0);
+
+    /* set current mode bit and generate irqs accordingly. */
+    if (ARRAY_FIELD_EX32(s->regs, MODE_SELECT_REGISTER, LBACK)) {
+        ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, LBACK, 1);
+    } else if (ARRAY_FIELD_EX32(s->regs, MODE_SELECT_REGISTER, SLEEP)) {
+        ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, SLEEP, 1);
+        ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, SLP,
+                         sleep_irq_val);
+    } else if (ARRAY_FIELD_EX32(s->regs, MODE_SELECT_REGISTER, SNOOP)) {
+        ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, SNOOP, 1);
+    } else {
+        /* If all bits are zero, XlnxVersalCANFDState is set in normal mode. */
+        ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, NORMAL, 1);
+        /* Set wakeup interrupt bit. */
+        ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, WKUP,
+                         wakeup_irq_val);
+    }
+
+    /* Put the CANFD in error active state. */
+    ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, ESTAT, 1);
+
+    canfd_update_irq(s);
+}
+
+static uint64_t canfd_msr_pre_write(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+    uint32_t val = val64;
+    uint8_t multi_mode = 0;
+
+    /*
+     * Multiple mode set check. This is done to make sure user doesn't set
+     * multiple modes.
+     */
+    multi_mode = FIELD_EX32(val, MODE_SELECT_REGISTER, LBACK) +
+                 FIELD_EX32(val, MODE_SELECT_REGISTER, SLEEP) +
+                 FIELD_EX32(val, MODE_SELECT_REGISTER, SNOOP);
+
+    if (multi_mode > 1) {
+        qemu_log_mask(LOG_GUEST_ERROR, "Attempting to configure several modes"
+                      " simultaneously. One mode will be selected according to"
+                      " their priority: LBACK > SLEEP > SNOOP.\n");
+    }
+
+    if (ARRAY_FIELD_EX32(s->regs, SOFTWARE_RESET_REGISTER, CEN) == 0) {
+        /* In configuration mode, any mode can be selected. */
+        s->regs[R_MODE_SELECT_REGISTER] = val;
+    } else {
+        bool sleep_mode_bit = FIELD_EX32(val, MODE_SELECT_REGISTER, SLEEP);
+
+        ARRAY_FIELD_DP32(s->regs, MODE_SELECT_REGISTER, SLEEP, sleep_mode_bit);
+
+        if (FIELD_EX32(val, MODE_SELECT_REGISTER, LBACK)) {
+            qemu_log_mask(LOG_GUEST_ERROR, "Attempting to set LBACK mode"
+                          " without setting CEN bit as 0\n");
+        } else if (FIELD_EX32(val, MODE_SELECT_REGISTER, SNOOP)) {
+            qemu_log_mask(LOG_GUEST_ERROR, "Attempting to set SNOOP mode"
+                          " without setting CEN bit as 0\n");
+        }
+
+        update_status_register_mode_bits(s);
+    }
+
+    return s->regs[R_MODE_SELECT_REGISTER];
+}
+
+static void canfd_exit_sleep_mode(XlnxVersalCANFDState *s)
+{
+    ARRAY_FIELD_DP32(s->regs, MODE_SELECT_REGISTER, SLEEP, 0);
+    update_status_register_mode_bits(s);
+}
+
+static void regs2frame(XlnxVersalCANFDState *s, qemu_can_frame *frame,
+                       uint32_t reg_num)
+{
+    uint32_t i = 0;
+    uint32_t j = 0;
+    uint32_t val = 0;
+    uint32_t dlc_reg_val = 0;
+    uint32_t dlc_value = 0;
+
+    /* Check that reg_num should be within TX register space. */
+    assert(reg_num <= R_TB_ID_REGISTER + (NUM_REGS_PER_MSG_SPACE *
+                                          s->cfg.tx_fifo));
+
+    dlc_reg_val = s->regs[reg_num + 1];
+    dlc_value = FIELD_EX32(dlc_reg_val, TB0_DLC_REGISTER, DLC);
+
+    frame->can_id = s->regs[reg_num];
+
+    if (FIELD_EX32(dlc_reg_val, TB0_DLC_REGISTER, FDF)) {
+        /*
+         * CANFD frame.
+         * Converting dlc(0 to 15) 4 Byte data to plain length(i.e. 0 to 64)
+         * 1 Byte data. This is done to make it work with SocketCAN.
+         * On actual CANFD frame, this value can't be more than 0xF.
+         * Conversion table for DLC to plain length:
+         *
+         *  DLC                        Plain Length
+         *  0 - 8                      0 - 8
+         *  9                          9 - 12
+         *  10                         13 - 16
+         *  11                         17 - 20
+         *  12                         21 - 24
+         *  13                         25 - 32
+         *  14                         33 - 48
+         *  15                         49 - 64
+         */
+
+        frame->flags = QEMU_CAN_FRMF_TYPE_FD;
+
+        if (dlc_value < 8) {
+            frame->can_dlc = dlc_value;
+        } else {
+            assert((dlc_value - 8) < ARRAY_SIZE(canfd_dlc_array));
+            frame->can_dlc = canfd_dlc_array[dlc_value - 8];
+        }
+    } else {
+        /*
+         * FD Format bit not set that means it is a CAN Frame.
+         * Conversion table for classic CAN:
+         *
+         *  DLC                        Plain Length
+         *  0 - 7                      0 - 7
+         *  8 - 15                     8
+         */
+
+        if (dlc_value > 8) {
+            frame->can_dlc = 8;
+            qemu_log_mask(LOG_GUEST_ERROR, "Maximum DLC value for Classic CAN"
+                          " frame is 8. Only 8 byte data will be sent.\n");
+        } else {
+            frame->can_dlc = dlc_value;
+        }
+    }
+
+    for (j = 0; j < frame->can_dlc; j++) {
+        val = 8 * i;
+
+        frame->data[j] = extract32(s->regs[reg_num + 2 + (j / 4)], val, 8);
+        i++;
+
+        if (i % 4 == 0) {
+            i = 0;
+        }
+    }
+}
+
+static void process_cancellation_requests(XlnxVersalCANFDState *s)
+{
+    uint32_t clear_mask = s->regs[R_TX_BUFFER_READY_REQUEST_REGISTER] &
+                                 s->regs[R_TX_BUFFER_CANCEL_REQUEST_REGISTER];
+
+    s->regs[R_TX_BUFFER_READY_REQUEST_REGISTER] &= ~clear_mask;
+    s->regs[R_TX_BUFFER_CANCEL_REQUEST_REGISTER] &= ~clear_mask;
+
+    canfd_update_irq(s);
+}
+
+static void store_rx_sequential(XlnxVersalCANFDState *s,
+                                const qemu_can_frame *frame,
+                                uint32_t fill_level, uint32_t read_index,
+                                uint32_t store_location, uint8_t rx_fifo,
+                                bool rx_fifo_id, uint8_t filter_index)
+{
+    int i;
+    bool is_canfd_frame;
+    uint8_t dlc = frame->can_dlc;
+    uint8_t rx_reg_num = 0;
+    uint32_t dlc_reg_val = 0;
+    uint32_t data_reg_val = 0;
+
+    /* Getting RX0/1 fill level */
+    if ((fill_level) > rx_fifo - 1) {
+        g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: RX%d Buffer is full. Discarding the"
+                      " message\n", path, rx_fifo_id);
+
+        /* Set the corresponding RF buffer overflow interrupt. */
+        if (rx_fifo_id == 0) {
+            ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXFOFLW, 1);
+        } else {
+            ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXFOFLW_1, 1);
+        }
+    } else {
+        uint16_t rx_timestamp = CANFD_TIMER_MAX -
+                                    ptimer_get_count(s->canfd_timer);
+
+        if (rx_timestamp == 0xFFFF) {
+            ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, TSCNT_OFLW, 1);
+        } else {
+            ARRAY_FIELD_DP32(s->regs, TIMESTAMP_REGISTER, TIMESTAMP_CNT,
+                             rx_timestamp);
+        }
+
+        if (rx_fifo_id == 0) {
+            ARRAY_FIELD_DP32(s->regs, RX_FIFO_STATUS_REGISTER, FL,
+                             fill_level + 1);
+            assert(store_location <=
+                              R_RB_ID_REGISTER + (s->cfg.rx0_fifo *
+                                                  NUM_REGS_PER_MSG_SPACE));
+        } else {
+            ARRAY_FIELD_DP32(s->regs, RX_FIFO_STATUS_REGISTER, FL_1,
+                             fill_level + 1);
+            assert(store_location <=
+                              R_RB_ID_REGISTER_1 + (s->cfg.rx1_fifo *
+                                                    NUM_REGS_PER_MSG_SPACE));
+        }
+
+        s->regs[store_location] = frame->can_id;
+
+        dlc = frame->can_dlc;
+
+        if (frame->flags == QEMU_CAN_FRMF_TYPE_FD) {
+            is_canfd_frame = true;
+
+            /* Store dlc value in Xilinx specific format. */
+            for (i = 0; i < ARRAY_SIZE(canfd_dlc_array); i++) {
+                if (canfd_dlc_array[i] == frame->can_dlc) {
+                    dlc_reg_val = FIELD_DP32(0, RB_DLC_REGISTER, DLC, 8 + i);
+                }
+            }
+        } else {
+            is_canfd_frame = false;
+
+            if (frame->can_dlc > 8) {
+                dlc = 8;
+            }
+
+            dlc_reg_val = FIELD_DP32(0, RB_DLC_REGISTER, DLC, dlc);
+        }
+
+        dlc_reg_val |= FIELD_DP32(0, RB_DLC_REGISTER, FDF, is_canfd_frame);
+        dlc_reg_val |= FIELD_DP32(0, RB_DLC_REGISTER, TIMESTAMP, rx_timestamp);
+        dlc_reg_val |= FIELD_DP32(0, RB_DLC_REGISTER, MATCHED_FILTER_INDEX,
+                                  filter_index);
+        s->regs[store_location + 1] = dlc_reg_val;
+
+        for (i = 0; i < dlc; i++) {
+            /* Register size is 4 byte but frame->data each is 1 byte. */
+            switch (i % 4) {
+            case 0:
+                rx_reg_num = i / 4;
+
+                data_reg_val = FIELD_DP32(0, RB_DW0_REGISTER, DATA_BYTES3,
+                                          frame->data[i]);
+                break;
+            case 1:
+                data_reg_val |= FIELD_DP32(0, RB_DW0_REGISTER, DATA_BYTES2,
+                                           frame->data[i]);
+                break;
+            case 2:
+                data_reg_val |= FIELD_DP32(0, RB_DW0_REGISTER, DATA_BYTES1,
+                                           frame->data[i]);
+                break;
+            case 3:
+                data_reg_val |= FIELD_DP32(0, RB_DW0_REGISTER, DATA_BYTES0,
+                                           frame->data[i]);
+                /*
+                 * Last Bytes data which means we have all 4 bytes ready to
+                 * store in one rx regs.
+                 */
+                s->regs[store_location + rx_reg_num + 2] = data_reg_val;
+                break;
+            }
+        }
+
+        if (i % 4) {
+            /*
+             * In case DLC is not multiplier of 4, data is not saved to RX FIFO
+             * in above switch case. Store the remaining bytes here.
+             */
+            s->regs[store_location + rx_reg_num + 2] = data_reg_val;
+        }
+
+        /* set the interrupt as RXOK. */
+        ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXOK, 1);
+    }
+}
+
+static void update_rx_sequential(XlnxVersalCANFDState *s,
+                                 const qemu_can_frame *frame)
+{
+    bool filter_pass = false;
+    uint8_t filter_index = 0;
+    int i;
+    int filter_partition = ARRAY_FIELD_EX32(s->regs,
+                                            RX_FIFO_WATERMARK_REGISTER, RXFP);
+    uint32_t store_location;
+    uint32_t fill_level;
+    uint32_t read_index;
+    uint8_t store_index = 0;
+    g_autofree char *path = NULL;
+    /*
+     * If all UAF bits are set to 0, then received messages are not stored
+     * in the RX buffers.
+     */
+    if (s->regs[R_ACCEPTANCE_FILTER_CONTROL_REGISTER]) {
+        uint32_t acceptance_filter_status =
+                                s->regs[R_ACCEPTANCE_FILTER_CONTROL_REGISTER];
+
+        for (i = 0; i < 32; i++) {
+            if (acceptance_filter_status & 0x1) {
+                uint32_t msg_id_masked = s->regs[R_AFMR_REGISTER + 2 * i] &
+                                         frame->can_id;
+                uint32_t afir_id_masked = s->regs[R_AFIR_REGISTER + 2 * i] &
+                                          s->regs[R_AFMR_REGISTER + 2 * i];
+                uint16_t std_msg_id_masked = FIELD_EX32(msg_id_masked,
+                                                        AFIR_REGISTER, AIID);
+                uint16_t std_afir_id_masked = FIELD_EX32(afir_id_masked,
+                                                         AFIR_REGISTER, AIID);
+                uint32_t ext_msg_id_masked = FIELD_EX32(msg_id_masked,
+                                                        AFIR_REGISTER,
+                                                        AIID_EXT);
+                uint32_t ext_afir_id_masked = FIELD_EX32(afir_id_masked,
+                                                         AFIR_REGISTER,
+                                                         AIID_EXT);
+                bool ext_ide = FIELD_EX32(s->regs[R_AFMR_REGISTER + 2 * i],
+                                          AFMR_REGISTER, AMIDE);
+
+                if (std_msg_id_masked == std_afir_id_masked) {
+                    if (ext_ide) {
+                        /* Extended message ID message. */
+                        if (ext_msg_id_masked == ext_afir_id_masked) {
+                            filter_pass = true;
+                            filter_index = i;
+
+                            break;
+                        }
+                    } else {
+                        /* Standard message ID. */
+                        filter_pass = true;
+                        filter_index = i;
+
+                        break;
+                    }
+                }
+            }
+            acceptance_filter_status >>= 1;
+        }
+    }
+
+    if (!filter_pass) {
+        path = object_get_canonical_path(OBJECT(s));
+
+        trace_xlnx_canfd_rx_fifo_filter_reject(path, frame->can_id,
+                                               frame->can_dlc);
+    } else {
+        if (filter_index <= filter_partition) {
+            fill_level = ARRAY_FIELD_EX32(s->regs, RX_FIFO_STATUS_REGISTER, FL);
+            read_index = ARRAY_FIELD_EX32(s->regs, RX_FIFO_STATUS_REGISTER, RI);
+            store_index = read_index + fill_level;
+
+            if (read_index == s->cfg.rx0_fifo - 1) {
+                /*
+                 * When ri is s->cfg.rx0_fifo - 1 i.e. max, it goes cyclic that
+                 * means we reset the ri to 0x0.
+                 */
+                read_index = 0;
+                ARRAY_FIELD_DP32(s->regs, RX_FIFO_STATUS_REGISTER, RI,
+                                 read_index);
+            }
+
+            if (store_index > s->cfg.rx0_fifo - 1) {
+                store_index -= s->cfg.rx0_fifo - 1;
+            }
+
+            store_location = R_RB_ID_REGISTER +
+                          (store_index * NUM_REGS_PER_MSG_SPACE);
+
+            store_rx_sequential(s, frame, fill_level, read_index,
+                                store_location, s->cfg.rx0_fifo, 0,
+                                filter_index);
+        } else {
+            /* RX 1 fill level message */
+            fill_level = ARRAY_FIELD_EX32(s->regs, RX_FIFO_STATUS_REGISTER,
+                                          FL_1);
+            read_index = ARRAY_FIELD_EX32(s->regs, RX_FIFO_STATUS_REGISTER,
+                                          RI_1);
+            store_index = read_index + fill_level;
+
+            if (read_index == s->cfg.rx1_fifo - 1) {
+                /*
+                 * When ri is s->cfg.rx1_fifo - 1 i.e. max, it goes cyclic that
+                 * means we reset the ri to 0x0.
+                 */
+                read_index = 0;
+                ARRAY_FIELD_DP32(s->regs, RX_FIFO_STATUS_REGISTER, RI_1,
+                                 read_index);
+            }
+
+            if (store_index > s->cfg.rx1_fifo - 1) {
+                store_index -= s->cfg.rx1_fifo - 1;
+            }
+
+            store_location = R_RB_ID_REGISTER_1 +
+                          (store_index * NUM_REGS_PER_MSG_SPACE);
+
+            store_rx_sequential(s, frame, fill_level, read_index,
+                                store_location, s->cfg.rx1_fifo, 1,
+                                filter_index);
+        }
+
+        path = object_get_canonical_path(OBJECT(s));
+
+        trace_xlnx_canfd_rx_data(path, frame->can_id, frame->can_dlc,
+                                 frame->flags);
+        canfd_update_irq(s);
+    }
+}
+
+static bool tx_ready_check(XlnxVersalCANFDState *s)
+{
+    if (ARRAY_FIELD_EX32(s->regs, SOFTWARE_RESET_REGISTER, SRST)) {
+        g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: Attempting to transfer data while"
+                      " XlnxVersalCANFDState is in reset mode\n", path);
+
+        return false;
+    }
+
+    if (ARRAY_FIELD_EX32(s->regs, SOFTWARE_RESET_REGISTER, CEN) == 0) {
+        g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: Attempting to transfer data while"
+                      " XlnxVersalCANFDState is in configuration mode."
+                      " Reset the core so operations can start fresh\n",
+                      path);
+        return false;
+    }
+
+    if (ARRAY_FIELD_EX32(s->regs, MODE_SELECT_REGISTER, SNOOP)) {
+        g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: Attempting to transfer data while"
+                      " XlnxVersalCANFDState is in SNOOP MODE\n",
+                      path);
+        return false;
+    }
+
+    return true;
+}
+
+static void tx_fifo_stamp(XlnxVersalCANFDState *s, uint32_t tb0_regid)
+{
+    /*
+     * If EFC bit in DLC message is set, this means we will store the
+     * event of this transmitted message with time stamp.
+     */
+    uint32_t dlc_reg_val = 0;
+
+    if (FIELD_EX32(s->regs[tb0_regid + 1], TB0_DLC_REGISTER, EFC)) {
+        uint8_t dlc_val = FIELD_EX32(s->regs[tb0_regid + 1], TB0_DLC_REGISTER,
+                                     DLC);
+        bool fdf_val = FIELD_EX32(s->regs[tb0_regid + 1], TB0_DLC_REGISTER,
+                                  FDF);
+        bool brs_val = FIELD_EX32(s->regs[tb0_regid + 1], TB0_DLC_REGISTER,
+                                  BRS);
+        uint8_t mm_val = FIELD_EX32(s->regs[tb0_regid + 1], TB0_DLC_REGISTER,
+                                    MM);
+        uint8_t fill_level = ARRAY_FIELD_EX32(s->regs,
+                                              TX_EVENT_FIFO_STATUS_REGISTER,
+                                              TXE_FL);
+        uint8_t read_index = ARRAY_FIELD_EX32(s->regs,
+                                              TX_EVENT_FIFO_STATUS_REGISTER,
+                                              TXE_RI);
+        uint8_t store_index = fill_level + read_index;
+
+        if ((fill_level) > s->cfg.tx_fifo - 1) {
+            qemu_log_mask(LOG_GUEST_ERROR, "TX Event Buffer is full."
+                          " Discarding the message\n");
+            ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, TXEOFLW, 1);
+        } else {
+            if (read_index == s->cfg.tx_fifo - 1) {
+                /*
+                 * When ri is s->cfg.tx_fifo - 1 i.e. max, it goes cyclic that
+                 * means we reset the ri to 0x0.
+                 */
+                read_index = 0;
+                ARRAY_FIELD_DP32(s->regs, TX_EVENT_FIFO_STATUS_REGISTER, TXE_RI,
+                                 read_index);
+            }
+
+            if (store_index > s->cfg.tx_fifo - 1) {
+                store_index -= s->cfg.tx_fifo - 1;
+            }
+
+            assert(store_index < s->cfg.tx_fifo);
+
+            uint32_t tx_event_reg0_id = R_TXE_FIFO_TB_ID_REGISTER +
+                                        (store_index * 2);
+
+            /* Store message ID in TX event register. */
+            s->regs[tx_event_reg0_id] = s->regs[tb0_regid];
+
+            uint16_t tx_timestamp = CANFD_TIMER_MAX -
+                                            ptimer_get_count(s->canfd_timer);
+
+            /* Store DLC with time stamp in DLC regs. */
+            dlc_reg_val = FIELD_DP32(0, TXE_FIFO_TB_DLC_REGISTER, DLC, dlc_val);
+            dlc_reg_val |= FIELD_DP32(0, TXE_FIFO_TB_DLC_REGISTER, FDF,
+                                      fdf_val);
+            dlc_reg_val |= FIELD_DP32(0, TXE_FIFO_TB_DLC_REGISTER, BRS,
+                                      brs_val);
+            dlc_reg_val |= FIELD_DP32(0, TXE_FIFO_TB_DLC_REGISTER, ET, 0x3);
+            dlc_reg_val |= FIELD_DP32(0, TXE_FIFO_TB_DLC_REGISTER, MM, mm_val);
+            dlc_reg_val |= FIELD_DP32(0, TXE_FIFO_TB_DLC_REGISTER, TIMESTAMP,
+                                      tx_timestamp);
+            s->regs[tx_event_reg0_id + 1] = dlc_reg_val;
+
+            ARRAY_FIELD_DP32(s->regs, TX_EVENT_FIFO_STATUS_REGISTER, TXE_FL,
+                             fill_level + 1);
+        }
+    }
+}
+
+static gint g_cmp_ids(gconstpointer data1, gconstpointer data2)
+{
+    tx_ready_reg_info *tx_reg_1 = (tx_ready_reg_info *) data1;
+    tx_ready_reg_info *tx_reg_2 = (tx_ready_reg_info *) data2;
+
+    return tx_reg_1->can_id - tx_reg_2->can_id;
+}
+
+static void free_list(GSList *list)
+{
+    GSList *iterator = NULL;
+
+    for (iterator = list; iterator != NULL; iterator = iterator->next) {
+        g_free((tx_ready_reg_info *)iterator->data);
+    }
+
+    g_slist_free(list);
+
+    return;
+}
+
+static GSList *prepare_tx_data(XlnxVersalCANFDState *s)
+{
+    uint8_t i = 0;
+    GSList *list = NULL;
+    uint32_t reg_num = 0;
+    uint32_t reg_ready = s->regs[R_TX_BUFFER_READY_REQUEST_REGISTER];
+
+    /* First find the messages which are ready for transmission. */
+    for (i = 0; i < s->cfg.tx_fifo; i++) {
+        if (reg_ready & 1) {
+            reg_num = R_TB_ID_REGISTER + (NUM_REGS_PER_MSG_SPACE * i);
+            tx_ready_reg_info *temp = g_new(tx_ready_reg_info, 1);
+
+            temp->can_id = s->regs[reg_num];
+            temp->reg_num = reg_num;
+            list = g_slist_prepend(list, temp);
+            list = g_slist_sort(list, g_cmp_ids);
+        }
+
+        reg_ready >>= 1;
+    }
+
+    s->regs[R_TX_BUFFER_READY_REQUEST_REGISTER] = 0;
+    s->regs[R_TX_BUFFER_CANCEL_REQUEST_REGISTER] = 0;
+
+    return list;
+}
+
+static void transfer_data(XlnxVersalCANFDState *s)
+{
+    bool canfd_tx = tx_ready_check(s);
+    GSList *list, *iterator = NULL;
+    qemu_can_frame frame;
+
+    if (!canfd_tx) {
+        g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: Controller not enabled for data"
+                      " transfer\n", path);
+        return;
+    }
+
+    list = prepare_tx_data(s);
+    if (list == NULL) {
+        return;
+    }
+
+    for (iterator = list; iterator != NULL; iterator = iterator->next) {
+        regs2frame(s, &frame,
+                   ((tx_ready_reg_info *)iterator->data)->reg_num);
+
+        if (ARRAY_FIELD_EX32(s->regs, STATUS_REGISTER, LBACK)) {
+            update_rx_sequential(s, &frame);
+            tx_fifo_stamp(s, ((tx_ready_reg_info *)iterator->data)->reg_num);
+
+            ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, RXOK, 1);
+        } else {
+            g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+            trace_xlnx_canfd_tx_data(path, frame.can_id, frame.can_dlc,
+                                     frame.flags);
+            can_bus_client_send(&s->bus_client, &frame, 1);
+            tx_fifo_stamp(s,
+                          ((tx_ready_reg_info *)iterator->data)->reg_num);
+
+            ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, TXRRS, 1);
+
+            if (ARRAY_FIELD_EX32(s->regs, STATUS_REGISTER, SLEEP)) {
+                canfd_exit_sleep_mode(s);
+            }
+        }
+    }
+
+    ARRAY_FIELD_DP32(s->regs, INTERRUPT_STATUS_REGISTER, TXOK, 1);
+    free_list(list);
+
+    canfd_update_irq(s);
+}
+
+static uint64_t canfd_srr_pre_write(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+    uint32_t val = val64;
+
+    ARRAY_FIELD_DP32(s->regs, SOFTWARE_RESET_REGISTER, CEN,
+                     FIELD_EX32(val, SOFTWARE_RESET_REGISTER, CEN));
+
+    if (FIELD_EX32(val, SOFTWARE_RESET_REGISTER, SRST)) {
+        g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+        trace_xlnx_canfd_reset(path, val64);
+
+        /* First, core will do software reset then will enter in config mode. */
+        canfd_config_reset(s);
+    } else if (ARRAY_FIELD_EX32(s->regs, SOFTWARE_RESET_REGISTER, CEN) == 0) {
+        canfd_config_mode(s);
+    } else {
+        /*
+         * Leave config mode. Now XlnxVersalCANFD core will enter Normal, Sleep,
+         * snoop or Loopback mode depending upon LBACK, SLEEP, SNOOP register
+         * states.
+         */
+        ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, CONFIG, 0);
+
+        ptimer_transaction_begin(s->canfd_timer);
+        ptimer_set_count(s->canfd_timer, 0);
+        ptimer_transaction_commit(s->canfd_timer);
+        update_status_register_mode_bits(s);
+        transfer_data(s);
+    }
+
+    return s->regs[R_SOFTWARE_RESET_REGISTER];
+}
+
+static uint64_t filter_mask(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+    uint32_t reg_idx = (reg->access->addr) / 4;
+    uint32_t val = val64;
+    uint32_t filter_offset = (reg_idx - R_AFMR_REGISTER) / 2;
+
+    if (!(s->regs[R_ACCEPTANCE_FILTER_CONTROL_REGISTER] &
+        (1 << filter_offset))) {
+        s->regs[reg_idx] = val;
+    } else {
+        g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: Acceptance filter %d not enabled\n",
+                      path, filter_offset + 1);
+    }
+
+    return s->regs[reg_idx];
+}
+
+static uint64_t filter_id(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+    hwaddr reg_idx = (reg->access->addr) / 4;
+    uint32_t val = val64;
+    uint32_t filter_offset = (reg_idx - R_AFIR_REGISTER) / 2;
+
+    if (!(s->regs[R_ACCEPTANCE_FILTER_CONTROL_REGISTER] &
+        (1 << filter_offset))) {
+        s->regs[reg_idx] = val;
+    } else {
+        g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: Acceptance filter %d not enabled\n",
+                      path, filter_offset + 1);
+    }
+
+    return s->regs[reg_idx];
+}
+
+static uint64_t canfd_tx_fifo_status_prew(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+    uint32_t val = val64;
+    uint8_t read_ind = 0;
+    uint8_t fill_ind = ARRAY_FIELD_EX32(s->regs, TX_EVENT_FIFO_STATUS_REGISTER,
+                                        TXE_FL);
+
+    if (FIELD_EX32(val, TX_EVENT_FIFO_STATUS_REGISTER, TXE_IRI) && fill_ind) {
+        read_ind = ARRAY_FIELD_EX32(s->regs, TX_EVENT_FIFO_STATUS_REGISTER,
+                                    TXE_RI) + 1;
+
+        if (read_ind > s->cfg.tx_fifo - 1) {
+            read_ind = 0;
+        }
+
+        /*
+         * Increase the read index by 1 and decrease the fill level by 1.
+         */
+        ARRAY_FIELD_DP32(s->regs, TX_EVENT_FIFO_STATUS_REGISTER, TXE_RI,
+                         read_ind);
+        ARRAY_FIELD_DP32(s->regs, TX_EVENT_FIFO_STATUS_REGISTER, TXE_FL,
+                         fill_ind - 1);
+    }
+
+    return s->regs[R_TX_EVENT_FIFO_STATUS_REGISTER];
+}
+
+static uint64_t canfd_rx_fifo_status_prew(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+    uint32_t val = val64;
+    uint8_t read_ind = 0;
+    uint8_t fill_ind = 0;
+
+    if (FIELD_EX32(val, RX_FIFO_STATUS_REGISTER, IRI)) {
+        /* FL index is zero, setting IRI bit has no effect. */
+        if (FIELD_EX32(val, RX_FIFO_STATUS_REGISTER, FL) != 0) {
+            read_ind = FIELD_EX32(val, RX_FIFO_STATUS_REGISTER, RI) + 1;
+
+            if (read_ind > s->cfg.rx0_fifo - 1) {
+                read_ind = 0;
+            }
+
+            fill_ind = FIELD_EX32(val, RX_FIFO_STATUS_REGISTER, FL) - 1;
+
+            ARRAY_FIELD_DP32(s->regs, RX_FIFO_STATUS_REGISTER, RI, read_ind);
+            ARRAY_FIELD_DP32(s->regs, RX_FIFO_STATUS_REGISTER, FL, fill_ind);
+        }
+    }
+
+    if (FIELD_EX32(val, RX_FIFO_STATUS_REGISTER, IRI_1)) {
+        /* FL_1 index is zero, setting IRI_1 bit has no effect. */
+        if (FIELD_EX32(val, RX_FIFO_STATUS_REGISTER, FL_1) != 0) {
+            read_ind = FIELD_EX32(val, RX_FIFO_STATUS_REGISTER, RI_1) + 1;
+
+            if (read_ind > s->cfg.rx1_fifo - 1) {
+                read_ind = 0;
+            }
+
+            fill_ind = FIELD_EX32(val, RX_FIFO_STATUS_REGISTER, FL_1) - 1;
+
+            ARRAY_FIELD_DP32(s->regs, RX_FIFO_STATUS_REGISTER, RI_1, read_ind);
+            ARRAY_FIELD_DP32(s->regs, RX_FIFO_STATUS_REGISTER, FL_1, fill_ind);
+        }
+    }
+
+    return s->regs[R_RX_FIFO_STATUS_REGISTER];
+}
+
+static uint64_t canfd_tsr_pre_write(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+    uint32_t val = val64;
+
+    if (FIELD_EX32(val, TIMESTAMP_REGISTER, CTS)) {
+        ARRAY_FIELD_DP32(s->regs, TIMESTAMP_REGISTER, TIMESTAMP_CNT, 0);
+        ptimer_transaction_begin(s->canfd_timer);
+        ptimer_set_count(s->canfd_timer, 0);
+        ptimer_transaction_commit(s->canfd_timer);
+    }
+
+    return 0;
+}
+
+static uint64_t canfd_trr_reg_prew(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+
+    if (ARRAY_FIELD_EX32(s->regs, MODE_SELECT_REGISTER, SNOOP)) {
+        g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: Controller is in SNOOP mode."
+                      " tx_ready_register will stay in reset mode\n", path);
+        return 0;
+    } else {
+        return val64;
+    }
+}
+
+static void canfd_trr_reg_postw(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+
+    transfer_data(s);
+}
+
+static void canfd_cancel_reg_postw(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+
+    process_cancellation_requests(s);
+}
+
+static uint64_t canfd_write_check_prew(RegisterInfo *reg, uint64_t val64)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(reg->opaque);
+    uint32_t val = val64;
+
+    if (ARRAY_FIELD_EX32(s->regs, SOFTWARE_RESET_REGISTER, CEN) == 0) {
+        return val;
+    }
+    return 0;
+}
+
+static const RegisterAccessInfo canfd_tx_regs[] = {
+    {   .name = "TB_ID_REGISTER",  .addr = A_TB_ID_REGISTER,
+    },{ .name = "TB0_DLC_REGISTER",  .addr = A_TB0_DLC_REGISTER,
+    },{ .name = "TB_DW0_REGISTER",  .addr = A_TB_DW0_REGISTER,
+    },{ .name = "TB_DW1_REGISTER",  .addr = A_TB_DW1_REGISTER,
+    },{ .name = "TB_DW2_REGISTER",  .addr = A_TB_DW2_REGISTER,
+    },{ .name = "TB_DW3_REGISTER",  .addr = A_TB_DW3_REGISTER,
+    },{ .name = "TB_DW4_REGISTER",  .addr = A_TB_DW4_REGISTER,
+    },{ .name = "TB_DW5_REGISTER",  .addr = A_TB_DW5_REGISTER,
+    },{ .name = "TB_DW6_REGISTER",  .addr = A_TB_DW6_REGISTER,
+    },{ .name = "TB_DW7_REGISTER",  .addr = A_TB_DW7_REGISTER,
+    },{ .name = "TB_DW8_REGISTER",  .addr = A_TB_DW8_REGISTER,
+    },{ .name = "TB_DW9_REGISTER",  .addr = A_TB_DW9_REGISTER,
+    },{ .name = "TB_DW10_REGISTER",  .addr = A_TB_DW10_REGISTER,
+    },{ .name = "TB_DW11_REGISTER",  .addr = A_TB_DW11_REGISTER,
+    },{ .name = "TB_DW12_REGISTER",  .addr = A_TB_DW12_REGISTER,
+    },{ .name = "TB_DW13_REGISTER",  .addr = A_TB_DW13_REGISTER,
+    },{ .name = "TB_DW14_REGISTER",  .addr = A_TB_DW14_REGISTER,
+    },{ .name = "TB_DW15_REGISTER",  .addr = A_TB_DW15_REGISTER,
+    }
+};
+
+static const RegisterAccessInfo canfd_rx0_regs[] = {
+    {   .name = "RB_ID_REGISTER",  .addr = A_RB_ID_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DLC_REGISTER",  .addr = A_RB_DLC_REGISTER,
+        .ro = 0xfe1fffff,
+    },{ .name = "RB_DW0_REGISTER",  .addr = A_RB_DW0_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW1_REGISTER",  .addr = A_RB_DW1_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW2_REGISTER",  .addr = A_RB_DW2_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW3_REGISTER",  .addr = A_RB_DW3_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW4_REGISTER",  .addr = A_RB_DW4_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW5_REGISTER",  .addr = A_RB_DW5_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW6_REGISTER",  .addr = A_RB_DW6_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW7_REGISTER",  .addr = A_RB_DW7_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW8_REGISTER",  .addr = A_RB_DW8_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW9_REGISTER",  .addr = A_RB_DW9_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW10_REGISTER",  .addr = A_RB_DW10_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW11_REGISTER",  .addr = A_RB_DW11_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW12_REGISTER",  .addr = A_RB_DW12_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW13_REGISTER",  .addr = A_RB_DW13_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW14_REGISTER",  .addr = A_RB_DW14_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW15_REGISTER",  .addr = A_RB_DW15_REGISTER,
+        .ro = 0xffffffff,
+    }
+};
+
+static const RegisterAccessInfo canfd_rx1_regs[] = {
+    {   .name = "RB_ID_REGISTER_1",  .addr = A_RB_ID_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DLC_REGISTER_1",  .addr = A_RB_DLC_REGISTER_1,
+        .ro = 0xfe1fffff,
+    },{ .name = "RB0_DW0_REGISTER_1",  .addr = A_RB0_DW0_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW1_REGISTER_1",  .addr = A_RB_DW1_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW2_REGISTER_1",  .addr = A_RB_DW2_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW3_REGISTER_1",  .addr = A_RB_DW3_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW4_REGISTER_1",  .addr = A_RB_DW4_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW5_REGISTER_1",  .addr = A_RB_DW5_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW6_REGISTER_1",  .addr = A_RB_DW6_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW7_REGISTER_1",  .addr = A_RB_DW7_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW8_REGISTER_1",  .addr = A_RB_DW8_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW9_REGISTER_1",  .addr = A_RB_DW9_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW10_REGISTER_1",  .addr = A_RB_DW10_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW11_REGISTER_1",  .addr = A_RB_DW11_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW12_REGISTER_1",  .addr = A_RB_DW12_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW13_REGISTER_1",  .addr = A_RB_DW13_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW14_REGISTER_1",  .addr = A_RB_DW14_REGISTER_1,
+        .ro = 0xffffffff,
+    },{ .name = "RB_DW15_REGISTER_1",  .addr = A_RB_DW15_REGISTER_1,
+        .ro = 0xffffffff,
+    }
+};
+
+/* Acceptance filter registers. */
+static const RegisterAccessInfo canfd_af_regs[] = {
+    {   .name = "AFMR_REGISTER",  .addr = A_AFMR_REGISTER,
+        .pre_write = filter_mask,
+    },{ .name = "AFIR_REGISTER",  .addr = A_AFIR_REGISTER,
+        .pre_write = filter_id,
+    }
+};
+
+static const RegisterAccessInfo canfd_txe_regs[] = {
+    {   .name = "TXE_FIFO_TB_ID_REGISTER",  .addr = A_TXE_FIFO_TB_ID_REGISTER,
+        .ro = 0xffffffff,
+    },{ .name = "TXE_FIFO_TB_DLC_REGISTER",  .addr = A_TXE_FIFO_TB_DLC_REGISTER,
+        .ro = 0xffffffff,
+    }
+};
+
+static const RegisterAccessInfo canfd_regs_info[] = {
+    {   .name = "SOFTWARE_RESET_REGISTER",  .addr = A_SOFTWARE_RESET_REGISTER,
+        .pre_write = canfd_srr_pre_write,
+    },{ .name = "MODE_SELECT_REGISTER",  .addr = A_MODE_SELECT_REGISTER,
+        .pre_write = canfd_msr_pre_write,
+    },{ .name = "ARBITRATION_PHASE_BAUD_RATE_PRESCALER_REGISTER",
+        .addr = A_ARBITRATION_PHASE_BAUD_RATE_PRESCALER_REGISTER,
+        .pre_write = canfd_write_check_prew,
+    },{ .name = "ARBITRATION_PHASE_BIT_TIMING_REGISTER",
+        .addr = A_ARBITRATION_PHASE_BIT_TIMING_REGISTER,
+        .pre_write = canfd_write_check_prew,
+    },{ .name = "ERROR_COUNTER_REGISTER",  .addr = A_ERROR_COUNTER_REGISTER,
+        .ro = 0xffff,
+    },{ .name = "ERROR_STATUS_REGISTER",  .addr = A_ERROR_STATUS_REGISTER,
+        .w1c = 0xf1f,
+    },{ .name = "STATUS_REGISTER",  .addr = A_STATUS_REGISTER,
+        .reset = 0x1,
+        .ro = 0x7f17ff,
+    },{ .name = "INTERRUPT_STATUS_REGISTER",
+        .addr = A_INTERRUPT_STATUS_REGISTER,
+        .ro = 0xffffff7f,
+    },{ .name = "INTERRUPT_ENABLE_REGISTER",
+        .addr = A_INTERRUPT_ENABLE_REGISTER,
+        .post_write = canfd_ier_post_write,
+    },{ .name = "INTERRUPT_CLEAR_REGISTER",
+        .addr = A_INTERRUPT_CLEAR_REGISTER, .pre_write = canfd_icr_pre_write,
+    },{ .name = "TIMESTAMP_REGISTER",  .addr = A_TIMESTAMP_REGISTER,
+        .ro = 0xffff0000,
+        .pre_write = canfd_tsr_pre_write,
+    },{ .name = "DATA_PHASE_BAUD_RATE_PRESCALER_REGISTER",
+        .addr = A_DATA_PHASE_BAUD_RATE_PRESCALER_REGISTER,
+        .pre_write = canfd_write_check_prew,
+    },{ .name = "DATA_PHASE_BIT_TIMING_REGISTER",
+        .addr = A_DATA_PHASE_BIT_TIMING_REGISTER,
+        .pre_write = canfd_write_check_prew,
+    },{ .name = "TX_BUFFER_READY_REQUEST_REGISTER",
+        .addr = A_TX_BUFFER_READY_REQUEST_REGISTER,
+        .pre_write = canfd_trr_reg_prew,
+        .post_write = canfd_trr_reg_postw,
+    },{ .name = "INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER",
+        .addr = A_INTERRUPT_ENABLE_TX_BUFFER_READY_REQUEST_REGISTER,
+    },{ .name = "TX_BUFFER_CANCEL_REQUEST_REGISTER",
+        .addr = A_TX_BUFFER_CANCEL_REQUEST_REGISTER,
+        .post_write = canfd_cancel_reg_postw,
+    },{ .name = "INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER",
+        .addr = A_INTERRUPT_ENABLE_TX_BUFFER_CANCELLATION_REQUEST_REGISTER,
+    },{ .name = "TX_EVENT_FIFO_STATUS_REGISTER",
+        .addr = A_TX_EVENT_FIFO_STATUS_REGISTER,
+        .ro = 0x3f1f, .pre_write = canfd_tx_fifo_status_prew,
+    },{ .name = "TX_EVENT_FIFO_WATERMARK_REGISTER",
+        .addr = A_TX_EVENT_FIFO_WATERMARK_REGISTER,
+        .reset = 0xf,
+        .pre_write = canfd_write_check_prew,
+    },{ .name = "ACCEPTANCE_FILTER_CONTROL_REGISTER",
+        .addr = A_ACCEPTANCE_FILTER_CONTROL_REGISTER,
+    },{ .name = "RX_FIFO_STATUS_REGISTER",  .addr = A_RX_FIFO_STATUS_REGISTER,
+        .ro = 0x7f3f7f3f, .pre_write = canfd_rx_fifo_status_prew,
+    },{ .name = "RX_FIFO_WATERMARK_REGISTER",
+        .addr = A_RX_FIFO_WATERMARK_REGISTER,
+        .reset = 0x1f0f0f,
+        .pre_write = canfd_write_check_prew,
+    }
+};
+
+static void xlnx_versal_canfd_ptimer_cb(void *opaque)
+{
+    /* No action required on the timer rollover. */
+}
+
+static const MemoryRegionOps canfd_ops = {
+    .read = register_read_memory,
+    .write = register_write_memory,
+    .endianness = DEVICE_LITTLE_ENDIAN,
+    .valid = {
+        .min_access_size = 4,
+        .max_access_size = 4,
+    },
+};
+
+static void canfd_reset(DeviceState *dev)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(dev);
+    unsigned int i;
+
+    for (i = 0; i < ARRAY_SIZE(s->reg_info); ++i) {
+        register_reset(&s->reg_info[i]);
+    }
+
+    ptimer_transaction_begin(s->canfd_timer);
+    ptimer_set_count(s->canfd_timer, 0);
+    ptimer_transaction_commit(s->canfd_timer);
+}
+
+static bool can_xilinx_canfd_receive(CanBusClientState *client)
+{
+    XlnxVersalCANFDState *s = container_of(client, XlnxVersalCANFDState,
+                                           bus_client);
+
+    bool reset_state = ARRAY_FIELD_EX32(s->regs, SOFTWARE_RESET_REGISTER, SRST);
+    bool can_enabled = ARRAY_FIELD_EX32(s->regs, SOFTWARE_RESET_REGISTER, CEN);
+
+    return !reset_state && can_enabled;
+}
+
+static ssize_t canfd_xilinx_receive(CanBusClientState *client,
+                                    const qemu_can_frame *buf,
+                                    size_t buf_size)
+{
+    XlnxVersalCANFDState *s = container_of(client, XlnxVersalCANFDState,
+                                           bus_client);
+    const qemu_can_frame *frame = buf;
+
+    assert(buf_size > 0);
+
+    if (ARRAY_FIELD_EX32(s->regs, STATUS_REGISTER, LBACK)) {
+        /*
+         * XlnxVersalCANFDState will not participate in normal bus communication
+         * and does not receive any messages transmitted by other CAN nodes.
+         */
+        return 1;
+    }
+
+    /* Update the status register that we are receiving message. */
+    ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, BBSY, 1);
+
+    if (ARRAY_FIELD_EX32(s->regs, STATUS_REGISTER, SNOOP)) {
+        /* Snoop Mode: Just keep the data. no response back. */
+        update_rx_sequential(s, frame);
+    } else {
+        if ((ARRAY_FIELD_EX32(s->regs, STATUS_REGISTER, SLEEP))) {
+            /*
+             * XlnxVersalCANFDState is in sleep mode. Any data on bus will bring
+             * it to the wake up state.
+             */
+            canfd_exit_sleep_mode(s);
+        }
+
+        update_rx_sequential(s, frame);
+    }
+
+    /* Message processing done. Update the status back to !busy */
+    ARRAY_FIELD_DP32(s->regs, STATUS_REGISTER, BBSY, 0);
+    return 1;
+}
+
+static CanBusClientInfo canfd_xilinx_bus_client_info = {
+    .can_receive = can_xilinx_canfd_receive,
+    .receive = canfd_xilinx_receive,
+};
+
+static int xlnx_canfd_connect_to_bus(XlnxVersalCANFDState *s,
+                                     CanBusState *bus)
+{
+    s->bus_client.info = &canfd_xilinx_bus_client_info;
+
+    return can_bus_insert_client(bus, &s->bus_client);
+}
+
+#define NUM_REG_PER_AF      ARRAY_SIZE(canfd_af_regs)
+#define NUM_AF              32
+#define NUM_REG_PER_TXE     ARRAY_SIZE(canfd_txe_regs)
+#define NUM_TXE             32
+
+static int canfd_populate_regarray(XlnxVersalCANFDState *s,
+                                  RegisterInfoArray *r_array, int pos,
+                                  const RegisterAccessInfo *rae,
+                                  int num_rae)
+{
+    int i;
+
+    for (i = 0; i < num_rae; i++) {
+        int index = rae[i].addr / 4;
+        RegisterInfo *r = &s->reg_info[index];
+
+        object_initialize(r, sizeof(*r), TYPE_REGISTER);
+
+        *r = (RegisterInfo) {
+            .data = &s->regs[index],
+            .data_size = sizeof(uint32_t),
+            .access = &rae[i],
+            .opaque = OBJECT(s),
+        };
+
+        r_array->r[i + pos] = r;
+    }
+    return i + pos;
+}
+
+static void canfd_create_rai(RegisterAccessInfo *rai_array,
+                                const RegisterAccessInfo *canfd_regs,
+                                int template_rai_array_sz,
+                                int num_template_to_copy)
+{
+    int i;
+    int reg_num;
+
+    for (reg_num = 0; reg_num < num_template_to_copy; reg_num++) {
+        int pos = reg_num * template_rai_array_sz;
+
+        memcpy(rai_array + pos, canfd_regs,
+               template_rai_array_sz * sizeof(RegisterAccessInfo));
+
+        for (i = 0; i < template_rai_array_sz; i++) {
+            const char *name = canfd_regs[i].name;
+            uint64_t addr = canfd_regs[i].addr;
+            rai_array[i + pos].name = g_strdup_printf("%s%d", name, reg_num);
+            rai_array[i + pos].addr = addr + pos * 4;
+        }
+    }
+}
+
+static RegisterInfoArray *canfd_create_regarray(XlnxVersalCANFDState *s)
+{
+    const char *device_prefix = object_get_typename(OBJECT(s));
+    uint64_t memory_size = XLNX_VERSAL_CANFD_R_MAX * 4;
+    int num_regs;
+    int pos = 0;
+    RegisterInfoArray *r_array;
+
+    num_regs = ARRAY_SIZE(canfd_regs_info) +
+                s->cfg.tx_fifo * NUM_REGS_PER_MSG_SPACE +
+                s->cfg.rx0_fifo * NUM_REGS_PER_MSG_SPACE +
+                NUM_AF * NUM_REG_PER_AF +
+                NUM_TXE * NUM_REG_PER_TXE;
+
+    s->tx_regs = g_new0(RegisterAccessInfo,
+                        s->cfg.tx_fifo * ARRAY_SIZE(canfd_tx_regs));
+
+    canfd_create_rai(s->tx_regs, canfd_tx_regs,
+                     ARRAY_SIZE(canfd_tx_regs), s->cfg.tx_fifo);
+
+    s->rx0_regs = g_new0(RegisterAccessInfo,
+                         s->cfg.rx0_fifo * ARRAY_SIZE(canfd_rx0_regs));
+
+    canfd_create_rai(s->rx0_regs, canfd_rx0_regs,
+                     ARRAY_SIZE(canfd_rx0_regs), s->cfg.rx0_fifo);
+
+    s->af_regs = g_new0(RegisterAccessInfo,
+                        NUM_AF * ARRAY_SIZE(canfd_af_regs));
+
+    canfd_create_rai(s->af_regs, canfd_af_regs,
+                     ARRAY_SIZE(canfd_af_regs), NUM_AF);
+
+    s->txe_regs = g_new0(RegisterAccessInfo,
+                         NUM_TXE * ARRAY_SIZE(canfd_txe_regs));
+
+    canfd_create_rai(s->txe_regs, canfd_txe_regs,
+                     ARRAY_SIZE(canfd_txe_regs), NUM_TXE);
+
+    if (s->cfg.enable_rx_fifo1) {
+        num_regs += s->cfg.rx1_fifo * NUM_REGS_PER_MSG_SPACE;
+
+        s->rx1_regs = g_new0(RegisterAccessInfo,
+                             s->cfg.rx1_fifo * ARRAY_SIZE(canfd_rx1_regs));
+
+        canfd_create_rai(s->rx1_regs, canfd_rx1_regs,
+                         ARRAY_SIZE(canfd_rx1_regs), s->cfg.rx1_fifo);
+    }
+
+    r_array = g_new0(RegisterInfoArray, 1);
+    r_array->r = g_new0(RegisterInfo * , num_regs);
+    r_array->num_elements = num_regs;
+    r_array->prefix = device_prefix;
+
+    pos = canfd_populate_regarray(s, r_array, pos,
+                                  canfd_regs_info,
+                                  ARRAY_SIZE(canfd_regs_info));
+    pos = canfd_populate_regarray(s, r_array, pos,
+                                  s->tx_regs, s->cfg.tx_fifo *
+                                  NUM_REGS_PER_MSG_SPACE);
+    pos = canfd_populate_regarray(s, r_array, pos,
+                                  s->rx0_regs, s->cfg.rx0_fifo *
+                                  NUM_REGS_PER_MSG_SPACE);
+    if (s->cfg.enable_rx_fifo1) {
+        pos = canfd_populate_regarray(s, r_array, pos,
+                                      s->rx1_regs, s->cfg.rx1_fifo *
+                                      NUM_REGS_PER_MSG_SPACE);
+    }
+    pos = canfd_populate_regarray(s, r_array, pos,
+                                  s->af_regs, NUM_AF * NUM_REG_PER_AF);
+    pos = canfd_populate_regarray(s, r_array, pos,
+                                  s->txe_regs, NUM_TXE * NUM_REG_PER_TXE);
+
+    memory_region_init_io(&r_array->mem, OBJECT(s), &canfd_ops, r_array,
+                          device_prefix, memory_size);
+    return r_array;
+}
+
+static void canfd_realize(DeviceState *dev, Error **errp)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(dev);
+    RegisterInfoArray *reg_array;
+
+    reg_array = canfd_create_regarray(s);
+    memory_region_add_subregion(&s->iomem, 0x00, &reg_array->mem);
+    sysbus_init_mmio(SYS_BUS_DEVICE(dev), &s->iomem);
+    sysbus_init_irq(SYS_BUS_DEVICE(dev), &s->irq_canfd_int);
+
+    if (s->canfdbus) {
+        if (xlnx_canfd_connect_to_bus(s, s->canfdbus) < 0) {
+            g_autofree char *path = object_get_canonical_path(OBJECT(s));
+
+            error_setg(errp, "%s: xlnx_canfd_connect_to_bus failed", path);
+            return;
+        }
+
+    }
+
+    /* Allocate a new timer. */
+    s->canfd_timer = ptimer_init(xlnx_versal_canfd_ptimer_cb, s,
+                                 PTIMER_POLICY_WRAP_AFTER_ONE_PERIOD |
+                                 PTIMER_POLICY_TRIGGER_ONLY_ON_DECREMENT |
+                                 PTIMER_POLICY_NO_IMMEDIATE_RELOAD);
+
+    ptimer_transaction_begin(s->canfd_timer);
+
+    ptimer_set_freq(s->canfd_timer, s->cfg.ext_clk_freq);
+    ptimer_set_limit(s->canfd_timer, CANFD_TIMER_MAX, 1);
+    ptimer_run(s->canfd_timer, 0);
+    ptimer_transaction_commit(s->canfd_timer);
+}
+
+static void canfd_init(Object *obj)
+{
+    XlnxVersalCANFDState *s = XILINX_CANFD(obj);
+
+    memory_region_init(&s->iomem, obj, TYPE_XILINX_CANFD,
+                       XLNX_VERSAL_CANFD_R_MAX * 4);
+}
+
+static const VMStateDescription vmstate_canfd = {
+    .name = TYPE_XILINX_CANFD,
+    .version_id = 1,
+    .minimum_version_id = 1,
+    .fields = (VMStateField[]) {
+        VMSTATE_UINT32_ARRAY(regs, XlnxVersalCANFDState,
+                             XLNX_VERSAL_CANFD_R_MAX),
+        VMSTATE_PTIMER(canfd_timer, XlnxVersalCANFDState),
+        VMSTATE_END_OF_LIST(),
+    }
+};
+
+static Property canfd_core_properties[] = {
+    DEFINE_PROP_UINT8("rx-fifo0", XlnxVersalCANFDState, cfg.rx0_fifo, 0x40),
+    DEFINE_PROP_UINT8("rx-fifo1", XlnxVersalCANFDState, cfg.rx1_fifo, 0x40),
+    DEFINE_PROP_UINT8("tx-fifo", XlnxVersalCANFDState, cfg.tx_fifo, 0x20),
+    DEFINE_PROP_BOOL("enable-rx-fifo1", XlnxVersalCANFDState,
+                     cfg.enable_rx_fifo1, true),
+    DEFINE_PROP_UINT32("ext_clk_freq", XlnxVersalCANFDState, cfg.ext_clk_freq,
+                       CANFD_DEFAULT_CLOCK),
+    DEFINE_PROP_LINK("canfdbus", XlnxVersalCANFDState, canfdbus, TYPE_CAN_BUS,
+                     CanBusState *),
+    DEFINE_PROP_END_OF_LIST(),
+};
+
+static void canfd_class_init(ObjectClass *klass, void *data)
+{
+    DeviceClass *dc = DEVICE_CLASS(klass);
+
+    dc->reset = canfd_reset;
+    dc->realize = canfd_realize;
+    device_class_set_props(dc, canfd_core_properties);
+    dc->vmsd = &vmstate_canfd;
+}
+
+static const TypeInfo canfd_info = {
+    .name          = TYPE_XILINX_CANFD,
+    .parent        = TYPE_SYS_BUS_DEVICE,
+    .instance_size = sizeof(XlnxVersalCANFDState),
+    .class_init    = canfd_class_init,
+    .instance_init = canfd_init,
+};
+
+static void canfd_register_types(void)
+{
+    type_register_static(&canfd_info);
+}
+
+type_init(canfd_register_types)
diff --git a/hw/sd/allwinner-sdhost.c b/hw/sd/allwinner-sdhost.c
index 92a0f42..286e009 100644
--- a/hw/sd/allwinner-sdhost.c
+++ b/hw/sd/allwinner-sdhost.c
@@ -77,6 +77,7 @@ enum {
     REG_SD_DATA1_CRC  = 0x12C, /* CRC Data 1 from card/eMMC */
     REG_SD_DATA0_CRC  = 0x130, /* CRC Data 0 from card/eMMC */
     REG_SD_CRC_STA    = 0x134, /* CRC status from card/eMMC during write */
+    REG_SD_SAMP_DL    = 0x144, /* Sample Delay Control (sun50i-a64) */
     REG_SD_FIFO       = 0x200, /* Read/Write FIFO */
 };
 
@@ -158,6 +159,7 @@ enum {
     REG_SD_RES_CRC_RST      = 0x0,
     REG_SD_DATA_CRC_RST     = 0x0,
     REG_SD_CRC_STA_RST      = 0x0,
+    REG_SD_SAMPLE_DL_RST    = 0x00002000,
     REG_SD_FIFO_RST         = 0x0,
 };
 
@@ -459,6 +461,7 @@ static uint64_t allwinner_sdhost_read(void *opaque, hwaddr offset,
 {
     AwSdHostState *s = AW_SDHOST(opaque);
     AwSdHostClass *sc = AW_SDHOST_GET_CLASS(s);
+    bool out_of_bounds = false;
     uint32_t res = 0;
 
     switch (offset) {
@@ -577,13 +580,24 @@ static uint64_t allwinner_sdhost_read(void *opaque, hwaddr offset,
     case REG_SD_FIFO:      /* Read/Write FIFO */
         res = allwinner_sdhost_fifo_read(s);
         break;
+    case REG_SD_SAMP_DL: /* Sample Delay */
+        if (sc->can_calibrate) {
+            res = s->sample_delay;
+        } else {
+            out_of_bounds = true;
+        }
+        break;
     default:
-        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset %"
-                      HWADDR_PRIx"\n", __func__, offset);
+        out_of_bounds = true;
         res = 0;
         break;
     }
 
+    if (out_of_bounds) {
+        qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset %"
+                      HWADDR_PRIx"\n", __func__, offset);
+    }
+
     trace_allwinner_sdhost_read(offset, res, size);
     return res;
 }
@@ -602,6 +616,7 @@ static void allwinner_sdhost_write(void *opaque, hwaddr offset,
 {
     AwSdHostState *s = AW_SDHOST(opaque);
     AwSdHostClass *sc = AW_SDHOST_GET_CLASS(s);
+    bool out_of_bounds = false;
 
     trace_allwinner_sdhost_write(offset, value, size);
 
@@ -725,10 +740,21 @@ static void allwinner_sdhost_write(void *opaque, hwaddr offset,
     case REG_SD_DATA0_CRC: /* CRC Data 0 from card/eMMC */
     case REG_SD_CRC_STA:   /* CRC status from card/eMMC in write operation */
         break;
+    case REG_SD_SAMP_DL: /* Sample delay control */
+        if (sc->can_calibrate) {
+            s->sample_delay = value;
+        } else {
+            out_of_bounds = true;
+        }
+        break;
     default:
+        out_of_bounds = true;
+        break;
+    }
+
+    if (out_of_bounds) {
         qemu_log_mask(LOG_GUEST_ERROR, "%s: out-of-bounds offset %"
                       HWADDR_PRIx"\n", __func__, offset);
-        break;
     }
 }
 
@@ -777,6 +803,7 @@ static const VMStateDescription vmstate_allwinner_sdhost = {
         VMSTATE_UINT32(response_crc, AwSdHostState),
         VMSTATE_UINT32_ARRAY(data_crc, AwSdHostState, 8),
         VMSTATE_UINT32(status_crc, AwSdHostState),
+        VMSTATE_UINT32(sample_delay, AwSdHostState),
         VMSTATE_END_OF_LIST()
     }
 };
@@ -815,6 +842,7 @@ static void allwinner_sdhost_realize(DeviceState *dev, Error **errp)
 static void allwinner_sdhost_reset(DeviceState *dev)
 {
     AwSdHostState *s = AW_SDHOST(dev);
+    AwSdHostClass *sc = AW_SDHOST_GET_CLASS(s);
 
     s->global_ctl = REG_SD_GCTL_RST;
     s->clock_ctl = REG_SD_CKCR_RST;
@@ -855,6 +883,10 @@ static void allwinner_sdhost_reset(DeviceState *dev)
     }
 
     s->status_crc = REG_SD_CRC_STA_RST;
+
+    if (sc->can_calibrate) {
+        s->sample_delay = REG_SD_SAMPLE_DL_RST;
+    }
 }
 
 static void allwinner_sdhost_bus_class_init(ObjectClass *klass, void *data)
@@ -879,6 +911,7 @@ static void allwinner_sdhost_sun4i_class_init(ObjectClass *klass, void *data)
     AwSdHostClass *sc = AW_SDHOST_CLASS(klass);
     sc->max_desc_size = 8 * KiB;
     sc->is_sun4i = true;
+    sc->can_calibrate = false;
 }
 
 static void allwinner_sdhost_sun5i_class_init(ObjectClass *klass, void *data)
@@ -886,6 +919,25 @@ static void allwinner_sdhost_sun5i_class_init(ObjectClass *klass, void *data)
     AwSdHostClass *sc = AW_SDHOST_CLASS(klass);
     sc->max_desc_size = 64 * KiB;
     sc->is_sun4i = false;
+    sc->can_calibrate = false;
+}
+
+static void allwinner_sdhost_sun50i_a64_class_init(ObjectClass *klass,
+                                                   void *data)
+{
+    AwSdHostClass *sc = AW_SDHOST_CLASS(klass);
+    sc->max_desc_size = 64 * KiB;
+    sc->is_sun4i = false;
+    sc->can_calibrate = true;
+}
+
+static void allwinner_sdhost_sun50i_a64_emmc_class_init(ObjectClass *klass,
+                                                        void *data)
+{
+    AwSdHostClass *sc = AW_SDHOST_CLASS(klass);
+    sc->max_desc_size = 8 * KiB;
+    sc->is_sun4i = false;
+    sc->can_calibrate = true;
 }
 
 static const TypeInfo allwinner_sdhost_info = {
@@ -910,6 +962,18 @@ static const TypeInfo allwinner_sdhost_sun5i_info = {
     .class_init    = allwinner_sdhost_sun5i_class_init,
 };
 
+static const TypeInfo allwinner_sdhost_sun50i_a64_info = {
+    .name          = TYPE_AW_SDHOST_SUN50I_A64,
+    .parent        = TYPE_AW_SDHOST,
+    .class_init    = allwinner_sdhost_sun50i_a64_class_init,
+};
+
+static const TypeInfo allwinner_sdhost_sun50i_a64_emmc_info = {
+    .name          = TYPE_AW_SDHOST_SUN50I_A64_EMMC,
+    .parent        = TYPE_AW_SDHOST,
+    .class_init    = allwinner_sdhost_sun50i_a64_emmc_class_init,
+};
+
 static const TypeInfo allwinner_sdhost_bus_info = {
     .name = TYPE_AW_SDHOST_BUS,
     .parent = TYPE_SD_BUS,
@@ -922,6 +986,8 @@ static void allwinner_sdhost_register_types(void)
     type_register_static(&allwinner_sdhost_info);
     type_register_static(&allwinner_sdhost_sun4i_info);
     type_register_static(&allwinner_sdhost_sun5i_info);
+    type_register_static(&allwinner_sdhost_sun50i_a64_info);
+    type_register_static(&allwinner_sdhost_sun50i_a64_emmc_info);
     type_register_static(&allwinner_sdhost_bus_info);
 }
 
diff --git a/include/hw/arm/allwinner-r40.h b/include/hw/arm/allwinner-r40.h
new file mode 100644
index 0000000..72710d3
--- /dev/null
+++ b/include/hw/arm/allwinner-r40.h
@@ -0,0 +1,143 @@
+/*
+ * Allwinner R40/A40i/T3 System on Chip emulation
+ *
+ * Copyright (C) 2023 qianfan Zhao <qianfanguijin@163.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef HW_ARM_ALLWINNER_R40_H
+#define HW_ARM_ALLWINNER_R40_H
+
+#include "qom/object.h"
+#include "hw/arm/boot.h"
+#include "hw/timer/allwinner-a10-pit.h"
+#include "hw/intc/arm_gic.h"
+#include "hw/sd/allwinner-sdhost.h"
+#include "hw/misc/allwinner-r40-ccu.h"
+#include "hw/misc/allwinner-r40-dramc.h"
+#include "hw/misc/allwinner-sramc.h"
+#include "hw/i2c/allwinner-i2c.h"
+#include "hw/net/allwinner_emac.h"
+#include "hw/net/allwinner-sun8i-emac.h"
+#include "target/arm/cpu.h"
+#include "sysemu/block-backend.h"
+
+enum {
+    AW_R40_DEV_SRAM_A1,
+    AW_R40_DEV_SRAM_A2,
+    AW_R40_DEV_SRAM_A3,
+    AW_R40_DEV_SRAM_A4,
+    AW_R40_DEV_SRAMC,
+    AW_R40_DEV_EMAC,
+    AW_R40_DEV_MMC0,
+    AW_R40_DEV_MMC1,
+    AW_R40_DEV_MMC2,
+    AW_R40_DEV_MMC3,
+    AW_R40_DEV_CCU,
+    AW_R40_DEV_PIT,
+    AW_R40_DEV_UART0,
+    AW_R40_DEV_UART1,
+    AW_R40_DEV_UART2,
+    AW_R40_DEV_UART3,
+    AW_R40_DEV_UART4,
+    AW_R40_DEV_UART5,
+    AW_R40_DEV_UART6,
+    AW_R40_DEV_UART7,
+    AW_R40_DEV_TWI0,
+    AW_R40_DEV_GMAC,
+    AW_R40_DEV_GIC_DIST,
+    AW_R40_DEV_GIC_CPU,
+    AW_R40_DEV_GIC_HYP,
+    AW_R40_DEV_GIC_VCPU,
+    AW_R40_DEV_SDRAM,
+    AW_R40_DEV_DRAMCOM,
+    AW_R40_DEV_DRAMCTL,
+    AW_R40_DEV_DRAMPHY,
+};
+
+#define AW_R40_NUM_CPUS      (4)
+
+/**
+ * Allwinner R40 object model
+ * @{
+ */
+
+/** Object type for the Allwinner R40 SoC */
+#define TYPE_AW_R40 "allwinner-r40"
+
+/** Convert input object to Allwinner R40 state object */
+OBJECT_DECLARE_SIMPLE_TYPE(AwR40State, AW_R40)
+
+/** @} */
+
+/**
+ * Allwinner R40 object
+ *
+ * This struct contains the state of all the devices
+ * which are currently emulated by the R40 SoC code.
+ */
+#define AW_R40_NUM_MMCS         4
+#define AW_R40_NUM_UARTS        8
+
+struct AwR40State {
+    /*< private >*/
+    DeviceState parent_obj;
+    /*< public >*/
+
+    /** Physical base address for start of RAM */
+    hwaddr ram_addr;
+
+    /** Total RAM size in megabytes */
+    uint32_t ram_size;
+
+    ARMCPU cpus[AW_R40_NUM_CPUS];
+    const hwaddr *memmap;
+    AwSRAMCState sramc;
+    AwA10PITState timer;
+    AwSdHostState mmc[AW_R40_NUM_MMCS];
+    AwR40ClockCtlState ccu;
+    AwR40DramCtlState dramc;
+    AWI2CState i2c0;
+    AwEmacState emac;
+    AwSun8iEmacState gmac;
+    GICState gic;
+    MemoryRegion sram_a1;
+    MemoryRegion sram_a2;
+    MemoryRegion sram_a3;
+    MemoryRegion sram_a4;
+};
+
+/**
+ * Emulate Boot ROM firmware setup functionality.
+ *
+ * A real Allwinner R40 SoC contains a Boot ROM
+ * which is the first code that runs right after
+ * the SoC is powered on. The Boot ROM is responsible
+ * for loading user code (e.g. a bootloader) from any
+ * of the supported external devices and writing the
+ * downloaded code to internal SRAM. After loading the SoC
+ * begins executing the code written to SRAM.
+ *
+ * This function emulates the Boot ROM by copying 32 KiB
+ * of data from the given block device and writes it to
+ * the start of the first internal SRAM memory.
+ *
+ * @s: Allwinner R40 state object pointer
+ * @blk: Block backend device object pointer
+ * @unit: the mmc control's unit
+ */
+bool allwinner_r40_bootrom_setup(AwR40State *s, BlockBackend *blk, int unit);
+
+#endif /* HW_ARM_ALLWINNER_R40_H */
diff --git a/include/hw/arm/xlnx-versal.h b/include/hw/arm/xlnx-versal.h
index b6786e9..39ee311 100644
--- a/include/hw/arm/xlnx-versal.h
+++ b/include/hw/arm/xlnx-versal.h
@@ -31,6 +31,7 @@
 #include "hw/dma/xlnx_csu_dma.h"
 #include "hw/misc/xlnx-versal-crl.h"
 #include "hw/misc/xlnx-versal-pmc-iou-slcr.h"
+#include "hw/net/xlnx-versal-canfd.h"
 
 #define TYPE_XLNX_VERSAL "xlnx-versal"
 OBJECT_DECLARE_SIMPLE_TYPE(Versal, XLNX_VERSAL)
@@ -43,6 +44,8 @@ OBJECT_DECLARE_SIMPLE_TYPE(Versal, XLNX_VERSAL)
 #define XLNX_VERSAL_NR_SDS     2
 #define XLNX_VERSAL_NR_XRAM    4
 #define XLNX_VERSAL_NR_IRQS    192
+#define XLNX_VERSAL_NR_CANFD   2
+#define XLNX_VERSAL_CANFD_REF_CLK (24 * 1000 * 1000)
 
 struct Versal {
     /*< private >*/
@@ -73,6 +76,8 @@ struct Versal {
             CadenceGEMState gem[XLNX_VERSAL_NR_GEMS];
             XlnxZDMA adma[XLNX_VERSAL_NR_ADMAS];
             VersalUsb2 usb;
+            CanBusState *canbus[XLNX_VERSAL_NR_CANFD];
+            XlnxVersalCANFDState canfd[XLNX_VERSAL_NR_CANFD];
         } iou;
 
         /* Real-time Processing Unit.  */
@@ -133,6 +138,8 @@ struct Versal {
 #define VERSAL_CRL_IRQ             10
 #define VERSAL_UART0_IRQ_0         18
 #define VERSAL_UART1_IRQ_0         19
+#define VERSAL_CANFD0_IRQ_0        20
+#define VERSAL_CANFD1_IRQ_0        21
 #define VERSAL_USB0_IRQ_0          22
 #define VERSAL_GEM0_IRQ_0          56
 #define VERSAL_GEM0_WAKE_IRQ_0     57
@@ -163,6 +170,11 @@ struct Versal {
 #define MM_UART1                    0xff010000U
 #define MM_UART1_SIZE               0x10000
 
+#define MM_CANFD0                   0xff060000U
+#define MM_CANFD0_SIZE              0x10000
+#define MM_CANFD1                   0xff070000U
+#define MM_CANFD1_SIZE              0x10000
+
 #define MM_GEM0                     0xff0c0000U
 #define MM_GEM0_SIZE                0x10000
 #define MM_GEM1                     0xff0d0000U
diff --git a/include/hw/misc/allwinner-r40-ccu.h b/include/hw/misc/allwinner-r40-ccu.h
new file mode 100644
index 0000000..ceb74ef
--- /dev/null
+++ b/include/hw/misc/allwinner-r40-ccu.h
@@ -0,0 +1,65 @@
+/*
+ * Allwinner R40 Clock Control Unit emulation
+ *
+ * Copyright (C) 2023 qianfan Zhao <qianfanguijin@163.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef HW_MISC_ALLWINNER_R40_CCU_H
+#define HW_MISC_ALLWINNER_R40_CCU_H
+
+#include "qom/object.h"
+#include "hw/sysbus.h"
+
+/**
+ * @name Constants
+ * @{
+ */
+
+/** Size of register I/O address space used by CCU device */
+#define AW_R40_CCU_IOSIZE        (0x400)
+
+/** Total number of known registers */
+#define AW_R40_CCU_REGS_NUM      (AW_R40_CCU_IOSIZE / sizeof(uint32_t))
+
+/** @} */
+
+/**
+ * @name Object model
+ * @{
+ */
+
+#define TYPE_AW_R40_CCU    "allwinner-r40-ccu"
+OBJECT_DECLARE_SIMPLE_TYPE(AwR40ClockCtlState, AW_R40_CCU)
+
+/** @} */
+
+/**
+ * Allwinner R40 CCU object instance state.
+ */
+struct AwR40ClockCtlState {
+    /*< private >*/
+    SysBusDevice parent_obj;
+    /*< public >*/
+
+    /** Maps I/O registers in physical memory */
+    MemoryRegion iomem;
+
+    /** Array of hardware registers */
+    uint32_t regs[AW_R40_CCU_REGS_NUM];
+
+};
+
+#endif /* HW_MISC_ALLWINNER_R40_CCU_H */
diff --git a/include/hw/misc/allwinner-r40-dramc.h b/include/hw/misc/allwinner-r40-dramc.h
new file mode 100644
index 0000000..6a1a3a7
--- /dev/null
+++ b/include/hw/misc/allwinner-r40-dramc.h
@@ -0,0 +1,108 @@
+/*
+ * Allwinner R40 SDRAM Controller emulation
+ *
+ * Copyright (C) 2023 qianfan Zhao <qianfanguijin@163.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef HW_MISC_ALLWINNER_R40_DRAMC_H
+#define HW_MISC_ALLWINNER_R40_DRAMC_H
+
+#include "qom/object.h"
+#include "hw/sysbus.h"
+#include "exec/hwaddr.h"
+
+/**
+ * Constants
+ * @{
+ */
+
+/** Highest register address used by DRAMCOM module */
+#define AW_R40_DRAMCOM_REGS_MAXADDR  (0x804)
+
+/** Total number of known DRAMCOM registers */
+#define AW_R40_DRAMCOM_REGS_NUM      (AW_R40_DRAMCOM_REGS_MAXADDR / \
+                                     sizeof(uint32_t))
+
+/** Highest register address used by DRAMCTL module */
+#define AW_R40_DRAMCTL_REGS_MAXADDR  (0x88c)
+
+/** Total number of known DRAMCTL registers */
+#define AW_R40_DRAMCTL_REGS_NUM      (AW_R40_DRAMCTL_REGS_MAXADDR / \
+                                     sizeof(uint32_t))
+
+/** Highest register address used by DRAMPHY module */
+#define AW_R40_DRAMPHY_REGS_MAXADDR  (0x4)
+
+/** Total number of known DRAMPHY registers */
+#define AW_R40_DRAMPHY_REGS_NUM      (AW_R40_DRAMPHY_REGS_MAXADDR / \
+                                     sizeof(uint32_t))
+
+/** @} */
+
+/**
+ * Object model
+ * @{
+ */
+
+#define TYPE_AW_R40_DRAMC "allwinner-r40-dramc"
+OBJECT_DECLARE_SIMPLE_TYPE(AwR40DramCtlState, AW_R40_DRAMC)
+
+/** @} */
+
+/**
+ * Allwinner R40 SDRAM Controller object instance state.
+ */
+struct AwR40DramCtlState {
+    /*< private >*/
+    SysBusDevice parent_obj;
+    /*< public >*/
+
+    /** Physical base address for start of RAM */
+    hwaddr ram_addr;
+
+    /** Total RAM size in megabytes */
+    uint32_t ram_size;
+
+    uint8_t set_row_bits;
+    uint8_t set_bank_bits;
+    uint8_t set_col_bits;
+
+    /**
+     * @name Memory Regions
+     * @{
+     */
+    MemoryRegion dramcom_iomem;    /**< DRAMCOM module I/O registers */
+    MemoryRegion dramctl_iomem;    /**< DRAMCTL module I/O registers */
+    MemoryRegion dramphy_iomem;    /**< DRAMPHY module I/O registers */
+    MemoryRegion dram_high;        /**< The high 1G dram for dualrank detect */
+    MemoryRegion detect_cells;     /**< DRAM memory cells for auto detect */
+
+    /** @} */
+
+    /**
+     * @name Hardware Registers
+     * @{
+     */
+
+    uint32_t dramcom[AW_R40_DRAMCOM_REGS_NUM]; /**< DRAMCOM registers */
+    uint32_t dramctl[AW_R40_DRAMCTL_REGS_NUM]; /**< DRAMCTL registers */
+    uint32_t dramphy[AW_R40_DRAMPHY_REGS_NUM] ;/**< DRAMPHY registers */
+
+    /** @} */
+
+};
+
+#endif /* HW_MISC_ALLWINNER_R40_DRAMC_H */
diff --git a/include/hw/misc/allwinner-sramc.h b/include/hw/misc/allwinner-sramc.h
new file mode 100644
index 0000000..66b01b8
--- /dev/null
+++ b/include/hw/misc/allwinner-sramc.h
@@ -0,0 +1,69 @@
+/*
+ * Allwinner SRAM controller emulation
+ *
+ * Copyright (C) 2023 qianfan Zhao <qianfanguijin@163.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef HW_MISC_ALLWINNER_SRAMC_H
+#define HW_MISC_ALLWINNER_SRAMC_H
+
+#include "qom/object.h"
+#include "hw/sysbus.h"
+#include "qemu/uuid.h"
+
+/**
+ * Object model
+ * @{
+ */
+#define TYPE_AW_SRAMC               "allwinner-sramc"
+#define TYPE_AW_SRAMC_SUN8I_R40     TYPE_AW_SRAMC "-sun8i-r40"
+OBJECT_DECLARE_TYPE(AwSRAMCState, AwSRAMCClass, AW_SRAMC)
+
+/** @} */
+
+/**
+ * Allwinner SRAMC object instance state
+ */
+struct AwSRAMCState {
+    /*< private >*/
+    SysBusDevice parent_obj;
+    /*< public >*/
+
+    /** Maps I/O registers in physical memory */
+    MemoryRegion iomem;
+
+    /* registers */
+    uint32_t sram_ctl1;
+    uint32_t sram_ver;
+    uint32_t sram_soft_entry_reg0;
+};
+
+/**
+ * Allwinner SRAM Controller class-level struct.
+ *
+ * This struct is filled by each sunxi device specific code
+ * such that the generic code can use this struct to support
+ * all devices.
+ */
+struct AwSRAMCClass {
+    /*< private >*/
+    SysBusDeviceClass parent_class;
+    /*< public >*/
+
+    uint32_t sram_version_code;
+};
+
+#endif /* HW_MISC_ALLWINNER_SRAMC_H */
diff --git a/include/hw/net/xlnx-versal-canfd.h b/include/hw/net/xlnx-versal-canfd.h
new file mode 100644
index 0000000..ad3104d
--- /dev/null
+++ b/include/hw/net/xlnx-versal-canfd.h
@@ -0,0 +1,87 @@
+/*
+ * QEMU model of the Xilinx Versal CANFD Controller.
+ *
+ * Copyright (c) 2023 Advanced Micro Devices, Inc.
+ *
+ * Written-by: Vikram Garhwal<vikram.garhwal@amd.com>
+ * Based on QEMU CANFD Device emulation implemented by Jin Yang, Deniz Eren and
+ * Pavel Pisa.
+ * 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.
+ */
+
+#ifndef HW_CANFD_XILINX_H
+#define HW_CANFD_XILINX_H
+
+#include "hw/register.h"
+#include "hw/ptimer.h"
+#include "net/can_emu.h"
+#include "hw/qdev-clock.h"
+
+#define TYPE_XILINX_CANFD "xlnx.versal-canfd"
+
+OBJECT_DECLARE_SIMPLE_TYPE(XlnxVersalCANFDState, XILINX_CANFD)
+
+#define NUM_REGS_PER_MSG_SPACE 18 /* 1 ID + 1 DLC + 16 Data(DW0 - DW15) regs. */
+#define MAX_NUM_RX             64
+#define OFFSET_RX1_DW15        (0x4144 / 4)
+#define CANFD_TIMER_MAX        0xFFFFUL
+#define CANFD_DEFAULT_CLOCK    (25 * 1000 * 1000)
+
+#define XLNX_VERSAL_CANFD_R_MAX (OFFSET_RX1_DW15 + \
+                    ((MAX_NUM_RX - 1) * NUM_REGS_PER_MSG_SPACE) + 1)
+
+typedef struct XlnxVersalCANFDState {
+    SysBusDevice            parent_obj;
+    MemoryRegion            iomem;
+
+    qemu_irq                irq_canfd_int;
+    qemu_irq                irq_addr_err;
+
+    RegisterInfo            reg_info[XLNX_VERSAL_CANFD_R_MAX];
+    RegisterAccessInfo      *tx_regs;
+    RegisterAccessInfo      *rx0_regs;
+    RegisterAccessInfo      *rx1_regs;
+    RegisterAccessInfo      *af_regs;
+    RegisterAccessInfo      *txe_regs;
+    RegisterAccessInfo      *rx_mailbox_regs;
+    RegisterAccessInfo      *af_mask_regs_mailbox;
+
+    uint32_t                regs[XLNX_VERSAL_CANFD_R_MAX];
+
+    ptimer_state            *canfd_timer;
+
+    CanBusClientState       bus_client;
+    CanBusState             *canfdbus;
+
+    struct {
+        uint8_t             rx0_fifo;
+        uint8_t             rx1_fifo;
+        uint8_t             tx_fifo;
+        bool                enable_rx_fifo1;
+        uint32_t            ext_clk_freq;
+   } cfg;
+
+} XlnxVersalCANFDState;
+
+typedef struct tx_ready_reg_info {
+    uint32_t can_id;
+    uint32_t reg_num;
+} tx_ready_reg_info;
+
+#endif
diff --git a/include/hw/sd/allwinner-sdhost.h b/include/hw/sd/allwinner-sdhost.h
index 30c1e60..1b95117 100644
--- a/include/hw/sd/allwinner-sdhost.h
+++ b/include/hw/sd/allwinner-sdhost.h
@@ -38,6 +38,12 @@
 /** Allwinner sun5i family and newer (A13, H2+, H3, etc) */
 #define TYPE_AW_SDHOST_SUN5I TYPE_AW_SDHOST "-sun5i"
 
+/** Allwinner sun50i-a64 */
+#define TYPE_AW_SDHOST_SUN50I_A64 TYPE_AW_SDHOST "-sun50i-a64"
+
+/** Allwinner sun50i-a64 emmc */
+#define TYPE_AW_SDHOST_SUN50I_A64_EMMC  TYPE_AW_SDHOST "-sun50i-a64-emmc"
+
 /** @} */
 
 /**
@@ -110,6 +116,7 @@ struct AwSdHostState {
     uint32_t startbit_detect;   /**< eMMC DDR Start Bit Detection Control */
     uint32_t response_crc;      /**< Response CRC */
     uint32_t data_crc[8];       /**< Data CRC */
+    uint32_t sample_delay;      /**< Sample delay control */
     uint32_t status_crc;        /**< Status CRC */
 
     /** @} */
@@ -132,6 +139,8 @@ struct AwSdHostClass {
     size_t max_desc_size;
     bool   is_sun4i;
 
+    /** does the IP block support autocalibration? */
+    bool can_calibrate;
 };
 
 #endif /* HW_SD_ALLWINNER_SDHOST_H */
diff --git a/include/sysemu/hvf.h b/include/sysemu/hvf.h
index bb70082..70549b9 100644
--- a/include/sysemu/hvf.h
+++ b/include/sysemu/hvf.h
@@ -17,6 +17,7 @@
 #include "qom/object.h"
 
 #ifdef NEED_CPU_H
+#include "cpu.h"
 
 #ifdef CONFIG_HVF
 uint32_t hvf_get_supported_cpuid(uint32_t func, uint32_t idx,
@@ -36,4 +37,40 @@ typedef struct HVFState HVFState;
 DECLARE_INSTANCE_CHECKER(HVFState, HVF_STATE,
                          TYPE_HVF_ACCEL)
 
+#ifdef NEED_CPU_H
+struct hvf_sw_breakpoint {
+    target_ulong pc;
+    target_ulong saved_insn;
+    int use_count;
+    QTAILQ_ENTRY(hvf_sw_breakpoint) entry;
+};
+
+struct hvf_sw_breakpoint *hvf_find_sw_breakpoint(CPUState *cpu,
+                                                 target_ulong pc);
+int hvf_sw_breakpoints_active(CPUState *cpu);
+
+int hvf_arch_insert_sw_breakpoint(CPUState *cpu, struct hvf_sw_breakpoint *bp);
+int hvf_arch_remove_sw_breakpoint(CPUState *cpu, struct hvf_sw_breakpoint *bp);
+int hvf_arch_insert_hw_breakpoint(target_ulong addr, target_ulong len,
+                                  int type);
+int hvf_arch_remove_hw_breakpoint(target_ulong addr, target_ulong len,
+                                  int type);
+void hvf_arch_remove_all_hw_breakpoints(void);
+
+/*
+ * hvf_update_guest_debug:
+ * @cs: CPUState for the CPU to update
+ *
+ * Update guest to enable or disable debugging. Per-arch specifics will be
+ * handled by calling down to hvf_arch_update_guest_debug.
+ */
+int hvf_update_guest_debug(CPUState *cpu);
+void hvf_arch_update_guest_debug(CPUState *cpu);
+
+/*
+ * Return whether the guest supports debugging.
+ */
+bool hvf_arch_supports_guest_debug(void);
+#endif /* NEED_CPU_H */
+
 #endif
diff --git a/include/sysemu/hvf_int.h b/include/sysemu/hvf_int.h
index 6545f7c..6ab119e 100644
--- a/include/sysemu/hvf_int.h
+++ b/include/sysemu/hvf_int.h
@@ -45,6 +45,7 @@ struct HVFState {
 
     hvf_vcpu_caps *hvf_caps;
     uint64_t vtimer_offset;
+    QTAILQ_HEAD(, hvf_sw_breakpoint) hvf_sw_breakpoints;
 };
 extern HVFState *hvf_state;
 
@@ -53,6 +54,7 @@ struct hvf_vcpu_state {
     void *exit;
     bool vtimer_masked;
     sigset_t unblock_ipi_mask;
+    bool guest_debug_enabled;
 };
 
 void assert_hvf_ok(hv_return_t ret);
diff --git a/target/arm/cpu.c b/target/arm/cpu.c
index 5182ed0..4d5bb57 100644
--- a/target/arm/cpu.c
+++ b/target/arm/cpu.c
@@ -289,6 +289,8 @@ static void arm_cpu_reset_hold(Object *obj)
          * This is not yet exposed from the Linux kernel in any way.
          */
         env->cp15.sctlr_el[1] |= SCTLR_TSCXT;
+        /* Disable access to Debug Communication Channel (DCC). */
+        env->cp15.mdscr_el1 |= 1 << 12;
 #else
         /* Reset into the highest available EL */
         if (arm_feature(env, ARM_FEATURE_EL3)) {
diff --git a/target/arm/cpu.h b/target/arm/cpu.h
index d469a26..36c608f 100644
--- a/target/arm/cpu.h
+++ b/target/arm/cpu.h
@@ -677,8 +677,16 @@ typedef struct CPUArchState {
         uint64_t zcr_el[4];   /* ZCR_EL[1-3] */
         uint64_t smcr_el[4];  /* SMCR_EL[1-3] */
     } vfp;
+
     uint64_t exclusive_addr;
     uint64_t exclusive_val;
+    /*
+     * Contains the 'val' for the second 64-bit register of LDXP, which comes
+     * from the higher address, not the high part of a complete 128-bit value.
+     * In some ways it might be more convenient to record the exclusive value
+     * as the low and high halves of a 128 bit data value, but the current
+     * semantics of these fields are baked into the migration format.
+     */
     uint64_t exclusive_high;
 
     /* iwMMXt coprocessor state.  */
@@ -1240,7 +1248,7 @@ void pmu_init(ARMCPU *cpu);
 #define SCTLR_D       (1U << 5) /* up to v5; RAO in v6 */
 #define SCTLR_CP15BEN (1U << 5) /* v7 onward */
 #define SCTLR_L       (1U << 6) /* up to v5; RAO in v6 and v7; RAZ in v8 */
-#define SCTLR_nAA     (1U << 6) /* when v8.4-LSE is implemented */
+#define SCTLR_nAA     (1U << 6) /* when FEAT_LSE2 is implemented */
 #define SCTLR_B       (1U << 7) /* up to v6; RAZ in v7 */
 #define SCTLR_ITD     (1U << 7) /* v8 onward */
 #define SCTLR_S       (1U << 8) /* up to v6; RAZ in v7 */
@@ -3036,6 +3044,7 @@ FIELD(TBFLAG_A64, SVL, 24, 4)
 /* Indicates that SME Streaming mode is active, and SMCR_ELx.FA64 is not. */
 FIELD(TBFLAG_A64, SME_TRAP_NONSTREAMING, 28, 1)
 FIELD(TBFLAG_A64, FGT_ERET, 29, 1)
+FIELD(TBFLAG_A64, NAA, 30, 1)
 
 /*
  * Helpers for using the above.
@@ -3843,6 +3852,11 @@ static inline bool isar_feature_aa64_st(const ARMISARegisters *id)
     return FIELD_EX64(id->id_aa64mmfr2, ID_AA64MMFR2, ST) != 0;
 }
 
+static inline bool isar_feature_aa64_lse2(const ARMISARegisters *id)
+{
+    return FIELD_EX64(id->id_aa64mmfr2, ID_AA64MMFR2, AT) != 0;
+}
+
 static inline bool isar_feature_aa64_fwb(const ARMISARegisters *id)
 {
     return FIELD_EX64(id->id_aa64mmfr2, ID_AA64MMFR2, FWB) != 0;
diff --git a/target/arm/debug_helper.c b/target/arm/debug_helper.c
index d41cc64..8362462 100644
--- a/target/arm/debug_helper.c
+++ b/target/arm/debug_helper.c
@@ -842,12 +842,14 @@ static CPAccessResult access_tda(CPUARMState *env, const ARMCPRegInfo *ri,
  * is implemented then these are controlled by MDCR_EL2.TDCC for
  * EL2 and MDCR_EL3.TDCC for EL3. They are also controlled by
  * the general debug access trap bits MDCR_EL2.TDA and MDCR_EL3.TDA.
+ * For EL0, they are also controlled by MDSCR_EL1.TDCC.
  */
 static CPAccessResult access_tdcc(CPUARMState *env, const ARMCPRegInfo *ri,
                                   bool isread)
 {
     int el = arm_current_el(env);
     uint64_t mdcr_el2 = arm_mdcr_el2_eff(env);
+    bool mdscr_el1_tdcc = extract32(env->cp15.mdscr_el1, 12, 1);
     bool mdcr_el2_tda = (mdcr_el2 & MDCR_TDA) || (mdcr_el2 & MDCR_TDE) ||
         (arm_hcr_el2_eff(env) & HCR_TGE);
     bool mdcr_el2_tdcc = cpu_isar_feature(aa64_fgt, env_archcpu(env)) &&
@@ -855,6 +857,9 @@ static CPAccessResult access_tdcc(CPUARMState *env, const ARMCPRegInfo *ri,
     bool mdcr_el3_tdcc = cpu_isar_feature(aa64_fgt, env_archcpu(env)) &&
                                           (env->cp15.mdcr_el3 & MDCR_TDCC);
 
+    if (el < 1 && mdscr_el1_tdcc) {
+        return CP_ACCESS_TRAP;
+    }
     if (el < 2 && (mdcr_el2_tda || mdcr_el2_tdcc)) {
         return CP_ACCESS_TRAP_EL2;
     }
diff --git a/target/arm/helper.c b/target/arm/helper.c
index 0b7fd2e..d4bee43 100644
--- a/target/arm/helper.c
+++ b/target/arm/helper.c
@@ -7405,7 +7405,6 @@ static const ARMCPRegInfo rndr_reginfo[] = {
       .access = PL0_R, .readfn = rndr_readfn },
 };
 
-#ifndef CONFIG_USER_ONLY
 static void dccvap_writefn(CPUARMState *env, const ARMCPRegInfo *opaque,
                           uint64_t value)
 {
@@ -7420,6 +7419,7 @@ static void dccvap_writefn(CPUARMState *env, const ARMCPRegInfo *opaque,
     /* This won't be crossing page boundaries */
     haddr = probe_read(env, vaddr, dline_size, mem_idx, GETPC());
     if (haddr) {
+#ifndef CONFIG_USER_ONLY
 
         ram_addr_t offset;
         MemoryRegion *mr;
@@ -7430,6 +7430,7 @@ static void dccvap_writefn(CPUARMState *env, const ARMCPRegInfo *opaque,
         if (mr) {
             memory_region_writeback(mr, offset, dline_size);
         }
+#endif /*CONFIG_USER_ONLY*/
     }
 }
 
@@ -7448,7 +7449,6 @@ static const ARMCPRegInfo dcpodp_reg[] = {
       .fgt = FGT_DCCVADP,
       .accessfn = aa64_cacheop_poc_access, .writefn = dccvap_writefn },
 };
-#endif /*CONFIG_USER_ONLY*/
 
 static CPAccessResult access_aa64_tid5(CPUARMState *env, const ARMCPRegInfo *ri,
                                        bool isread)
@@ -9092,7 +9092,6 @@ void register_cp_regs_for_features(ARMCPU *cpu)
     if (cpu_isar_feature(aa64_tlbios, cpu)) {
         define_arm_cp_regs(cpu, tlbios_reginfo);
     }
-#ifndef CONFIG_USER_ONLY
     /* Data Cache clean instructions up to PoP */
     if (cpu_isar_feature(aa64_dcpop, cpu)) {
         define_one_arm_cp_reg(cpu, dcpop_reg);
@@ -9101,7 +9100,6 @@ void register_cp_regs_for_features(ARMCPU *cpu)
             define_one_arm_cp_reg(cpu, dcpodp_reg);
         }
     }
-#endif /*CONFIG_USER_ONLY*/
 
     /*
      * If full MTE is enabled, add all of the system registers.
diff --git a/target/arm/hvf/hvf.c b/target/arm/hvf/hvf.c
index 5900dc7..8f72624 100644
--- a/target/arm/hvf/hvf.c
+++ b/target/arm/hvf/hvf.c
@@ -31,6 +31,118 @@
 #include "trace/trace-target_arm_hvf.h"
 #include "migration/vmstate.h"
 
+#include "exec/gdbstub.h"
+
+#define MDSCR_EL1_SS_SHIFT  0
+#define MDSCR_EL1_MDE_SHIFT 15
+
+static uint16_t dbgbcr_regs[] = {
+    HV_SYS_REG_DBGBCR0_EL1,
+    HV_SYS_REG_DBGBCR1_EL1,
+    HV_SYS_REG_DBGBCR2_EL1,
+    HV_SYS_REG_DBGBCR3_EL1,
+    HV_SYS_REG_DBGBCR4_EL1,
+    HV_SYS_REG_DBGBCR5_EL1,
+    HV_SYS_REG_DBGBCR6_EL1,
+    HV_SYS_REG_DBGBCR7_EL1,
+    HV_SYS_REG_DBGBCR8_EL1,
+    HV_SYS_REG_DBGBCR9_EL1,
+    HV_SYS_REG_DBGBCR10_EL1,
+    HV_SYS_REG_DBGBCR11_EL1,
+    HV_SYS_REG_DBGBCR12_EL1,
+    HV_SYS_REG_DBGBCR13_EL1,
+    HV_SYS_REG_DBGBCR14_EL1,
+    HV_SYS_REG_DBGBCR15_EL1,
+};
+static uint16_t dbgbvr_regs[] = {
+    HV_SYS_REG_DBGBVR0_EL1,
+    HV_SYS_REG_DBGBVR1_EL1,
+    HV_SYS_REG_DBGBVR2_EL1,
+    HV_SYS_REG_DBGBVR3_EL1,
+    HV_SYS_REG_DBGBVR4_EL1,
+    HV_SYS_REG_DBGBVR5_EL1,
+    HV_SYS_REG_DBGBVR6_EL1,
+    HV_SYS_REG_DBGBVR7_EL1,
+    HV_SYS_REG_DBGBVR8_EL1,
+    HV_SYS_REG_DBGBVR9_EL1,
+    HV_SYS_REG_DBGBVR10_EL1,
+    HV_SYS_REG_DBGBVR11_EL1,
+    HV_SYS_REG_DBGBVR12_EL1,
+    HV_SYS_REG_DBGBVR13_EL1,
+    HV_SYS_REG_DBGBVR14_EL1,
+    HV_SYS_REG_DBGBVR15_EL1,
+};
+static uint16_t dbgwcr_regs[] = {
+    HV_SYS_REG_DBGWCR0_EL1,
+    HV_SYS_REG_DBGWCR1_EL1,
+    HV_SYS_REG_DBGWCR2_EL1,
+    HV_SYS_REG_DBGWCR3_EL1,
+    HV_SYS_REG_DBGWCR4_EL1,
+    HV_SYS_REG_DBGWCR5_EL1,
+    HV_SYS_REG_DBGWCR6_EL1,
+    HV_SYS_REG_DBGWCR7_EL1,
+    HV_SYS_REG_DBGWCR8_EL1,
+    HV_SYS_REG_DBGWCR9_EL1,
+    HV_SYS_REG_DBGWCR10_EL1,
+    HV_SYS_REG_DBGWCR11_EL1,
+    HV_SYS_REG_DBGWCR12_EL1,
+    HV_SYS_REG_DBGWCR13_EL1,
+    HV_SYS_REG_DBGWCR14_EL1,
+    HV_SYS_REG_DBGWCR15_EL1,
+};
+static uint16_t dbgwvr_regs[] = {
+    HV_SYS_REG_DBGWVR0_EL1,
+    HV_SYS_REG_DBGWVR1_EL1,
+    HV_SYS_REG_DBGWVR2_EL1,
+    HV_SYS_REG_DBGWVR3_EL1,
+    HV_SYS_REG_DBGWVR4_EL1,
+    HV_SYS_REG_DBGWVR5_EL1,
+    HV_SYS_REG_DBGWVR6_EL1,
+    HV_SYS_REG_DBGWVR7_EL1,
+    HV_SYS_REG_DBGWVR8_EL1,
+    HV_SYS_REG_DBGWVR9_EL1,
+    HV_SYS_REG_DBGWVR10_EL1,
+    HV_SYS_REG_DBGWVR11_EL1,
+    HV_SYS_REG_DBGWVR12_EL1,
+    HV_SYS_REG_DBGWVR13_EL1,
+    HV_SYS_REG_DBGWVR14_EL1,
+    HV_SYS_REG_DBGWVR15_EL1,
+};
+
+static inline int hvf_arm_num_brps(hv_vcpu_config_t config)
+{
+    uint64_t val;
+    hv_return_t ret;
+    ret = hv_vcpu_config_get_feature_reg(config, HV_FEATURE_REG_ID_AA64DFR0_EL1,
+                                         &val);
+    assert_hvf_ok(ret);
+    return FIELD_EX64(val, ID_AA64DFR0, BRPS) + 1;
+}
+
+static inline int hvf_arm_num_wrps(hv_vcpu_config_t config)
+{
+    uint64_t val;
+    hv_return_t ret;
+    ret = hv_vcpu_config_get_feature_reg(config, HV_FEATURE_REG_ID_AA64DFR0_EL1,
+                                         &val);
+    assert_hvf_ok(ret);
+    return FIELD_EX64(val, ID_AA64DFR0, WRPS) + 1;
+}
+
+void hvf_arm_init_debug(void)
+{
+    hv_vcpu_config_t config;
+    config = hv_vcpu_config_create();
+
+    max_hw_bps = hvf_arm_num_brps(config);
+    hw_breakpoints =
+        g_array_sized_new(true, true, sizeof(HWBreakpoint), max_hw_bps);
+
+    max_hw_wps = hvf_arm_num_wrps(config);
+    hw_watchpoints =
+        g_array_sized_new(true, true, sizeof(HWWatchpoint), max_hw_wps);
+}
+
 #define HVF_SYSREG(crn, crm, op0, op1, op2) \
         ENCODE_AA64_CP_REG(CP_REG_ARM64_SYSREG_CP, crn, crm, op0, op1, op2)
 #define PL1_WRITE_MASK 0x4
@@ -107,6 +219,72 @@
 #define SYSREG_ICC_SGI1R_EL1     SYSREG(3, 0, 12, 11, 5)
 #define SYSREG_ICC_SRE_EL1       SYSREG(3, 0, 12, 12, 5)
 
+#define SYSREG_MDSCR_EL1      SYSREG(2, 0, 0, 2, 2)
+#define SYSREG_DBGBVR0_EL1    SYSREG(2, 0, 0, 0, 4)
+#define SYSREG_DBGBCR0_EL1    SYSREG(2, 0, 0, 0, 5)
+#define SYSREG_DBGWVR0_EL1    SYSREG(2, 0, 0, 0, 6)
+#define SYSREG_DBGWCR0_EL1    SYSREG(2, 0, 0, 0, 7)
+#define SYSREG_DBGBVR1_EL1    SYSREG(2, 0, 0, 1, 4)
+#define SYSREG_DBGBCR1_EL1    SYSREG(2, 0, 0, 1, 5)
+#define SYSREG_DBGWVR1_EL1    SYSREG(2, 0, 0, 1, 6)
+#define SYSREG_DBGWCR1_EL1    SYSREG(2, 0, 0, 1, 7)
+#define SYSREG_DBGBVR2_EL1    SYSREG(2, 0, 0, 2, 4)
+#define SYSREG_DBGBCR2_EL1    SYSREG(2, 0, 0, 2, 5)
+#define SYSREG_DBGWVR2_EL1    SYSREG(2, 0, 0, 2, 6)
+#define SYSREG_DBGWCR2_EL1    SYSREG(2, 0, 0, 2, 7)
+#define SYSREG_DBGBVR3_EL1    SYSREG(2, 0, 0, 3, 4)
+#define SYSREG_DBGBCR3_EL1    SYSREG(2, 0, 0, 3, 5)
+#define SYSREG_DBGWVR3_EL1    SYSREG(2, 0, 0, 3, 6)
+#define SYSREG_DBGWCR3_EL1    SYSREG(2, 0, 0, 3, 7)
+#define SYSREG_DBGBVR4_EL1    SYSREG(2, 0, 0, 4, 4)
+#define SYSREG_DBGBCR4_EL1    SYSREG(2, 0, 0, 4, 5)
+#define SYSREG_DBGWVR4_EL1    SYSREG(2, 0, 0, 4, 6)
+#define SYSREG_DBGWCR4_EL1    SYSREG(2, 0, 0, 4, 7)
+#define SYSREG_DBGBVR5_EL1    SYSREG(2, 0, 0, 5, 4)
+#define SYSREG_DBGBCR5_EL1    SYSREG(2, 0, 0, 5, 5)
+#define SYSREG_DBGWVR5_EL1    SYSREG(2, 0, 0, 5, 6)
+#define SYSREG_DBGWCR5_EL1    SYSREG(2, 0, 0, 5, 7)
+#define SYSREG_DBGBVR6_EL1    SYSREG(2, 0, 0, 6, 4)
+#define SYSREG_DBGBCR6_EL1    SYSREG(2, 0, 0, 6, 5)
+#define SYSREG_DBGWVR6_EL1    SYSREG(2, 0, 0, 6, 6)
+#define SYSREG_DBGWCR6_EL1    SYSREG(2, 0, 0, 6, 7)
+#define SYSREG_DBGBVR7_EL1    SYSREG(2, 0, 0, 7, 4)
+#define SYSREG_DBGBCR7_EL1    SYSREG(2, 0, 0, 7, 5)
+#define SYSREG_DBGWVR7_EL1    SYSREG(2, 0, 0, 7, 6)
+#define SYSREG_DBGWCR7_EL1    SYSREG(2, 0, 0, 7, 7)
+#define SYSREG_DBGBVR8_EL1    SYSREG(2, 0, 0, 8, 4)
+#define SYSREG_DBGBCR8_EL1    SYSREG(2, 0, 0, 8, 5)
+#define SYSREG_DBGWVR8_EL1    SYSREG(2, 0, 0, 8, 6)
+#define SYSREG_DBGWCR8_EL1    SYSREG(2, 0, 0, 8, 7)
+#define SYSREG_DBGBVR9_EL1    SYSREG(2, 0, 0, 9, 4)
+#define SYSREG_DBGBCR9_EL1    SYSREG(2, 0, 0, 9, 5)
+#define SYSREG_DBGWVR9_EL1    SYSREG(2, 0, 0, 9, 6)
+#define SYSREG_DBGWCR9_EL1    SYSREG(2, 0, 0, 9, 7)
+#define SYSREG_DBGBVR10_EL1   SYSREG(2, 0, 0, 10, 4)
+#define SYSREG_DBGBCR10_EL1   SYSREG(2, 0, 0, 10, 5)
+#define SYSREG_DBGWVR10_EL1   SYSREG(2, 0, 0, 10, 6)
+#define SYSREG_DBGWCR10_EL1   SYSREG(2, 0, 0, 10, 7)
+#define SYSREG_DBGBVR11_EL1   SYSREG(2, 0, 0, 11, 4)
+#define SYSREG_DBGBCR11_EL1   SYSREG(2, 0, 0, 11, 5)
+#define SYSREG_DBGWVR11_EL1   SYSREG(2, 0, 0, 11, 6)
+#define SYSREG_DBGWCR11_EL1   SYSREG(2, 0, 0, 11, 7)
+#define SYSREG_DBGBVR12_EL1   SYSREG(2, 0, 0, 12, 4)
+#define SYSREG_DBGBCR12_EL1   SYSREG(2, 0, 0, 12, 5)
+#define SYSREG_DBGWVR12_EL1   SYSREG(2, 0, 0, 12, 6)
+#define SYSREG_DBGWCR12_EL1   SYSREG(2, 0, 0, 12, 7)
+#define SYSREG_DBGBVR13_EL1   SYSREG(2, 0, 0, 13, 4)
+#define SYSREG_DBGBCR13_EL1   SYSREG(2, 0, 0, 13, 5)
+#define SYSREG_DBGWVR13_EL1   SYSREG(2, 0, 0, 13, 6)
+#define SYSREG_DBGWCR13_EL1   SYSREG(2, 0, 0, 13, 7)
+#define SYSREG_DBGBVR14_EL1   SYSREG(2, 0, 0, 14, 4)
+#define SYSREG_DBGBCR14_EL1   SYSREG(2, 0, 0, 14, 5)
+#define SYSREG_DBGWVR14_EL1   SYSREG(2, 0, 0, 14, 6)
+#define SYSREG_DBGWCR14_EL1   SYSREG(2, 0, 0, 14, 7)
+#define SYSREG_DBGBVR15_EL1   SYSREG(2, 0, 0, 15, 4)
+#define SYSREG_DBGBCR15_EL1   SYSREG(2, 0, 0, 15, 5)
+#define SYSREG_DBGWVR15_EL1   SYSREG(2, 0, 0, 15, 6)
+#define SYSREG_DBGWCR15_EL1   SYSREG(2, 0, 0, 15, 7)
+
 #define WFX_IS_WFE (1 << 0)
 
 #define TMR_CTL_ENABLE  (1 << 0)
@@ -397,6 +575,92 @@ int hvf_get_registers(CPUState *cpu)
             continue;
         }
 
+        if (cpu->hvf->guest_debug_enabled) {
+            /* Handle debug registers */
+            switch (hvf_sreg_match[i].reg) {
+            case HV_SYS_REG_DBGBVR0_EL1:
+            case HV_SYS_REG_DBGBCR0_EL1:
+            case HV_SYS_REG_DBGWVR0_EL1:
+            case HV_SYS_REG_DBGWCR0_EL1:
+            case HV_SYS_REG_DBGBVR1_EL1:
+            case HV_SYS_REG_DBGBCR1_EL1:
+            case HV_SYS_REG_DBGWVR1_EL1:
+            case HV_SYS_REG_DBGWCR1_EL1:
+            case HV_SYS_REG_DBGBVR2_EL1:
+            case HV_SYS_REG_DBGBCR2_EL1:
+            case HV_SYS_REG_DBGWVR2_EL1:
+            case HV_SYS_REG_DBGWCR2_EL1:
+            case HV_SYS_REG_DBGBVR3_EL1:
+            case HV_SYS_REG_DBGBCR3_EL1:
+            case HV_SYS_REG_DBGWVR3_EL1:
+            case HV_SYS_REG_DBGWCR3_EL1:
+            case HV_SYS_REG_DBGBVR4_EL1:
+            case HV_SYS_REG_DBGBCR4_EL1:
+            case HV_SYS_REG_DBGWVR4_EL1:
+            case HV_SYS_REG_DBGWCR4_EL1:
+            case HV_SYS_REG_DBGBVR5_EL1:
+            case HV_SYS_REG_DBGBCR5_EL1:
+            case HV_SYS_REG_DBGWVR5_EL1:
+            case HV_SYS_REG_DBGWCR5_EL1:
+            case HV_SYS_REG_DBGBVR6_EL1:
+            case HV_SYS_REG_DBGBCR6_EL1:
+            case HV_SYS_REG_DBGWVR6_EL1:
+            case HV_SYS_REG_DBGWCR6_EL1:
+            case HV_SYS_REG_DBGBVR7_EL1:
+            case HV_SYS_REG_DBGBCR7_EL1:
+            case HV_SYS_REG_DBGWVR7_EL1:
+            case HV_SYS_REG_DBGWCR7_EL1:
+            case HV_SYS_REG_DBGBVR8_EL1:
+            case HV_SYS_REG_DBGBCR8_EL1:
+            case HV_SYS_REG_DBGWVR8_EL1:
+            case HV_SYS_REG_DBGWCR8_EL1:
+            case HV_SYS_REG_DBGBVR9_EL1:
+            case HV_SYS_REG_DBGBCR9_EL1:
+            case HV_SYS_REG_DBGWVR9_EL1:
+            case HV_SYS_REG_DBGWCR9_EL1:
+            case HV_SYS_REG_DBGBVR10_EL1:
+            case HV_SYS_REG_DBGBCR10_EL1:
+            case HV_SYS_REG_DBGWVR10_EL1:
+            case HV_SYS_REG_DBGWCR10_EL1:
+            case HV_SYS_REG_DBGBVR11_EL1:
+            case HV_SYS_REG_DBGBCR11_EL1:
+            case HV_SYS_REG_DBGWVR11_EL1:
+            case HV_SYS_REG_DBGWCR11_EL1:
+            case HV_SYS_REG_DBGBVR12_EL1:
+            case HV_SYS_REG_DBGBCR12_EL1:
+            case HV_SYS_REG_DBGWVR12_EL1:
+            case HV_SYS_REG_DBGWCR12_EL1:
+            case HV_SYS_REG_DBGBVR13_EL1:
+            case HV_SYS_REG_DBGBCR13_EL1:
+            case HV_SYS_REG_DBGWVR13_EL1:
+            case HV_SYS_REG_DBGWCR13_EL1:
+            case HV_SYS_REG_DBGBVR14_EL1:
+            case HV_SYS_REG_DBGBCR14_EL1:
+            case HV_SYS_REG_DBGWVR14_EL1:
+            case HV_SYS_REG_DBGWCR14_EL1:
+            case HV_SYS_REG_DBGBVR15_EL1:
+            case HV_SYS_REG_DBGBCR15_EL1:
+            case HV_SYS_REG_DBGWVR15_EL1:
+            case HV_SYS_REG_DBGWCR15_EL1: {
+                /*
+                 * If the guest is being debugged, the vCPU's debug registers
+                 * are holding the gdbstub's view of the registers (set in
+                 * hvf_arch_update_guest_debug()).
+                 * Since the environment is used to store only the guest's view
+                 * of the registers, don't update it with the values from the
+                 * vCPU but simply keep the values from the previous
+                 * environment.
+                 */
+                const ARMCPRegInfo *ri;
+                ri = get_arm_cp_reginfo(arm_cpu->cp_regs, hvf_sreg_match[i].key);
+                val = read_raw_cp_reg(env, ri);
+
+                arm_cpu->cpreg_values[hvf_sreg_match[i].cp_idx] = val;
+                continue;
+            }
+            }
+        }
+
         ret = hv_vcpu_get_sys_reg(cpu->hvf->fd, hvf_sreg_match[i].reg, &val);
         assert_hvf_ok(ret);
 
@@ -448,6 +712,82 @@ int hvf_put_registers(CPUState *cpu)
             continue;
         }
 
+        if (cpu->hvf->guest_debug_enabled) {
+            /* Handle debug registers */
+            switch (hvf_sreg_match[i].reg) {
+            case HV_SYS_REG_DBGBVR0_EL1:
+            case HV_SYS_REG_DBGBCR0_EL1:
+            case HV_SYS_REG_DBGWVR0_EL1:
+            case HV_SYS_REG_DBGWCR0_EL1:
+            case HV_SYS_REG_DBGBVR1_EL1:
+            case HV_SYS_REG_DBGBCR1_EL1:
+            case HV_SYS_REG_DBGWVR1_EL1:
+            case HV_SYS_REG_DBGWCR1_EL1:
+            case HV_SYS_REG_DBGBVR2_EL1:
+            case HV_SYS_REG_DBGBCR2_EL1:
+            case HV_SYS_REG_DBGWVR2_EL1:
+            case HV_SYS_REG_DBGWCR2_EL1:
+            case HV_SYS_REG_DBGBVR3_EL1:
+            case HV_SYS_REG_DBGBCR3_EL1:
+            case HV_SYS_REG_DBGWVR3_EL1:
+            case HV_SYS_REG_DBGWCR3_EL1:
+            case HV_SYS_REG_DBGBVR4_EL1:
+            case HV_SYS_REG_DBGBCR4_EL1:
+            case HV_SYS_REG_DBGWVR4_EL1:
+            case HV_SYS_REG_DBGWCR4_EL1:
+            case HV_SYS_REG_DBGBVR5_EL1:
+            case HV_SYS_REG_DBGBCR5_EL1:
+            case HV_SYS_REG_DBGWVR5_EL1:
+            case HV_SYS_REG_DBGWCR5_EL1:
+            case HV_SYS_REG_DBGBVR6_EL1:
+            case HV_SYS_REG_DBGBCR6_EL1:
+            case HV_SYS_REG_DBGWVR6_EL1:
+            case HV_SYS_REG_DBGWCR6_EL1:
+            case HV_SYS_REG_DBGBVR7_EL1:
+            case HV_SYS_REG_DBGBCR7_EL1:
+            case HV_SYS_REG_DBGWVR7_EL1:
+            case HV_SYS_REG_DBGWCR7_EL1:
+            case HV_SYS_REG_DBGBVR8_EL1:
+            case HV_SYS_REG_DBGBCR8_EL1:
+            case HV_SYS_REG_DBGWVR8_EL1:
+            case HV_SYS_REG_DBGWCR8_EL1:
+            case HV_SYS_REG_DBGBVR9_EL1:
+            case HV_SYS_REG_DBGBCR9_EL1:
+            case HV_SYS_REG_DBGWVR9_EL1:
+            case HV_SYS_REG_DBGWCR9_EL1:
+            case HV_SYS_REG_DBGBVR10_EL1:
+            case HV_SYS_REG_DBGBCR10_EL1:
+            case HV_SYS_REG_DBGWVR10_EL1:
+            case HV_SYS_REG_DBGWCR10_EL1:
+            case HV_SYS_REG_DBGBVR11_EL1:
+            case HV_SYS_REG_DBGBCR11_EL1:
+            case HV_SYS_REG_DBGWVR11_EL1:
+            case HV_SYS_REG_DBGWCR11_EL1:
+            case HV_SYS_REG_DBGBVR12_EL1:
+            case HV_SYS_REG_DBGBCR12_EL1:
+            case HV_SYS_REG_DBGWVR12_EL1:
+            case HV_SYS_REG_DBGWCR12_EL1:
+            case HV_SYS_REG_DBGBVR13_EL1:
+            case HV_SYS_REG_DBGBCR13_EL1:
+            case HV_SYS_REG_DBGWVR13_EL1:
+            case HV_SYS_REG_DBGWCR13_EL1:
+            case HV_SYS_REG_DBGBVR14_EL1:
+            case HV_SYS_REG_DBGBCR14_EL1:
+            case HV_SYS_REG_DBGWVR14_EL1:
+            case HV_SYS_REG_DBGWCR14_EL1:
+            case HV_SYS_REG_DBGBVR15_EL1:
+            case HV_SYS_REG_DBGBCR15_EL1:
+            case HV_SYS_REG_DBGWVR15_EL1:
+            case HV_SYS_REG_DBGWCR15_EL1:
+                /*
+                 * If the guest is being debugged, the vCPU's debug registers
+                 * are already holding the gdbstub's view of the registers (set
+                 * in hvf_arch_update_guest_debug()).
+                 */
+                continue;
+            }
+        }
+
         val = arm_cpu->cpreg_values[hvf_sreg_match[i].cp_idx];
         ret = hv_vcpu_set_sys_reg(cpu->hvf->fd, hvf_sreg_match[i].reg, val);
         assert_hvf_ok(ret);
@@ -933,6 +1273,78 @@ static int hvf_sysreg_read(CPUState *cpu, uint32_t reg, uint32_t rt)
             hvf_raise_exception(cpu, EXCP_UDEF, syn_uncategorized());
         }
         break;
+    case SYSREG_DBGBVR0_EL1:
+    case SYSREG_DBGBVR1_EL1:
+    case SYSREG_DBGBVR2_EL1:
+    case SYSREG_DBGBVR3_EL1:
+    case SYSREG_DBGBVR4_EL1:
+    case SYSREG_DBGBVR5_EL1:
+    case SYSREG_DBGBVR6_EL1:
+    case SYSREG_DBGBVR7_EL1:
+    case SYSREG_DBGBVR8_EL1:
+    case SYSREG_DBGBVR9_EL1:
+    case SYSREG_DBGBVR10_EL1:
+    case SYSREG_DBGBVR11_EL1:
+    case SYSREG_DBGBVR12_EL1:
+    case SYSREG_DBGBVR13_EL1:
+    case SYSREG_DBGBVR14_EL1:
+    case SYSREG_DBGBVR15_EL1:
+        val = env->cp15.dbgbvr[SYSREG_CRM(reg)];
+        break;
+    case SYSREG_DBGBCR0_EL1:
+    case SYSREG_DBGBCR1_EL1:
+    case SYSREG_DBGBCR2_EL1:
+    case SYSREG_DBGBCR3_EL1:
+    case SYSREG_DBGBCR4_EL1:
+    case SYSREG_DBGBCR5_EL1:
+    case SYSREG_DBGBCR6_EL1:
+    case SYSREG_DBGBCR7_EL1:
+    case SYSREG_DBGBCR8_EL1:
+    case SYSREG_DBGBCR9_EL1:
+    case SYSREG_DBGBCR10_EL1:
+    case SYSREG_DBGBCR11_EL1:
+    case SYSREG_DBGBCR12_EL1:
+    case SYSREG_DBGBCR13_EL1:
+    case SYSREG_DBGBCR14_EL1:
+    case SYSREG_DBGBCR15_EL1:
+        val = env->cp15.dbgbcr[SYSREG_CRM(reg)];
+        break;
+    case SYSREG_DBGWVR0_EL1:
+    case SYSREG_DBGWVR1_EL1:
+    case SYSREG_DBGWVR2_EL1:
+    case SYSREG_DBGWVR3_EL1:
+    case SYSREG_DBGWVR4_EL1:
+    case SYSREG_DBGWVR5_EL1:
+    case SYSREG_DBGWVR6_EL1:
+    case SYSREG_DBGWVR7_EL1:
+    case SYSREG_DBGWVR8_EL1:
+    case SYSREG_DBGWVR9_EL1:
+    case SYSREG_DBGWVR10_EL1:
+    case SYSREG_DBGWVR11_EL1:
+    case SYSREG_DBGWVR12_EL1:
+    case SYSREG_DBGWVR13_EL1:
+    case SYSREG_DBGWVR14_EL1:
+    case SYSREG_DBGWVR15_EL1:
+        val = env->cp15.dbgwvr[SYSREG_CRM(reg)];
+        break;
+    case SYSREG_DBGWCR0_EL1:
+    case SYSREG_DBGWCR1_EL1:
+    case SYSREG_DBGWCR2_EL1:
+    case SYSREG_DBGWCR3_EL1:
+    case SYSREG_DBGWCR4_EL1:
+    case SYSREG_DBGWCR5_EL1:
+    case SYSREG_DBGWCR6_EL1:
+    case SYSREG_DBGWCR7_EL1:
+    case SYSREG_DBGWCR8_EL1:
+    case SYSREG_DBGWCR9_EL1:
+    case SYSREG_DBGWCR10_EL1:
+    case SYSREG_DBGWCR11_EL1:
+    case SYSREG_DBGWCR12_EL1:
+    case SYSREG_DBGWCR13_EL1:
+    case SYSREG_DBGWCR14_EL1:
+    case SYSREG_DBGWCR15_EL1:
+        val = env->cp15.dbgwcr[SYSREG_CRM(reg)];
+        break;
     default:
         if (is_id_sysreg(reg)) {
             /* ID system registers read as RES0 */
@@ -1172,6 +1584,81 @@ static int hvf_sysreg_write(CPUState *cpu, uint32_t reg, uint64_t val)
             hvf_raise_exception(cpu, EXCP_UDEF, syn_uncategorized());
         }
         break;
+    case SYSREG_MDSCR_EL1:
+        env->cp15.mdscr_el1 = val;
+        break;
+    case SYSREG_DBGBVR0_EL1:
+    case SYSREG_DBGBVR1_EL1:
+    case SYSREG_DBGBVR2_EL1:
+    case SYSREG_DBGBVR3_EL1:
+    case SYSREG_DBGBVR4_EL1:
+    case SYSREG_DBGBVR5_EL1:
+    case SYSREG_DBGBVR6_EL1:
+    case SYSREG_DBGBVR7_EL1:
+    case SYSREG_DBGBVR8_EL1:
+    case SYSREG_DBGBVR9_EL1:
+    case SYSREG_DBGBVR10_EL1:
+    case SYSREG_DBGBVR11_EL1:
+    case SYSREG_DBGBVR12_EL1:
+    case SYSREG_DBGBVR13_EL1:
+    case SYSREG_DBGBVR14_EL1:
+    case SYSREG_DBGBVR15_EL1:
+        env->cp15.dbgbvr[SYSREG_CRM(reg)] = val;
+        break;
+    case SYSREG_DBGBCR0_EL1:
+    case SYSREG_DBGBCR1_EL1:
+    case SYSREG_DBGBCR2_EL1:
+    case SYSREG_DBGBCR3_EL1:
+    case SYSREG_DBGBCR4_EL1:
+    case SYSREG_DBGBCR5_EL1:
+    case SYSREG_DBGBCR6_EL1:
+    case SYSREG_DBGBCR7_EL1:
+    case SYSREG_DBGBCR8_EL1:
+    case SYSREG_DBGBCR9_EL1:
+    case SYSREG_DBGBCR10_EL1:
+    case SYSREG_DBGBCR11_EL1:
+    case SYSREG_DBGBCR12_EL1:
+    case SYSREG_DBGBCR13_EL1:
+    case SYSREG_DBGBCR14_EL1:
+    case SYSREG_DBGBCR15_EL1:
+        env->cp15.dbgbcr[SYSREG_CRM(reg)] = val;
+        break;
+    case SYSREG_DBGWVR0_EL1:
+    case SYSREG_DBGWVR1_EL1:
+    case SYSREG_DBGWVR2_EL1:
+    case SYSREG_DBGWVR3_EL1:
+    case SYSREG_DBGWVR4_EL1:
+    case SYSREG_DBGWVR5_EL1:
+    case SYSREG_DBGWVR6_EL1:
+    case SYSREG_DBGWVR7_EL1:
+    case SYSREG_DBGWVR8_EL1:
+    case SYSREG_DBGWVR9_EL1:
+    case SYSREG_DBGWVR10_EL1:
+    case SYSREG_DBGWVR11_EL1:
+    case SYSREG_DBGWVR12_EL1:
+    case SYSREG_DBGWVR13_EL1:
+    case SYSREG_DBGWVR14_EL1:
+    case SYSREG_DBGWVR15_EL1:
+        env->cp15.dbgwvr[SYSREG_CRM(reg)] = val;
+        break;
+    case SYSREG_DBGWCR0_EL1:
+    case SYSREG_DBGWCR1_EL1:
+    case SYSREG_DBGWCR2_EL1:
+    case SYSREG_DBGWCR3_EL1:
+    case SYSREG_DBGWCR4_EL1:
+    case SYSREG_DBGWCR5_EL1:
+    case SYSREG_DBGWCR6_EL1:
+    case SYSREG_DBGWCR7_EL1:
+    case SYSREG_DBGWCR8_EL1:
+    case SYSREG_DBGWCR9_EL1:
+    case SYSREG_DBGWCR10_EL1:
+    case SYSREG_DBGWCR11_EL1:
+    case SYSREG_DBGWCR12_EL1:
+    case SYSREG_DBGWCR13_EL1:
+    case SYSREG_DBGWCR14_EL1:
+    case SYSREG_DBGWCR15_EL1:
+        env->cp15.dbgwcr[SYSREG_CRM(reg)] = val;
+        break;
     default:
         cpu_synchronize_state(cpu);
         trace_hvf_unhandled_sysreg_write(env->pc, reg,
@@ -1317,11 +1804,13 @@ int hvf_vcpu_exec(CPUState *cpu)
 {
     ARMCPU *arm_cpu = ARM_CPU(cpu);
     CPUARMState *env = &arm_cpu->env;
+    int ret;
     hv_vcpu_exit_t *hvf_exit = cpu->hvf->exit;
     hv_return_t r;
     bool advance_pc = false;
 
-    if (hvf_inject_interrupts(cpu)) {
+    if (!(cpu->singlestep_enabled & SSTEP_NOIRQ) &&
+        hvf_inject_interrupts(cpu)) {
         return EXCP_INTERRUPT;
     }
 
@@ -1339,6 +1828,7 @@ int hvf_vcpu_exec(CPUState *cpu)
     uint64_t syndrome = hvf_exit->exception.syndrome;
     uint32_t ec = syn_get_ec(syndrome);
 
+    ret = 0;
     qemu_mutex_lock_iothread();
     switch (exit_reason) {
     case HV_EXIT_REASON_EXCEPTION:
@@ -1358,6 +1848,49 @@ int hvf_vcpu_exec(CPUState *cpu)
     hvf_sync_vtimer(cpu);
 
     switch (ec) {
+    case EC_SOFTWARESTEP: {
+        ret = EXCP_DEBUG;
+
+        if (!cpu->singlestep_enabled) {
+            error_report("EC_SOFTWARESTEP but single-stepping not enabled");
+        }
+        break;
+    }
+    case EC_AA64_BKPT: {
+        ret = EXCP_DEBUG;
+
+        cpu_synchronize_state(cpu);
+
+        if (!hvf_find_sw_breakpoint(cpu, env->pc)) {
+            /* Re-inject into the guest */
+            ret = 0;
+            hvf_raise_exception(cpu, EXCP_BKPT, syn_aa64_bkpt(0));
+        }
+        break;
+    }
+    case EC_BREAKPOINT: {
+        ret = EXCP_DEBUG;
+
+        cpu_synchronize_state(cpu);
+
+        if (!find_hw_breakpoint(cpu, env->pc)) {
+            error_report("EC_BREAKPOINT but unknown hw breakpoint");
+        }
+        break;
+    }
+    case EC_WATCHPOINT: {
+        ret = EXCP_DEBUG;
+
+        cpu_synchronize_state(cpu);
+
+        CPUWatchpoint *wp =
+            find_hw_watchpoint(cpu, hvf_exit->exception.virtual_address);
+        if (!wp) {
+            error_report("EXCP_DEBUG but unknown hw watchpoint");
+        }
+        cpu->watchpoint_hit = wp;
+        break;
+    }
     case EC_DATAABORT: {
         bool isv = syndrome & ARM_EL_ISV;
         bool iswrite = (syndrome >> 6) & 1;
@@ -1462,9 +1995,14 @@ int hvf_vcpu_exec(CPUState *cpu)
         pc += 4;
         r = hv_vcpu_set_reg(cpu->hvf->fd, HV_REG_PC, pc);
         assert_hvf_ok(r);
+
+        /* Handle single-stepping over instructions which trigger a VM exit */
+        if (cpu->singlestep_enabled) {
+            ret = EXCP_DEBUG;
+        }
     }
 
-    return 0;
+    return ret;
 }
 
 static const VMStateDescription vmstate_hvf_vtimer = {
@@ -1496,5 +2034,213 @@ int hvf_arch_init(void)
     hvf_state->vtimer_offset = mach_absolute_time();
     vmstate_register(NULL, 0, &vmstate_hvf_vtimer, &vtimer);
     qemu_add_vm_change_state_handler(hvf_vm_state_change, &vtimer);
+
+    hvf_arm_init_debug();
+
+    return 0;
+}
+
+static const uint32_t brk_insn = 0xd4200000;
+
+int hvf_arch_insert_sw_breakpoint(CPUState *cpu, struct hvf_sw_breakpoint *bp)
+{
+    if (cpu_memory_rw_debug(cpu, bp->pc, (uint8_t *)&bp->saved_insn, 4, 0) ||
+        cpu_memory_rw_debug(cpu, bp->pc, (uint8_t *)&brk_insn, 4, 1)) {
+        return -EINVAL;
+    }
     return 0;
 }
+
+int hvf_arch_remove_sw_breakpoint(CPUState *cpu, struct hvf_sw_breakpoint *bp)
+{
+    static uint32_t brk;
+
+    if (cpu_memory_rw_debug(cpu, bp->pc, (uint8_t *)&brk, 4, 0) ||
+        brk != brk_insn ||
+        cpu_memory_rw_debug(cpu, bp->pc, (uint8_t *)&bp->saved_insn, 4, 1)) {
+        return -EINVAL;
+    }
+    return 0;
+}
+
+int hvf_arch_insert_hw_breakpoint(target_ulong addr, target_ulong len, int type)
+{
+    switch (type) {
+    case GDB_BREAKPOINT_HW:
+        return insert_hw_breakpoint(addr);
+    case GDB_WATCHPOINT_READ:
+    case GDB_WATCHPOINT_WRITE:
+    case GDB_WATCHPOINT_ACCESS:
+        return insert_hw_watchpoint(addr, len, type);
+    default:
+        return -ENOSYS;
+    }
+}
+
+int hvf_arch_remove_hw_breakpoint(target_ulong addr, target_ulong len, int type)
+{
+    switch (type) {
+    case GDB_BREAKPOINT_HW:
+        return delete_hw_breakpoint(addr);
+    case GDB_WATCHPOINT_READ:
+    case GDB_WATCHPOINT_WRITE:
+    case GDB_WATCHPOINT_ACCESS:
+        return delete_hw_watchpoint(addr, len, type);
+    default:
+        return -ENOSYS;
+    }
+}
+
+void hvf_arch_remove_all_hw_breakpoints(void)
+{
+    if (cur_hw_wps > 0) {
+        g_array_remove_range(hw_watchpoints, 0, cur_hw_wps);
+    }
+    if (cur_hw_bps > 0) {
+        g_array_remove_range(hw_breakpoints, 0, cur_hw_bps);
+    }
+}
+
+/*
+ * Update the vCPU with the gdbstub's view of debug registers. This view
+ * consists of all hardware breakpoints and watchpoints inserted so far while
+ * debugging the guest.
+ */
+static void hvf_put_gdbstub_debug_registers(CPUState *cpu)
+{
+    hv_return_t r = HV_SUCCESS;
+    int i;
+
+    for (i = 0; i < cur_hw_bps; i++) {
+        HWBreakpoint *bp = get_hw_bp(i);
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgbcr_regs[i], bp->bcr);
+        assert_hvf_ok(r);
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgbvr_regs[i], bp->bvr);
+        assert_hvf_ok(r);
+    }
+    for (i = cur_hw_bps; i < max_hw_bps; i++) {
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgbcr_regs[i], 0);
+        assert_hvf_ok(r);
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgbvr_regs[i], 0);
+        assert_hvf_ok(r);
+    }
+
+    for (i = 0; i < cur_hw_wps; i++) {
+        HWWatchpoint *wp = get_hw_wp(i);
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgwcr_regs[i], wp->wcr);
+        assert_hvf_ok(r);
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgwvr_regs[i], wp->wvr);
+        assert_hvf_ok(r);
+    }
+    for (i = cur_hw_wps; i < max_hw_wps; i++) {
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgwcr_regs[i], 0);
+        assert_hvf_ok(r);
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgwvr_regs[i], 0);
+        assert_hvf_ok(r);
+    }
+}
+
+/*
+ * Update the vCPU with the guest's view of debug registers. This view is kept
+ * in the environment at all times.
+ */
+static void hvf_put_guest_debug_registers(CPUState *cpu)
+{
+    ARMCPU *arm_cpu = ARM_CPU(cpu);
+    CPUARMState *env = &arm_cpu->env;
+    hv_return_t r = HV_SUCCESS;
+    int i;
+
+    for (i = 0; i < max_hw_bps; i++) {
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgbcr_regs[i],
+                                env->cp15.dbgbcr[i]);
+        assert_hvf_ok(r);
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgbvr_regs[i],
+                                env->cp15.dbgbvr[i]);
+        assert_hvf_ok(r);
+    }
+
+    for (i = 0; i < max_hw_wps; i++) {
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgwcr_regs[i],
+                                env->cp15.dbgwcr[i]);
+        assert_hvf_ok(r);
+        r = hv_vcpu_set_sys_reg(cpu->hvf->fd, dbgwvr_regs[i],
+                                env->cp15.dbgwvr[i]);
+        assert_hvf_ok(r);
+    }
+}
+
+static inline bool hvf_arm_hw_debug_active(CPUState *cpu)
+{
+    return ((cur_hw_wps > 0) || (cur_hw_bps > 0));
+}
+
+static void hvf_arch_set_traps(void)
+{
+    CPUState *cpu;
+    bool should_enable_traps = false;
+    hv_return_t r = HV_SUCCESS;
+
+    /* Check whether guest debugging is enabled for at least one vCPU; if it
+     * is, enable exiting the guest on all vCPUs */
+    CPU_FOREACH(cpu) {
+        should_enable_traps |= cpu->hvf->guest_debug_enabled;
+    }
+    CPU_FOREACH(cpu) {
+        /* Set whether debug exceptions exit the guest */
+        r = hv_vcpu_set_trap_debug_exceptions(cpu->hvf->fd,
+                                              should_enable_traps);
+        assert_hvf_ok(r);
+
+        /* Set whether accesses to debug registers exit the guest */
+        r = hv_vcpu_set_trap_debug_reg_accesses(cpu->hvf->fd,
+                                                should_enable_traps);
+        assert_hvf_ok(r);
+    }
+}
+
+void hvf_arch_update_guest_debug(CPUState *cpu)
+{
+    ARMCPU *arm_cpu = ARM_CPU(cpu);
+    CPUARMState *env = &arm_cpu->env;
+
+    /* Check whether guest debugging is enabled */
+    cpu->hvf->guest_debug_enabled = cpu->singlestep_enabled ||
+                                    hvf_sw_breakpoints_active(cpu) ||
+                                    hvf_arm_hw_debug_active(cpu);
+
+    /* Update debug registers */
+    if (cpu->hvf->guest_debug_enabled) {
+        hvf_put_gdbstub_debug_registers(cpu);
+    } else {
+        hvf_put_guest_debug_registers(cpu);
+    }
+
+    cpu_synchronize_state(cpu);
+
+    /* Enable/disable single-stepping */
+    if (cpu->singlestep_enabled) {
+        env->cp15.mdscr_el1 =
+            deposit64(env->cp15.mdscr_el1, MDSCR_EL1_SS_SHIFT, 1, 1);
+        pstate_write(env, pstate_read(env) | PSTATE_SS);
+    } else {
+        env->cp15.mdscr_el1 =
+            deposit64(env->cp15.mdscr_el1, MDSCR_EL1_SS_SHIFT, 1, 0);
+    }
+
+    /* Enable/disable Breakpoint exceptions */
+    if (hvf_arm_hw_debug_active(cpu)) {
+        env->cp15.mdscr_el1 =
+            deposit64(env->cp15.mdscr_el1, MDSCR_EL1_MDE_SHIFT, 1, 1);
+    } else {
+        env->cp15.mdscr_el1 =
+            deposit64(env->cp15.mdscr_el1, MDSCR_EL1_MDE_SHIFT, 1, 0);
+    }
+
+    hvf_arch_set_traps();
+}
+
+inline bool hvf_arch_supports_guest_debug(void)
+{
+    return true;
+}
diff --git a/target/arm/hvf_arm.h b/target/arm/hvf_arm.h
index 9a9d1a0..e848c1d 100644
--- a/target/arm/hvf_arm.h
+++ b/target/arm/hvf_arm.h
@@ -13,6 +13,13 @@
 
 #include "cpu.h"
 
+/**
+ * hvf_arm_init_debug() - initialize guest debug capabilities
+ *
+ * Should be called only once before using guest debug capabilities.
+ */
+void hvf_arm_init_debug(void);
+
 void hvf_arm_set_cpu_features_from_host(ARMCPU *cpu);
 
 #endif
diff --git a/target/arm/hyp_gdbstub.c b/target/arm/hyp_gdbstub.c
new file mode 100644
index 0000000..ebde289
--- /dev/null
+++ b/target/arm/hyp_gdbstub.c
@@ -0,0 +1,253 @@
+/*
+ * ARM implementation of KVM and HVF hooks, 64 bit specific code
+ *
+ * Copyright Mian-M. Hamayun 2013, Virtual Open Systems
+ * Copyright Alex Bennée 2014, Linaro
+ *
+ * This work is licensed under the terms of the GNU GPL, version 2 or later.
+ * See the COPYING file in the top-level directory.
+ *
+ */
+
+#include "qemu/osdep.h"
+#include "cpu.h"
+#include "internals.h"
+#include "exec/gdbstub.h"
+
+/* Maximum and current break/watch point counts */
+int max_hw_bps, max_hw_wps;
+GArray *hw_breakpoints, *hw_watchpoints;
+
+/**
+ * insert_hw_breakpoint()
+ * @addr: address of breakpoint
+ *
+ * See ARM ARM D2.9.1 for details but here we are only going to create
+ * simple un-linked breakpoints (i.e. we don't chain breakpoints
+ * together to match address and context or vmid). The hardware is
+ * capable of fancier matching but that will require exposing that
+ * fanciness to GDB's interface
+ *
+ * DBGBCR<n>_EL1, Debug Breakpoint Control Registers
+ *
+ *  31  24 23  20 19   16 15 14  13  12   9 8   5 4    3 2   1  0
+ * +------+------+-------+-----+----+------+-----+------+-----+---+
+ * | RES0 |  BT  |  LBN  | SSC | HMC| RES0 | BAS | RES0 | PMC | E |
+ * +------+------+-------+-----+----+------+-----+------+-----+---+
+ *
+ * BT: Breakpoint type (0 = unlinked address match)
+ * LBN: Linked BP number (0 = unused)
+ * SSC/HMC/PMC: Security, Higher and Priv access control (Table D-12)
+ * BAS: Byte Address Select (RES1 for AArch64)
+ * E: Enable bit
+ *
+ * DBGBVR<n>_EL1, Debug Breakpoint Value Registers
+ *
+ *  63  53 52       49 48       2  1 0
+ * +------+-----------+----------+-----+
+ * | RESS | VA[52:49] | VA[48:2] | 0 0 |
+ * +------+-----------+----------+-----+
+ *
+ * Depending on the addressing mode bits the top bits of the register
+ * are a sign extension of the highest applicable VA bit. Some
+ * versions of GDB don't do it correctly so we ensure they are correct
+ * here so future PC comparisons will work properly.
+ */
+
+int insert_hw_breakpoint(target_ulong addr)
+{
+    HWBreakpoint brk = {
+        .bcr = 0x1,                             /* BCR E=1, enable */
+        .bvr = sextract64(addr, 0, 53)
+    };
+
+    if (cur_hw_bps >= max_hw_bps) {
+        return -ENOBUFS;
+    }
+
+    brk.bcr = deposit32(brk.bcr, 1, 2, 0x3);   /* PMC = 11 */
+    brk.bcr = deposit32(brk.bcr, 5, 4, 0xf);   /* BAS = RES1 */
+
+    g_array_append_val(hw_breakpoints, brk);
+
+    return 0;
+}
+
+/**
+ * delete_hw_breakpoint()
+ * @pc: address of breakpoint
+ *
+ * Delete a breakpoint and shuffle any above down
+ */
+
+int delete_hw_breakpoint(target_ulong pc)
+{
+    int i;
+    for (i = 0; i < hw_breakpoints->len; i++) {
+        HWBreakpoint *brk = get_hw_bp(i);
+        if (brk->bvr == pc) {
+            g_array_remove_index(hw_breakpoints, i);
+            return 0;
+        }
+    }
+    return -ENOENT;
+}
+
+/**
+ * insert_hw_watchpoint()
+ * @addr: address of watch point
+ * @len: size of area
+ * @type: type of watch point
+ *
+ * See ARM ARM D2.10. As with the breakpoints we can do some advanced
+ * stuff if we want to. The watch points can be linked with the break
+ * points above to make them context aware. However for simplicity
+ * currently we only deal with simple read/write watch points.
+ *
+ * D7.3.11 DBGWCR<n>_EL1, Debug Watchpoint Control Registers
+ *
+ *  31  29 28   24 23  21  20  19 16 15 14  13   12  5 4   3 2   1  0
+ * +------+-------+------+----+-----+-----+-----+-----+-----+-----+---+
+ * | RES0 |  MASK | RES0 | WT | LBN | SSC | HMC | BAS | LSC | PAC | E |
+ * +------+-------+------+----+-----+-----+-----+-----+-----+-----+---+
+ *
+ * MASK: num bits addr mask (0=none,01/10=res,11=3 bits (8 bytes))
+ * WT: 0 - unlinked, 1 - linked (not currently used)
+ * LBN: Linked BP number (not currently used)
+ * SSC/HMC/PAC: Security, Higher and Priv access control (Table D2-11)
+ * BAS: Byte Address Select
+ * LSC: Load/Store control (01: load, 10: store, 11: both)
+ * E: Enable
+ *
+ * The bottom 2 bits of the value register are masked. Therefore to
+ * break on any sizes smaller than an unaligned word you need to set
+ * MASK=0, BAS=bit per byte in question. For larger regions (^2) you
+ * need to ensure you mask the address as required and set BAS=0xff
+ */
+
+int insert_hw_watchpoint(target_ulong addr, target_ulong len, int type)
+{
+    HWWatchpoint wp = {
+        .wcr = R_DBGWCR_E_MASK, /* E=1, enable */
+        .wvr = addr & (~0x7ULL),
+        .details = { .vaddr = addr, .len = len }
+    };
+
+    if (cur_hw_wps >= max_hw_wps) {
+        return -ENOBUFS;
+    }
+
+    /*
+     * HMC=0 SSC=0 PAC=3 will hit EL0 or EL1, any security state,
+     * valid whether EL3 is implemented or not
+     */
+    wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, PAC, 3);
+
+    switch (type) {
+    case GDB_WATCHPOINT_READ:
+        wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, LSC, 1);
+        wp.details.flags = BP_MEM_READ;
+        break;
+    case GDB_WATCHPOINT_WRITE:
+        wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, LSC, 2);
+        wp.details.flags = BP_MEM_WRITE;
+        break;
+    case GDB_WATCHPOINT_ACCESS:
+        wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, LSC, 3);
+        wp.details.flags = BP_MEM_ACCESS;
+        break;
+    default:
+        g_assert_not_reached();
+        break;
+    }
+    if (len <= 8) {
+        /* we align the address and set the bits in BAS */
+        int off = addr & 0x7;
+        int bas = (1 << len) - 1;
+
+        wp.wcr = deposit32(wp.wcr, 5 + off, 8 - off, bas);
+    } else {
+        /* For ranges above 8 bytes we need to be a power of 2 */
+        if (is_power_of_2(len)) {
+            int bits = ctz64(len);
+
+            wp.wvr &= ~((1 << bits) - 1);
+            wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, MASK, bits);
+            wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, BAS, 0xff);
+        } else {
+            return -ENOBUFS;
+        }
+    }
+
+    g_array_append_val(hw_watchpoints, wp);
+    return 0;
+}
+
+bool check_watchpoint_in_range(int i, target_ulong addr)
+{
+    HWWatchpoint *wp = get_hw_wp(i);
+    uint64_t addr_top, addr_bottom = wp->wvr;
+    int bas = extract32(wp->wcr, 5, 8);
+    int mask = extract32(wp->wcr, 24, 4);
+
+    if (mask) {
+        addr_top = addr_bottom + (1 << mask);
+    } else {
+        /*
+         * BAS must be contiguous but can offset against the base
+         * address in DBGWVR
+         */
+        addr_bottom = addr_bottom + ctz32(bas);
+        addr_top = addr_bottom + clo32(bas);
+    }
+
+    if (addr >= addr_bottom && addr <= addr_top) {
+        return true;
+    }
+
+    return false;
+}
+
+/**
+ * delete_hw_watchpoint()
+ * @addr: address of breakpoint
+ *
+ * Delete a breakpoint and shuffle any above down
+ */
+
+int delete_hw_watchpoint(target_ulong addr, target_ulong len, int type)
+{
+    int i;
+    for (i = 0; i < cur_hw_wps; i++) {
+        if (check_watchpoint_in_range(i, addr)) {
+            g_array_remove_index(hw_watchpoints, i);
+            return 0;
+        }
+    }
+    return -ENOENT;
+}
+
+bool find_hw_breakpoint(CPUState *cpu, target_ulong pc)
+{
+    int i;
+
+    for (i = 0; i < cur_hw_bps; i++) {
+        HWBreakpoint *bp = get_hw_bp(i);
+        if (bp->bvr == pc) {
+            return true;
+        }
+    }
+    return false;
+}
+
+CPUWatchpoint *find_hw_watchpoint(CPUState *cpu, target_ulong addr)
+{
+    int i;
+
+    for (i = 0; i < cur_hw_wps; i++) {
+        if (check_watchpoint_in_range(i, addr)) {
+            return &get_hw_wp(i)->details;
+        }
+    }
+    return NULL;
+}
diff --git a/target/arm/internals.h b/target/arm/internals.h
index c869d18..e3029bd 100644
--- a/target/arm/internals.h
+++ b/target/arm/internals.h
@@ -1242,7 +1242,8 @@ FIELD(MTEDESC, MIDX,  0, 4)
 FIELD(MTEDESC, TBI,   4, 2)
 FIELD(MTEDESC, TCMA,  6, 2)
 FIELD(MTEDESC, WRITE, 8, 1)
-FIELD(MTEDESC, SIZEM1, 9, SIMD_DATA_BITS - 9)  /* size - 1 */
+FIELD(MTEDESC, ALIGN, 9, 3)
+FIELD(MTEDESC, SIZEM1, 12, SIMD_DATA_BITS - 12)  /* size - 1 */
 
 bool mte_probe(CPUARMState *env, uint32_t desc, uint64_t ptr);
 uint64_t mte_check(CPUARMState *env, uint32_t desc, uint64_t ptr, uintptr_t ra);
@@ -1447,4 +1448,54 @@ static inline bool arm_fgt_active(CPUARMState *env, int el)
 }
 
 void assert_hflags_rebuild_correctly(CPUARMState *env);
+
+/*
+ * Although the ARM implementation of hardware assisted debugging
+ * allows for different breakpoints per-core, the current GDB
+ * interface treats them as a global pool of registers (which seems to
+ * be the case for x86, ppc and s390). As a result we store one copy
+ * of registers which is used for all active cores.
+ *
+ * Write access is serialised by virtue of the GDB protocol which
+ * updates things. Read access (i.e. when the values are copied to the
+ * vCPU) is also gated by GDB's run control.
+ *
+ * This is not unreasonable as most of the time debugging kernels you
+ * never know which core will eventually execute your function.
+ */
+
+typedef struct {
+    uint64_t bcr;
+    uint64_t bvr;
+} HWBreakpoint;
+
+/*
+ * The watchpoint registers can cover more area than the requested
+ * watchpoint so we need to store the additional information
+ * somewhere. We also need to supply a CPUWatchpoint to the GDB stub
+ * when the watchpoint is hit.
+ */
+typedef struct {
+    uint64_t wcr;
+    uint64_t wvr;
+    CPUWatchpoint details;
+} HWWatchpoint;
+
+/* Maximum and current break/watch point counts */
+extern int max_hw_bps, max_hw_wps;
+extern GArray *hw_breakpoints, *hw_watchpoints;
+
+#define cur_hw_wps      (hw_watchpoints->len)
+#define cur_hw_bps      (hw_breakpoints->len)
+#define get_hw_bp(i)    (&g_array_index(hw_breakpoints, HWBreakpoint, i))
+#define get_hw_wp(i)    (&g_array_index(hw_watchpoints, HWWatchpoint, i))
+
+bool find_hw_breakpoint(CPUState *cpu, target_ulong pc);
+int insert_hw_breakpoint(target_ulong pc);
+int delete_hw_breakpoint(target_ulong pc);
+
+bool check_watchpoint_in_range(int i, target_ulong addr);
+CPUWatchpoint *find_hw_watchpoint(CPUState *cpu, target_ulong addr);
+int insert_hw_watchpoint(target_ulong addr, target_ulong len, int type);
+int delete_hw_watchpoint(target_ulong addr, target_ulong len, int type);
 #endif
diff --git a/target/arm/kvm64.c b/target/arm/kvm64.c
index 810db33..94bbd96 100644
--- a/target/arm/kvm64.c
+++ b/target/arm/kvm64.c
@@ -34,46 +34,6 @@
 
 static bool have_guest_debug;
 
-/*
- * Although the ARM implementation of hardware assisted debugging
- * allows for different breakpoints per-core, the current GDB
- * interface treats them as a global pool of registers (which seems to
- * be the case for x86, ppc and s390). As a result we store one copy
- * of registers which is used for all active cores.
- *
- * Write access is serialised by virtue of the GDB protocol which
- * updates things. Read access (i.e. when the values are copied to the
- * vCPU) is also gated by GDB's run control.
- *
- * This is not unreasonable as most of the time debugging kernels you
- * never know which core will eventually execute your function.
- */
-
-typedef struct {
-    uint64_t bcr;
-    uint64_t bvr;
-} HWBreakpoint;
-
-/* The watchpoint registers can cover more area than the requested
- * watchpoint so we need to store the additional information
- * somewhere. We also need to supply a CPUWatchpoint to the GDB stub
- * when the watchpoint is hit.
- */
-typedef struct {
-    uint64_t wcr;
-    uint64_t wvr;
-    CPUWatchpoint details;
-} HWWatchpoint;
-
-/* Maximum and current break/watch point counts */
-int max_hw_bps, max_hw_wps;
-GArray *hw_breakpoints, *hw_watchpoints;
-
-#define cur_hw_wps      (hw_watchpoints->len)
-#define cur_hw_bps      (hw_breakpoints->len)
-#define get_hw_bp(i)    (&g_array_index(hw_breakpoints, HWBreakpoint, i))
-#define get_hw_wp(i)    (&g_array_index(hw_watchpoints, HWWatchpoint, i))
-
 void kvm_arm_init_debug(KVMState *s)
 {
     have_guest_debug = kvm_check_extension(s,
@@ -89,217 +49,6 @@ void kvm_arm_init_debug(KVMState *s)
     return;
 }
 
-/**
- * insert_hw_breakpoint()
- * @addr: address of breakpoint
- *
- * See ARM ARM D2.9.1 for details but here we are only going to create
- * simple un-linked breakpoints (i.e. we don't chain breakpoints
- * together to match address and context or vmid). The hardware is
- * capable of fancier matching but that will require exposing that
- * fanciness to GDB's interface
- *
- * DBGBCR<n>_EL1, Debug Breakpoint Control Registers
- *
- *  31  24 23  20 19   16 15 14  13  12   9 8   5 4    3 2   1  0
- * +------+------+-------+-----+----+------+-----+------+-----+---+
- * | RES0 |  BT  |  LBN  | SSC | HMC| RES0 | BAS | RES0 | PMC | E |
- * +------+------+-------+-----+----+------+-----+------+-----+---+
- *
- * BT: Breakpoint type (0 = unlinked address match)
- * LBN: Linked BP number (0 = unused)
- * SSC/HMC/PMC: Security, Higher and Priv access control (Table D-12)
- * BAS: Byte Address Select (RES1 for AArch64)
- * E: Enable bit
- *
- * DBGBVR<n>_EL1, Debug Breakpoint Value Registers
- *
- *  63  53 52       49 48       2  1 0
- * +------+-----------+----------+-----+
- * | RESS | VA[52:49] | VA[48:2] | 0 0 |
- * +------+-----------+----------+-----+
- *
- * Depending on the addressing mode bits the top bits of the register
- * are a sign extension of the highest applicable VA bit. Some
- * versions of GDB don't do it correctly so we ensure they are correct
- * here so future PC comparisons will work properly.
- */
-
-static int insert_hw_breakpoint(target_ulong addr)
-{
-    HWBreakpoint brk = {
-        .bcr = 0x1,                             /* BCR E=1, enable */
-        .bvr = sextract64(addr, 0, 53)
-    };
-
-    if (cur_hw_bps >= max_hw_bps) {
-        return -ENOBUFS;
-    }
-
-    brk.bcr = deposit32(brk.bcr, 1, 2, 0x3);   /* PMC = 11 */
-    brk.bcr = deposit32(brk.bcr, 5, 4, 0xf);   /* BAS = RES1 */
-
-    g_array_append_val(hw_breakpoints, brk);
-
-    return 0;
-}
-
-/**
- * delete_hw_breakpoint()
- * @pc: address of breakpoint
- *
- * Delete a breakpoint and shuffle any above down
- */
-
-static int delete_hw_breakpoint(target_ulong pc)
-{
-    int i;
-    for (i = 0; i < hw_breakpoints->len; i++) {
-        HWBreakpoint *brk = get_hw_bp(i);
-        if (brk->bvr == pc) {
-            g_array_remove_index(hw_breakpoints, i);
-            return 0;
-        }
-    }
-    return -ENOENT;
-}
-
-/**
- * insert_hw_watchpoint()
- * @addr: address of watch point
- * @len: size of area
- * @type: type of watch point
- *
- * See ARM ARM D2.10. As with the breakpoints we can do some advanced
- * stuff if we want to. The watch points can be linked with the break
- * points above to make them context aware. However for simplicity
- * currently we only deal with simple read/write watch points.
- *
- * D7.3.11 DBGWCR<n>_EL1, Debug Watchpoint Control Registers
- *
- *  31  29 28   24 23  21  20  19 16 15 14  13   12  5 4   3 2   1  0
- * +------+-------+------+----+-----+-----+-----+-----+-----+-----+---+
- * | RES0 |  MASK | RES0 | WT | LBN | SSC | HMC | BAS | LSC | PAC | E |
- * +------+-------+------+----+-----+-----+-----+-----+-----+-----+---+
- *
- * MASK: num bits addr mask (0=none,01/10=res,11=3 bits (8 bytes))
- * WT: 0 - unlinked, 1 - linked (not currently used)
- * LBN: Linked BP number (not currently used)
- * SSC/HMC/PAC: Security, Higher and Priv access control (Table D2-11)
- * BAS: Byte Address Select
- * LSC: Load/Store control (01: load, 10: store, 11: both)
- * E: Enable
- *
- * The bottom 2 bits of the value register are masked. Therefore to
- * break on any sizes smaller than an unaligned word you need to set
- * MASK=0, BAS=bit per byte in question. For larger regions (^2) you
- * need to ensure you mask the address as required and set BAS=0xff
- */
-
-static int insert_hw_watchpoint(target_ulong addr,
-                                target_ulong len, int type)
-{
-    HWWatchpoint wp = {
-        .wcr = R_DBGWCR_E_MASK, /* E=1, enable */
-        .wvr = addr & (~0x7ULL),
-        .details = { .vaddr = addr, .len = len }
-    };
-
-    if (cur_hw_wps >= max_hw_wps) {
-        return -ENOBUFS;
-    }
-
-    /*
-     * HMC=0 SSC=0 PAC=3 will hit EL0 or EL1, any security state,
-     * valid whether EL3 is implemented or not
-     */
-    wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, PAC, 3);
-
-    switch (type) {
-    case GDB_WATCHPOINT_READ:
-        wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, LSC, 1);
-        wp.details.flags = BP_MEM_READ;
-        break;
-    case GDB_WATCHPOINT_WRITE:
-        wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, LSC, 2);
-        wp.details.flags = BP_MEM_WRITE;
-        break;
-    case GDB_WATCHPOINT_ACCESS:
-        wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, LSC, 3);
-        wp.details.flags = BP_MEM_ACCESS;
-        break;
-    default:
-        g_assert_not_reached();
-        break;
-    }
-    if (len <= 8) {
-        /* we align the address and set the bits in BAS */
-        int off = addr & 0x7;
-        int bas = (1 << len) - 1;
-
-        wp.wcr = deposit32(wp.wcr, 5 + off, 8 - off, bas);
-    } else {
-        /* For ranges above 8 bytes we need to be a power of 2 */
-        if (is_power_of_2(len)) {
-            int bits = ctz64(len);
-
-            wp.wvr &= ~((1 << bits) - 1);
-            wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, MASK, bits);
-            wp.wcr = FIELD_DP64(wp.wcr, DBGWCR, BAS, 0xff);
-        } else {
-            return -ENOBUFS;
-        }
-    }
-
-    g_array_append_val(hw_watchpoints, wp);
-    return 0;
-}
-
-
-static bool check_watchpoint_in_range(int i, target_ulong addr)
-{
-    HWWatchpoint *wp = get_hw_wp(i);
-    uint64_t addr_top, addr_bottom = wp->wvr;
-    int bas = extract32(wp->wcr, 5, 8);
-    int mask = extract32(wp->wcr, 24, 4);
-
-    if (mask) {
-        addr_top = addr_bottom + (1 << mask);
-    } else {
-        /* BAS must be contiguous but can offset against the base
-         * address in DBGWVR */
-        addr_bottom = addr_bottom + ctz32(bas);
-        addr_top = addr_bottom + clo32(bas);
-    }
-
-    if (addr >= addr_bottom && addr <= addr_top) {
-        return true;
-    }
-
-    return false;
-}
-
-/**
- * delete_hw_watchpoint()
- * @addr: address of breakpoint
- *
- * Delete a breakpoint and shuffle any above down
- */
-
-static int delete_hw_watchpoint(target_ulong addr,
-                                target_ulong len, int type)
-{
-    int i;
-    for (i = 0; i < cur_hw_wps; i++) {
-        if (check_watchpoint_in_range(i, addr)) {
-            g_array_remove_index(hw_watchpoints, i);
-            return 0;
-        }
-    }
-    return -ENOENT;
-}
-
-
 int kvm_arch_insert_hw_breakpoint(target_ulong addr,
                                   target_ulong len, int type)
 {
@@ -364,31 +113,6 @@ bool kvm_arm_hw_debug_active(CPUState *cs)
     return ((cur_hw_wps > 0) || (cur_hw_bps > 0));
 }
 
-static bool find_hw_breakpoint(CPUState *cpu, target_ulong pc)
-{
-    int i;
-
-    for (i = 0; i < cur_hw_bps; i++) {
-        HWBreakpoint *bp = get_hw_bp(i);
-        if (bp->bvr == pc) {
-            return true;
-        }
-    }
-    return false;
-}
-
-static CPUWatchpoint *find_hw_watchpoint(CPUState *cpu, target_ulong addr)
-{
-    int i;
-
-    for (i = 0; i < cur_hw_wps; i++) {
-        if (check_watchpoint_in_range(i, addr)) {
-            return &get_hw_wp(i)->details;
-        }
-    }
-    return NULL;
-}
-
 static bool kvm_arm_set_device_attr(CPUState *cs, struct kvm_device_attr *attr,
                                     const char *name)
 {
diff --git a/target/arm/meson.build b/target/arm/meson.build
index 359a649..011e8ca 100644
--- a/target/arm/meson.build
+++ b/target/arm/meson.build
@@ -8,7 +8,8 @@ arm_ss.add(files(
 ))
 arm_ss.add(zlib)
 
-arm_ss.add(when: 'CONFIG_KVM', if_true: files('kvm.c', 'kvm64.c'), if_false: files('kvm-stub.c'))
+arm_ss.add(when: 'CONFIG_KVM', if_true: files('hyp_gdbstub.c', 'kvm.c', 'kvm64.c'), if_false: files('kvm-stub.c'))
+arm_ss.add(when: 'CONFIG_HVF', if_true: files('hyp_gdbstub.c'))
 
 arm_ss.add(when: 'TARGET_AARCH64', if_true: files(
   'cpu64.c',
diff --git a/target/arm/tcg/cpu64.c b/target/arm/tcg/cpu64.c
index 886674a..2976f94 100644
--- a/target/arm/tcg/cpu64.c
+++ b/target/arm/tcg/cpu64.c
@@ -644,6 +644,7 @@ void aarch64_max_tcg_initfn(Object *obj)
     t = FIELD_DP64(t, ID_AA64MMFR2, IESB, 1);     /* FEAT_IESB */
     t = FIELD_DP64(t, ID_AA64MMFR2, VARANGE, 1);  /* FEAT_LVA */
     t = FIELD_DP64(t, ID_AA64MMFR2, ST, 1);       /* FEAT_TTST */
+    t = FIELD_DP64(t, ID_AA64MMFR2, AT, 1);       /* FEAT_LSE2 */
     t = FIELD_DP64(t, ID_AA64MMFR2, IDS, 1);      /* FEAT_IDST */
     t = FIELD_DP64(t, ID_AA64MMFR2, FWB, 1);      /* FEAT_S2FWB */
     t = FIELD_DP64(t, ID_AA64MMFR2, TTL, 1);      /* FEAT_TTL */
diff --git a/target/arm/tcg/helper-a64.c b/target/arm/tcg/helper-a64.c
index c3edf16..1c9370f 100644
--- a/target/arm/tcg/helper-a64.c
+++ b/target/arm/tcg/helper-a64.c
@@ -952,3 +952,10 @@ void HELPER(dc_zva)(CPUARMState *env, uint64_t vaddr_in)
 
     memset(mem, 0, blocklen);
 }
+
+void HELPER(unaligned_access)(CPUARMState *env, uint64_t addr,
+                              uint32_t access_type, uint32_t mmu_idx)
+{
+    arm_cpu_do_unaligned_access(env_cpu(env), addr, access_type,
+                                mmu_idx, GETPC());
+}
diff --git a/target/arm/tcg/helper-a64.h b/target/arm/tcg/helper-a64.h
index ff56807..3d5957c 100644
--- a/target/arm/tcg/helper-a64.h
+++ b/target/arm/tcg/helper-a64.h
@@ -110,3 +110,6 @@ DEF_HELPER_FLAGS_2(st2g_stub, TCG_CALL_NO_WG, void, env, i64)
 DEF_HELPER_FLAGS_2(ldgm, TCG_CALL_NO_WG, i64, env, i64)
 DEF_HELPER_FLAGS_3(stgm, TCG_CALL_NO_WG, void, env, i64, i64)
 DEF_HELPER_FLAGS_3(stzgm_tags, TCG_CALL_NO_WG, void, env, i64, i64)
+
+DEF_HELPER_FLAGS_4(unaligned_access, TCG_CALL_NO_WG,
+                   noreturn, env, i64, i32, i32)
diff --git a/target/arm/tcg/hflags.c b/target/arm/tcg/hflags.c
index b2ccd77..616c5fa 100644
--- a/target/arm/tcg/hflags.c
+++ b/target/arm/tcg/hflags.c
@@ -248,6 +248,12 @@ static CPUARMTBFlags rebuild_hflags_a64(CPUARMState *env, int el, int fp_el,
         }
     }
 
+    if (cpu_isar_feature(aa64_lse2, env_archcpu(env))) {
+        if (sctlr & SCTLR_nAA) {
+            DP_TBFLAG_A64(flags, NAA, 1);
+        }
+    }
+
     /* Compute the condition for using AccType_UNPRIV for LDTR et al. */
     if (!(env->pstate & PSTATE_UAO)) {
         switch (mmu_idx) {
diff --git a/target/arm/tcg/mte_helper.c b/target/arm/tcg/mte_helper.c
index a4f3f92..9c64def 100644
--- a/target/arm/tcg/mte_helper.c
+++ b/target/arm/tcg/mte_helper.c
@@ -785,6 +785,24 @@ uint64_t mte_check(CPUARMState *env, uint32_t desc, uint64_t ptr, uintptr_t ra)
 
 uint64_t HELPER(mte_check)(CPUARMState *env, uint32_t desc, uint64_t ptr)
 {
+    /*
+     * R_XCHFJ: Alignment check not caused by memory type is priority 1,
+     * higher than any translation fault.  When MTE is disabled, tcg
+     * performs the alignment check during the code generated for the
+     * memory access.  With MTE enabled, we must check this here before
+     * raising any translation fault in allocation_tag_mem.
+     */
+    unsigned align = FIELD_EX32(desc, MTEDESC, ALIGN);
+    if (unlikely(align)) {
+        align = (1u << align) - 1;
+        if (unlikely(ptr & align)) {
+            int idx = FIELD_EX32(desc, MTEDESC, MIDX);
+            bool w = FIELD_EX32(desc, MTEDESC, WRITE);
+            MMUAccessType type = w ? MMU_DATA_STORE : MMU_DATA_LOAD;
+            arm_cpu_do_unaligned_access(env_cpu(env), ptr, type, idx, GETPC());
+        }
+    }
+
     return mte_check(env, desc, ptr, GETPC());
 }
 
diff --git a/target/arm/tcg/translate-a64.c b/target/arm/tcg/translate-a64.c
index d980033..aa93f37 100644
--- a/target/arm/tcg/translate-a64.c
+++ b/target/arm/tcg/translate-a64.c
@@ -253,7 +253,7 @@ static void gen_probe_access(DisasContext *s, TCGv_i64 ptr,
  */
 static TCGv_i64 gen_mte_check1_mmuidx(DisasContext *s, TCGv_i64 addr,
                                       bool is_write, bool tag_checked,
-                                      int log2_size, bool is_unpriv,
+                                      MemOp memop, bool is_unpriv,
                                       int core_idx)
 {
     if (tag_checked && s->mte_active[is_unpriv]) {
@@ -264,7 +264,8 @@ static TCGv_i64 gen_mte_check1_mmuidx(DisasContext *s, TCGv_i64 addr,
         desc = FIELD_DP32(desc, MTEDESC, TBI, s->tbid);
         desc = FIELD_DP32(desc, MTEDESC, TCMA, s->tcma);
         desc = FIELD_DP32(desc, MTEDESC, WRITE, is_write);
-        desc = FIELD_DP32(desc, MTEDESC, SIZEM1, (1 << log2_size) - 1);
+        desc = FIELD_DP32(desc, MTEDESC, ALIGN, get_alignment_bits(memop));
+        desc = FIELD_DP32(desc, MTEDESC, SIZEM1, memop_size(memop) - 1);
 
         ret = tcg_temp_new_i64();
         gen_helper_mte_check(ret, cpu_env, tcg_constant_i32(desc), addr);
@@ -275,9 +276,9 @@ static TCGv_i64 gen_mte_check1_mmuidx(DisasContext *s, TCGv_i64 addr,
 }
 
 TCGv_i64 gen_mte_check1(DisasContext *s, TCGv_i64 addr, bool is_write,
-                        bool tag_checked, int log2_size)
+                        bool tag_checked, MemOp memop)
 {
-    return gen_mte_check1_mmuidx(s, addr, is_write, tag_checked, log2_size,
+    return gen_mte_check1_mmuidx(s, addr, is_write, tag_checked, memop,
                                  false, get_mem_index(s));
 }
 
@@ -285,7 +286,7 @@ TCGv_i64 gen_mte_check1(DisasContext *s, TCGv_i64 addr, bool is_write,
  * For MTE, check multiple logical sequential accesses.
  */
 TCGv_i64 gen_mte_checkN(DisasContext *s, TCGv_i64 addr, bool is_write,
-                        bool tag_checked, int size)
+                        bool tag_checked, int total_size, MemOp single_mop)
 {
     if (tag_checked && s->mte_active[0]) {
         TCGv_i64 ret;
@@ -295,7 +296,8 @@ TCGv_i64 gen_mte_checkN(DisasContext *s, TCGv_i64 addr, bool is_write,
         desc = FIELD_DP32(desc, MTEDESC, TBI, s->tbid);
         desc = FIELD_DP32(desc, MTEDESC, TCMA, s->tcma);
         desc = FIELD_DP32(desc, MTEDESC, WRITE, is_write);
-        desc = FIELD_DP32(desc, MTEDESC, SIZEM1, size - 1);
+        desc = FIELD_DP32(desc, MTEDESC, ALIGN, get_alignment_bits(single_mop));
+        desc = FIELD_DP32(desc, MTEDESC, SIZEM1, total_size - 1);
 
         ret = tcg_temp_new_i64();
         gen_helper_mte_check(ret, cpu_env, tcg_constant_i32(desc), addr);
@@ -305,6 +307,89 @@ TCGv_i64 gen_mte_checkN(DisasContext *s, TCGv_i64 addr, bool is_write,
     return clean_data_tbi(s, addr);
 }
 
+/*
+ * Generate the special alignment check that applies to AccType_ATOMIC
+ * and AccType_ORDERED insns under FEAT_LSE2: the access need not be
+ * naturally aligned, but it must not cross a 16-byte boundary.
+ * See AArch64.CheckAlignment().
+ */
+static void check_lse2_align(DisasContext *s, int rn, int imm,
+                             bool is_write, MemOp mop)
+{
+    TCGv_i32 tmp;
+    TCGv_i64 addr;
+    TCGLabel *over_label;
+    MMUAccessType type;
+    int mmu_idx;
+
+    tmp = tcg_temp_new_i32();
+    tcg_gen_extrl_i64_i32(tmp, cpu_reg_sp(s, rn));
+    tcg_gen_addi_i32(tmp, tmp, imm & 15);
+    tcg_gen_andi_i32(tmp, tmp, 15);
+    tcg_gen_addi_i32(tmp, tmp, memop_size(mop));
+
+    over_label = gen_new_label();
+    tcg_gen_brcondi_i32(TCG_COND_LEU, tmp, 16, over_label);
+
+    addr = tcg_temp_new_i64();
+    tcg_gen_addi_i64(addr, cpu_reg_sp(s, rn), imm);
+
+    type = is_write ? MMU_DATA_STORE : MMU_DATA_LOAD,
+    mmu_idx = get_mem_index(s);
+    gen_helper_unaligned_access(cpu_env, addr, tcg_constant_i32(type),
+                                tcg_constant_i32(mmu_idx));
+
+    gen_set_label(over_label);
+
+}
+
+/* Handle the alignment check for AccType_ATOMIC instructions. */
+static MemOp check_atomic_align(DisasContext *s, int rn, MemOp mop)
+{
+    MemOp size = mop & MO_SIZE;
+
+    if (size == MO_8) {
+        return mop;
+    }
+
+    /*
+     * If size == MO_128, this is a LDXP, and the operation is single-copy
+     * atomic for each doubleword, not the entire quadword; it still must
+     * be quadword aligned.
+     */
+    if (size == MO_128) {
+        return finalize_memop_atom(s, MO_128 | MO_ALIGN,
+                                   MO_ATOM_IFALIGN_PAIR);
+    }
+    if (dc_isar_feature(aa64_lse2, s)) {
+        check_lse2_align(s, rn, 0, true, mop);
+    } else {
+        mop |= MO_ALIGN;
+    }
+    return finalize_memop(s, mop);
+}
+
+/* Handle the alignment check for AccType_ORDERED instructions. */
+static MemOp check_ordered_align(DisasContext *s, int rn, int imm,
+                                 bool is_write, MemOp mop)
+{
+    MemOp size = mop & MO_SIZE;
+
+    if (size == MO_8) {
+        return mop;
+    }
+    if (size == MO_128) {
+        return finalize_memop_atom(s, MO_128 | MO_ALIGN,
+                                   MO_ATOM_IFALIGN_PAIR);
+    }
+    if (!dc_isar_feature(aa64_lse2, s)) {
+        mop |= MO_ALIGN;
+    } else if (!s->naa) {
+        check_lse2_align(s, rn, imm, is_write, mop);
+    }
+    return finalize_memop(s, mop);
+}
+
 typedef struct DisasCompare64 {
     TCGCond cond;
     TCGv_i64 value;
@@ -838,7 +923,6 @@ static void do_gpr_st_memidx(DisasContext *s, TCGv_i64 source,
                              unsigned int iss_srt,
                              bool iss_sf, bool iss_ar)
 {
-    memop = finalize_memop(s, memop);
     tcg_gen_qemu_st_i64(source, tcg_addr, memidx, memop);
 
     if (iss_valid) {
@@ -873,7 +957,6 @@ static void do_gpr_ld_memidx(DisasContext *s, TCGv_i64 dest, TCGv_i64 tcg_addr,
                              bool iss_valid, unsigned int iss_srt,
                              bool iss_sf, bool iss_ar)
 {
-    memop = finalize_memop(s, memop);
     tcg_gen_qemu_ld_i64(dest, tcg_addr, memidx, memop);
 
     if (extend && (memop & MO_SIGN)) {
@@ -907,59 +990,44 @@ static void do_gpr_ld(DisasContext *s, TCGv_i64 dest, TCGv_i64 tcg_addr,
 /*
  * Store from FP register to memory
  */
-static void do_fp_st(DisasContext *s, int srcidx, TCGv_i64 tcg_addr, int size)
+static void do_fp_st(DisasContext *s, int srcidx, TCGv_i64 tcg_addr, MemOp mop)
 {
     /* This writes the bottom N bits of a 128 bit wide vector to memory */
     TCGv_i64 tmplo = tcg_temp_new_i64();
-    MemOp mop;
 
     tcg_gen_ld_i64(tmplo, cpu_env, fp_reg_offset(s, srcidx, MO_64));
 
-    if (size < 4) {
-        mop = finalize_memop(s, size);
+    if ((mop & MO_SIZE) < MO_128) {
         tcg_gen_qemu_st_i64(tmplo, tcg_addr, get_mem_index(s), mop);
     } else {
-        bool be = s->be_data == MO_BE;
-        TCGv_i64 tcg_hiaddr = tcg_temp_new_i64();
         TCGv_i64 tmphi = tcg_temp_new_i64();
+        TCGv_i128 t16 = tcg_temp_new_i128();
 
         tcg_gen_ld_i64(tmphi, cpu_env, fp_reg_hi_offset(s, srcidx));
+        tcg_gen_concat_i64_i128(t16, tmplo, tmphi);
 
-        mop = s->be_data | MO_UQ;
-        tcg_gen_qemu_st_i64(be ? tmphi : tmplo, tcg_addr, get_mem_index(s),
-                            mop | (s->align_mem ? MO_ALIGN_16 : 0));
-        tcg_gen_addi_i64(tcg_hiaddr, tcg_addr, 8);
-        tcg_gen_qemu_st_i64(be ? tmplo : tmphi, tcg_hiaddr,
-                            get_mem_index(s), mop);
+        tcg_gen_qemu_st_i128(t16, tcg_addr, get_mem_index(s), mop);
     }
 }
 
 /*
  * Load from memory to FP register
  */
-static void do_fp_ld(DisasContext *s, int destidx, TCGv_i64 tcg_addr, int size)
+static void do_fp_ld(DisasContext *s, int destidx, TCGv_i64 tcg_addr, MemOp mop)
 {
     /* This always zero-extends and writes to a full 128 bit wide vector */
     TCGv_i64 tmplo = tcg_temp_new_i64();
     TCGv_i64 tmphi = NULL;
-    MemOp mop;
 
-    if (size < 4) {
-        mop = finalize_memop(s, size);
+    if ((mop & MO_SIZE) < MO_128) {
         tcg_gen_qemu_ld_i64(tmplo, tcg_addr, get_mem_index(s), mop);
     } else {
-        bool be = s->be_data == MO_BE;
-        TCGv_i64 tcg_hiaddr;
+        TCGv_i128 t16 = tcg_temp_new_i128();
 
-        tmphi = tcg_temp_new_i64();
-        tcg_hiaddr = tcg_temp_new_i64();
+        tcg_gen_qemu_ld_i128(t16, tcg_addr, get_mem_index(s), mop);
 
-        mop = s->be_data | MO_UQ;
-        tcg_gen_qemu_ld_i64(be ? tmphi : tmplo, tcg_addr, get_mem_index(s),
-                            mop | (s->align_mem ? MO_ALIGN_16 : 0));
-        tcg_gen_addi_i64(tcg_hiaddr, tcg_addr, 8);
-        tcg_gen_qemu_ld_i64(be ? tmplo : tmphi, tcg_hiaddr,
-                            get_mem_index(s), mop);
+        tmphi = tcg_temp_new_i64();
+        tcg_gen_extr_i128_i64(tmplo, tmphi, t16);
     }
 
     tcg_gen_st_i64(tmplo, cpu_env, fp_reg_offset(s, destidx, MO_64));
@@ -2382,19 +2450,22 @@ static void disas_b_exc_sys(DisasContext *s, uint32_t insn)
  * races in multi-threaded linux-user and when MTTCG softmmu is
  * enabled.
  */
-static void gen_load_exclusive(DisasContext *s, int rt, int rt2,
-                               TCGv_i64 addr, int size, bool is_pair)
+static void gen_load_exclusive(DisasContext *s, int rt, int rt2, int rn,
+                               int size, bool is_pair)
 {
     int idx = get_mem_index(s);
-    MemOp memop = s->be_data;
+    TCGv_i64 dirty_addr, clean_addr;
+    MemOp memop = check_atomic_align(s, rn, size + is_pair);
+
+    s->is_ldex = true;
+    dirty_addr = cpu_reg_sp(s, rn);
+    clean_addr = gen_mte_check1(s, dirty_addr, false, rn != 31, memop);
 
     g_assert(size <= 3);
     if (is_pair) {
         g_assert(size >= 2);
         if (size == 2) {
-            /* The pair must be single-copy atomic for the doubleword.  */
-            memop |= MO_64 | MO_ALIGN;
-            tcg_gen_qemu_ld_i64(cpu_exclusive_val, addr, idx, memop);
+            tcg_gen_qemu_ld_i64(cpu_exclusive_val, clean_addr, idx, memop);
             if (s->be_data == MO_LE) {
                 tcg_gen_extract_i64(cpu_reg(s, rt), cpu_exclusive_val, 0, 32);
                 tcg_gen_extract_i64(cpu_reg(s, rt2), cpu_exclusive_val, 32, 32);
@@ -2403,29 +2474,29 @@ static void gen_load_exclusive(DisasContext *s, int rt, int rt2,
                 tcg_gen_extract_i64(cpu_reg(s, rt2), cpu_exclusive_val, 0, 32);
             }
         } else {
-            /* The pair must be single-copy atomic for *each* doubleword, not
-               the entire quadword, however it must be quadword aligned.  */
-            memop |= MO_64;
-            tcg_gen_qemu_ld_i64(cpu_exclusive_val, addr, idx,
-                                memop | MO_ALIGN_16);
+            TCGv_i128 t16 = tcg_temp_new_i128();
 
-            TCGv_i64 addr2 = tcg_temp_new_i64();
-            tcg_gen_addi_i64(addr2, addr, 8);
-            tcg_gen_qemu_ld_i64(cpu_exclusive_high, addr2, idx, memop);
+            tcg_gen_qemu_ld_i128(t16, clean_addr, idx, memop);
 
+            if (s->be_data == MO_LE) {
+                tcg_gen_extr_i128_i64(cpu_exclusive_val,
+                                      cpu_exclusive_high, t16);
+            } else {
+                tcg_gen_extr_i128_i64(cpu_exclusive_high,
+                                      cpu_exclusive_val, t16);
+            }
             tcg_gen_mov_i64(cpu_reg(s, rt), cpu_exclusive_val);
             tcg_gen_mov_i64(cpu_reg(s, rt2), cpu_exclusive_high);
         }
     } else {
-        memop |= size | MO_ALIGN;
-        tcg_gen_qemu_ld_i64(cpu_exclusive_val, addr, idx, memop);
+        tcg_gen_qemu_ld_i64(cpu_exclusive_val, clean_addr, idx, memop);
         tcg_gen_mov_i64(cpu_reg(s, rt), cpu_exclusive_val);
     }
-    tcg_gen_mov_i64(cpu_exclusive_addr, addr);
+    tcg_gen_mov_i64(cpu_exclusive_addr, clean_addr);
 }
 
 static void gen_store_exclusive(DisasContext *s, int rd, int rt, int rt2,
-                                TCGv_i64 addr, int size, int is_pair)
+                                int rn, int size, int is_pair)
 {
     /* if (env->exclusive_addr == addr && env->exclusive_val == [addr]
      *     && (!is_pair || env->exclusive_high == [addr + datasize])) {
@@ -2441,9 +2512,46 @@ static void gen_store_exclusive(DisasContext *s, int rd, int rt, int rt2,
      */
     TCGLabel *fail_label = gen_new_label();
     TCGLabel *done_label = gen_new_label();
-    TCGv_i64 tmp;
+    TCGv_i64 tmp, clean_addr;
+    MemOp memop;
+
+    /*
+     * FIXME: We are out of spec here.  We have recorded only the address
+     * from load_exclusive, not the entire range, and we assume that the
+     * size of the access on both sides match.  The architecture allows the
+     * store to be smaller than the load, so long as the stored bytes are
+     * within the range recorded by the load.
+     */
+
+    /* See AArch64.ExclusiveMonitorsPass() and AArch64.IsExclusiveVA(). */
+    clean_addr = clean_data_tbi(s, cpu_reg_sp(s, rn));
+    tcg_gen_brcond_i64(TCG_COND_NE, clean_addr, cpu_exclusive_addr, fail_label);
 
-    tcg_gen_brcond_i64(TCG_COND_NE, addr, cpu_exclusive_addr, fail_label);
+    /*
+     * The write, and any associated faults, only happen if the virtual
+     * and physical addresses pass the exclusive monitor check.  These
+     * faults are exceedingly unlikely, because normally the guest uses
+     * the exact same address register for the load_exclusive, and we
+     * would have recognized these faults there.
+     *
+     * It is possible to trigger an alignment fault pre-LSE2, e.g. with an
+     * unaligned 4-byte write within the range of an aligned 8-byte load.
+     * With LSE2, the store would need to cross a 16-byte boundary when the
+     * load did not, which would mean the store is outside the range
+     * recorded for the monitor, which would have failed a corrected monitor
+     * check above.  For now, we assume no size change and retain the
+     * MO_ALIGN to let tcg know what we checked in the load_exclusive.
+     *
+     * It is possible to trigger an MTE fault, by performing the load with
+     * a virtual address with a valid tag and performing the store with the
+     * same virtual address and a different invalid tag.
+     */
+    memop = size + is_pair;
+    if (memop == MO_128 || !dc_isar_feature(aa64_lse2, s)) {
+        memop |= MO_ALIGN;
+    }
+    memop = finalize_memop(s, memop);
+    gen_mte_check1(s, cpu_reg_sp(s, rn), true, rn != 31, memop);
 
     tmp = tcg_temp_new_i64();
     if (is_pair) {
@@ -2455,8 +2563,7 @@ static void gen_store_exclusive(DisasContext *s, int rd, int rt, int rt2,
             }
             tcg_gen_atomic_cmpxchg_i64(tmp, cpu_exclusive_addr,
                                        cpu_exclusive_val, tmp,
-                                       get_mem_index(s),
-                                       MO_64 | MO_ALIGN | s->be_data);
+                                       get_mem_index(s), memop);
             tcg_gen_setcond_i64(TCG_COND_NE, tmp, tmp, cpu_exclusive_val);
         } else {
             TCGv_i128 t16 = tcg_temp_new_i128();
@@ -2474,8 +2581,7 @@ static void gen_store_exclusive(DisasContext *s, int rd, int rt, int rt2,
             }
 
             tcg_gen_atomic_cmpxchg_i128(t16, cpu_exclusive_addr, c16, t16,
-                                        get_mem_index(s),
-                                        MO_128 | MO_ALIGN | s->be_data);
+                                        get_mem_index(s), memop);
 
             a = tcg_temp_new_i64();
             b = tcg_temp_new_i64();
@@ -2493,8 +2599,7 @@ static void gen_store_exclusive(DisasContext *s, int rd, int rt, int rt2,
         }
     } else {
         tcg_gen_atomic_cmpxchg_i64(tmp, cpu_exclusive_addr, cpu_exclusive_val,
-                                   cpu_reg(s, rt), get_mem_index(s),
-                                   size | MO_ALIGN | s->be_data);
+                                   cpu_reg(s, rt), get_mem_index(s), memop);
         tcg_gen_setcond_i64(TCG_COND_NE, tmp, tmp, cpu_exclusive_val);
     }
     tcg_gen_mov_i64(cpu_reg(s, rd), tmp);
@@ -2513,13 +2618,15 @@ static void gen_compare_and_swap(DisasContext *s, int rs, int rt,
     TCGv_i64 tcg_rt = cpu_reg(s, rt);
     int memidx = get_mem_index(s);
     TCGv_i64 clean_addr;
+    MemOp memop;
 
     if (rn == 31) {
         gen_check_sp_alignment(s);
     }
-    clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn), true, rn != 31, size);
-    tcg_gen_atomic_cmpxchg_i64(tcg_rs, clean_addr, tcg_rs, tcg_rt, memidx,
-                               size | MO_ALIGN | s->be_data);
+    memop = check_atomic_align(s, rn, size);
+    clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn), true, rn != 31, memop);
+    tcg_gen_atomic_cmpxchg_i64(tcg_rs, clean_addr, tcg_rs, tcg_rt,
+                               memidx, memop);
 }
 
 static void gen_compare_and_swap_pair(DisasContext *s, int rs, int rt,
@@ -2531,13 +2638,15 @@ static void gen_compare_and_swap_pair(DisasContext *s, int rs, int rt,
     TCGv_i64 t2 = cpu_reg(s, rt + 1);
     TCGv_i64 clean_addr;
     int memidx = get_mem_index(s);
+    MemOp memop;
 
     if (rn == 31) {
         gen_check_sp_alignment(s);
     }
 
     /* This is a single atomic access, despite the "pair". */
-    clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn), true, rn != 31, size + 1);
+    memop = check_atomic_align(s, rn, size + 1);
+    clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn), true, rn != 31, memop);
 
     if (size == 2) {
         TCGv_i64 cmp = tcg_temp_new_i64();
@@ -2551,8 +2660,7 @@ static void gen_compare_and_swap_pair(DisasContext *s, int rs, int rt,
             tcg_gen_concat32_i64(cmp, s2, s1);
         }
 
-        tcg_gen_atomic_cmpxchg_i64(cmp, clean_addr, cmp, val, memidx,
-                                   MO_64 | MO_ALIGN | s->be_data);
+        tcg_gen_atomic_cmpxchg_i64(cmp, clean_addr, cmp, val, memidx, memop);
 
         if (s->be_data == MO_LE) {
             tcg_gen_extr32_i64(s1, s2, cmp);
@@ -2571,8 +2679,7 @@ static void gen_compare_and_swap_pair(DisasContext *s, int rs, int rt,
             tcg_gen_concat_i64_i128(cmp, s2, s1);
         }
 
-        tcg_gen_atomic_cmpxchg_i128(cmp, clean_addr, cmp, val, memidx,
-                                    MO_128 | MO_ALIGN | s->be_data);
+        tcg_gen_atomic_cmpxchg_i128(cmp, clean_addr, cmp, val, memidx, memop);
 
         if (s->be_data == MO_LE) {
             tcg_gen_extr_i128_i64(s1, s2, cmp);
@@ -2621,6 +2728,7 @@ static void disas_ldst_excl(DisasContext *s, uint32_t insn)
     int o2_L_o1_o0 = extract32(insn, 21, 3) * 2 | is_lasr;
     int size = extract32(insn, 30, 2);
     TCGv_i64 clean_addr;
+    MemOp memop;
 
     switch (o2_L_o1_o0) {
     case 0x0: /* STXR */
@@ -2631,9 +2739,7 @@ static void disas_ldst_excl(DisasContext *s, uint32_t insn)
         if (is_lasr) {
             tcg_gen_mb(TCG_MO_ALL | TCG_BAR_STRL);
         }
-        clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn),
-                                    true, rn != 31, size);
-        gen_store_exclusive(s, rs, rt, rt2, clean_addr, size, false);
+        gen_store_exclusive(s, rs, rt, rt2, rn, size, false);
         return;
 
     case 0x4: /* LDXR */
@@ -2641,10 +2747,7 @@ static void disas_ldst_excl(DisasContext *s, uint32_t insn)
         if (rn == 31) {
             gen_check_sp_alignment(s);
         }
-        clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn),
-                                    false, rn != 31, size);
-        s->is_ldex = true;
-        gen_load_exclusive(s, rt, rt2, clean_addr, size, false);
+        gen_load_exclusive(s, rt, rt2, rn, size, false);
         if (is_lasr) {
             tcg_gen_mb(TCG_MO_ALL | TCG_BAR_LDAQ);
         }
@@ -2662,10 +2765,10 @@ static void disas_ldst_excl(DisasContext *s, uint32_t insn)
             gen_check_sp_alignment(s);
         }
         tcg_gen_mb(TCG_MO_ALL | TCG_BAR_STRL);
+        memop = check_ordered_align(s, rn, 0, true, size);
         clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn),
-                                    true, rn != 31, size);
-        /* TODO: ARMv8.4-LSE SCTLR.nAA */
-        do_gpr_st(s, cpu_reg(s, rt), clean_addr, size | MO_ALIGN, true, rt,
+                                    true, rn != 31, memop);
+        do_gpr_st(s, cpu_reg(s, rt), clean_addr, memop, true, rt,
                   disas_ldst_compute_iss_sf(size, false, 0), is_lasr);
         return;
 
@@ -2680,10 +2783,10 @@ static void disas_ldst_excl(DisasContext *s, uint32_t insn)
         if (rn == 31) {
             gen_check_sp_alignment(s);
         }
+        memop = check_ordered_align(s, rn, 0, false, size);
         clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn),
-                                    false, rn != 31, size);
-        /* TODO: ARMv8.4-LSE SCTLR.nAA */
-        do_gpr_ld(s, cpu_reg(s, rt), clean_addr, size | MO_ALIGN, false, true,
+                                    false, rn != 31, memop);
+        do_gpr_ld(s, cpu_reg(s, rt), clean_addr, memop, false, true,
                   rt, disas_ldst_compute_iss_sf(size, false, 0), is_lasr);
         tcg_gen_mb(TCG_MO_ALL | TCG_BAR_LDAQ);
         return;
@@ -2696,9 +2799,7 @@ static void disas_ldst_excl(DisasContext *s, uint32_t insn)
             if (is_lasr) {
                 tcg_gen_mb(TCG_MO_ALL | TCG_BAR_STRL);
             }
-            clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn),
-                                        true, rn != 31, size);
-            gen_store_exclusive(s, rs, rt, rt2, clean_addr, size, true);
+            gen_store_exclusive(s, rs, rt, rt2, rn, size, true);
             return;
         }
         if (rt2 == 31
@@ -2715,10 +2816,7 @@ static void disas_ldst_excl(DisasContext *s, uint32_t insn)
             if (rn == 31) {
                 gen_check_sp_alignment(s);
             }
-            clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn),
-                                        false, rn != 31, size);
-            s->is_ldex = true;
-            gen_load_exclusive(s, rt, rt2, clean_addr, size, true);
+            gen_load_exclusive(s, rt, rt2, rn, size, true);
             if (is_lasr) {
                 tcg_gen_mb(TCG_MO_ALL | TCG_BAR_LDAQ);
             }
@@ -2768,6 +2866,7 @@ static void disas_ld_lit(DisasContext *s, uint32_t insn)
     bool is_signed = false;
     int size = 2;
     TCGv_i64 tcg_rt, clean_addr;
+    MemOp memop;
 
     if (is_vector) {
         if (opc == 3) {
@@ -2778,6 +2877,7 @@ static void disas_ld_lit(DisasContext *s, uint32_t insn)
         if (!fp_access_check(s)) {
             return;
         }
+        memop = finalize_memop_asimd(s, size);
     } else {
         if (opc == 3) {
             /* PRFM (literal) : prefetch */
@@ -2785,20 +2885,20 @@ static void disas_ld_lit(DisasContext *s, uint32_t insn)
         }
         size = 2 + extract32(opc, 0, 1);
         is_signed = extract32(opc, 1, 1);
+        memop = finalize_memop(s, size + is_signed * MO_SIGN);
     }
 
     tcg_rt = cpu_reg(s, rt);
 
     clean_addr = tcg_temp_new_i64();
     gen_pc_plus_diff(s, clean_addr, imm);
+
     if (is_vector) {
-        do_fp_ld(s, rt, clean_addr, size);
+        do_fp_ld(s, rt, clean_addr, memop);
     } else {
         /* Only unsigned 32bit loads target 32bit registers.  */
         bool iss_sf = opc != 0;
-
-        do_gpr_ld(s, tcg_rt, clean_addr, size + is_signed * MO_SIGN,
-                  false, true, rt, iss_sf, false);
+        do_gpr_ld(s, tcg_rt, clean_addr, memop, false, true, rt, iss_sf, false);
     }
 }
 
@@ -2840,14 +2940,12 @@ static void disas_ldst_pair(DisasContext *s, uint32_t insn)
     bool is_vector = extract32(insn, 26, 1);
     bool is_load = extract32(insn, 22, 1);
     int opc = extract32(insn, 30, 2);
-
     bool is_signed = false;
     bool postindex = false;
     bool wback = false;
     bool set_tag = false;
-
     TCGv_i64 clean_addr, dirty_addr;
-
+    MemOp mop;
     int size;
 
     if (opc == 3) {
@@ -2930,44 +3028,94 @@ static void disas_ldst_pair(DisasContext *s, uint32_t insn)
         }
     }
 
+    if (is_vector) {
+        mop = finalize_memop_asimd(s, size);
+    } else {
+        mop = finalize_memop(s, size);
+    }
     clean_addr = gen_mte_checkN(s, dirty_addr, !is_load,
-                                (wback || rn != 31) && !set_tag, 2 << size);
+                                (wback || rn != 31) && !set_tag,
+                                2 << size, mop);
 
     if (is_vector) {
+        /* LSE2 does not merge FP pairs; leave these as separate operations. */
         if (is_load) {
-            do_fp_ld(s, rt, clean_addr, size);
+            do_fp_ld(s, rt, clean_addr, mop);
         } else {
-            do_fp_st(s, rt, clean_addr, size);
+            do_fp_st(s, rt, clean_addr, mop);
         }
         tcg_gen_addi_i64(clean_addr, clean_addr, 1 << size);
         if (is_load) {
-            do_fp_ld(s, rt2, clean_addr, size);
+            do_fp_ld(s, rt2, clean_addr, mop);
         } else {
-            do_fp_st(s, rt2, clean_addr, size);
+            do_fp_st(s, rt2, clean_addr, mop);
         }
     } else {
         TCGv_i64 tcg_rt = cpu_reg(s, rt);
         TCGv_i64 tcg_rt2 = cpu_reg(s, rt2);
 
+        /*
+         * We built mop above for the single logical access -- rebuild it
+         * now for the paired operation.
+         *
+         * With LSE2, non-sign-extending pairs are treated atomically if
+         * aligned, and if unaligned one of the pair will be completely
+         * within a 16-byte block and that element will be atomic.
+         * Otherwise each element is separately atomic.
+         * In all cases, issue one operation with the correct atomicity.
+         *
+         * This treats sign-extending loads like zero-extending loads,
+         * since that reuses the most code below.
+         */
+        mop = size + 1;
+        if (s->align_mem) {
+            mop |= (size == 2 ? MO_ALIGN_4 : MO_ALIGN_8);
+        }
+        mop = finalize_memop_pair(s, mop);
+
         if (is_load) {
-            TCGv_i64 tmp = tcg_temp_new_i64();
+            if (size == 2) {
+                int o2 = s->be_data == MO_LE ? 32 : 0;
+                int o1 = o2 ^ 32;
 
-            /* Do not modify tcg_rt before recognizing any exception
-             * from the second load.
-             */
-            do_gpr_ld(s, tmp, clean_addr, size + is_signed * MO_SIGN,
-                      false, false, 0, false, false);
-            tcg_gen_addi_i64(clean_addr, clean_addr, 1 << size);
-            do_gpr_ld(s, tcg_rt2, clean_addr, size + is_signed * MO_SIGN,
-                      false, false, 0, false, false);
+                tcg_gen_qemu_ld_i64(tcg_rt, clean_addr, get_mem_index(s), mop);
+                if (is_signed) {
+                    tcg_gen_sextract_i64(tcg_rt2, tcg_rt, o2, 32);
+                    tcg_gen_sextract_i64(tcg_rt, tcg_rt, o1, 32);
+                } else {
+                    tcg_gen_extract_i64(tcg_rt2, tcg_rt, o2, 32);
+                    tcg_gen_extract_i64(tcg_rt, tcg_rt, o1, 32);
+                }
+            } else {
+                TCGv_i128 tmp = tcg_temp_new_i128();
 
-            tcg_gen_mov_i64(tcg_rt, tmp);
+                tcg_gen_qemu_ld_i128(tmp, clean_addr, get_mem_index(s), mop);
+                if (s->be_data == MO_LE) {
+                    tcg_gen_extr_i128_i64(tcg_rt, tcg_rt2, tmp);
+                } else {
+                    tcg_gen_extr_i128_i64(tcg_rt2, tcg_rt, tmp);
+                }
+            }
         } else {
-            do_gpr_st(s, tcg_rt, clean_addr, size,
-                      false, 0, false, false);
-            tcg_gen_addi_i64(clean_addr, clean_addr, 1 << size);
-            do_gpr_st(s, tcg_rt2, clean_addr, size,
-                      false, 0, false, false);
+            if (size == 2) {
+                TCGv_i64 tmp = tcg_temp_new_i64();
+
+                if (s->be_data == MO_LE) {
+                    tcg_gen_concat32_i64(tmp, tcg_rt, tcg_rt2);
+                } else {
+                    tcg_gen_concat32_i64(tmp, tcg_rt2, tcg_rt);
+                }
+                tcg_gen_qemu_st_i64(tmp, clean_addr, get_mem_index(s), mop);
+            } else {
+                TCGv_i128 tmp = tcg_temp_new_i128();
+
+                if (s->be_data == MO_LE) {
+                    tcg_gen_concat_i64_i128(tmp, tcg_rt, tcg_rt2);
+                } else {
+                    tcg_gen_concat_i64_i128(tmp, tcg_rt2, tcg_rt);
+                }
+                tcg_gen_qemu_st_i128(tmp, clean_addr, get_mem_index(s), mop);
+            }
         }
     }
 
@@ -3012,7 +3160,7 @@ static void disas_ldst_reg_imm9(DisasContext *s, uint32_t insn,
     bool post_index;
     bool writeback;
     int memidx;
-
+    MemOp memop;
     TCGv_i64 clean_addr, dirty_addr;
 
     if (is_vector) {
@@ -3025,6 +3173,7 @@ static void disas_ldst_reg_imm9(DisasContext *s, uint32_t insn,
         if (!fp_access_check(s)) {
             return;
         }
+        memop = finalize_memop_asimd(s, size);
     } else {
         if (size == 3 && opc == 2) {
             /* PRFM - prefetch */
@@ -3039,8 +3188,9 @@ static void disas_ldst_reg_imm9(DisasContext *s, uint32_t insn,
             return;
         }
         is_store = (opc == 0);
-        is_signed = extract32(opc, 1, 1);
+        is_signed = !is_store && extract32(opc, 1, 1);
         is_extended = (size < 3) && extract32(opc, 0, 1);
+        memop = finalize_memop(s, size + is_signed * MO_SIGN);
     }
 
     switch (idx) {
@@ -3073,25 +3223,26 @@ static void disas_ldst_reg_imm9(DisasContext *s, uint32_t insn,
     }
 
     memidx = is_unpriv ? get_a64_user_mem_index(s) : get_mem_index(s);
+
     clean_addr = gen_mte_check1_mmuidx(s, dirty_addr, is_store,
                                        writeback || rn != 31,
                                        size, is_unpriv, memidx);
 
     if (is_vector) {
         if (is_store) {
-            do_fp_st(s, rt, clean_addr, size);
+            do_fp_st(s, rt, clean_addr, memop);
         } else {
-            do_fp_ld(s, rt, clean_addr, size);
+            do_fp_ld(s, rt, clean_addr, memop);
         }
     } else {
         TCGv_i64 tcg_rt = cpu_reg(s, rt);
         bool iss_sf = disas_ldst_compute_iss_sf(size, is_signed, opc);
 
         if (is_store) {
-            do_gpr_st_memidx(s, tcg_rt, clean_addr, size, memidx,
+            do_gpr_st_memidx(s, tcg_rt, clean_addr, memop, memidx,
                              iss_valid, rt, iss_sf, false);
         } else {
-            do_gpr_ld_memidx(s, tcg_rt, clean_addr, size + is_signed * MO_SIGN,
+            do_gpr_ld_memidx(s, tcg_rt, clean_addr, memop,
                              is_extended, memidx,
                              iss_valid, rt, iss_sf, false);
         }
@@ -3140,8 +3291,8 @@ static void disas_ldst_reg_roffset(DisasContext *s, uint32_t insn,
     bool is_signed = false;
     bool is_store = false;
     bool is_extended = false;
-
     TCGv_i64 tcg_rm, clean_addr, dirty_addr;
+    MemOp memop;
 
     if (extract32(opt, 1, 1) == 0) {
         unallocated_encoding(s);
@@ -3168,7 +3319,7 @@ static void disas_ldst_reg_roffset(DisasContext *s, uint32_t insn,
             return;
         }
         is_store = (opc == 0);
-        is_signed = extract32(opc, 1, 1);
+        is_signed = !is_store && extract32(opc, 1, 1);
         is_extended = (size < 3) && extract32(opc, 0, 1);
     }
 
@@ -3181,22 +3332,25 @@ static void disas_ldst_reg_roffset(DisasContext *s, uint32_t insn,
     ext_and_shift_reg(tcg_rm, tcg_rm, opt, shift ? size : 0);
 
     tcg_gen_add_i64(dirty_addr, dirty_addr, tcg_rm);
-    clean_addr = gen_mte_check1(s, dirty_addr, is_store, true, size);
+
+    memop = finalize_memop(s, size + is_signed * MO_SIGN);
+    clean_addr = gen_mte_check1(s, dirty_addr, is_store, true, memop);
 
     if (is_vector) {
         if (is_store) {
-            do_fp_st(s, rt, clean_addr, size);
+            do_fp_st(s, rt, clean_addr, memop);
         } else {
-            do_fp_ld(s, rt, clean_addr, size);
+            do_fp_ld(s, rt, clean_addr, memop);
         }
     } else {
         TCGv_i64 tcg_rt = cpu_reg(s, rt);
         bool iss_sf = disas_ldst_compute_iss_sf(size, is_signed, opc);
+
         if (is_store) {
-            do_gpr_st(s, tcg_rt, clean_addr, size,
+            do_gpr_st(s, tcg_rt, clean_addr, memop,
                       true, rt, iss_sf, false);
         } else {
-            do_gpr_ld(s, tcg_rt, clean_addr, size + is_signed * MO_SIGN,
+            do_gpr_ld(s, tcg_rt, clean_addr, memop,
                       is_extended, true, rt, iss_sf, false);
         }
     }
@@ -3228,12 +3382,11 @@ static void disas_ldst_reg_unsigned_imm(DisasContext *s, uint32_t insn,
     int rn = extract32(insn, 5, 5);
     unsigned int imm12 = extract32(insn, 10, 12);
     unsigned int offset;
-
     TCGv_i64 clean_addr, dirty_addr;
-
     bool is_store;
     bool is_signed = false;
     bool is_extended = false;
+    MemOp memop;
 
     if (is_vector) {
         size |= (opc & 2) << 1;
@@ -3255,7 +3408,7 @@ static void disas_ldst_reg_unsigned_imm(DisasContext *s, uint32_t insn,
             return;
         }
         is_store = (opc == 0);
-        is_signed = extract32(opc, 1, 1);
+        is_signed = !is_store && extract32(opc, 1, 1);
         is_extended = (size < 3) && extract32(opc, 0, 1);
     }
 
@@ -3265,22 +3418,23 @@ static void disas_ldst_reg_unsigned_imm(DisasContext *s, uint32_t insn,
     dirty_addr = read_cpu_reg_sp(s, rn, 1);
     offset = imm12 << size;
     tcg_gen_addi_i64(dirty_addr, dirty_addr, offset);
-    clean_addr = gen_mte_check1(s, dirty_addr, is_store, rn != 31, size);
+
+    memop = finalize_memop(s, size + is_signed * MO_SIGN);
+    clean_addr = gen_mte_check1(s, dirty_addr, is_store, rn != 31, memop);
 
     if (is_vector) {
         if (is_store) {
-            do_fp_st(s, rt, clean_addr, size);
+            do_fp_st(s, rt, clean_addr, memop);
         } else {
-            do_fp_ld(s, rt, clean_addr, size);
+            do_fp_ld(s, rt, clean_addr, memop);
         }
     } else {
         TCGv_i64 tcg_rt = cpu_reg(s, rt);
         bool iss_sf = disas_ldst_compute_iss_sf(size, is_signed, opc);
         if (is_store) {
-            do_gpr_st(s, tcg_rt, clean_addr, size,
-                      true, rt, iss_sf, false);
+            do_gpr_st(s, tcg_rt, clean_addr, memop, true, rt, iss_sf, false);
         } else {
-            do_gpr_ld(s, tcg_rt, clean_addr, size + is_signed * MO_SIGN,
+            do_gpr_ld(s, tcg_rt, clean_addr, memop,
                       is_extended, true, rt, iss_sf, false);
         }
     }
@@ -3310,7 +3464,7 @@ static void disas_ldst_atomic(DisasContext *s, uint32_t insn,
     bool a = extract32(insn, 23, 1);
     TCGv_i64 tcg_rs, tcg_rt, clean_addr;
     AtomicThreeOpFn *fn = NULL;
-    MemOp mop = s->be_data | size | MO_ALIGN;
+    MemOp mop = size;
 
     if (is_vector || !dc_isar_feature(aa64_atomics, s)) {
         unallocated_encoding(s);
@@ -3361,7 +3515,9 @@ static void disas_ldst_atomic(DisasContext *s, uint32_t insn,
     if (rn == 31) {
         gen_check_sp_alignment(s);
     }
-    clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn), false, rn != 31, size);
+
+    mop = check_atomic_align(s, rn, mop);
+    clean_addr = gen_mte_check1(s, cpu_reg_sp(s, rn), false, rn != 31, mop);
 
     if (o3_opc == 014) {
         /*
@@ -3371,7 +3527,7 @@ static void disas_ldst_atomic(DisasContext *s, uint32_t insn,
          * full load-acquire (we only need "load-acquire processor consistent"),
          * but we choose to implement them as full LDAQ.
          */
-        do_gpr_ld(s, cpu_reg(s, rt), clean_addr, size, false,
+        do_gpr_ld(s, cpu_reg(s, rt), clean_addr, mop, false,
                   true, rt, disas_ldst_compute_iss_sf(size, false, 0), true);
         tcg_gen_mb(TCG_MO_ALL | TCG_BAR_LDAQ);
         return;
@@ -3417,6 +3573,7 @@ static void disas_ldst_pac(DisasContext *s, uint32_t insn,
     bool use_key_a = !extract32(insn, 23, 1);
     int offset;
     TCGv_i64 clean_addr, dirty_addr, tcg_rt;
+    MemOp memop;
 
     if (size != 3 || is_vector || !dc_isar_feature(aa64_pauth, s)) {
         unallocated_encoding(s);
@@ -3443,12 +3600,14 @@ static void disas_ldst_pac(DisasContext *s, uint32_t insn,
     offset = sextract32(offset << size, 0, 10 + size);
     tcg_gen_addi_i64(dirty_addr, dirty_addr, offset);
 
+    memop = finalize_memop(s, size);
+
     /* Note that "clean" and "dirty" here refer to TBI not PAC.  */
     clean_addr = gen_mte_check1(s, dirty_addr, false,
-                                is_wback || rn != 31, size);
+                                is_wback || rn != 31, memop);
 
     tcg_rt = cpu_reg(s, rt);
-    do_gpr_ld(s, tcg_rt, clean_addr, size,
+    do_gpr_ld(s, tcg_rt, clean_addr, memop,
               /* extend */ false, /* iss_valid */ !is_wback,
               /* iss_srt */ rt, /* iss_sf */ true, /* iss_ar */ false);
 
@@ -3482,16 +3641,13 @@ static void disas_ldst_ldapr_stlr(DisasContext *s, uint32_t insn)
     bool is_store = false;
     bool extend = false;
     bool iss_sf;
-    MemOp mop;
+    MemOp mop = size;
 
     if (!dc_isar_feature(aa64_rcpc_8_4, s)) {
         unallocated_encoding(s);
         return;
     }
 
-    /* TODO: ARMv8.4-LSE SCTLR.nAA */
-    mop = size | MO_ALIGN;
-
     switch (opc) {
     case 0: /* STLURB */
         is_store = true;
@@ -3523,6 +3679,8 @@ static void disas_ldst_ldapr_stlr(DisasContext *s, uint32_t insn)
         gen_check_sp_alignment(s);
     }
 
+    mop = check_ordered_align(s, rn, offset, is_store, mop);
+
     dirty_addr = read_cpu_reg_sp(s, rn, 1);
     tcg_gen_addi_i64(dirty_addr, dirty_addr, offset);
     clean_addr = clean_data_tbi(s, dirty_addr);
@@ -3689,7 +3847,7 @@ static void disas_ldst_multiple_struct(DisasContext *s, uint32_t insn)
      * promote consecutive little-endian elements below.
      */
     clean_addr = gen_mte_checkN(s, tcg_rn, is_store, is_postidx || rn != 31,
-                                total);
+                                total, finalize_memop(s, size));
 
     /*
      * Consecutive little-endian elements from a single register
@@ -3847,10 +4005,11 @@ static void disas_ldst_single_struct(DisasContext *s, uint32_t insn)
     total = selem << scale;
     tcg_rn = cpu_reg_sp(s, rn);
 
-    clean_addr = gen_mte_checkN(s, tcg_rn, !is_load, is_postidx || rn != 31,
-                                total);
     mop = finalize_memop(s, scale);
 
+    clean_addr = gen_mte_checkN(s, tcg_rn, !is_load, is_postidx || rn != 31,
+                                total, mop);
+
     tcg_ebytes = tcg_constant_i64(1 << scale);
     for (xs = 0; xs < selem; xs++) {
         if (replicate) {
@@ -4062,15 +4221,18 @@ static void disas_ldst_tag(DisasContext *s, uint32_t insn)
 
     if (is_zero) {
         TCGv_i64 clean_addr = clean_data_tbi(s, addr);
-        TCGv_i64 tcg_zero = tcg_constant_i64(0);
+        TCGv_i64 zero64 = tcg_constant_i64(0);
+        TCGv_i128 zero128 = tcg_temp_new_i128();
         int mem_index = get_mem_index(s);
-        int i, n = (1 + is_pair) << LOG2_TAG_GRANULE;
+        MemOp mop = finalize_memop(s, MO_128 | MO_ALIGN);
+
+        tcg_gen_concat_i64_i128(zero128, zero64, zero64);
 
-        tcg_gen_qemu_st_i64(tcg_zero, clean_addr, mem_index,
-                            MO_UQ | MO_ALIGN_16);
-        for (i = 8; i < n; i += 8) {
-            tcg_gen_addi_i64(clean_addr, clean_addr, 8);
-            tcg_gen_qemu_st_i64(tcg_zero, clean_addr, mem_index, MO_UQ);
+        /* This is 1 or 2 atomic 16-byte operations. */
+        tcg_gen_qemu_st_i128(zero128, clean_addr, mem_index, mop);
+        if (is_pair) {
+            tcg_gen_addi_i64(clean_addr, clean_addr, 16);
+            tcg_gen_qemu_st_i128(zero128, clean_addr, mem_index, mop);
         }
     }
 
@@ -14087,6 +14249,7 @@ static void aarch64_tr_init_disas_context(DisasContextBase *dcbase,
     dc->pstate_sm = EX_TBFLAG_A64(tb_flags, PSTATE_SM);
     dc->pstate_za = EX_TBFLAG_A64(tb_flags, PSTATE_ZA);
     dc->sme_trap_nonstreaming = EX_TBFLAG_A64(tb_flags, SME_TRAP_NONSTREAMING);
+    dc->naa = EX_TBFLAG_A64(tb_flags, NAA);
     dc->vec_len = 0;
     dc->vec_stride = 0;
     dc->cp_regs = arm_cpu->cp_regs;
@@ -14098,6 +14261,8 @@ static void aarch64_tr_init_disas_context(DisasContextBase *dcbase,
     tcg_debug_assert(dc->tbid & 1);
 #endif
 
+    dc->lse2 = dc_isar_feature(aa64_lse2, dc);
+
     /* Single step state. The code-generation logic here is:
      *  SS_ACTIVE == 0:
      *   generate code with no special handling for single-stepping (except
diff --git a/target/arm/tcg/translate-a64.h b/target/arm/tcg/translate-a64.h
index 0576c4e..b55dc43 100644
--- a/target/arm/tcg/translate-a64.h
+++ b/target/arm/tcg/translate-a64.h
@@ -49,9 +49,9 @@ static inline bool sme_smza_enabled_check(DisasContext *s)
 
 TCGv_i64 clean_data_tbi(DisasContext *s, TCGv_i64 addr);
 TCGv_i64 gen_mte_check1(DisasContext *s, TCGv_i64 addr, bool is_write,
-                        bool tag_checked, int log2_size);
+                        bool tag_checked, MemOp memop);
 TCGv_i64 gen_mte_checkN(DisasContext *s, TCGv_i64 addr, bool is_write,
-                        bool tag_checked, int size);
+                        bool tag_checked, int total_size, MemOp memop);
 
 /* We should have at some point before trying to access an FP register
  * done the necessary access check, so assert that
diff --git a/target/arm/tcg/translate-sve.c b/target/arm/tcg/translate-sve.c
index d9d5810..ff05062 100644
--- a/target/arm/tcg/translate-sve.c
+++ b/target/arm/tcg/translate-sve.c
@@ -4167,15 +4167,16 @@ TRANS_FEAT(UCVTF_dd, aa64_sve, gen_gvec_fpst_arg_zpz,
 void gen_sve_ldr(DisasContext *s, TCGv_ptr base, int vofs,
                  int len, int rn, int imm)
 {
-    int len_align = QEMU_ALIGN_DOWN(len, 8);
-    int len_remain = len % 8;
-    int nparts = len / 8 + ctpop8(len_remain);
+    int len_align = QEMU_ALIGN_DOWN(len, 16);
+    int len_remain = len % 16;
+    int nparts = len / 16 + ctpop8(len_remain);
     int midx = get_mem_index(s);
     TCGv_i64 dirty_addr, clean_addr, t0, t1;
+    TCGv_i128 t16;
 
     dirty_addr = tcg_temp_new_i64();
     tcg_gen_addi_i64(dirty_addr, cpu_reg_sp(s, rn), imm);
-    clean_addr = gen_mte_checkN(s, dirty_addr, false, rn != 31, len);
+    clean_addr = gen_mte_checkN(s, dirty_addr, false, rn != 31, len, MO_8);
 
     /*
      * Note that unpredicated load/store of vector/predicate registers
@@ -4188,10 +4189,16 @@ void gen_sve_ldr(DisasContext *s, TCGv_ptr base, int vofs,
         int i;
 
         t0 = tcg_temp_new_i64();
-        for (i = 0; i < len_align; i += 8) {
-            tcg_gen_qemu_ld_i64(t0, clean_addr, midx, MO_LEUQ);
+        t1 = tcg_temp_new_i64();
+        t16 = tcg_temp_new_i128();
+
+        for (i = 0; i < len_align; i += 16) {
+            tcg_gen_qemu_ld_i128(t16, clean_addr, midx,
+                                 MO_LE | MO_128 | MO_ATOM_NONE);
+            tcg_gen_extr_i128_i64(t0, t1, t16);
             tcg_gen_st_i64(t0, base, vofs + i);
-            tcg_gen_addi_i64(clean_addr, clean_addr, 8);
+            tcg_gen_st_i64(t1, base, vofs + i + 8);
+            tcg_gen_addi_i64(clean_addr, clean_addr, 16);
         }
     } else {
         TCGLabel *loop = gen_new_label();
@@ -4200,14 +4207,21 @@ void gen_sve_ldr(DisasContext *s, TCGv_ptr base, int vofs,
         tcg_gen_movi_ptr(i, 0);
         gen_set_label(loop);
 
-        t0 = tcg_temp_new_i64();
-        tcg_gen_qemu_ld_i64(t0, clean_addr, midx, MO_LEUQ);
-        tcg_gen_addi_i64(clean_addr, clean_addr, 8);
+        t16 = tcg_temp_new_i128();
+        tcg_gen_qemu_ld_i128(t16, clean_addr, midx,
+                             MO_LE | MO_128 | MO_ATOM_NONE);
+        tcg_gen_addi_i64(clean_addr, clean_addr, 16);
 
         tp = tcg_temp_new_ptr();
         tcg_gen_add_ptr(tp, base, i);
-        tcg_gen_addi_ptr(i, i, 8);
+        tcg_gen_addi_ptr(i, i, 16);
+
+        t0 = tcg_temp_new_i64();
+        t1 = tcg_temp_new_i64();
+        tcg_gen_extr_i128_i64(t0, t1, t16);
+
         tcg_gen_st_i64(t0, tp, vofs);
+        tcg_gen_st_i64(t1, tp, vofs + 8);
 
         tcg_gen_brcondi_ptr(TCG_COND_LTU, i, len_align, loop);
     }
@@ -4216,6 +4230,16 @@ void gen_sve_ldr(DisasContext *s, TCGv_ptr base, int vofs,
      * Predicate register loads can be any multiple of 2.
      * Note that we still store the entire 64-bit unit into cpu_env.
      */
+    if (len_remain >= 8) {
+        t0 = tcg_temp_new_i64();
+        tcg_gen_qemu_ld_i64(t0, clean_addr, midx, MO_LEUQ | MO_ATOM_NONE);
+        tcg_gen_st_i64(t0, base, vofs + len_align);
+        len_remain -= 8;
+        len_align += 8;
+        if (len_remain) {
+            tcg_gen_addi_i64(clean_addr, clean_addr, 8);
+        }
+    }
     if (len_remain) {
         t0 = tcg_temp_new_i64();
         switch (len_remain) {
@@ -4223,14 +4247,14 @@ void gen_sve_ldr(DisasContext *s, TCGv_ptr base, int vofs,
         case 4:
         case 8:
             tcg_gen_qemu_ld_i64(t0, clean_addr, midx,
-                                MO_LE | ctz32(len_remain));
+                                MO_LE | ctz32(len_remain) | MO_ATOM_NONE);
             break;
 
         case 6:
             t1 = tcg_temp_new_i64();
-            tcg_gen_qemu_ld_i64(t0, clean_addr, midx, MO_LEUL);
+            tcg_gen_qemu_ld_i64(t0, clean_addr, midx, MO_LEUL | MO_ATOM_NONE);
             tcg_gen_addi_i64(clean_addr, clean_addr, 4);
-            tcg_gen_qemu_ld_i64(t1, clean_addr, midx, MO_LEUW);
+            tcg_gen_qemu_ld_i64(t1, clean_addr, midx, MO_LEUW | MO_ATOM_NONE);
             tcg_gen_deposit_i64(t0, t0, t1, 32, 32);
             break;
 
@@ -4245,15 +4269,16 @@ void gen_sve_ldr(DisasContext *s, TCGv_ptr base, int vofs,
 void gen_sve_str(DisasContext *s, TCGv_ptr base, int vofs,
                  int len, int rn, int imm)
 {
-    int len_align = QEMU_ALIGN_DOWN(len, 8);
-    int len_remain = len % 8;
-    int nparts = len / 8 + ctpop8(len_remain);
+    int len_align = QEMU_ALIGN_DOWN(len, 16);
+    int len_remain = len % 16;
+    int nparts = len / 16 + ctpop8(len_remain);
     int midx = get_mem_index(s);
-    TCGv_i64 dirty_addr, clean_addr, t0;
+    TCGv_i64 dirty_addr, clean_addr, t0, t1;
+    TCGv_i128 t16;
 
     dirty_addr = tcg_temp_new_i64();
     tcg_gen_addi_i64(dirty_addr, cpu_reg_sp(s, rn), imm);
-    clean_addr = gen_mte_checkN(s, dirty_addr, false, rn != 31, len);
+    clean_addr = gen_mte_checkN(s, dirty_addr, false, rn != 31, len, MO_8);
 
     /* Note that unpredicated load/store of vector/predicate registers
      * are defined as a stream of bytes, which equates to little-endian
@@ -4267,10 +4292,15 @@ void gen_sve_str(DisasContext *s, TCGv_ptr base, int vofs,
         int i;
 
         t0 = tcg_temp_new_i64();
+        t1 = tcg_temp_new_i64();
+        t16 = tcg_temp_new_i128();
         for (i = 0; i < len_align; i += 8) {
             tcg_gen_ld_i64(t0, base, vofs + i);
-            tcg_gen_qemu_st_i64(t0, clean_addr, midx, MO_LEUQ);
-            tcg_gen_addi_i64(clean_addr, clean_addr, 8);
+            tcg_gen_ld_i64(t1, base, vofs + i + 8);
+            tcg_gen_concat_i64_i128(t16, t0, t1);
+            tcg_gen_qemu_st_i128(t16, clean_addr, midx,
+                                 MO_LE | MO_128 | MO_ATOM_NONE);
+            tcg_gen_addi_i64(clean_addr, clean_addr, 16);
         }
     } else {
         TCGLabel *loop = gen_new_label();
@@ -4280,18 +4310,33 @@ void gen_sve_str(DisasContext *s, TCGv_ptr base, int vofs,
         gen_set_label(loop);
 
         t0 = tcg_temp_new_i64();
+        t1 = tcg_temp_new_i64();
         tp = tcg_temp_new_ptr();
         tcg_gen_add_ptr(tp, base, i);
         tcg_gen_ld_i64(t0, tp, vofs);
-        tcg_gen_addi_ptr(i, i, 8);
+        tcg_gen_ld_i64(t1, tp, vofs + 8);
+        tcg_gen_addi_ptr(i, i, 16);
 
-        tcg_gen_qemu_st_i64(t0, clean_addr, midx, MO_LEUQ);
-        tcg_gen_addi_i64(clean_addr, clean_addr, 8);
+        t16 = tcg_temp_new_i128();
+        tcg_gen_concat_i64_i128(t16, t0, t1);
+
+        tcg_gen_qemu_st_i128(t16, clean_addr, midx, MO_LEUQ);
+        tcg_gen_addi_i64(clean_addr, clean_addr, 16);
 
         tcg_gen_brcondi_ptr(TCG_COND_LTU, i, len_align, loop);
     }
 
     /* Predicate register stores can be any multiple of 2.  */
+    if (len_remain >= 8) {
+        t0 = tcg_temp_new_i64();
+        tcg_gen_st_i64(t0, base, vofs + len_align);
+        tcg_gen_qemu_st_i64(t0, clean_addr, midx, MO_LEUQ | MO_ATOM_NONE);
+        len_remain -= 8;
+        len_align += 8;
+        if (len_remain) {
+            tcg_gen_addi_i64(clean_addr, clean_addr, 8);
+        }
+    }
     if (len_remain) {
         t0 = tcg_temp_new_i64();
         tcg_gen_ld_i64(t0, base, vofs + len_align);
@@ -4301,14 +4346,14 @@ void gen_sve_str(DisasContext *s, TCGv_ptr base, int vofs,
         case 4:
         case 8:
             tcg_gen_qemu_st_i64(t0, clean_addr, midx,
-                                MO_LE | ctz32(len_remain));
+                                MO_LE | ctz32(len_remain) | MO_ATOM_NONE);
             break;
 
         case 6:
-            tcg_gen_qemu_st_i64(t0, clean_addr, midx, MO_LEUL);
+            tcg_gen_qemu_st_i64(t0, clean_addr, midx, MO_LEUL | MO_ATOM_NONE);
             tcg_gen_addi_i64(clean_addr, clean_addr, 4);
             tcg_gen_shri_i64(t0, t0, 32);
-            tcg_gen_qemu_st_i64(t0, clean_addr, midx, MO_LEUW);
+            tcg_gen_qemu_st_i64(t0, clean_addr, midx, MO_LEUW | MO_ATOM_NONE);
             break;
 
         default:
@@ -4964,6 +5009,7 @@ static bool trans_LD1R_zpri(DisasContext *s, arg_rpri_load *a)
     unsigned msz = dtype_msz(a->dtype);
     TCGLabel *over;
     TCGv_i64 temp, clean_addr;
+    MemOp memop;
 
     if (!dc_isar_feature(aa64_sve, s)) {
         return false;
@@ -4993,10 +5039,10 @@ static bool trans_LD1R_zpri(DisasContext *s, arg_rpri_load *a)
     /* Load the data.  */
     temp = tcg_temp_new_i64();
     tcg_gen_addi_i64(temp, cpu_reg_sp(s, a->rn), a->imm << msz);
-    clean_addr = gen_mte_check1(s, temp, false, true, msz);
 
-    tcg_gen_qemu_ld_i64(temp, clean_addr, get_mem_index(s),
-                        finalize_memop(s, dtype_mop[a->dtype]));
+    memop = finalize_memop(s, dtype_mop[a->dtype]);
+    clean_addr = gen_mte_check1(s, temp, false, true, memop);
+    tcg_gen_qemu_ld_i64(temp, clean_addr, get_mem_index(s), memop);
 
     /* Broadcast to *all* elements.  */
     tcg_gen_gvec_dup_i64(esz, vec_full_reg_offset(s, a->rd),
diff --git a/target/arm/tcg/translate.c b/target/arm/tcg/translate.c
index a68d3c7..13c88ba 100644
--- a/target/arm/tcg/translate.c
+++ b/target/arm/tcg/translate.c
@@ -9168,6 +9168,7 @@ static void arm_tr_init_disas_context(DisasContextBase *dcbase, CPUState *cs)
         dc->sme_trap_nonstreaming =
             EX_TBFLAG_A32(tb_flags, SME_TRAP_NONSTREAMING);
     }
+    dc->lse2 = false; /* applies only to aarch64 */
     dc->cp_regs = cpu->cp_regs;
     dc->features = env->features;
 
diff --git a/target/arm/tcg/translate.h b/target/arm/tcg/translate.h
index 4d88197..d1cacff 100644
--- a/target/arm/tcg/translate.h
+++ b/target/arm/tcg/translate.h
@@ -90,6 +90,7 @@ typedef struct DisasContext {
     uint64_t features; /* CPU features bits */
     bool aarch64;
     bool thumb;
+    bool lse2;
     /* Because unallocated encodings generate different exception syndrome
      * information from traps due to FP being disabled, we can't do a single
      * "is fp access disabled" check at a high level in the decode tree.
@@ -141,6 +142,8 @@ typedef struct DisasContext {
     bool fgt_eret;
     /* True if fine-grained trap on SVC is enabled */
     bool fgt_svc;
+    /* True if FEAT_LSE2 SCTLR_ELx.nAA is set */
+    bool naa;
     /*
      * >= 0, a copy of PSTATE.BTYPE, which will be 0 without v8.5-BTI.
      *  < 0, set by the current instruction.
@@ -557,12 +560,13 @@ static inline TCGv_ptr fpstatus_ptr(ARMFPStatusFlavour flavour)
 }
 
 /**
- * finalize_memop:
+ * finalize_memop_atom:
  * @s: DisasContext
  * @opc: size+sign+align of the memory operation
+ * @atom: atomicity of the memory operation
  *
- * Build the complete MemOp for a memory operation, including alignment
- * and endianness.
+ * Build the complete MemOp for a memory operation, including alignment,
+ * endianness, and atomicity.
  *
  * If (op & MO_AMASK) then the operation already contains the required
  * alignment, e.g. for AccType_ATOMIC.  Otherwise, this an optionally
@@ -572,12 +576,63 @@ static inline TCGv_ptr fpstatus_ptr(ARMFPStatusFlavour flavour)
  * and this is applied here.  Note that there is no way to indicate that
  * no alignment should ever be enforced; this must be handled manually.
  */
-static inline MemOp finalize_memop(DisasContext *s, MemOp opc)
+static inline MemOp finalize_memop_atom(DisasContext *s, MemOp opc, MemOp atom)
 {
     if (s->align_mem && !(opc & MO_AMASK)) {
         opc |= MO_ALIGN;
     }
-    return opc | s->be_data;
+    return opc | atom | s->be_data;
+}
+
+/**
+ * finalize_memop:
+ * @s: DisasContext
+ * @opc: size+sign+align of the memory operation
+ *
+ * Like finalize_memop_atom, but with default atomicity.
+ */
+static inline MemOp finalize_memop(DisasContext *s, MemOp opc)
+{
+    MemOp atom = s->lse2 ? MO_ATOM_WITHIN16 : MO_ATOM_IFALIGN;
+    return finalize_memop_atom(s, opc, atom);
+}
+
+/**
+ * finalize_memop_pair:
+ * @s: DisasContext
+ * @opc: size+sign+align of the memory operation
+ *
+ * Like finalize_memop_atom, but with atomicity for a pair.
+ * C.f. Pseudocode for Mem[], operand ispair.
+ */
+static inline MemOp finalize_memop_pair(DisasContext *s, MemOp opc)
+{
+    MemOp atom = s->lse2 ? MO_ATOM_WITHIN16_PAIR : MO_ATOM_IFALIGN_PAIR;
+    return finalize_memop_atom(s, opc, atom);
+}
+
+/**
+ * finalize_memop_asimd:
+ * @s: DisasContext
+ * @opc: size+sign+align of the memory operation
+ *
+ * Like finalize_memop_atom, but with atomicity of AccessType_ASIMD.
+ */
+static inline MemOp finalize_memop_asimd(DisasContext *s, MemOp opc)
+{
+    /*
+     * In the pseudocode for Mem[], with AccessType_ASIMD, size == 16,
+     * if IsAligned(8), the first case provides separate atomicity for
+     * the pair of 64-bit accesses.  If !IsAligned(8), the middle cases
+     * do not apply, and we're left with the final case of no atomicity.
+     * Thus MO_ATOM_IFALIGN_PAIR.
+     *
+     * For other sizes, normal LSE2 rules apply.
+     */
+    if ((opc & MO_SIZE) == MO_128) {
+        return finalize_memop_atom(s, opc, MO_ATOM_IFALIGN_PAIR);
+    }
+    return finalize_memop(s, opc);
 }
 
 /**
diff --git a/target/i386/hvf/hvf.c b/target/i386/hvf/hvf.c
index 8d2248b..f6775c9 100644
--- a/target/i386/hvf/hvf.c
+++ b/target/i386/hvf/hvf.c
@@ -679,3 +679,36 @@ int hvf_vcpu_exec(CPUState *cpu)
 
     return ret;
 }
+
+int hvf_arch_insert_sw_breakpoint(CPUState *cpu, struct hvf_sw_breakpoint *bp)
+{
+    return -ENOSYS;
+}
+
+int hvf_arch_remove_sw_breakpoint(CPUState *cpu, struct hvf_sw_breakpoint *bp)
+{
+    return -ENOSYS;
+}
+
+int hvf_arch_insert_hw_breakpoint(target_ulong addr, target_ulong len, int type)
+{
+    return -ENOSYS;
+}
+
+int hvf_arch_remove_hw_breakpoint(target_ulong addr, target_ulong len, int type)
+{
+    return -ENOSYS;
+}
+
+void hvf_arch_remove_all_hw_breakpoints(void)
+{
+}
+
+void hvf_arch_update_guest_debug(CPUState *cpu)
+{
+}
+
+inline bool hvf_arch_supports_guest_debug(void)
+{
+    return false;
+}
diff --git a/tests/avocado/boot_linux_console.py b/tests/avocado/boot_linux_console.py
index c067580..6ed6606 100644
--- a/tests/avocado/boot_linux_console.py
+++ b/tests/avocado/boot_linux_console.py
@@ -769,6 +769,182 @@ class BootLinuxConsole(LinuxKernelTest):
         self.wait_for_console_pattern(
                 'Give root password for system maintenance')
 
+    def test_arm_bpim2u(self):
+        """
+        :avocado: tags=arch:arm
+        :avocado: tags=machine:bpim2u
+        :avocado: tags=accel:tcg
+        """
+        deb_url = ('https://apt.armbian.com/pool/main/l/linux-5.10.16-sunxi/'
+                   'linux-image-current-sunxi_21.02.2_armhf.deb')
+        deb_hash = '9fa84beda245cabf0b4fa84cf6eaa7738ead1da0'
+        deb_path = self.fetch_asset(deb_url, asset_hash=deb_hash)
+        kernel_path = self.extract_from_deb(deb_path,
+                                            '/boot/vmlinuz-5.10.16-sunxi')
+        dtb_path = ('/usr/lib/linux-image-current-sunxi/'
+                    'sun8i-r40-bananapi-m2-ultra.dtb')
+        dtb_path = self.extract_from_deb(deb_path, dtb_path)
+
+        self.vm.set_console()
+        kernel_command_line = (self.KERNEL_COMMON_COMMAND_LINE +
+                               'console=ttyS0,115200n8 '
+                               'earlycon=uart,mmio32,0x1c28000')
+        self.vm.add_args('-kernel', kernel_path,
+                         '-dtb', dtb_path,
+                         '-append', kernel_command_line)
+        self.vm.launch()
+        console_pattern = 'Kernel command line: %s' % kernel_command_line
+        self.wait_for_console_pattern(console_pattern)
+
+    def test_arm_bpim2u_initrd(self):
+        """
+        :avocado: tags=arch:arm
+        :avocado: tags=accel:tcg
+        :avocado: tags=machine:bpim2u
+        """
+        deb_url = ('https://apt.armbian.com/pool/main/l/linux-5.10.16-sunxi/'
+                   'linux-image-current-sunxi_21.02.2_armhf.deb')
+        deb_hash = '9fa84beda245cabf0b4fa84cf6eaa7738ead1da0'
+        deb_path = self.fetch_asset(deb_url, asset_hash=deb_hash)
+        kernel_path = self.extract_from_deb(deb_path,
+                                            '/boot/vmlinuz-5.10.16-sunxi')
+        dtb_path = ('/usr/lib/linux-image-current-sunxi/'
+                    'sun8i-r40-bananapi-m2-ultra.dtb')
+        dtb_path = self.extract_from_deb(deb_path, dtb_path)
+        initrd_url = ('https://github.com/groeck/linux-build-test/raw/'
+                      '2eb0a73b5d5a28df3170c546ddaaa9757e1e0848/rootfs/'
+                      'arm/rootfs-armv7a.cpio.gz')
+        initrd_hash = '604b2e45cdf35045846b8bbfbf2129b1891bdc9c'
+        initrd_path_gz = self.fetch_asset(initrd_url, asset_hash=initrd_hash)
+        initrd_path = os.path.join(self.workdir, 'rootfs.cpio')
+        archive.gzip_uncompress(initrd_path_gz, initrd_path)
+
+        self.vm.set_console()
+        kernel_command_line = (self.KERNEL_COMMON_COMMAND_LINE +
+                               'console=ttyS0,115200 '
+                               'panic=-1 noreboot')
+        self.vm.add_args('-kernel', kernel_path,
+                         '-dtb', dtb_path,
+                         '-initrd', initrd_path,
+                         '-append', kernel_command_line,
+                         '-no-reboot')
+        self.vm.launch()
+        self.wait_for_console_pattern('Boot successful.')
+
+        exec_command_and_wait_for_pattern(self, 'cat /proc/cpuinfo',
+                                                'Allwinner sun8i Family')
+        exec_command_and_wait_for_pattern(self, 'cat /proc/iomem',
+                                                'system-control@1c00000')
+        exec_command_and_wait_for_pattern(self, 'reboot',
+                                                'reboot: Restarting system')
+        # Wait for VM to shut down gracefully
+        self.vm.wait()
+
+    def test_arm_bpim2u_gmac(self):
+        """
+        :avocado: tags=arch:arm
+        :avocado: tags=accel:tcg
+        :avocado: tags=machine:bpim2u
+        :avocado: tags=device:sd
+        """
+        self.require_netdev('user')
+
+        deb_url = ('https://apt.armbian.com/pool/main/l/linux-5.10.16-sunxi/'
+                   'linux-image-current-sunxi_21.02.2_armhf.deb')
+        deb_hash = '9fa84beda245cabf0b4fa84cf6eaa7738ead1da0'
+        deb_path = self.fetch_asset(deb_url, asset_hash=deb_hash)
+        kernel_path = self.extract_from_deb(deb_path,
+                                            '/boot/vmlinuz-5.10.16-sunxi')
+        dtb_path = ('/usr/lib/linux-image-current-sunxi/'
+                    'sun8i-r40-bananapi-m2-ultra.dtb')
+        dtb_path = self.extract_from_deb(deb_path, dtb_path)
+        rootfs_url = ('http://storage.kernelci.org/images/rootfs/buildroot/'
+                      'buildroot-baseline/20221116.0/armel/rootfs.ext2.xz')
+        rootfs_hash = 'fae32f337c7b87547b10f42599acf109da8b6d9a'
+        rootfs_path_xz = self.fetch_asset(rootfs_url, asset_hash=rootfs_hash)
+        rootfs_path = os.path.join(self.workdir, 'rootfs.cpio')
+        archive.lzma_uncompress(rootfs_path_xz, rootfs_path)
+        image_pow2ceil_expand(rootfs_path)
+
+        self.vm.set_console()
+        kernel_command_line = (self.KERNEL_COMMON_COMMAND_LINE +
+                               'console=ttyS0,115200 '
+                               'root=/dev/mmcblk0 rootwait rw '
+                               'panic=-1 noreboot')
+        self.vm.add_args('-kernel', kernel_path,
+                         '-dtb', dtb_path,
+                         '-drive', 'file=' + rootfs_path + ',if=sd,format=raw',
+                         '-net', 'nic,model=gmac,netdev=host_gmac',
+                         '-netdev', 'user,id=host_gmac',
+                         '-append', kernel_command_line,
+                         '-no-reboot')
+        self.vm.launch()
+        shell_ready = "/bin/sh: can't access tty; job control turned off"
+        self.wait_for_console_pattern(shell_ready)
+
+        exec_command_and_wait_for_pattern(self, 'cat /proc/cpuinfo',
+                                                'Allwinner sun8i Family')
+        exec_command_and_wait_for_pattern(self, 'cat /proc/partitions',
+                                                'mmcblk0')
+        exec_command_and_wait_for_pattern(self, 'ifconfig eth0 up',
+                                                 'eth0: Link is Up')
+        exec_command_and_wait_for_pattern(self, 'udhcpc eth0',
+            'udhcpc: lease of 10.0.2.15 obtained')
+        exec_command_and_wait_for_pattern(self, 'ping -c 3 10.0.2.2',
+            '3 packets transmitted, 3 packets received, 0% packet loss')
+        exec_command_and_wait_for_pattern(self, 'reboot',
+                                                'reboot: Restarting system')
+        # Wait for VM to shut down gracefully
+        self.vm.wait()
+
+    @skipUnless(os.getenv('AVOCADO_ALLOW_LARGE_STORAGE'), 'storage limited')
+    def test_arm_bpim2u_openwrt_22_03_3(self):
+        """
+        :avocado: tags=arch:arm
+        :avocado: tags=machine:bpim2u
+        :avocado: tags=device:sd
+        """
+
+        # This test download a 8.9 MiB compressed image and expand it
+        # to 127 MiB.
+        image_url = ('https://downloads.openwrt.org/releases/22.03.3/targets/'
+                     'sunxi/cortexa7/openwrt-22.03.3-sunxi-cortexa7-'
+                     'sinovoip_bananapi-m2-ultra-ext4-sdcard.img.gz')
+        image_hash = ('5b41b4e11423e562c6011640f9a7cd3b'
+                      'dd0a3d42b83430f7caa70a432e6cd82c')
+        image_path_gz = self.fetch_asset(image_url, asset_hash=image_hash,
+                                         algorithm='sha256')
+        image_path = archive.extract(image_path_gz, self.workdir)
+        image_pow2ceil_expand(image_path)
+
+        self.vm.set_console()
+        self.vm.add_args('-drive', 'file=' + image_path + ',if=sd,format=raw',
+                         '-nic', 'user',
+                         '-no-reboot')
+        self.vm.launch()
+
+        kernel_command_line = (self.KERNEL_COMMON_COMMAND_LINE +
+                               'usbcore.nousb '
+                               'noreboot')
+
+        self.wait_for_console_pattern('U-Boot SPL')
+
+        interrupt_interactive_console_until_pattern(
+                self, 'Hit any key to stop autoboot:', '=>')
+        exec_command_and_wait_for_pattern(self, "setenv extraargs '" +
+                                                kernel_command_line + "'", '=>')
+        exec_command_and_wait_for_pattern(self, 'boot', 'Starting kernel ...');
+
+        self.wait_for_console_pattern(
+            'Please press Enter to activate this console.')
+
+        exec_command_and_wait_for_pattern(self, ' ', 'root@')
+
+        exec_command_and_wait_for_pattern(self, 'cat /proc/cpuinfo',
+                                                'Allwinner sun8i Family')
+        exec_command_and_wait_for_pattern(self, 'cat /proc/iomem',
+                                                'system-control@1c00000')
+
     def test_arm_orangepi(self):
         """
         :avocado: tags=arch:arm
diff --git a/tests/qtest/meson.build b/tests/qtest/meson.build
index 1d8a53a..5fa6833 100644
--- a/tests/qtest/meson.build
+++ b/tests/qtest/meson.build
@@ -216,6 +216,7 @@ qtests_aarch64 = \
   (config_all.has_key('CONFIG_TCG') and config_all_devices.has_key('CONFIG_TPM_TIS_SYSBUS') ?            \
     ['tpm-tis-device-test', 'tpm-tis-device-swtpm-test'] : []) +                                         \
   (config_all_devices.has_key('CONFIG_XLNX_ZYNQMP_ARM') ? ['xlnx-can-test', 'fuzz-xlnx-dp-test'] : []) + \
+  (config_all_devices.has_key('CONFIG_XLNX_VERSAL') ? ['xlnx-canfd-test'] : []) + \
   (config_all_devices.has_key('CONFIG_RASPI') ? ['bcm2835-dma-test'] : []) +  \
   (config_all.has_key('CONFIG_TCG') and                                            \
    config_all_devices.has_key('CONFIG_TPM_TIS_I2C') ? ['tpm-tis-i2c-test'] : []) + \
diff --git a/tests/qtest/xlnx-canfd-test.c b/tests/qtest/xlnx-canfd-test.c
new file mode 100644
index 0000000..76ee106
--- /dev/null
+++ b/tests/qtest/xlnx-canfd-test.c
@@ -0,0 +1,423 @@
+/*
+ * SPDX-License-Identifier: MIT
+ *
+ * QTests for the Xilinx Versal CANFD controller.
+ *
+ * Copyright (c) 2022 AMD Inc.
+ *
+ * Written-by: Vikram Garhwal<vikram.garhwal@amd.com>
+ *
+ * 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.
+ */
+
+#include "qemu/osdep.h"
+#include "libqtest.h"
+
+/* Base address. */
+#define CANFD0_BASE_ADDR                0xff060000
+#define CANFD1_BASE_ADDR                0xff070000
+
+/* Register addresses. */
+#define R_SRR_OFFSET                    0x00
+#define R_MSR_OFFSET                    0x04
+#define R_FILTER_CONTROL_REGISTER       0xe0
+#define R_SR_OFFSET                     0x18
+#define R_ISR_OFFSET                    0x1c
+#define R_IER_OFFSET                    0x20
+#define R_ICR_OFFSET                    0x24
+#define R_TX_READY_REQ_REGISTER         0x90
+#define RX_FIFO_STATUS_REGISTER         0xe8
+#define R_TXID_OFFSET                   0x100
+#define R_TXDLC_OFFSET                  0x104
+#define R_TXDATA1_OFFSET                0x108
+#define R_TXDATA2_OFFSET                0x10c
+#define R_AFMR_REGISTER0                0xa00
+#define R_AFIR_REGISTER0                0xa04
+#define R_RX0_ID_OFFSET                 0x2100
+#define R_RX0_DLC_OFFSET                0x2104
+#define R_RX0_DATA1_OFFSET              0x2108
+#define R_RX0_DATA2_OFFSET              0x210c
+
+/* CANFD modes. */
+#define SRR_CONFIG_MODE                 0x00
+#define MSR_NORMAL_MODE                 0x00
+#define MSR_LOOPBACK_MODE               (1 << 1)
+#define ENABLE_CANFD                    (1 << 1)
+
+/* CANFD status. */
+#define STATUS_CONFIG_MODE              (1 << 0)
+#define STATUS_NORMAL_MODE              (1 << 3)
+#define STATUS_LOOPBACK_MODE            (1 << 1)
+#define ISR_TXOK                        (1 << 1)
+#define ISR_RXOK                        (1 << 4)
+
+#define ENABLE_ALL_FILTERS              0xffffffff
+#define ENABLE_ALL_INTERRUPTS           0xffffffff
+
+/* We are sending one canfd message. */
+#define TX_READY_REG_VAL                0x1
+
+#define FIRST_RX_STORE_INDEX            0x1
+#define STATUS_REG_MASK                 0xf
+#define DLC_FD_BIT_SHIFT                0x1b
+#define DLC_FD_BIT_MASK                 0xf8000000
+#define FIFO_STATUS_READ_INDEX_MASK     0x3f
+#define FIFO_STATUS_FILL_LEVEL_MASK     0x7f00
+#define FILL_LEVEL_SHIFT                0x8
+
+/* CANFD frame size ID, DLC and 16 DATA word. */
+#define CANFD_FRAME_SIZE        18
+/* CAN frame size ID, DLC and 2 DATA word. */
+#define CAN_FRAME_SIZE          4
+
+/* Set the filters for CANFD controller. */
+static void enable_filters(QTestState *qts)
+{
+     const uint32_t arr_afmr[32] = { 0xb423deaa, 0xa2a40bdc, 0x1b64f486,
+                                     0x95c0d4ee, 0xe0c44528, 0x4b407904,
+                                     0xd2673f46, 0x9fc638d6, 0x8844f3d8,
+                                     0xa607d1e8, 0x67871bf4, 0xc2557dc,
+                                     0x9ea5b53e, 0x3643c0cc, 0x5a05ea8e,
+                                     0x83a46d84, 0x4a25c2b8, 0x93a66008,
+                                     0x2e467470, 0xedc66118, 0x9086f9f2,
+                                     0xfa23dd36, 0xb6654b90, 0xb221b8ca,
+                                     0x3467d1e2, 0xa3a55542, 0x5b26a012,
+                                     0x2281ea7e, 0xcea0ece8, 0xdc61e588,
+                                     0x2e5676a,  0x16821320 };
+
+    const uint32_t arr_afir[32] = { 0xa833dfa1, 0x255a477e, 0x3a4bb1c5,
+                                    0x8f560a6c, 0x27f38903, 0x2fecec4d,
+                                    0xa014c66d, 0xec289b8,  0x7e52dead,
+                                    0x82e94f3c, 0xcf3e3c5c, 0x66059871,
+                                    0x3f213df4, 0x25ac3959, 0xa12e9bef,
+                                    0xa3ad3af,  0xbafd7fe,  0xb3cb40fd,
+                                    0x5d9caa81, 0x2ed61902, 0x7cd64a0,
+                                    0x4b1fa538, 0x9b5ced8c, 0x150de059,
+                                    0xd2794227, 0x635e820a, 0xbb6b02cf,
+                                    0xbb58176,  0x570025bb, 0xa78d9658,
+                                    0x49d735df, 0xe5399d2f };
+
+    /* Passing the respective array values to all the AFMR and AFIR pairs. */
+    for (int i = 0; i < 32; i++) {
+        /* For CANFD0. */
+       qtest_writel(qts, CANFD0_BASE_ADDR + R_AFMR_REGISTER0 + 8 * i,
+                    arr_afmr[i]);
+       qtest_writel(qts, CANFD0_BASE_ADDR + R_AFIR_REGISTER0 + 8 * i,
+                    arr_afir[i]);
+
+        /* For CANFD1. */
+       qtest_writel(qts, CANFD1_BASE_ADDR + R_AFMR_REGISTER0 + 8 * i,
+                    arr_afmr[i]);
+       qtest_writel(qts, CANFD1_BASE_ADDR + R_AFIR_REGISTER0 + 8 * i,
+                    arr_afir[i]);
+    }
+
+    /* Enable all the pairs from AFR register. */
+    qtest_writel(qts, CANFD0_BASE_ADDR + R_FILTER_CONTROL_REGISTER,
+                 ENABLE_ALL_FILTERS);
+    qtest_writel(qts, CANFD1_BASE_ADDR + R_FILTER_CONTROL_REGISTER,
+                 ENABLE_ALL_FILTERS);
+}
+
+static void configure_canfd(QTestState *qts, uint8_t mode)
+{
+    uint32_t status = 0;
+
+    /* Put CANFD0 and CANFD1 in config mode. */
+    qtest_writel(qts, CANFD0_BASE_ADDR + R_SRR_OFFSET, SRR_CONFIG_MODE);
+    qtest_writel(qts, CANFD1_BASE_ADDR + R_SRR_OFFSET, SRR_CONFIG_MODE);
+
+    /* Write mode of operation in Mode select register. */
+    qtest_writel(qts, CANFD0_BASE_ADDR + R_MSR_OFFSET, mode);
+    qtest_writel(qts, CANFD1_BASE_ADDR + R_MSR_OFFSET, mode);
+
+    enable_filters(qts);
+
+    /* Check here if CANFD0 and CANFD1 are in config mode. */
+    status = qtest_readl(qts, CANFD0_BASE_ADDR + R_SR_OFFSET);
+    status = status & STATUS_REG_MASK;
+    g_assert_cmpint(status, ==, STATUS_CONFIG_MODE);
+
+    status = qtest_readl(qts, CANFD1_BASE_ADDR + R_SR_OFFSET);
+    status = status & STATUS_REG_MASK;
+    g_assert_cmpint(status, ==, STATUS_CONFIG_MODE);
+
+    qtest_writel(qts, CANFD1_BASE_ADDR + R_IER_OFFSET, ENABLE_ALL_INTERRUPTS);
+    qtest_writel(qts, CANFD1_BASE_ADDR + R_IER_OFFSET, ENABLE_ALL_INTERRUPTS);
+
+    qtest_writel(qts, CANFD0_BASE_ADDR + R_SRR_OFFSET, ENABLE_CANFD);
+    qtest_writel(qts, CANFD1_BASE_ADDR + R_SRR_OFFSET, ENABLE_CANFD);
+}
+
+static void generate_random_data(uint32_t *buf_tx, bool is_canfd_frame)
+{
+    /* Generate random TX data for CANFD frame. */
+    if (is_canfd_frame) {
+        for (int i = 0; i < CANFD_FRAME_SIZE - 2; i++) {
+            buf_tx[2 + i] = rand();
+        }
+    } else {
+        /* Generate random TX data for CAN frame. */
+        for (int i = 0; i < CAN_FRAME_SIZE - 2; i++) {
+            buf_tx[2 + i] = rand();
+        }
+    }
+}
+
+static void read_data(QTestState *qts, uint64_t can_base_addr, uint32_t *buf_rx)
+{
+    uint32_t int_status;
+    uint32_t fifo_status_reg_value;
+    /* At which RX FIFO the received data is stored. */
+    uint8_t store_ind = 0;
+    bool is_canfd_frame = false;
+
+    /* Read the interrupt on CANFD rx. */
+    int_status = qtest_readl(qts, can_base_addr + R_ISR_OFFSET) & ISR_RXOK;
+
+    g_assert_cmpint(int_status, ==, ISR_RXOK);
+
+    /* Find the fill level and read index. */
+    fifo_status_reg_value = qtest_readl(qts, can_base_addr +
+                                        RX_FIFO_STATUS_REGISTER);
+
+    store_ind = (fifo_status_reg_value & FIFO_STATUS_READ_INDEX_MASK) +
+                ((fifo_status_reg_value & FIFO_STATUS_FILL_LEVEL_MASK) >>
+                  FILL_LEVEL_SHIFT);
+
+    g_assert_cmpint(store_ind, ==, FIRST_RX_STORE_INDEX);
+
+    /* Read the RX register data for CANFD. */
+    buf_rx[0] = qtest_readl(qts, can_base_addr + R_RX0_ID_OFFSET);
+    buf_rx[1] = qtest_readl(qts, can_base_addr + R_RX0_DLC_OFFSET);
+
+    is_canfd_frame = (buf_rx[1] >> DLC_FD_BIT_SHIFT) & 1;
+
+    if (is_canfd_frame) {
+        for (int i = 0; i < CANFD_FRAME_SIZE - 2; i++) {
+            buf_rx[i + 2] = qtest_readl(qts,
+                                    can_base_addr + R_RX0_DATA1_OFFSET + 4 * i);
+        }
+    } else {
+        buf_rx[2] = qtest_readl(qts, can_base_addr + R_RX0_DATA1_OFFSET);
+        buf_rx[3] = qtest_readl(qts, can_base_addr + R_RX0_DATA2_OFFSET);
+    }
+
+    /* Clear the RX interrupt. */
+    qtest_writel(qts, CANFD1_BASE_ADDR + R_ICR_OFFSET, ISR_RXOK);
+}
+
+static void write_data(QTestState *qts, uint64_t can_base_addr,
+                       const uint32_t *buf_tx, bool is_canfd_frame)
+{
+    /* Write the TX register data for CANFD. */
+    qtest_writel(qts, can_base_addr + R_TXID_OFFSET, buf_tx[0]);
+    qtest_writel(qts, can_base_addr + R_TXDLC_OFFSET, buf_tx[1]);
+
+    if (is_canfd_frame) {
+        for (int i = 0; i < CANFD_FRAME_SIZE - 2; i++) {
+            qtest_writel(qts, can_base_addr + R_TXDATA1_OFFSET + 4 * i,
+                         buf_tx[2 + i]);
+        }
+    } else {
+        qtest_writel(qts, can_base_addr + R_TXDATA1_OFFSET, buf_tx[2]);
+        qtest_writel(qts, can_base_addr + R_TXDATA2_OFFSET, buf_tx[3]);
+    }
+}
+
+static void send_data(QTestState *qts, uint64_t can_base_addr)
+{
+    uint32_t int_status;
+
+    qtest_writel(qts, can_base_addr + R_TX_READY_REQ_REGISTER,
+                 TX_READY_REG_VAL);
+
+    /* Read the interrupt on CANFD for tx. */
+    int_status = qtest_readl(qts, can_base_addr + R_ISR_OFFSET) & ISR_TXOK;
+
+    g_assert_cmpint(int_status, ==, ISR_TXOK);
+
+    /* Clear the interrupt for tx. */
+    qtest_writel(qts, CANFD0_BASE_ADDR + R_ICR_OFFSET, ISR_TXOK);
+}
+
+static void match_rx_tx_data(const uint32_t *buf_tx, const uint32_t *buf_rx,
+                             bool is_canfd_frame)
+{
+    uint16_t size = 0;
+    uint8_t len = CAN_FRAME_SIZE;
+
+    if (is_canfd_frame) {
+        len = CANFD_FRAME_SIZE;
+    }
+
+    while (size < len) {
+        if (R_RX0_ID_OFFSET + 4 * size == R_RX0_DLC_OFFSET)  {
+            g_assert_cmpint((buf_rx[size] & DLC_FD_BIT_MASK), ==,
+                            (buf_tx[size] & DLC_FD_BIT_MASK));
+        } else {
+            if (!is_canfd_frame && size == 4) {
+                break;
+            }
+
+            g_assert_cmpint(buf_rx[size], ==, buf_tx[size]);
+        }
+
+        size++;
+    }
+}
+/*
+ * Xilinx CANFD supports both CAN and CANFD frames. This test will be
+ * transferring CAN frame i.e. 8 bytes of data from CANFD0 and CANFD1 through
+ * canbus. CANFD0 initiate the data transfer to can-bus, CANFD1 receives the
+ * data. Test compares the can frame data sent from CANFD0 and received on
+ * CANFD1.
+ */
+static void test_can_data_transfer(void)
+{
+    uint32_t buf_tx[CAN_FRAME_SIZE] = { 0x5a5bb9a4, 0x80000000,
+                                        0x12345678, 0x87654321 };
+    uint32_t buf_rx[CAN_FRAME_SIZE] = { 0x00, 0x00, 0x00, 0x00 };
+    uint32_t status = 0;
+
+    generate_random_data(buf_tx, false);
+
+    QTestState *qts = qtest_init("-machine xlnx-versal-virt"
+                " -object can-bus,id=canbus"
+                " -machine canbus0=canbus"
+                " -machine canbus1=canbus"
+                );
+
+    configure_canfd(qts, MSR_NORMAL_MODE);
+
+    /* Check if CANFD0 and CANFD1 are in Normal mode. */
+    status = qtest_readl(qts, CANFD0_BASE_ADDR + R_SR_OFFSET);
+    status = status & STATUS_REG_MASK;
+    g_assert_cmpint(status, ==, STATUS_NORMAL_MODE);
+
+    status = qtest_readl(qts, CANFD1_BASE_ADDR + R_SR_OFFSET);
+    status = status & STATUS_REG_MASK;
+    g_assert_cmpint(status, ==, STATUS_NORMAL_MODE);
+
+    write_data(qts, CANFD0_BASE_ADDR, buf_tx, false);
+
+    send_data(qts, CANFD0_BASE_ADDR);
+    read_data(qts, CANFD1_BASE_ADDR, buf_rx);
+    match_rx_tx_data(buf_tx, buf_rx, false);
+
+    qtest_quit(qts);
+}
+
+/*
+ * This test will be transferring CANFD frame i.e. 64 bytes of data from CANFD0
+ * and CANFD1 through canbus. CANFD0 initiate the data transfer to can-bus,
+ * CANFD1 receives the data. Test compares the CANFD frame data sent from CANFD0
+ * with received on CANFD1.
+ */
+static void test_canfd_data_transfer(void)
+{
+    uint32_t buf_tx[CANFD_FRAME_SIZE] = { 0x5a5bb9a4, 0xf8000000 };
+    uint32_t buf_rx[CANFD_FRAME_SIZE] = { 0x00, 0x00, 0x00, 0x00 };
+    uint32_t status = 0;
+
+    generate_random_data(buf_tx, true);
+
+    QTestState *qts = qtest_init("-machine xlnx-versal-virt"
+                " -object can-bus,id=canbus"
+                " -machine canbus0=canbus"
+                " -machine canbus1=canbus"
+                );
+
+    configure_canfd(qts, MSR_NORMAL_MODE);
+
+    /* Check if CANFD0 and CANFD1 are in Normal mode. */
+    status = qtest_readl(qts, CANFD0_BASE_ADDR + R_SR_OFFSET);
+    status = status & STATUS_REG_MASK;
+    g_assert_cmpint(status, ==, STATUS_NORMAL_MODE);
+
+    status = qtest_readl(qts, CANFD1_BASE_ADDR + R_SR_OFFSET);
+    status = status & STATUS_REG_MASK;
+    g_assert_cmpint(status, ==, STATUS_NORMAL_MODE);
+
+    write_data(qts, CANFD0_BASE_ADDR, buf_tx, true);
+
+    send_data(qts, CANFD0_BASE_ADDR);
+    read_data(qts, CANFD1_BASE_ADDR, buf_rx);
+    match_rx_tx_data(buf_tx, buf_rx, true);
+
+    qtest_quit(qts);
+}
+
+/*
+ * This test is performing loopback mode on CANFD0 and CANFD1. Data sent from
+ * TX of each CANFD0 and CANFD1 are compared with RX register data for
+ * respective CANFD Controller.
+ */
+static void test_can_loopback(void)
+{
+    uint32_t buf_tx[CANFD_FRAME_SIZE] = { 0x5a5bb9a4, 0xf8000000 };
+    uint32_t buf_rx[CANFD_FRAME_SIZE] = { 0x00, 0x00, 0x00, 0x00 };
+    uint32_t status = 0;
+
+    generate_random_data(buf_tx, true);
+
+    QTestState *qts = qtest_init("-machine xlnx-versal-virt"
+                " -object can-bus,id=canbus"
+                " -machine canbus0=canbus"
+                " -machine canbus1=canbus"
+                );
+
+    configure_canfd(qts, MSR_LOOPBACK_MODE);
+
+    /* Check if CANFD0 and CANFD1 are set in correct loopback mode. */
+    status = qtest_readl(qts, CANFD0_BASE_ADDR + R_SR_OFFSET);
+    status = status & STATUS_REG_MASK;
+    g_assert_cmpint(status, ==, STATUS_LOOPBACK_MODE);
+
+    status = qtest_readl(qts, CANFD1_BASE_ADDR + R_SR_OFFSET);
+    status = status & STATUS_REG_MASK;
+    g_assert_cmpint(status, ==, STATUS_LOOPBACK_MODE);
+
+    write_data(qts, CANFD0_BASE_ADDR, buf_tx, true);
+
+    send_data(qts, CANFD0_BASE_ADDR);
+    read_data(qts, CANFD0_BASE_ADDR, buf_rx);
+    match_rx_tx_data(buf_tx, buf_rx, true);
+
+    generate_random_data(buf_tx, true);
+
+    write_data(qts, CANFD1_BASE_ADDR, buf_tx, true);
+
+    send_data(qts, CANFD1_BASE_ADDR);
+    read_data(qts, CANFD1_BASE_ADDR, buf_rx);
+    match_rx_tx_data(buf_tx, buf_rx, true);
+
+    qtest_quit(qts);
+}
+
+int main(int argc, char **argv)
+{
+    g_test_init(&argc, &argv, NULL);
+
+    qtest_add_func("/net/canfd/can_data_transfer", test_can_data_transfer);
+    qtest_add_func("/net/canfd/canfd_data_transfer", test_canfd_data_transfer);
+    qtest_add_func("/net/canfd/can_loopback", test_can_loopback);
+
+    return g_test_run();
+}
diff --git a/tests/tcg/aarch64/Makefile.target b/tests/tcg/aarch64/Makefile.target
index 0315795..3430fd3 100644
--- a/tests/tcg/aarch64/Makefile.target
+++ b/tests/tcg/aarch64/Makefile.target
@@ -21,12 +21,23 @@ config-cc.mak: Makefile
 	$(quiet-@)( \
 	    $(call cc-option,-march=armv8.1-a+sve,          CROSS_CC_HAS_SVE); \
 	    $(call cc-option,-march=armv8.1-a+sve2,         CROSS_CC_HAS_SVE2); \
+	    $(call cc-option,-march=armv8.2-a,              CROSS_CC_HAS_ARMV8_2); \
 	    $(call cc-option,-march=armv8.3-a,              CROSS_CC_HAS_ARMV8_3); \
+	    $(call cc-option,-march=armv8.5-a,              CROSS_CC_HAS_ARMV8_5); \
 	    $(call cc-option,-mbranch-protection=standard,  CROSS_CC_HAS_ARMV8_BTI); \
 	    $(call cc-option,-march=armv8.5-a+memtag,       CROSS_CC_HAS_ARMV8_MTE); \
 	    $(call cc-option,-march=armv9-a+sme,            CROSS_CC_HAS_ARMV9_SME)) 3> config-cc.mak
 -include config-cc.mak
 
+ifneq ($(CROSS_CC_HAS_ARMV8_2),)
+AARCH64_TESTS += dcpop
+dcpop: CFLAGS += -march=armv8.2-a
+endif
+ifneq ($(CROSS_CC_HAS_ARMV8_5),)
+AARCH64_TESTS += dcpodp
+dcpodp: CFLAGS += -march=armv8.5-a
+endif
+
 # Pauth Tests
 ifneq ($(CROSS_CC_HAS_ARMV8_3),)
 AARCH64_TESTS += pauth-1 pauth-2 pauth-4 pauth-5
diff --git a/tests/tcg/aarch64/dcpodp.c b/tests/tcg/aarch64/dcpodp.c
new file mode 100644
index 0000000..2cf7df2
--- /dev/null
+++ b/tests/tcg/aarch64/dcpodp.c
@@ -0,0 +1,63 @@
+/*
+ * Test execution of DC CVADP instruction.
+ *
+ * Copyright (c) 2023 Zhuojia Shen <chaosdefinition@hotmail.com>
+ * SPDX-License-Identifier: GPL-2.0-or-later
+ */
+
+#include <asm/hwcap.h>
+#include <sys/auxv.h>
+
+#include <signal.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#ifndef HWCAP2_DCPODP
+#define HWCAP2_DCPODP (1 << 0)
+#endif
+
+bool should_fail = false;
+
+static void signal_handler(int sig, siginfo_t *si, void *data)
+{
+    ucontext_t *uc = (ucontext_t *)data;
+
+    if (should_fail) {
+        uc->uc_mcontext.pc += 4;
+    } else {
+        exit(EXIT_FAILURE);
+    }
+}
+
+static int do_dc_cvadp(void)
+{
+    struct sigaction sa = {
+        .sa_flags = SA_SIGINFO,
+        .sa_sigaction = signal_handler,
+    };
+
+    sigemptyset(&sa.sa_mask);
+    if (sigaction(SIGSEGV, &sa, NULL) < 0) {
+        perror("sigaction");
+        return EXIT_FAILURE;
+    }
+
+    asm volatile("dc cvadp, %0\n\t" :: "r"(&sa));
+
+    should_fail = true;
+    asm volatile("dc cvadp, %0\n\t" :: "r"(NULL));
+    should_fail = false;
+
+    return EXIT_SUCCESS;
+}
+
+int main(void)
+{
+    if (getauxval(AT_HWCAP2) & HWCAP2_DCPODP) {
+        return do_dc_cvadp();
+    } else {
+        printf("SKIP: no HWCAP2_DCPODP on this system\n");
+        return EXIT_SUCCESS;
+    }
+}
diff --git a/tests/tcg/aarch64/dcpop.c b/tests/tcg/aarch64/dcpop.c
new file mode 100644
index 0000000..a332a80
--- /dev/null
+++ b/tests/tcg/aarch64/dcpop.c
@@ -0,0 +1,63 @@
+/*
+ * Test execution of DC CVAP instruction.
+ *
+ * Copyright (c) 2023 Zhuojia Shen <chaosdefinition@hotmail.com>
+ * SPDX-License-Identifier: GPL-2.0-or-later
+ */
+
+#include <asm/hwcap.h>
+#include <sys/auxv.h>
+
+#include <signal.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#ifndef HWCAP_DCPOP
+#define HWCAP_DCPOP (1 << 16)
+#endif
+
+bool should_fail = false;
+
+static void signal_handler(int sig, siginfo_t *si, void *data)
+{
+    ucontext_t *uc = (ucontext_t *)data;
+
+    if (should_fail) {
+        uc->uc_mcontext.pc += 4;
+    } else {
+        exit(EXIT_FAILURE);
+    }
+}
+
+static int do_dc_cvap(void)
+{
+    struct sigaction sa = {
+        .sa_flags = SA_SIGINFO,
+        .sa_sigaction = signal_handler,
+    };
+
+    sigemptyset(&sa.sa_mask);
+    if (sigaction(SIGSEGV, &sa, NULL) < 0) {
+        perror("sigaction");
+        return EXIT_FAILURE;
+    }
+
+    asm volatile("dc cvap, %0\n\t" :: "r"(&sa));
+
+    should_fail = true;
+    asm volatile("dc cvap, %0\n\t" :: "r"(NULL));
+    should_fail = false;
+
+    return EXIT_SUCCESS;
+}
+
+int main(void)
+{
+    if (getauxval(AT_HWCAP) & HWCAP_DCPOP) {
+        return do_dc_cvap();
+    } else {
+        printf("SKIP: no HWCAP_DCPOP on this system\n");
+        return EXIT_SUCCESS;
+    }
+}
diff --git a/tests/tcg/aarch64/mte-7.c b/tests/tcg/aarch64/mte-7.c
index a981de6..04974f9 100644
--- a/tests/tcg/aarch64/mte-7.c
+++ b/tests/tcg/aarch64/mte-7.c
@@ -19,8 +19,7 @@ int main(int ac, char **av)
     p = (void *)((unsigned long)p | (1ul << 56));
 
     /* Store tag in sequential granules. */
-    asm("stg %0, [%0]" : : "r"(p + 0x0ff0));
-    asm("stg %0, [%0]" : : "r"(p + 0x1000));
+    asm("stz2g %0, [%0]" : : "r"(p + 0x0ff0));
 
     /*
      * Perform an unaligned store with tag 1 crossing the pages.
diff --git a/tests/tcg/multiarch/sigbus.c b/tests/tcg/multiarch/sigbus.c
index 8134c5f..f47c739 100644
--- a/tests/tcg/multiarch/sigbus.c
+++ b/tests/tcg/multiarch/sigbus.c
@@ -6,8 +6,13 @@
 #include <endian.h>
 
 
-unsigned long long x = 0x8877665544332211ull;
-void * volatile p = (void *)&x + 1;
+char x[32] __attribute__((aligned(16))) = {
+  0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+  0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f, 0x10,
+  0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18,
+  0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20,
+};
+void * volatile p = (void *)&x + 15;
 
 void sigbus(int sig, siginfo_t *info, void *uc)
 {
@@ -60,9 +65,9 @@ int main()
      * We might as well validate the unaligned load worked.
      */
     if (BYTE_ORDER == LITTLE_ENDIAN) {
-        assert(tmp == 0x55443322);
+        assert(tmp == 0x13121110);
     } else {
-        assert(tmp == 0x77665544);
+        assert(tmp == 0x10111213);
     }
     return EXIT_SUCCESS;
 }