diff --git a/arch/arm/core/isr_wrapper.S b/arch/arm/core/isr_wrapper.S
index 68a0e846192..83556f8337c 100644
--- a/arch/arm/core/isr_wrapper.S
+++ b/arch/arm/core/isr_wrapper.S
@@ -110,18 +110,16 @@ _idle_state_cleared:
 
 	ldm r1!,{r0,r3}	/* arg in r0, ISR in r3 */
 #ifdef CONFIG_EXECUTION_BENCHMARKING
-#if defined(CONFIG_ARMV6_M)
-	push {r3}
-#endif
+	stm sp!,{r0-r3} /* Save r0 to r4 into stack */
 	push {lr}
 	bl read_timer_end_of_isr
 #if defined(CONFIG_ARMV6_M)
 	pop {r3}
 	mov lr,r3
-	pop {r3}
 #else
 	pop {lr}
 #endif
+	ldm sp!,{r0-r3} /* Restore r0 to r4 regs */
 #endif
 	blx r3		/* call ISR */
 
diff --git a/arch/arm/core/swap.S b/arch/arm/core/swap.S
index d05dfbc57ec..22d5bbac1c8 100644
--- a/arch/arm/core/swap.S
+++ b/arch/arm/core/swap.S
@@ -185,21 +185,18 @@ _thread_irq_disabled:
     msr PSP, ip
 
 #ifdef CONFIG_EXECUTION_BENCHMARKING
-#if defined(CONFIG_ARMV6_M)
-    push {r3}
-#endif
+    stm sp!,{r0-r3} /* Save regs r0 to r4 on stack */
     push {lr}
-
     bl read_timer_end_of_swap
 
 #if defined(CONFIG_ARMV6_M)
     pop {r3}
     mov lr,r3
-    pop {r3}
 #else
     pop {lr}
-#endif
-#endif
+#endif /* CONFIG_ARMV6_M */
+    ldm sp!,{r0-r3} /* Load back regs ro to r4 */
+#endif /* CONFIG_EXECUTION_BENCHMARKING */
 
     /* exc return */
     bx lr
@@ -363,19 +360,15 @@ _oops:
 SECTION_FUNC(TEXT, __swap)
 
 #ifdef CONFIG_EXECUTION_BENCHMARKING
-#if defined(CONFIG_ARMV6_M)
-    push {r3}
-#endif
     push {lr}
     bl read_timer_start_of_swap
 #if defined(CONFIG_ARMV6_M)
     pop {r3}
     mov lr,r3
-    pop {r3}
 #else
     pop {lr}
-#endif
-#endif
+#endif /* CONFIG_ARMV6_M */
+#endif /* CONFIG_EXECUTION_BENCHMARKING */
     ldr r1, =_kernel
     ldr r2, [r1, #_kernel_offset_to_current]
     str r0, [r2, #_thread_offset_to_basepri]