diff --git a/iapy.py b/iapy.py
index 491f460..5efa8ad 100644
--- a/iapy.py
+++ b/iapy.py
@@ -4,23 +4,38 @@ import time
 # get the current inferior, i.e. the connection to target
 inferior = gdb.selected_inferior()
 
+def target_break():
+    if inferior.threads()[0].is_running():
+        gdb.execute(interrupt)
+
+def target_continue():
+    t = inferior.threads()
+    if not(inferior.threads()[0].is_running()):
+        gdb.execute('continue')
+
 def target_u32read(address, endian='little'):
+    target_break()
     value = inferior.read_memory(address, 4)
     return int.from_bytes(value, byteorder=endian)
 
 def target_u32write(address, value, endian='little'):
+    target_break()
     data = value.to_bytes(4, byteorder=endian)  # Adjust byteorder as needed
     inferior.write_memory(address, data)
 
 def target_regset(name, value):
+    target_break()
     gdb.execute(f'set ${name} = {value}')
 
+
+
 def target_prepare():
     """
     Prepares the target for IAP operations.
     This implementation is an example for the LPC546xx series of chips.
     """
-
+    
+    LPC546XX_WDT_MODE = 0x4000C000
     LPC546XX_WDT_PERIOD_MAX = 0xFFFFFF
     LPC546XX_WDT_PROTECT = (1 << 4)
     LPC546XX_MAINCLKSELA = 0x40000280
@@ -28,24 +43,29 @@ def target_prepare():
     LPC546XX_AHBCLKDIV =   0x40000380
     LPC546XX_FLASHCFG  =   0x40000400
 
-    gdb.execute('interrupt')
+    gdb.execute('set mem inaccessible-by-default off')
+    target_break()
+    gdb.Breakpoint("*0x00020000")
+    gdb.Breakpoint("*0x00020001")
     # check if watchdog timer's on
     wdt_mode = target_u32read(LPC546XX_WDT_MODE)
-	# if WDT on, we can't disable it, but we may be able to set a long period
-	if (wdt_mode and not(wdt_mode & LPC546XX_WDT_PROTECT)):
+    # if WDT on, we can't disable it, but we may be able to set a long period
+    if (wdt_mode and not(wdt_mode & LPC546XX_WDT_PROTECT)):
         target_u32write(LPC546XX_WDT_CNT, LPC546XX_WDT_PERIOD_MAX)
 
     # SYSCLK: 12MHz FRO
     target_u32write(LPC546XX_MAINCLKSELA, 0)
     # use MAINCLKSELA
-	target_u32write(LPC546XX_MAINCLKSELB, 0)
+    target_u32write(LPC546XX_MAINCLKSELB, 0)
 	# div by 1
-	target_u32write(LPC546XX_AHBCLKDIV, 0)
+    target_u32write(LPC546XX_AHBCLKDIV, 0)
 	# recommended default configuration
-	target_u32write(LPC546XX_FLASHCFG, 0x1A)
+    target_u32write(LPC546XX_FLASHCFG, 0x1A)
+    
 
 def iap_command(a0, a1=0, a2=0, a3=0, a4=0):
-    gdb.execute('interrupt')
+    print(f"IAPy: Command {a0} {a1} {a2} {a3} {a4}")
+    target_break()
     # setup request
     target_u32write(0x2000ffa0, a0)
     target_u32write(0x2000ffa4, a1)
@@ -61,17 +81,25 @@ def iap_command(a0, a1=0, a2=0, a3=0, a4=0):
     target_u32write(0x2000ffd0, 0xAAAAAAAA)
 
     # call setup
+    target_break()    
     target_regset('lr', 0x00020001)
     target_regset('r0', 0x2000ffa0)
     target_regset('r1', 0x2000ffc0)
-    target_regset('pc', 0x03000205)
-    target_regset('primask', 1)
-    target_regset('msp', 0x2000ffa0)
+    # set PC
+    gdb.execute('jump *0x03000205')
+    # use PSP instead because MSP is most-likely not accessible
+    target_regset('psp', 0x2000ffa0) 
+    target_regset('control', 0x2)
 
     # execute and wait
-    gdb.execute('continue')
+    target_continue()
     time.sleep(0.5)
 
+    # restore execution to MSP
+    target_break()
+    target_regset('msp', 0x2000ffa0)
+    target_regset('control', 0x0)
+
     # gather results
     res0 = target_u32read(0x2000ffc0)
     res1 = target_u32read(0x2000ffc4)
@@ -88,7 +116,4 @@ def iap_flash_sector_erase(start_sector, end_sector, sysclk=12000):
     iap_command(50, start_sector, end_sector)
     # erase sectors
     iap_command(52, start_sector, end_sector, sysclk)
-    gdb.execute('interrupt')
-
-
-
+    target_break()