diff --git a/arch/arm/soc/atmel_sam3/Kconfig b/arch/arm/soc/atmel_sam3/Kconfig
index bf4ec1ec392..2563cc5be08 100644
--- a/arch/arm/soc/atmel_sam3/Kconfig
+++ b/arch/arm/soc/atmel_sam3/Kconfig
@@ -80,4 +80,13 @@ config SOC_ATMEL_SAM3_PLLA_DIVA
 	With default of MULA == 6, and DIVA == 1,
 	PLL is running at 7 times of main clock.
 
+config SOC_ATMEL_SAM3_WAIT_MODE
+	bool "Atmel SAM3 goes to Wait mode instead of Sleep mode"
+	depends on SOC_ATMEL_SAM3_EXT_MAINCK
+	default y if DEBUG
+	help
+	For JTAG debugging CPU clock (HCLK) should not stop. In order
+	to achieve this, make CPU go to Wait mode instead of Sleep
+	mode while using external crystal oscillator for main clock.
+
 endif # SOC_ATMEL_SAM3
diff --git a/arch/arm/soc/atmel_sam3/soc.c b/arch/arm/soc/atmel_sam3/soc.c
index e9eb4948e30..16edb71d221 100644
--- a/arch/arm/soc/atmel_sam3/soc.c
+++ b/arch/arm/soc/atmel_sam3/soc.c
@@ -81,6 +81,14 @@ static ALWAYS_INLINE void clock_init(void)
 	/* Wait for main oscillator to be selected */
 	while (!(__PMC->sr & PMC_INT_MOSCSELS))
 		;
+#ifdef CONFIG_SOC_ATMEL_SAM3_WAIT_MODE
+	/*
+	 * Instruct CPU enter Wait mode instead of Sleep mode to
+	 * keep Processor Clock (HCLK) and thus be able to debug
+	 * CPU using JTAG
+	 */
+	__PMC->fsmr |= PMC_FSMR_LPM;
+#endif
 #else
 	/* Set main fast RC oscillator to 12 MHz */
 	__PMC->ckgr_mor = PMC_CKGR_MOR_KEY | PMC_CKGR_MOR_MOSCRCF_12MHZ
diff --git a/arch/arm/soc/atmel_sam3/soc.h b/arch/arm/soc/atmel_sam3/soc.h
index 4f1ace0125e..ebc282c1aa7 100644
--- a/arch/arm/soc/atmel_sam3/soc.h
+++ b/arch/arm/soc/atmel_sam3/soc.h
@@ -162,6 +162,8 @@
 #define PMC_MCKR_PLLADIV2		(1 << 12)
 #define PMC_MCKR_UPLLDIV2		(1 << 13)
 
+#define PMC_FSMR_LPM			(1 << 20)
+
 #define PMC_INT_MOSCXTS			(1 << 0)
 #define PMC_INT_LOCKA			(1 << 1)
 #define PMC_INT_MCKRDY			(1 << 3)