diff --git a/os/hal/platforms/SAM4L/hal_lld.c b/os/hal/platforms/SAM4L/hal_lld.c
index edc40ecb0..095195706 100644
--- a/os/hal/platforms/SAM4L/hal_lld.c
+++ b/os/hal/platforms/SAM4L/hal_lld.c
@@ -53,6 +53,34 @@
 /* Driver local functions.                                                   */
 /*===========================================================================*/
 
+void sam_set_optimal_flash_mode(void) {
+
+  if (SAM_PMCON_PS == SAM_PMCON_PS0) {
+    /* Scalability Level 0.*/
+    if (SAM_CPU_CLK > SAM_FLASH0WS_MAX) {
+      /* One wait state required.*/
+      if (SAM_CPU_CLK <= SAM_FLASH1WS_MAX) {
+        /* Fast read mode disabled.*/
+      }
+      else {
+        /* Fast read mode enabled.*/
+      }
+    }
+    else {
+      /* No wait state required.*/
+    }
+  }
+  else {
+    /* Scalability Level 1.*/
+    if (SAM_CPU_CLK > SAM_FLASH0WS_MAX) {
+
+    }
+    else {
+
+    }
+  }
+}
+
 /*===========================================================================*/
 /* Driver interrupt handlers.                                                */
 /*===========================================================================*/
@@ -130,23 +158,30 @@ void sam_clock_init(void) {
 #if SAM_MCCTRL_MCSEL == SAM_MCSEL_RCSYS
   /* Nothing to do, already running from SYSIRC.*/
 #endif
+
 #if SAM_MCCTRL_MCSEL == SAM_MCSEL_OSC0
   SAM_SCIF_UNLOCK(&SCIF->SCIF_OSCCTRL0);
   SCIF->SCIF_OSCCTRL0 = SAM_OSCCTRL_GAIN    | SAM_OSCCTRL_MODE |
                         SAM_OSCCTRL_STARTUP | SAM_OSCCTRL_OSCEN;
   while (!(SCIF->SCIF_PCLKSR & SCIF_PCLKSR_OSC0RDY))
     ;
-#endif
+  sam_set_optimal_flash_mode();
+#endif /* SAM_MCCTRL_MCSEL == SAM_MCSEL_RCSYS */
+
 #if SAM_MCCTRL_MCSEL == SAM_MCSEL_PLL
-#endif
+#endif /* SAM_MCCTRL_MCSEL == SAM_MCSEL_PLL */
+
 #if SAM_MCCTRL_MCSEL == SAM_MCSEL_DFLL
-#endif
+#endif /* SAM_MCCTRL_MCSEL == SAM_MCSEL_DFLL */
+
 #if SAM_MCCTRL_MCSEL == SAM_MCSEL_RC80M
-#endif
+#endif /* SAM_MCCTRL_MCSEL == SAM_MCSEL_RC80M */
+
 #if SAM_MCCTRL_MCSEL == SAM_MCSEL_RCFAST
-#endif
+#endif /* SAM_MCCTRL_MCSEL == SAM_MCSEL_RCFAST */
+
 #if SAM_MCCTRL_MCSEL == SAM_MCSEL_RC1M
-#endif
+#endif /* SAM_MCCTRL_MCSEL == SAM_MCSEL_RC1M */
 
 #endif /* SAM_NO_INIT */
 }
diff --git a/os/hal/platforms/SAM4L/hal_lld.h b/os/hal/platforms/SAM4L/hal_lld.h
index 879d31bde..db1e0a98c 100644
--- a/os/hal/platforms/SAM4L/hal_lld.h
+++ b/os/hal/platforms/SAM4L/hal_lld.h
@@ -251,7 +251,7 @@
 #endif
 
 /**
- * @brief   Power mode.
+ * @brief   Power Scalability mode.
  */
 #if !defined(SAM_PMCON_PS) || defined(__DOXYGEN__)
 #define SAM_PMCON_PS                        SAM_PMCON_PS0
@@ -321,6 +321,9 @@
 #define SAM_PBBCLK_MAX                      50000000
 #define SAM_PBCCLK_MAX                      50000000
 #define SAM_PBDCLK_MAX                      50000000
+#define SAM_FLASH0WS_MAX                    18000000
+#define SAM_FLASH1WS_MAX                    36000000
+#define SAM_FLASH1WS_FWU_MAX                12000000
 
 #elif SAM_PMCON_PS == SAM_PMCON_PS1
 #define SAM_CPUCLK_MAX                      12000000
@@ -328,6 +331,9 @@
 #define SAM_PBBCLK_MAX                      12000000
 #define SAM_PBCCLK_MAX                      12000000
 #define SAM_PBDCLK_MAX                      12000000
+#define SAM_FLASH0WS_MAX                    8000000
+#define SAM_FLASH1WS_MAX                    12000000
+#define SAM_FLASH1WS_FWU_MAX                0
 
 #else
 #error "invalid SAM_PMCON_PS value specified"
diff --git a/os/hal/platforms/SAM4L/platform.mk b/os/hal/platforms/SAM4L/platform.mk
index 26adf9a54..d97d0d018 100644
--- a/os/hal/platforms/SAM4L/platform.mk
+++ b/os/hal/platforms/SAM4L/platform.mk
@@ -1,6 +1,7 @@
 # List of all the STM32F3xx platform files.
-PLATFORMSRC = ${CHIBIOS}/os/hal/platforms/SAM4L/hal_lld.c
+PLATFORMSRC = ${CHIBIOS}/os/hal/platforms/SAM4L/hal_lld.c \
+              ${CHIBIOS}/os/hal/platforms/SAM4L/asf/flashcalw.c
 
 # Required include directories
-PLATFORMINC = ${CHIBIOS}/os/hal/platforms/SAM4L
-
+PLATFORMINC = ${CHIBIOS}/os/hal/platforms/SAM4L \
+              ${CHIBIOS}/os/hal/platforms/SAM4L/asf