From 2de41265e3306a9bf9574b15fb431b32150c20b2 Mon Sep 17 00:00:00 2001
From: isiora <none@example.com>
Date: Wed, 16 Aug 2017 21:37:20 +0000
Subject: Added debug asserts. Fixed slow clock select. Added mainf measurement
 Tested XT main clock.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@10438 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/hal/ports/SAMA/SAMA5D2x/hal_lld.c    | 17 +++++++++++++----
 os/hal/ports/SAMA/SAMA5D2x/hal_st_lld.c |  6 ++++--
 2 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/os/hal/ports/SAMA/SAMA5D2x/hal_lld.c b/os/hal/ports/SAMA/SAMA5D2x/hal_lld.c
index e71e082ed..7cd4efa53 100644
--- a/os/hal/ports/SAMA/SAMA5D2x/hal_lld.c
+++ b/os/hal/ports/SAMA/SAMA5D2x/hal_lld.c
@@ -75,7 +75,7 @@ void hal_lld_init(void) {
  */
 void sama_clock_init(void) {
 #if !SAMA_NO_INIT
-  uint32_t mor, pllar, mckr;
+  uint32_t mor, pllar, mckr, mainf;
   /* Disabling PMC write protection. */
   pmcDisableWP();
 
@@ -107,7 +107,7 @@ void sama_clock_init(void) {
   while (!(PMC->PMC_SR & PMC_SR_MCKRDY))
     ;                                       /* Waits until Master Clock is stable.*/
 
-  /* Switching Main Frequency Source to MOSCRC. */
+  /* Counter Clock Source to MOSCRC. */
   PMC->CKGR_MCFR &= ~CKGR_MCFR_CCSS;
 
 }
@@ -116,12 +116,23 @@ void sama_clock_init(void) {
    * Main oscillator configuration block.
    */
 {
+  /* Setting Slow clock source. */
+  SCKC->SCKC_CR = SAMA_OSC_SEL;
+  while ((SAMA_OSC_SEL && !(PMC->PMC_SR & PMC_SR_OSCSELS)) ||
+        (!SAMA_OSC_SEL &&  (PMC->PMC_SR & PMC_SR_OSCSELS)))
+    ;                                       /* Waits until MOSCxxS switch is done.*/
   mor = PMC->CKGR_MOR | CKGR_MOR_KEY_PASSWD;
 #if SAMA_MOSCXT_ENABLED
   mor |= CKGR_MOR_MOSCXTEN;
   PMC->CKGR_MOR = mor;
   while (!(PMC->PMC_SR & PMC_SR_MOSCXTS))
     ;                                       /* Waits until MOSCXT is stable.*/
+  /* Counter Clock Source to MOSCXT. */
+  PMC->CKGR_MCFR |= CKGR_MCFR_CCSS;
+  PMC->CKGR_MCFR |= CKGR_MCFR_RCMEAS;
+  while (!(PMC->CKGR_MCFR & CKGR_MCFR_MAINFRDY))
+    ;
+  mainf = CKGR_MCFR_MAINF(PMC->CKGR_MCFR);
 #else
   mor &= ~CKGR_MOR_MOSCXTEN;
   PMC->CKGR_MOR = mor;
@@ -176,8 +187,6 @@ void sama_clock_init(void) {
   while (!(PMC->PMC_SR & PMC_SR_MCKRDY))
     ;                                       /* Waits until MCK is stable.   */
 
-  /* Setting Slow clock source. */
-  SCKC->SCKC_CR = SAMA_OSC_SEL;
 }
 
   /* Enabling write protection.  */
diff --git a/os/hal/ports/SAMA/SAMA5D2x/hal_st_lld.c b/os/hal/ports/SAMA/SAMA5D2x/hal_st_lld.c
index eabde5c4a..bb62f242b 100644
--- a/os/hal/ports/SAMA/SAMA5D2x/hal_st_lld.c
+++ b/os/hal/ports/SAMA/SAMA5D2x/hal_st_lld.c
@@ -61,11 +61,13 @@
  * @isr
  */
 OSAL_IRQ_HANDLER(PIT_Handler) {
-
+  uint32_t ivr;
   OSAL_IRQ_PROLOGUE();
 
   osalSysLockFromISR();
-  (void)PIT->PIT_PIVR;    /* acknowledge PIT interrupt */
+  ivr = PIT->PIT_PIVR;    /* acknowledge PIT interrupt */
+  osalDbgAssert((ivr & PIT_PIVR_PICNT_Msk) == (1 << PIT_PIVR_PICNT_Pos),
+      "check for lost tick");
   osalOsTimerHandlerI();
   osalSysUnlockFromISR();
   aicAckInt();
-- 
cgit v1.2.3