mcxn patch

mcxn.patch

diff --git a/probe-rs/src/vendor/nxp/sequences/mcx.rs b/probe-rs/src/vendor/nxp/sequences/mcx.rs
index a4f0e6b2..c1585f70 100644
--- a/probe-rs/src/vendor/nxp/sequences/mcx.rs
+++ b/probe-rs/src/vendor/nxp/sequences/mcx.rs
@@ -7,7 +7,6 @@ use std::{
 };
 
 use bitfield::BitMut;
-use debugmailbox::{DMCSW, DMREQUEST};
 use probe_rs_target::CoreType;
 
 use crate::{
@@ -63,6 +62,18 @@ mod debugmailbox {
         }),
         to: value => value.Request
     );
+
+    define_ap_register!(
+        name: DMRETURN,
+        address: 0x08,    // Register 2
+        fields: [
+            Response: u32
+        ],
+        from: value => Ok(DMRETURN {
+            Response: value
+        }),
+        to: value => value.Response
+    );
 }
 
 /// Debug sequences for MCX family MCUs.
@@ -72,6 +83,9 @@ pub struct MCX {
 }
 
 impl MCX {
+    const DWT_COMP0: u64 = 0xE000_1020;
+    const DWT_FUNCTION0: u64 = 0xE000_1028;
+
     /// Create a sequence handle for MCX MCUs.
     pub fn create(variant: String) -> Arc<dyn ArmDebugSequence> {
         Arc::new(Self { variant })
@@ -92,23 +106,6 @@ impl MCX {
         v.into_iter().any(|s| self.variant.starts_with(s))
     }
 
-    fn is_ap_enable(
-        &self,
-        interface: &mut dyn DapAccess,
-        mem_ap: &FullyQualifiedApAddress,
-    ) -> Result<bool, ArmError> {
-        use crate::architecture::arm::ap::CSW;
-
-        if mem_ap == &self.debug_mailbox_ap(mem_ap.dp())? {
-            // DebugMailbox AP is always enabled
-            return Ok(true);
-        }
-        let csw: CSW = interface
-            .read_raw_ap_register(mem_ap, CSW::ADDRESS)?
-            .try_into()?;
-        Ok(csw.DeviceEn)
-    }
-
     fn debug_mailbox_ap(&self, dp: DpAddress) -> Result<FullyQualifiedApAddress, ArmError> {
         if self.is_variant(Self::VARIANT_N) {
             Ok(FullyQualifiedApAddress::v1_with_dp(dp, 2))
@@ -125,42 +122,101 @@ impl MCX {
         interface: &mut dyn DapAccess,
         dp: DpAddress,
     ) -> Result<bool, ArmError> {
-        use crate::architecture::arm::{ap::IDR, dp::DPIDR};
+        use crate::architecture::arm::ap::IDR;
+        use debugmailbox::{DMCSW, DMREQUEST, DMRETURN};
 
-        tracing::info!("enable debug mailbox");
+        tracing::info!("Starting NXP debug mailbox protocol");
 
         let ap = self.debug_mailbox_ap(dp)?;
 
-        // Read APIDR
+        // Verify this is the correct debug mailbox AP
         let apidr: IDR = interface
             .read_raw_ap_register(&ap, IDR::ADDRESS)?
             .try_into()?;
-        tracing::info!("APIDR: {:?}", apidr);
-        tracing::info!("APIDR: 0x{:08X}", u32::from(apidr));
+        tracing::info!("Debug Mailbox APIDR: 0x{:08X}", u32::from(apidr));
+
         if u32::from(apidr) != 0x002A_0000 {
-            // This AP is not DebugMailbox!
-            tracing::error!("ap {:?} is not DebugMailbox!", ap);
+            tracing::error!(
+                "Expected debug mailbox APIDR 0x002A0000, got 0x{:08X}",
+                u32::from(apidr)
+            );
             return Err(ArmError::WrongApType);
         }
 
-        // Read DPIDR
-        let dpidr: DPIDR = interface
-            .read_raw_dp_register(dp, DPIDR::ADDRESS)?
-            .try_into()?;
-        tracing::info!("DPIDR: {:?}", dpidr);
-
-        tracing::info!("active DebugMailbox");
+        // Write RESYNC_REQ + CHIP_RESET_REQ (following NXP documentation)
+        tracing::info!("Writing RESYNC_REQ + CHIP_RESET_REQ (0x21) to CSW");
         interface.write_raw_ap_register(&ap, DMCSW::ADDRESS, 0x0000_0021)?;
-        thread::sleep(Duration::from_millis(30));
-        interface.read_raw_ap_register(&ap, 0x0)?;
         interface.flush()?;
 
-        tracing::info!("DebugMailbox command: start debug session");
+        // Poll CSW register until lower 16 bits are 0 (resync complete)
+        tracing::info!("Polling CSW for resynchronization completion");
+        let start = Instant::now();
+        let timeout = Duration::from_millis(2000); // Longer timeout for chip reset
+
+        loop {
+            let csw_val = interface.read_raw_ap_register(&ap, DMCSW::ADDRESS)?;
+            let lower_16_bits = csw_val & 0xFFFF;
+
+            tracing::debug!(
+                "CSW polling: 0x{:08X}, lower 16 bits: 0x{:04X}",
+                csw_val,
+                lower_16_bits
+            );
+
+            if lower_16_bits == 0 {
+                tracing::info!("Resynchronization completed successfully");
+                break;
+            }
+
+            if start.elapsed() > timeout {
+                tracing::error!(
+                    "Timeout waiting for resynchronization. CSW: 0x{:08X}",
+                    csw_val
+                );
+                return Err(ArmError::Timeout);
+            }
+
+            thread::sleep(Duration::from_millis(10));
+        }
+
+        // Write START_DBG_SESSION command (0x07) to REQUEST register
+        tracing::info!("Writing START_DBG_SESSION (0x07) to REQUEST register");
         interface.write_raw_ap_register(&ap, DMREQUEST::ADDRESS, 0x0000_0007)?;
-        thread::sleep(Duration::from_millis(30));
-        interface.read_raw_ap_register(&ap, 0x0)?;
         interface.flush()?;
 
+        // Poll RETURN register until lower 16 bits are 0 (command complete)
+        tracing::info!("Polling RETURN register for debug session start completion");
+        let start = Instant::now();
+        let timeout = Duration::from_millis(1000);
+
+        loop {
+            let return_val = interface.read_raw_ap_register(&ap, DMRETURN::ADDRESS)?;
+            let lower_16_bits = return_val & 0xFFFF;
+
+            tracing::debug!(
+                "RETURN polling: 0x{:08X}, lower 16 bits: 0x{:04X}",
+                return_val,
+                lower_16_bits
+            );
+
+            if lower_16_bits == 0 {
+                tracing::info!("Debug session started successfully");
+                break;
+            }
+
+            if start.elapsed() > timeout {
+                tracing::error!(
+                    "Timeout waiting for debug session start. RETURN: 0x{:08X}",
+                    return_val
+                );
+                return Err(ArmError::Timeout);
+            }
+
+            thread::sleep(Duration::from_millis(10));
+        }
+
+        // Verify we can access CPU0 AP
+        tracing::info!("Debug mailbox protocol completed successfully");
         Ok(true)
     }
 
@@ -168,45 +224,46 @@ impl MCX {
         &self,
         interface: &mut dyn ArmMemoryInterface,
     ) -> Result<(), ArmError> {
-        use crate::architecture::arm::armv8m::Dhcsr;
+        use crate::architecture::arm::core::armv8m::Dhcsr;
+        tracing::info!("Wait for stop after reset");
 
-        tracing::info!("wait for stop after reset");
+        thread::sleep(Duration::from_millis(10));
 
-        // Give bootloader time to do what it needs to do
-        thread::sleep(Duration::from_millis(100));
+        if interface.generic_status()?.DeviceEn {
+            let dp = interface.fully_qualified_address().dp();
+            self.enable_debug_mailbox(interface.get_dap_access()?, dp)?;
+        }
 
-        let ap = interface.fully_qualified_address();
-        let dp = ap.dp();
         let start = Instant::now();
-        while self.is_ap_enable(interface.get_dap_access()?, &ap)?
-            && start.elapsed() < Duration::from_millis(300)
-        {}
-        self.enable_debug_mailbox(interface.get_dap_access()?, dp)?;
-
-        // Halt the core in case it didn't stop at a breakpoint
-        let mut dhcsr = Dhcsr(0);
-        dhcsr.enable_write();
-        dhcsr.set_c_halt(true);
-        dhcsr.set_c_debugen(true);
-        interface.write_word_32(Dhcsr::get_mmio_address(), dhcsr.into())?;
-        interface.flush()?;
 
-        // Clear watch point
-        interface.write_word_32(0xE000_1020, 0x0)?;
-        interface.write_word_32(0xE000_1028, 0x0)?;
-        interface.write_word_32(0xE000_1030, 0x0)?;
-        interface.write_word_32(0xE000_1038, 0x0)?;
-        interface.flush()?;
+        tracing::debug!("Polling for reset");
 
-        // Clear XPSR to avoid undefined instruction fault caused by IT/ICI
-        interface.write_word_32(0xE000_EDF8, 0x0100_0000)?;
-        interface.write_word_32(0xE000_EDF4, 0x0001_0010)?;
-        interface.flush()?;
+        loop {
+            if let Ok(v) = interface.read_word_32(Dhcsr::get_mmio_address()) {
+                let dhcsr = Dhcsr(v);
 
-        // Set MSPLIM to 0
-        interface.write_word_32(0xE000_EDF8, 0x0000_0000)?;
-        interface.write_word_32(0xE000_EDF4, 0x0001_001C)?;
-        interface.flush()?;
+                // Wait until the S_RESET_ST bit is cleared on a read
+                if !dhcsr.s_reset_st() {
+                    break;
+                }
+            }
+
+            if start.elapsed() >= Duration::from_millis(500) {
+                return Err(ArmError::Timeout);
+            }
+        }
+
+        let dhcsr = Dhcsr(interface.read_word_32(Dhcsr::get_mmio_address())?);
+
+        if !dhcsr.s_halt() {
+            let mut dhcsr = Dhcsr(0);
+            dhcsr.enable_write();
+            dhcsr.set_c_halt(true);
+            dhcsr.set_c_debugen(true);
+
+            tracing::debug!("Force halt until finding a proper catch.");
+            interface.write_word_32(Dhcsr::get_mmio_address(), dhcsr.into())?;
+        }
 
         Ok(())
     }
@@ -218,11 +275,23 @@ impl ArmDebugSequence for MCX {
         interface: &mut dyn DapAccess,
         dp: DpAddress,
     ) -> Result<(), ArmError> {
-        use crate::architecture::arm::dp::{Abort, Ctrl, SelectV1};
+        use crate::architecture::arm::dp::{Abort, Ctrl, DPIDR, SelectV1};
 
-        tracing::info!("debug port start");
+        tracing::info!("MCX debug port start");
 
-        // Clear WDATAERR, STICKYORUN, STICKYCMP, STICKYERR
+        // Standard DP initialization
+        let dpidr: DPIDR = interface
+            .read_raw_dp_register(dp, DPIDR::ADDRESS)?
+            .try_into()?;
+        tracing::info!("DPIDR: 0x{:08X}", dpidr.0);
+
+        if dpidr.0 == 0 {
+            tracing::error!("Invalid DPIDR value: 0x{:08X}", dpidr.0);
+            // TODO: return a proper error
+            return Err(ArmError::NotImplemented(""));
+        }
+
+        // Clear any previous errors
         let mut abort = Abort(0);
         abort.set_wderrclr(true);
         abort.set_orunerrclr(true);
@@ -233,29 +302,25 @@ impl ArmDebugSequence for MCX {
         // Switch to DP Register Bank 0
         interface.write_dp_register(dp, SelectV1(0))?;
 
-        // Read DP CTRL/STAT Register and check if CSYSPWRUPACK and CDBGPWRUPACK bits are set
+        // Power up debug and system domains
         let ctrl: Ctrl = interface.read_dp_register(dp)?;
-        let powered_down = !ctrl.csyspwrupack() || !ctrl.cdbgpwrupack();
-
-        if !powered_down {
-            return Ok(());
-        }
-
-        // Request Debug/System Power-Up
-        let mut ctrl = Ctrl(0);
-        ctrl.set_csyspwrupreq(true);
-        ctrl.set_cdbgpwrupreq(true);
-        interface.write_dp_register(dp, ctrl)?;
+        if !ctrl.csyspwrupack() || !ctrl.cdbgpwrupack() {
+            let mut ctrl = Ctrl(0);
+            ctrl.set_csyspwrupreq(true);
+            ctrl.set_cdbgpwrupreq(true);
+            interface.write_dp_register(dp, ctrl)?;
 
-        // Wait for Power-Up request to be acknowledged
-        let start = Instant::now();
-        loop {
-            let ctrl: Ctrl = interface.read_dp_register(dp)?;
-            if ctrl.csyspwrupack() && ctrl.cdbgpwrupack() {
-                break;
-            }
-            if start.elapsed() > Duration::from_millis(1000) {
-                return Err(ArmError::Timeout);
+            // Wait for power-up acknowledgment
+            let start = Instant::now();
+            loop {
+                let ctrl: Ctrl = interface.read_dp_register(dp)?;
+                if ctrl.csyspwrupack() && ctrl.cdbgpwrupack() {
+                    break;
+                }
+                if start.elapsed() > Duration::from_millis(1000) {
+                    return Err(ArmError::Timeout);
+                }
+                thread::sleep(Duration::from_millis(10));
             }
         }
 
@@ -293,85 +358,56 @@ impl ArmDebugSequence for MCX {
                     interface.write_dp_register(dp, abort)?;
                 }
             }
+        }
 
-            let ap = FullyQualifiedApAddress::v1_with_dp(dp, 0);
-            if !self.is_ap_enable(interface, &ap)? {
-                self.enable_debug_mailbox(interface, dp)?;
+        // CRITICAL: Enable debug mailbox using NXP protocol
+        // This handles ISP mode and debug disabled scenarios
+        match self.enable_debug_mailbox(interface, dp) {
+            Ok(_) => {
+                tracing::info!("Debug mailbox enabled successfully");
+            }
+            Err(e) => {
+                tracing::error!("Failed to enable debug mailbox: {:?}", e);
+                return Err(e);
             }
         }
 
         Ok(())
     }
 
-    fn reset_system(
+    fn reset_catch_set(
         &self,
-        interface: &mut dyn ArmMemoryInterface,
+        core: &mut dyn ArmMemoryInterface,
         _core_type: CoreType,
         _debug_base: Option<u64>,
     ) -> Result<(), ArmError> {
-        use crate::architecture::arm::core::armv8m::{Aircr, Demcr, Dhcsr};
-
-        tracing::info!("reset system");
-
-        // Halt the core
-        let mut dhcsr = Dhcsr(0);
-        dhcsr.enable_write();
-        dhcsr.set_c_halt(true);
-        dhcsr.set_c_debugen(true);
-        interface.write_word_32(Dhcsr::get_mmio_address(), dhcsr.into())?;
-        interface.flush()?;
+        use crate::architecture::arm::armv8m::{Demcr, Dhcsr};
 
-        // Clear VECTOR CATCH and set TRCENA
-        let mut demcr: Demcr = interface.read_word_32(Demcr::get_mmio_address())?.into();
-        demcr.set_trcena(true);
-        interface.write_word_32(Demcr::get_mmio_address(), demcr.into())?;
-        interface.flush()?;
+        tracing::info!("reset catch set");
 
         // Set watch point
         if self.is_variant(Self::VARIANT_A0) || self.is_variant(Self::VARIANT_A1) {
-            interface.write_word_32(0xE000_1020, 0x4009_1036)?;
-            interface.write_word_32(0xE000_1028, 0xF000_0412)?;
-            interface.write_word_32(0xE000_1030, 0x4009_1040)?;
-            interface.write_word_32(0xE000_1038, 0xF000_0403)?;
+            core.write_word_32(0xE000_1020, 0x4009_1036)?;
+            core.write_word_32(0xE000_1028, 0xF000_0412)?;
+            core.write_word_32(0xE000_1030, 0x4009_1040)?;
+            core.write_word_32(0xE000_1038, 0xF000_0403)?;
+        } else if self.is_variant(Self::VARIANT_N) {
+            core.write_word_32(Self::DWT_COMP0, 0x5000_0040)?;
+            core.write_word_32(Self::DWT_FUNCTION0, 0x0000_0016)?;
         } else {
             tracing::warn!("unknwon variant, try to set watch point");
-            interface.write_word_32(0xE000_1020, 0x0000_0000)?;
-            interface.write_word_32(0xE000_1028, 0x0000_0412)?;
-            interface.write_word_32(0xE000_1030, 0x000F_FFFF)?;
-            interface.write_word_32(0xE000_1038, 0xF000_0403)?;
+            core.write_word_32(0xE000_1020, 0x0000_0000)?;
+            core.write_word_32(0xE000_1028, 0x0000_0412)?;
+            core.write_word_32(0xE000_1030, 0x000F_FFFF)?;
+            core.write_word_32(0xE000_1038, 0xF000_0403)?;
         }
-        interface.flush()?;
-
-        // Execute SYSRESETREQ via AIRCR
-        let mut aircr = Aircr(0);
-        aircr.vectkey();
-        aircr.set_sysresetreq(true);
-        let _ = interface.write_word_32(Aircr::get_mmio_address(), aircr.into());
-
-        let _ = self.wait_for_stop_after_reset(interface);
-
-        Ok(())
-    }
-
-    fn reset_catch_set(
-        &self,
-        core: &mut dyn ArmMemoryInterface,
-        _core_type: CoreType,
-        _debug_base: Option<u64>,
-    ) -> Result<(), ArmError> {
-        use crate::architecture::arm::armv8m::{Demcr, Dhcsr};
-
-        tracing::info!("reset catch set");
-
-        let mut demcr: Demcr = core.read_word_32(Demcr::get_mmio_address())?.into();
-        demcr.set_vc_corereset(false);
-        core.write_word_32(Demcr::get_mmio_address(), demcr.into())?;
         core.flush()?;
 
         let reset_vector = core.read_word_32(0x0000_0004)?;
 
         // Breakpoint on user application reset vector
         if reset_vector != 0xFFFF_FFFF {
+            tracing::info!("reset vector: {reset_vector:?}");
             core.write_word_32(0xE000_2008, reset_vector | 0x1)?;
             core.write_word_32(0xE000_2000, 0x0000_0003)?;
         }
@@ -383,7 +419,28 @@ impl ArmDebugSequence for MCX {
         }
         core.flush()?;
 
-        core.read_word_32(Dhcsr::get_mmio_address())?;
+        Ok(())
+    }
+
+    fn reset_system(
+        &self,
+        interface: &mut dyn ArmMemoryInterface,
+        _core_type: CoreType,
+        _debug_base: Option<u64>,
+    ) -> Result<(), ArmError> {
+        use crate::architecture::arm::core::armv8m::Aircr;
+
+        tracing::info!("reset system");
+
+        // Execute SYSRESETREQ via AIRCR
+        let mut aircr = Aircr(0);
+        aircr.vectkey();
+        aircr.set_sysresetreq(true);
+        let _ = interface.write_word_32(Aircr::get_mmio_address(), aircr.into());
+
+        std::thread::sleep(Duration::from_millis(100));
+
+        self.wait_for_stop_after_reset(interface)?;
 
         Ok(())
     }
@@ -401,9 +458,21 @@ impl ArmDebugSequence for MCX {
         core.write_word_32(0xE000_2008, 0x0000_0000)?;
         core.write_word_32(0xE000_2000, 0x0000_0002)?;
 
+        if self.is_variant(Self::VARIANT_A0) || self.is_variant(Self::VARIANT_A1) {
+            core.write_word_32(0xE000_1020, 0x0000_0000)?;
+            core.write_word_32(0xE000_1028, 0x0000_0000)?;
+            core.write_word_32(0xE000_1030, 0x0000_0000)?;
+            core.write_word_32(0xE000_1038, 0x0000_0000)?;
+        } else if self.is_variant(Self::VARIANT_N) {
+            core.write_word_32(Self::DWT_COMP0, 0x0000_0000)?;
+            core.write_word_32(Self::DWT_FUNCTION0, 0x0000_0000)?;
+        }
+        core.flush()?;
+
         let mut demcr: Demcr = core.read_word_32(Demcr::get_mmio_address())?.into();
         demcr.set_vc_corereset(false);
         core.write_word_32(Demcr::get_mmio_address(), demcr.into())?;
+        core.flush()?;
 
         Ok(())
     }