From 2e2136bf5fea95a25c13dbe0ef6bde511681f461 Mon Sep 17 00:00:00 2001
From: electronicayciencia <electronicayciencia@gmail.com>
Date: Sat, 25 Feb 2017 20:58:14 +0100
Subject: [PATCH] Select the source that give us the closest frequency

---
 gpio/gpio.c         |  3 ++-
 wiringPi/wiringPi.c | 35 +++++++++++++++++++++++++++++------
 wiringPi/wiringPi.h |  2 +-
 3 files changed, 32 insertions(+), 8 deletions(-)

diff --git a/gpio/gpio.c b/gpio/gpio.c
index 3327308..499da5f 100644
--- a/gpio/gpio.c
+++ b/gpio/gpio.c
@@ -85,6 +85,7 @@ char *usage = "Usage: gpio -v\n"
 	      "       gpio wb <value>\n"
 	      "       gpio usbp high/low\n"
 	      "       gpio gbr <channel>\n"
+	      "       gpio clock <pin> <freq>\n"
 	      "       gpio gbw <channel> <value>" ;	// No trailing newline needed here.
 
 
@@ -1170,7 +1171,7 @@ void doClock (int argc, char *argv [])
 
   freq = atoi (argv [3]) ;
 
-  gpioClockSet (pin, freq) ;
+  printf ("Actual frequency: %d\n", gpioClockSet (pin, freq)) ;
 }
 
 
diff --git a/wiringPi/wiringPi.c b/wiringPi/wiringPi.c
index 320f04b..5aa5230 100644
--- a/wiringPi/wiringPi.c
+++ b/wiringPi/wiringPi.c
@@ -1189,11 +1189,13 @@ void pwmSetClock (int divisor)
 
 /*
  * gpioClockSet:
- *	Set the freuency on a GPIO clock pin
+ *	Set the frequency on a GPIO clock pin
+ *	Choose source/divi combination to the closest frequency
+ *	Returns the actual frequency
  *********************************************************************************
  */
 
-void gpioClockSet (int pin, int freq)
+int gpioClockSet (int pin, int freq)
 {
   int divi, divr, divf, mash ;
   int clock_source, clock_frequency ;
@@ -1205,19 +1207,38 @@ void gpioClockSet (int pin, int freq)
   else if (wiringPiMode == WPI_MODE_PHYS)
     pin = physToGpio [pin] ;
   else if (wiringPiMode != WPI_MODE_GPIO)
-    return ;
+    return 0;
 
   mash = 0; // experimental
 
-  if (freq > CLOCK_PLLD_FREQ / 4096)
+  if (freq < CLOCK_PLLD_FREQ / 4096) {
+    clock_source    = CLOCK_INTOSC_SRC ;
+    clock_frequency = CLOCK_INTOSC_FREQ ;
+  }	
+  else if (freq > CLOCK_INTOSC_FREQ / 2)
   {
     clock_source    = CLOCK_PLLD_SRC ;
     clock_frequency = CLOCK_PLLD_FREQ ;
   }
   else
   {
-    clock_source    = CLOCK_INTOSC_SRC ;
-    clock_frequency = CLOCK_INTOSC_FREQ ;
+	int delta_intosc, delta_plld;
+
+    divi = rint((double) CLOCK_INTOSC_FREQ / freq) ;
+	delta_intosc = abs (freq - CLOCK_INTOSC_FREQ / divi);
+    
+	divi = rint((double) CLOCK_PLLD_FREQ / freq) ;
+	delta_plld = abs (freq - CLOCK_PLLD_FREQ / divi);
+
+	if (delta_plld < delta_intosc) {
+      clock_source    = CLOCK_PLLD_SRC ;
+      clock_frequency = CLOCK_PLLD_FREQ ;
+	}
+	else
+	{
+      clock_source    = CLOCK_INTOSC_SRC ;
+      clock_frequency = CLOCK_INTOSC_FREQ ;
+	}
   }
 
   if (freq > 25000000) mash = 0 ;
@@ -1261,6 +1282,8 @@ void gpioClockSet (int pin, int freq)
 
   *(clk + gpioToClkDiv [pin]) = BCM_PASSWORD | (divi << 12) | divf ; 				// Set dividers
   *(clk + gpioToClkCon [pin]) = BCM_PASSWORD | (mash << 9)  | 0x10 | clock_source ;	// Start Clock
+  
+  return (int) ((float)clock_frequency / divi + divf/1024.0) ;
 }
 
 
diff --git a/wiringPi/wiringPi.h b/wiringPi/wiringPi.h
index 1b50470..09ee364 100644
--- a/wiringPi/wiringPi.h
+++ b/wiringPi/wiringPi.h
@@ -209,7 +209,7 @@ extern unsigned int  digitalReadByte     (void) ;
 extern          void pwmSetMode          (int mode) ;
 extern          void pwmSetRange         (unsigned int range) ;
 extern          void pwmSetClock         (int divisor) ;
-extern          void gpioClockSet        (int pin, int freq) ;
+extern          int  gpioClockSet        (int pin, int freq) ;
 
 // Interrupts
 //	(Also Pi hardware specific)