diff --git a/firmware/stm32f4xx/c64_interface.c b/firmware/stm32f4xx/c64_interface.c
index e42deeb..deb8874 100644
--- a/firmware/stm32f4xx/c64_interface.c
+++ b/firmware/stm32f4xx/c64_interface.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2021 Kim Jørgensen
+ * Copyright (c) 2019-2022 Kim Jørgensen
  *
  * This software is provided 'as-is', without any express or implied
  * warranty.  In no event will the authors be held liable for any damages
@@ -194,7 +194,8 @@ static void c64_diag_handler(void)
 {
     // Clear the interrupt flag
     TIM1->SR = ~TIM_SR_CC3IF;
-    __DSB();
+    // Ensure interrupt flag is cleared
+    (void)TIM1->SR;
 
     if (diag_state == DIAG_RUN)
     {
@@ -230,9 +231,18 @@ static void c64_diag_handler(void)
 *************************************************/
 static void c64_interface_enable_no_check(void)
 {
+    u32 dier = TIM1->DIER | TIM_DIER_CC3IE;
+    COMPILER_BARRIER();
+
+    __disable_irq();
+    // Wait for next interrupt
+    TIM1->SR = ~TIM_SR_CC3IF;
+    while (!(TIM1->SR & TIM_SR_CC3IF));
+
     // Capture/Compare 3 interrupt enable
     TIM1->SR = ~(TIM_SR_CC3IF|TIM_SR_CC4IF);
-    TIM1->DIER |= TIM_DIER_CC3IE;
+    TIM1->DIER = dier;
+    __enable_irq();
 }
 
 static void c64_interface(bool state)
diff --git a/firmware/stm32f4xx/c64_interface.h b/firmware/stm32f4xx/c64_interface.h
index 9a5059a..0768813 100644
--- a/firmware/stm32f4xx/c64_interface.h
+++ b/firmware/stm32f4xx/c64_interface.h
@@ -127,17 +127,17 @@ static void handler(void)
     /* We need to clear the interrupt flag early otherwise the next */          \
     /* interrupt may be delayed */                                              \
     TIM1->SR = ~TIM_SR_CC3IF;                                                   \
-    __DSB();                                                                    \
+    u32 phi2_high = DWT->COMP0;                                                 \
+    COMPILER_BARRIER();                                                         \
     /* Use debug cycle counter which is faster to access than timer */          \
     DWT->CYCCNT = TIM1->CNT;                                                    \
     COMPILER_BARRIER();                                                         \
-    u32 phi2_high = DWT->COMP0;                                                 \
     while (DWT->CYCCNT < phi2_high);                                            \
     u32 addr = C64_ADDR_READ();                                                 \
+    COMPILER_BARRIER();                                                         \
     u32 control = C64_CONTROL_READ();                                           \
     if (control & C64_WRITE)                                                    \
     {                                                                           \
-        COMPILER_BARRIER();                                                     \
         if (read_handler(control, addr))                                        \
         {                                                                       \
             /* Wait for phi2 to go low */                                       \
@@ -149,7 +149,6 @@ static void handler(void)
     }                                                                           \
     else                                                                        \
     {                                                                           \
-        COMPILER_BARRIER();                                                     \
         u32 data = C64_DATA_READ();                                             \
         write_handler(control, addr, data);                                     \
     }                                                                           \