From 92ee843b70b0c712e2363a7d141e0ef3a41ad836 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Arnim=20L=C3=A4uger?= <arniml@users.sourceforge.net>
Date: Sat, 6 Sep 2008 12:00:25 +0000
Subject: [PATCH] fixed calibration loop

git-svn-id: https://urjtag.svn.sourceforge.net/svnroot/urjtag/trunk@1352 b68d4a1b-bc3d-0410-92ed-d4ac073336b7
---
 jtag/ChangeLog               |  2 ++
 jtag/src/tap/cable/generic.c | 32 +++++++++++++++++---------------
 2 files changed, 19 insertions(+), 15 deletions(-)

diff --git a/jtag/ChangeLog b/jtag/ChangeLog
index 39142189..df2f8cc5 100644
--- a/jtag/ChangeLog
+++ b/jtag/ChangeLog
@@ -1,5 +1,7 @@
 2008-09-06  Arnim Laeuger  <arniml@users.sourceforge.net>
 
+   * src/tap/cable/generic.c: fixed calibration loop
+
    * src/svf/svf.c: require frequency setting from cable for
      RUNTEST MIN SEC command, fixes
      [ 2090432 ] SVF programming problem with "x SEC" arg to runtest
diff --git a/jtag/src/tap/cable/generic.c b/jtag/src/tap/cable/generic.c
index d178bfdf..6008b920 100644
--- a/jtag/src/tap/cable/generic.c
+++ b/jtag/src/tap/cable/generic.c
@@ -357,26 +357,24 @@ generic_set_frequency( cable_t *cable, uint32_t new_frequency )
 {
 	if (new_frequency == 0) {
 		cable->delay = 0;
-		cable-> frequency = 0;
+		cable->frequency = 0;
 	} else {
 		const double tolerance = 0.1;
-		uint32_t loops;
+		const uint32_t loops = 2000;
 		uint32_t delay = cable->delay;
 		uint32_t frequency = cable->frequency;
 
-		printf("requested frequency %u, now calibrating delay loop\n", new_frequency);
+		if (new_frequency > (1.0 - tolerance) * frequency &&
+		    new_frequency < (1.0 + tolerance) * frequency)
+			return;
 
-		if (delay == 0) {
-			delay = 1000;
-			loops = 10000;
-		} else {
-			loops = 3 * frequency;
-		}
+		printf("requested frequency %u, now calibrating delay loop\n", new_frequency);
 
 		while (1) {
 			uint32_t i, new_delay;
 			long double start, end, real_frequency;
 
+			cable->delay = delay;
 			start = frealtime();	
 			for (i = 0; i < loops; ++i) {
 				cable->driver->clock(cable, 0, 0, 1);
@@ -388,11 +386,11 @@ generic_set_frequency( cable_t *cable, uint32_t new_frequency )
 			printf("new real frequency %Lg, delay %u\n", 
 			       real_frequency, delay);
 
-			loops = 3 * fmax(real_frequency, new_frequency);
 			new_delay = (long double)delay * real_frequency / new_frequency;
 
 			if (real_frequency >= (1.0 - tolerance)*new_frequency) {
 				if (real_frequency <= (1.0 + tolerance)*new_frequency) {
+					frequency = real_frequency;
 					break;
 				}
 				if (new_delay > delay) {
@@ -400,16 +398,20 @@ generic_set_frequency( cable_t *cable, uint32_t new_frequency )
 				} else {
 					delay++;
 				}
+
 			} else {
-				if (new_delay < delay) {
-					delay = new_delay;
-				} else {
-					delay--;
-				}			
 				if (delay == 0) {
 					printf("operating without delay\n");
+					frequency = real_frequency;
 					break;
 				}
+
+				if (new_delay < delay) {
+					delay = new_delay;
+				} else {
+					if (delay > 0)
+						delay--;
+				}			
 			}
 		}