Index: trunk/i386/libsaio/cpu.c
===================================================================
--- trunk/i386/libsaio/cpu.c	(revision 1943)
+++ trunk/i386/libsaio/cpu.c	(revision 1944)
@@ -20,6 +20,76 @@
 #endif
 
 /*
+ * timeRDTSC()
+ * This routine sets up PIT counter 2 to count down 1/20 of a second.
+ * It pauses until the value is latched in the counter
+ * and then reads the time stamp counter to return to the caller.
+ */
+uint64_t timeRDTSC(void)
+{
+    int		attempts = 0;
+    uint64_t    latchTime;
+    uint64_t	saveTime,intermediate;
+    unsigned int timerValue, lastValue;
+    //boolean_t	int_enabled;
+    /*
+     * Table of correction factors to account for
+     *	 - timer counter quantization errors, and
+     *	 - undercounts 0..5
+     */
+#define SAMPLE_CLKS_EXACT	(((double) CLKNUM) / 20.0)
+#define SAMPLE_CLKS_INT		((int) CLKNUM / 20)
+#define SAMPLE_NSECS		(2000000000LL)
+#define SAMPLE_MULTIPLIER	(((double)SAMPLE_NSECS)*SAMPLE_CLKS_EXACT)
+#define ROUND64(x)		((uint64_t)((x) + 0.5))
+    uint64_t	scale[6] = {
+		ROUND64(SAMPLE_MULTIPLIER/(double)(SAMPLE_CLKS_INT-0)), 
+		ROUND64(SAMPLE_MULTIPLIER/(double)(SAMPLE_CLKS_INT-1)), 
+		ROUND64(SAMPLE_MULTIPLIER/(double)(SAMPLE_CLKS_INT-2)), 
+		ROUND64(SAMPLE_MULTIPLIER/(double)(SAMPLE_CLKS_INT-3)), 
+		ROUND64(SAMPLE_MULTIPLIER/(double)(SAMPLE_CLKS_INT-4)), 
+		ROUND64(SAMPLE_MULTIPLIER/(double)(SAMPLE_CLKS_INT-5))
+    };
+    
+restart:
+    if (attempts >= 9) // increase to up to 9 attempts.
+        // This will flash-reboot. TODO: Use tscPanic instead.
+        printf("Timestamp counter calibation failed with %d attempts\n", attempts);
+    attempts++;
+    enable_PIT2();		// turn on PIT2
+    set_PIT2(0);		// reset timer 2 to be zero
+    latchTime = rdtsc64();	// get the time stamp to time 
+    latchTime = get_PIT2(&timerValue) - latchTime; // time how long this takes
+    set_PIT2(SAMPLE_CLKS_INT);	// set up the timer for (almost) 1/20th a second
+    saveTime = rdtsc64();	// now time how long a 20th a second is...
+    get_PIT2(&lastValue);
+    get_PIT2(&lastValue);	// read twice, first value may be unreliable
+    do {
+		intermediate = get_PIT2(&timerValue);
+		if (timerValue > lastValue) {
+			// Timer wrapped
+			set_PIT2(0);
+			disable_PIT2();
+			goto restart;
+		}
+		lastValue = timerValue;
+    } while (timerValue > 5);
+    printf("timerValue	  %d\n",timerValue);
+    printf("intermediate 0x%016llx\n",intermediate);
+    printf("saveTime	  0x%016llx\n",saveTime);
+    
+    intermediate -= saveTime;		// raw count for about 1/20 second
+    intermediate *= scale[timerValue];	// rescale measured time spent
+    intermediate /= SAMPLE_NSECS;	// so its exactly 1/20 a second
+    intermediate += latchTime;		// add on our save fudge
+    
+    set_PIT2(0);			// reset timer 2 to be zero
+    disable_PIT2();			// turn off PIT 2
+	
+    return intermediate;
+}
+
+/*
  * DFE: Measures the TSC frequency in Hz (64-bit) using the ACPI PM timer
  */
 static uint64_t measure_tsc_frequency(void)
@@ -281,6 +351,11 @@
 	}
 	
 	tscFrequency = measure_tsc_frequency();
+	/* if usual method failed */
+	if ( tscFrequency < 1000 )
+	{
+	tscFrequency = timeRDTSC() * 20;
+	}
 	fsbFrequency = 0;
 	cpuFrequency = 0;
 	
Index: trunk/i386/libsaio/cpu.h
===================================================================
--- trunk/i386/libsaio/cpu.h	(revision 1943)
+++ trunk/i386/libsaio/cpu.h	(revision 1944)
@@ -164,4 +164,70 @@
     return count;
 }
 
+
+inline static void
+set_PIT2(int value)
+{
+/*
+ * First, tell the clock we are going to write 16 bits to the counter
+ * and enable one-shot mode (command 0xB8 to port 0x43)
+ * Then write the two bytes into the PIT2 clock register (port 0x42).
+ * Loop until the value is "realized" in the clock,
+ * this happens on the next tick.
+ */
+    asm volatile(
+        " movb  $0xB8,%%al      \n\t"
+        " outb  %%al,$0x43      \n\t"
+        " movb  %%dl,%%al       \n\t"
+        " outb  %%al,$0x42      \n\t"
+        " movb  %%dh,%%al       \n\t"
+        " outb  %%al,$0x42      \n"
+"1:       inb   $0x42,%%al      \n\t" 
+        " inb   $0x42,%%al      \n\t"
+        " cmp   %%al,%%dh       \n\t"
+        " jne   1b"
+        : : "d"(value) : "%al");
+}
+
+
+inline static uint64_t
+get_PIT2(unsigned int *value)
+{
+    register uint64_t   result;
+/*
+ * This routine first latches the time (command 0x80 to port 0x43),
+ * then gets the time stamp so we know how long the read will take later.
+ * Read (from port 0x42) and return the current value of the timer.
+ */
+#ifdef __i386__
+    asm volatile(
+        " xorl  %%ecx,%%ecx     \n\t"
+        " movb  $0x80,%%al      \n\t"
+        " outb  %%al,$0x43      \n\t"
+        " rdtsc                 \n\t"
+        " pushl %%eax           \n\t"
+        " inb   $0x42,%%al      \n\t"
+        " movb  %%al,%%cl       \n\t"
+        " inb   $0x42,%%al      \n\t"
+        " movb  %%al,%%ch       \n\t"
+        " popl  %%eax   "
+        : "=A"(result), "=c"(*value));
+#else /* __x86_64__ */
+    asm volatile(
+		" xorq  %%rcx,%%rcx     \n\t"
+		" movb  $0x80,%%al      \n\t"
+		" outb  %%al,$0x43      \n\t"
+		" rdtsc                 \n\t"
+		" pushq  %%rax          \n\t"
+		" inb   $0x42,%%al      \n\t"
+		" movb  %%al,%%cl       \n\t"
+		" inb   $0x42,%%al      \n\t"
+		" movb  %%al,%%ch       \n\t"
+		" popq  %%rax   "
+		: "=A"(result), "=c"(*value));
+#endif
+
+    return result;
+}
+
 #endif /* !__LIBSAIO_CPU_H */