2 files changed, 60 insertions, 13 deletions
diff --git a/bsp/drivers/fe300prci/fe300prci_driver.c b/bsp/drivers/fe300prci/fe300prci_driver.c
index 9a16d5d..0e555a6 100644
--- a/bsp/drivers/fe300prci/fe300prci_driver.c
+++ b/bsp/drivers/fe300prci/fe300prci_driver.c
@@ -177,7 +177,7 @@ void PRCI_use_hfxosc(uint32_t finaldiv)
 // this way.
 // It returns the actual measured CPU frequency.
 
-uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu)
+uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu, PRCI_freq_target target )
 {
 
   int hfrosctrim = 16;
@@ -186,23 +186,62 @@ uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu)
   // Ignore the first run (for icache reasons)
   uint32_t cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ); 
   cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ);
+  int prev_trim = hfrosctrim;
+  int prev_freq = cpu_freq;
   
   if (cpu_freq > F_CPU) {
-    while((cpu_freq > F_CPU) && (hfrosctrim >= 0) ){  
-	PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, hfrosctrim );
-	cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ);
-	hfrosctrim--;
-      }
+   while((cpu_freq > F_CPU) && (hfrosctrim > 0) ){
+     prev_freq = cpu_freq;
+     prev_trim = hfrosctrim;
+     hfrosctrim--;
+     PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, hfrosctrim );
+     cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ);
+   }
+   
+   // Check for over/undershoot
+   switch(target) {
+     case(PRCI_FREQ_CLOSEST):
+       if ((prev_freq - F_CPU) < (F_CPU - cpu_freq) && target == PRCI_FREQ_CLOSEST) {
+	 PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim);
+	 cpu_freq =  PRCI_measure_mcycle_freq(1000, RTC_FREQ);
+       }
+       break;
+     case(PRCI_FREQ_OVERSHOOT):
+       PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim);
+       cpu_freq =  PRCI_measure_mcycle_freq(1000, RTC_FREQ);
+       break;
+       //default:
+       // Do Nothing
+   }
+    
     return cpu_freq;
   }
-  
-  while(cpu_freq < F_CPU) {
-    if (hfrosctrim >= 0x20) {
-      break;
-    }
+   
+  while(cpu_freq < F_CPU && hfrosctrim < 0x20) {
+    prev_freq = cpu_freq;
+    prev_trim = hfrosctrim;
+    hfrosctrim ++;
     PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, hfrosctrim );
     cpu_freq = PRCI_measure_mcycle_freq(1000, RTC_FREQ);
-    hfrosctrim++;
   }
+
+  
+  // Check for over/undershoot
+  switch(target) {
+  case(PRCI_FREQ_CLOSEST):
+    if ((F_CPU - prev_freq) < (cpu_freq - F_CPU)) {
+      PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim);
+      cpu_freq =  PRCI_measure_mcycle_freq(1000, RTC_FREQ);
+    }
+    break;
+  case(PRCI_FREQ_UNDERSHOOT):
+    PRCI_use_pll(0, 0, 1, 31, 1, 1, 4, prev_trim);
+    cpu_freq =  PRCI_measure_mcycle_freq(1000, RTC_FREQ);
+    break;
+    //default:
+    // Do Nothing
+  }
+  
   return cpu_freq;
+
 }
diff --git a/bsp/drivers/fe300prci/fe300prci_driver.h b/bsp/drivers/fe300prci/fe300prci_driver.h
index 75dcf00..243e6de 100644
--- a/bsp/drivers/fe300prci/fe300prci_driver.h
+++ b/bsp/drivers/fe300prci/fe300prci_driver.h
@@ -7,6 +7,14 @@ __BEGIN_DECLS
 
 #include <unistd.h>
 
+typedef enum prci_freq_target {
+  
+  PRCI_FREQ_OVERSHOOT,
+  PRCI_FREQ_CLOSEST,
+  PRCI_FREQ_UNDERSHOOT
+
+} PRCI_freq_target;
+
 /* Measure and return the approximate frequency of the 
  * CPU, as given by measuring the mcycle counter against 
  * the mtime ticks.
@@ -55,7 +63,7 @@ void PRCI_use_default_clocks();
  * achievable with this function, and not all 
  * are guaranteed to actually work.
  */
-uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu);
+uint32_t PRCI_set_hfrosctrim_for_f_cpu(uint32_t f_cpu, PRCI_freq_target target);
 
 __END_DECLS