diff --git a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED/debug/RT-SAMA5D2-XPLAINED (Load and Run).launch b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED/debug/RT-SAMA5D2-XPLAINED (Load and Run).launch
index ea0aca12c..b9d906be2 100644
--- a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED/debug/RT-SAMA5D2-XPLAINED (Load and Run).launch	
+++ b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED/debug/RT-SAMA5D2-XPLAINED (Load and Run).launch	
@@ -6,7 +6,7 @@
 <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.doReset" value="false"/>
 <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.imageFileName" value=""/>
 <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.imageOffset" value=""/>
-<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.initCommands" value="mon reg cpsr = 0xd3&#13;&#10;set *0xF8048000 = 0xA5000004&#13;&#10;set *0x00A00100 = 0&#13;&#10;set *0xF8048044 = 0x00008000&#13;&#10;mon cp15 1 0 0 0 = 0x00C50078&#13;&#10;set *0xF0014004 = 0x4&#13;&#10;set *0xF0014014 = 1&lt;&lt;13&#13;&#10;"/>
+<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.initCommands" value="mon reg cpsr = 0xd3&#13;&#10;set *0xF8048000 = 0xA5000004&#13;&#10;set *0x00A00100 = 0&#13;&#10;set *0xF8048044 = 0x00008000&#13;&#10;mon cp15 1 0 0 0 = 0x00C50078&#13;&#10;set *0xF0014004 = 0x4&#13;&#10;set *0xF0014014 = 1&lt;&lt;13"/>
 <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.ipAddress" value="localhost"/>
 <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.jtagDevice" value="Generic TCP/IP"/>
 <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.loadImage" value="true"/>
@@ -17,7 +17,7 @@
 <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setPcRegister" value="false"/>
 <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setResume" value="true"/>
 <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.setStopAt" value="true"/>
-<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.stopAt" value="sama_clock_init"/>
+<stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.stopAt" value="main"/>
 <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.symbolsFileName" value=""/>
 <stringAttribute key="org.eclipse.cdt.debug.gdbjtag.core.symbolsOffset" value=""/>
 <booleanAttribute key="org.eclipse.cdt.debug.gdbjtag.core.useFileForImage" value="false"/>
diff --git a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED/mcuconf.h b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED/mcuconf.h
index 3cf952cd7..0b8a9c79b 100644
--- a/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED/mcuconf.h
+++ b/demos/ATSAMA5D2/RT-SAMA5D2-XPLAINED/mcuconf.h
@@ -22,14 +22,14 @@
 /*
  * HAL driver system settings.
  */
-#define SAMA_NO_INIT                        TRUE
+#define SAMA_NO_INIT                        FALSE
 #define SAMA_MOSCRC_ENABLED                 TRUE
 #define SAMA_MOSCXT_ENABLED                 FALSE
 #define SAMA_MOSC_SEL                       SAMA_MOSC_MOSCRC
 #define SAMA_OSC_SEL                        SAMA_OSC_OSCXT
 #define SAMA_MCK_SEL                        SAMA_MCK_PLLA_CLK
-#define SAMA_MCK_PRES                       SAMA_MCK_PRE_DIV2
-#define SAMA_MCK_MDIV                       SAMA_MCK_MDIV_DIV1
+#define SAMA_MCK_PRES_VALUE                 1
+#define SAMA_MCK_MDIV_VALUE                 3
 #define SAMA_PLLA_MUL_VALUE                 83
-#define SAMA_PLLADIV2_EN                    FALSE
+#define SAMA_PLLADIV2_EN                    TRUE
 #endif /* MCUCONF_H */
diff --git a/os/hal/boards/ATSAMA5D2_XULT/board.h b/os/hal/boards/ATSAMA5D2_XULT/board.h
index 72eec3a4b..8f4f7a662 100644
--- a/os/hal/boards/ATSAMA5D2_XULT/board.h
+++ b/os/hal/boards/ATSAMA5D2_XULT/board.h
@@ -30,12 +30,12 @@
 /*
  * Board oscillators-related settings.
  */
-#if !defined(SAM_SLCK)
-#define SAM_SLCK                    32768U
+#if !defined(SAMA_OSCXTCLK)
+#define SAMA_OSCXTCLK               32768U
 #endif
 
-#if !defined(SAM_MAINCK)
-#define SAM_MAINCK                  12000000U
+#if !defined(SAMA_MOSCXTCLK)
+#define SAMA_MOSCXTCLK              12000000U
 #endif
 
 /*
diff --git a/os/hal/ports/SAMA/SAMA5D2x/hal_lld.c b/os/hal/ports/SAMA/SAMA5D2x/hal_lld.c
index 70fcf11ce..114c7dc4a 100644
--- a/os/hal/ports/SAMA/SAMA5D2x/hal_lld.c
+++ b/os/hal/ports/SAMA/SAMA5D2x/hal_lld.c
@@ -88,16 +88,26 @@ void sama_clock_init(void) {
     ;                                       /* Waits until MOSCRC is stable.*/
 
   /* Switching Main Oscillator Source to MOSRC. */
-  mor = PMC->CKGR_MOR;
+  mor = PMC->CKGR_MOR | CKGR_MOR_KEY_PASSWD;
   mor &= ~CKGR_MOR_MOSCSEL;
-  mor |= (SAMA_MOSC_MOSCRC | CKGR_MOR_KEY_PASSWD);
+  mor |= SAMA_MOSC_MOSCRC;
   PMC->CKGR_MOR = mor;
 
+  while (!(PMC->PMC_SR & PMC_SR_MOSCSELS))
+    ;                                       /* Waits until MOSCSEL has changed.*/
+
   /* Switching Master Clock source to Main Clock. */
   mckr = PMC->PMC_MCKR;
   mckr &= ~PMC_MCKR_CSS_Msk;
   mckr |= PMC_MCKR_CSS_MAIN_CLK;
   PMC->PMC_MCKR = mckr;
+
+  while (!(PMC->PMC_SR & PMC_SR_MCKRDY))
+    ;                                       /* Waits until Master Clock is stable.*/
+
+  /* Switching Main Frequency Source to MOSCRC. */
+  PMC->CKGR_MCFR &= ~CKGR_MCFR_CCSS;
+
 }
 
   /*
@@ -152,9 +162,13 @@ void sama_clock_init(void) {
   PMC->PMC_MCKR = mckr;
   while (!(PMC->PMC_SR & PMC_SR_MCKRDY))
     ;                                       /* Waits until MCK is stable.   */
-  mckr = SAMA_MCK_PRES | SAMA_MCK_MDIV | SAMA_MCK_SEL;
+
+  mckr &= ~(PMC_MCKR_PRES_Msk | PMC_MCKR_MDIV_Msk);
+  mckr |= (SAMA_MCK_PRES | SAMA_MCK_MDIV);
 #if SAMA_PLLADIV2_EN
   mckr |= PMC_MCKR_PLLADIV2;
+#else
+  mckr &= ~PMC_MCKR_PLLADIV2;
 #endif
   PMC->PMC_MCKR = mckr;
   while (!(PMC->PMC_SR & PMC_SR_MCKRDY))
diff --git a/os/hal/ports/SAMA/SAMA5D2x/hal_lld.h b/os/hal/ports/SAMA/SAMA5D2x/hal_lld.h
index 7fb03d864..0a7b8aae5 100644
--- a/os/hal/ports/SAMA/SAMA5D2x/hal_lld.h
+++ b/os/hal/ports/SAMA/SAMA5D2x/hal_lld.h
@@ -20,6 +20,7 @@
  * @pre     This module requires the following macros to be defined in the
  *          @p board.h file:
  *          - SAMA_MOSCXTCLK.
+ *          - SAMA_OSCXTCLK
  *          .
  *          One of the following macros must also be defined:
  *          - SAMA5D21, SAMA5D22, SAMA5D23, SAMA5D24, SAMA5D25, SAMA5D26,
@@ -95,14 +96,14 @@
 #define SAMA_PCK_MIN            250000000
 
 /**
- * @brief   Maximum processor clock frequency.
+ * @brief   Maximum master clock frequency.
  */
-#define SAMA_MCK_MAX            125000000
+#define SAMA_MCK_MAX            166000000
 
 /**
- * @brief   Minimum processor clock frequency.
+ * @brief   Minimum master clock frequency.
  */
-#define SAMA_MCK_MIN            166000000
+#define SAMA_MCK_MIN            125000000
 
 /**
  * @brief   Maximum Main Crystal Oscillator clock frequency.
@@ -114,11 +115,6 @@
  */
 #define SAMA_MOSCXTCLK_MIN      8000000
 
-/**
- * @brief   Crystal 32 clock frequency.
- */
-#define SAMA_OSCXTCLK           32768
-
 /**
  * @brief   Maximum PLLs input clock frequency.
  */
@@ -242,15 +238,15 @@
 /**
  * @brief   Master clock prescaler.
  */
-#if !defined(SAMA_MCK_PRES) || defined(__DOXYGEN__)
-#define SAMA_MCK_PRES                       SAMA_MCK_PRE_DIV2
+#if !defined(SAMA_MCK_PRES_VALUE) || defined(__DOXYGEN__)
+#define SAMA_MCK_PRES_VALUE                 1
 #endif
 
 /**
  * @brief   Master clock divider.
  */
-#if !defined(SAMA_MCK_MDIV) || defined(__DOXYGEN__)
-#define SAMA_MCK_MDIV                       SAMA_MCK_MDIV_DIV1
+#if !defined(SAMA_MCK_MDIV_VALUE) || defined(__DOXYGEN__)
+#define SAMA_MCK_MDIV_VALUE                 3
 #endif
 
 /**
@@ -264,7 +260,7 @@
  * @brief   PLLADIV2 clock divider.
  */
 #if !defined(SAMA_PLLADIV2_EN) || defined(__DOXYGEN__)
-#define SAMA_PLLADIV2_EN                    FALSE
+#define SAMA_PLLADIV2_EN                    TRUE
 #endif
 /** @} */
 
@@ -279,6 +275,18 @@
 #error "Using a wrong mcuconf.h file, SAMA5D2x_MCUCONF not defined"
 #endif
 
+/**
+ * @brief   Slow clock value.
+ */
+/* Main oscillator is fed by internal RC. */
+#if (SAMA_OSC_SEL == SAMA_OSC_OSCRC) || defined(__DOXYGEN__)
+#define SAMA_SLOW_CLK                       SAMA_OSCRCCLK
+#elif (SAMA_OSC_SEL == SAMA_OSC_OSCXT)
+#define SAMA_SLOW_CLK                       SAMA_OSCXTCLK
+#else
+#error "Wrong SAMA_OSC_SEL value."
+#endif
+
 /**
  * @brief   MAIN clock value.
  */
@@ -324,9 +332,9 @@
 
 /**
  * @brief   PLLA input clock frequency.
- * @todo    Condider to add DIVA to this. On SAMA5D27 DIVA is a nonsense since
+ * @todo    Consider to add DIVA to this. On SAMA5D27 DIVA is a nonsense since
  *          it could be only 1 or 0 whereas 0 means PLLA disabled. This could
- *          be useful for other chip beloging to this family
+ *          be useful for other chip belonging to this family
  */
 #define SAMA_PLLACLKIN                      SAMA_MAIN_CLK
 
@@ -354,10 +362,80 @@
 #define SAMA_PLLADIV                        0
 #endif
 
+/**
+ * @brief   Master Clock prescaler.
+ */
+#if (SAMA_MCK_PRES_VALUE == 1) || defined(__DOXYGEN__)
+#define SAMA_MCK_PRES                       SAMA_MCK_PRE_DIV1
+#elif (SAMA_MCK_PRES_VALUE == 2)
+#define SAMA_MCK_PRES                       SAMA_MCK_PRE_DIV2
+#elif (SAMA_MCK_PRES_VALUE == 4)
+#define SAMA_MCK_PRES                       SAMA_MCK_PRE_DIV4
+#elif (SAMA_MCK_PRES_VALUE == 8)
+#define SAMA_MCK_PRES                       SAMA_MCK_PRE_DIV8
+#elif (SAMA_MCK_PRES_VALUE == 16)
+#define SAMA_MCK_PRES                       SAMA_MCK_PRE_DIV16
+#elif (SAMA_MCK_PRES_VALUE == 32)
+#define SAMA_MCK_PRES                       SAMA_MCK_PRE_DIV32
+#elif (SAMA_MCK_PRES_VALUE == 64)
+#define SAMA_MCK_PRES                       SAMA_MCK_PRE_DIV64
+#else
+#error "Wrong SAMA_MCK_PRES_VALUE."
+#endif
+
+/**
+ * @brief   Master Clock divider.
+ */
+#if (SAMA_MCK_MDIV_VALUE == 1) || defined(__DOXYGEN__)
+#define SAMA_MCK_MDIV                       SAMA_MCK_MDIV_DIV1
+#elif (SAMA_MCK_MDIV_VALUE == 2)
+#define SAMA_MCK_MDIV                       SAMA_MCK_MDIV_DIV2
+#elif (SAMA_MCK_MDIV_VALUE == 3)
+#define SAMA_MCK_MDIV                       SAMA_MCK_MDIV_DIV3
+#elif (SAMA_MCK_MDIV_VALUE == 4)
+#define SAMA_MCK_MDIV                       SAMA_MCK_MDIV_DIV4
+#else
+#error "Wrong SAMA_MCK_MDIV_VALUE."
+#endif
+
+/* Check on MDIV and PLLADIV2 value. */
 #if (SAMA_MCK_MDIV == SAMA_MCK_MDIV_DIV3) && !SAMA_PLLADIV2_EN
 #error "PLLADIV2 must be always enabled when Main Clock Divider is 3"
 #endif
 
+/**
+ * @brief   Processor Clock frequency.
+ */
+#if (SAMA_MCK_SEL == SAMA_MCK_SLOW_CLK) || defined(__DOXYGEN__)
+#define SAMA_PCKOUT                         (SAMA_SLOW_CLK / SAMA_MCK_PRES_VALUE)
+#elif (SAMA_MCK_SEL == SAMA_MCK_MAIN_CLK)
+#define SAMA_PCKOUT                         (SAMA_MAIN_CLK / SAMA_MCK_PRES_VALUE)
+#elif (SAMA_MCK_SEL == SAMA_MCK_PLLA_CLK)
+#if SAMA_PLLADIV2_EN
+#define SAMA_PCKOUT                         (SAMA_PLLACLKOUT / SAMA_MCK_PRES_VALUE / 2)
+#else
+#define SAMA_PCKOUT                         (SAMA_PLLACLKOUT / SAMA_MCK_PRES_VALUE)
+#endif
+#elif (SAMA_MCK_SEL == SAMA_MCK_UPLL_CLK)
+#error "UPLL still unsupported"
+#else
+#error "Wrong SAMA_MCK_SEL."
+#endif
+
+/**
+ * @brief   Master Clock frequency.
+ */
+#define SAMA_MCKOUT                         (SAMA_PCKOUT / SAMA_MCK_MDIV_VALUE)
+
+/* Checks on Processor Clock crystal range. */
+#if (SAMA_PCKOUT > SAMA_PCK_MAX) || (SAMA_PCKOUT < SAMA_PCK_MIN)
+#error "Processor clock frequency out of range."
+#endif
+
+/* Checks on Master Clock crystal range. */
+#if (SAMA_MCKOUT > SAMA_MCK_MAX) || (SAMA_MCKOUT < SAMA_MCK_MIN)
+#error "Master clock frequency out of range."
+#endif
 /*===========================================================================*/
 /* Driver data structures and types.                                         */
 /*===========================================================================*/