2008-12-03 Joel Sherrill <joel.sherrill@OARcorp.com>

		Michael South <msouth@msouth.org>

	PR 1344/bsps
	* configure.ac, clock/ckinit.c, timer/timer.c: Add use of TSC for
	nanoseconds granularity. i8254 is very slow on some systems. TSC use
	is auto-detected by default.

4 files changed, 214 insertions, 36 deletions

diff --git a/c/src/lib/libbsp/i386/pc386/ChangeLog b/c/src/lib/libbsp/i386/pc386/ChangeLog
index 3e2d66c569..5b6782b917 100644
--- a/c/src/lib/libbsp/i386/pc386/ChangeLog
+++ b/c/src/lib/libbsp/i386/pc386/ChangeLog
@@ -1,3 +1,11 @@
+2008-12-03	Joel Sherrill <joel.sherrill@OARcorp.com>
+		Michael South <msouth@msouth.org>
+
+	PR 1344/bsps
+	* configure.ac, clock/ckinit.c, timer/timer.c: Add use of TSC for
+	nanoseconds granularity. i8254 is very slow on some systems. TSC use
+	is auto-detected by default.
+
 2008-12-02	Joel Sherrill <joel.sherrill@oarcorp.com>
 
 	* startup/bspgetworkarea.c: Remove debug prints.
diff --git a/c/src/lib/libbsp/i386/pc386/clock/ckinit.c b/c/src/lib/libbsp/i386/pc386/clock/ckinit.c
index d8756fc34f..8085c91444 100644
--- a/c/src/lib/libbsp/i386/pc386/clock/ckinit.c
+++ b/c/src/lib/libbsp/i386/pc386/clock/ckinit.c
@@ -24,6 +24,7 @@
 #include <bsp.h>
 #include <bsp/irq.h>
 #include <bspopts.h>
+#include <libcpu/cpuModel.h>
 
 #define CLOCK_VECTOR 0
 
@@ -31,12 +32,46 @@ volatile uint32_t pc386_microseconds_per_isr;
 volatile uint32_t pc386_isrs_per_tick;
 uint32_t pc386_clock_click_count;
 
+/*
+ * Roughly the number of cycles per tick and per nanosecond. Note that these
+ * will be wildly inaccurate if the chip speed changes due to power saving
+ * or thermal modes.
+ *
+ * NOTE: These are only used when the TSC method is used.
+ */
+uint64_t pc586_tsc_per_tick;
+uint64_t pc586_nanoseconds_per_tick;
 
-/* this driver may need to count ISRs per tick */
+uint64_t pc586_tsc_at_tick;
 
+/* this driver may need to count ISRs per tick */
 #define CLOCK_DRIVER_ISRS_PER_TICK pc386_isrs_per_tick
 
-#define Clock_driver_support_at_tick()
+#define READ_8254( _lsb, _msb )                               \
+  do { outport_byte(TIMER_MODE, TIMER_SEL0|TIMER_LATCH);      \
+     inport_byte(TIMER_CNTR0, _lsb);                          \
+     inport_byte(TIMER_CNTR0, _msb);                          \
+  } while (0)
+
+
+/*
+ *  Hooks which get swapped based upon which nanoseconds since last
+ *  tick method is preferred.
+ */
+void     (*Clock_driver_support_at_tick)(void) = NULL;
+uint32_t (*Clock_driver_nanoseconds_since_last_tick)(void) = NULL;
+
+/*
+ *  What do we do at each clock tick?
+ */
+void Clock_driver_support_at_tick_tsc(void)
+{
+  pc586_tsc_at_tick = rdtsc();
+}
+
+void Clock_driver_support_at_tick_empty(void)
+{
+}
 
 #define Clock_driver_support_install_isr( _new, _old ) \
   do { \
@@ -44,8 +79,41 @@ uint32_t pc386_clock_click_count;
 
 extern volatile uint32_t Clock_driver_isrs;
 
-uint32_t bsp_clock_nanoseconds_since_last_tick(void)
+uint32_t bsp_clock_nanoseconds_since_last_tick_tsc(void)
 {
+  /******
+   * Get nanoseconds using Pentium-compatible TSC register
+   ******/
+  
+  uint64_t                 diff_nsec;
+
+  diff_nsec = rdtsc() - pc586_tsc_at_tick;
+
+  /*
+   * At this point, with a hypothetical 10 GHz CPU clock and 100 Hz tick
+   * clock, diff_nsec <= 27 bits.
+   */
+  diff_nsec *= pc586_nanoseconds_per_tick; /* <= 54 bits */
+  diff_nsec /= pc586_tsc_per_tick;
+
+  if (diff_nsec > pc586_nanoseconds_per_tick)
+    /*
+     * Hmmm... Some drift or rounding. Pin the value to 1 nanosecond before
+     * the next tick.
+     */
+    /*    diff_nsec = pc586_nanoseconds_per_tick - 1; */
+    diff_nsec = 12345;
+
+  return (uint32_t)diff_nsec;
+}
+  
+uint32_t bsp_clock_nanoseconds_since_last_tick_i8254(void)
+{
+
+  /******
+   * Get nanoseconds using 8254 timer chip
+   ******/
+
   uint32_t                 usecs, clicks, isrs;
   uint32_t                 usecs1, usecs2;
   uint8_t                  lsb, msb;
@@ -55,9 +123,7 @@ uint32_t bsp_clock_nanoseconds_since_last_tick(void)
    * Fetch all the data in an interrupt critical section.
    */
   rtems_interrupt_disable(level);
-    outport_byte(TIMER_MODE, TIMER_SEL0|TIMER_LATCH);
-    inport_byte(TIMER_CNTR0, lsb);
-    inport_byte(TIMER_CNTR0, msb);
+    READ_8254(lsb, msb);
     isrs = Clock_driver_isrs;
   rtems_interrupt_enable(level);
 
@@ -85,10 +151,58 @@ uint32_t bsp_clock_nanoseconds_since_last_tick(void)
 
   /* return it in nanoseconds */
   return usecs * 1000;
+
 }
 
-#define Clock_driver_nanoseconds_since_last_tick \
-  bsp_clock_nanoseconds_since_last_tick
+/*
+ * Calibrate CPU cycles per tick. Interrupts should be disabled.
+ */
+static void calibrate_tsc(void)
+{
+  uint64_t              begin_time;
+  uint8_t               then_lsb, then_msb, now_lsb, now_msb;
+  uint32_t              i;
+
+  pc586_nanoseconds_per_tick =
+    rtems_configuration_get_microseconds_per_tick() * 1000;
+
+  /*
+   * We just reset the timer, so we know we're at the beginning of a tick.
+   */
+
+  /*
+   * Count cycles. Watching the timer introduces a several microsecond
+   * uncertaintity, so let it cook for a while and divide by the number of
+   * ticks actually executed.
+   */
+
+  begin_time = rdtsc();
+
+  for (i = rtems_clock_get_ticks_per_second() * pc386_isrs_per_tick;
+       i != 0; --i ) {   
+    /* We know we've just completed a tick when timer goes from low to high */
+    then_lsb = then_msb = 0xff;
+    do {
+      READ_8254(now_lsb, now_msb);
+      if ((then_msb < now_msb) ||
+          ((then_msb == now_msb) && (then_lsb < now_lsb)))
+        break;
+      then_lsb = now_lsb;
+      then_msb = now_msb;
+    } while (1);
+  }
+
+  pc586_tsc_per_tick = rdtsc() - begin_time;
+
+  /* Initialize "previous tick" counters */
+  pc586_tsc_at_tick = rdtsc();
+
+#if 1
+  printk( "CPU clock at %u MHz\n", (uint32_t)(pc586_tsc_per_tick / 1000000));
+#endif
+  
+  pc586_tsc_per_tick /= rtems_clock_get_ticks_per_second();
+}
 
 static void clockOn(
   const rtems_irq_connect_data* unused
@@ -114,6 +228,13 @@ static void clockOn(
   outport_byte(TIMER_MODE, TIMER_SEL0|TIMER_16BIT|TIMER_RATEGEN);
   outport_byte(TIMER_CNTR0, pc386_clock_click_count >> 0 & 0xff);
   outport_byte(TIMER_CNTR0, pc386_clock_click_count >> 8 & 0xff);
+
+  /*
+   * Now calibrate cycles per tick. Do this every time we
+   * turn the clock on in case the CPU clock speed has changed.
+   */
+  if ( x86_has_tsc() )
+    calibrate_tsc();
 }
 
 void clockOff(const rtems_irq_connect_data* unused)
@@ -142,13 +263,42 @@ static rtems_irq_connect_data clockIrqData = {
   clockIsOn
 };
 
-#define Clock_driver_support_initialize_hardware() \
-  do { \
-    if (!BSP_install_rtems_irq_handler (&clockIrqData)) { \
-      printk("Unable to initialize system clock\n"); \
-      rtems_fatal_error_occurred(1); \
-    } \
-  } while (0)
+void Clock_driver_support_initialize_hardware()
+{
+  bool use_tsc = false;
+  bool use_8254 = false;
+  
+  #if (CLOCK_DRIVER_USE_TSC == 1)
+    use_tsc = true;
+  #endif
+
+  #if (CLOCK_DRIVER_USE_8254 == 1)
+    use_8254 = true;
+  #endif
+ 
+  if ( !use_tsc && !use_8254 ) {
+    if ( x86_has_tsc() ) use_tsc  = true;
+    else                 use_8254 = true;
+  }
+
+  if ( use_8254 ) {
+    printk( "Use 8254\n" );
+    Clock_driver_support_at_tick = Clock_driver_support_at_tick_empty;
+    bsp_clock_nanoseconds_since_last_tick = 
+      bsp_clock_nanoseconds_since_last_tick_tsc;
+
+  } else {
+    printk( "Use TSC\n" );
+    Clock_driver_support_at_tick = Clock_driver_support_at_tick_tsc;
+    bsp_clock_nanoseconds_since_last_tick = 
+      bsp_clock_nanoseconds_since_last_tick_i88254;
+  }
+
+  if (!BSP_install_rtems_irq_handler (&clockIrqData)) {
+    printk("Unable to initialize system clock\n");
+    rtems_fatal_error_occurred(1);
+  }
+}
 
 #define Clock_driver_support_shutdown_hardware() \
   do { \
diff --git a/c/src/lib/libbsp/i386/pc386/configure.ac b/c/src/lib/libbsp/i386/pc386/configure.ac
index 729e7cd1e7..9988c1a4a0 100644
--- a/c/src/lib/libbsp/i386/pc386/configure.ac
+++ b/c/src/lib/libbsp/i386/pc386/configure.ac
@@ -46,6 +46,41 @@ RTEMS_BSPOPTS_HELP([BSP_PRESS_KEY_FOR_RESET],
  before rebooting the PC.  This is useful for unattended PC deployments
 ])
 
+RTEMS_BSPOPTS_SET([CLOCK_DRIVER_USE_TSC],[*],[0])
+RTEMS_BSPOPTS_HELP([CLOCK_DRIVER_USE_TSC],
+[If enabled, the clock driver will use the TSC register available
+ with Pentium-class CPUs to report close to nanosecond-accuracy
+ clock times.
+ Enable it, if:
+ - you have nanosecond timing enabled (you do NOT have
+   USE_TICKS_FOR_CPU_USAGE_STATISTICS enabled)
+ - you do NOT have CLOCK_DRIVER_USE_8254 enabled (use one, the other,
+   or neither)
+ - you have a Pentium which supports TSC (all Intels, and probably
+   all or most clones)
+ - you do not have a variable-speed CPU clock. Note that some
+   motherboard BIOS will automatically vary clock speed for thermal
+   control. Note also, however, that really new Pentium-class chips
+   from Intel and AMD will maintain a constant-rate TSC regardless.
+])
+
+RTEMS_BSPOPTS_SET([CLOCK_DRIVER_USE_8254],[*],[0])
+RTEMS_BSPOPTS_HELP([CLOCK_DRIVER_USE_8254],
+[If enabled, the clock driver will use the good old 8254 chip
+ to report microsecond-accuracy clock times.
+ Enable it, if:
+ - you have nanosecond timing enabled (you do NOT have
+   USE_TICKS_FOR_CPU_USAGE_STATISTICS enabled)
+ - you do NOT have CLOCK_DRIVER_USE_TSC enabled (use one, the other,
+   or neither)
+ - you do not mind adding roughly 5 microseconds to each context switch.
+])
+
+if test X${CLOCK_DRIVER_USE_TSC} = X1 -a X${CLOCK_DRIVER_USE_8254} = X1 ; then
+  AC_MSG_ERROR([pc386 both TSC and 8254 specified for clock driver])
+fi
+
+#define CLOCK_DRIVER_USE_8254 $CLOCK_DRIVER_USE_8254
 ## if this is an i386, does gas have good code16 support?
 RTEMS_I386_GAS_CODE16
 AM_CONDITIONAL(RTEMS_GAS_CODE16,[test "$RTEMS_GAS_CODE16" = "yes"])
diff --git a/c/src/lib/libbsp/i386/pc386/timer/timer.c b/c/src/lib/libbsp/i386/pc386/timer/timer.c
index d45074ec46..534813e789 100644
--- a/c/src/lib/libbsp/i386/pc386/timer/timer.c
+++ b/c/src/lib/libbsp/i386/pc386/timer/timer.c
@@ -42,6 +42,7 @@
 
 #include <bsp.h>
 #include <bsp/irq.h>
+#include <libcpu/cpuModel.h>
 
 /*-------------------------------------------------------------------------+
 | Constants
@@ -59,18 +60,18 @@
 | Global Variables
 +--------------------------------------------------------------------------*/
 volatile uint32_t         Ttimer_val;
-bool    	  benchmark_timer_find_average_overhead = true;
+bool    	          benchmark_timer_find_average_overhead = true;
 volatile unsigned int     fastLoop1ms, slowLoop1ms;
-void (*benchmark_timer_initialize_function)(void) = 0;
+
+void     (*benchmark_timer_initialize_function)(void) = 0;
 uint32_t (*benchmark_timer_read_function)(void) = 0;
-void (*Timer_exit_function)(void) = 0;
+void     (*Timer_exit_function)(void) = 0;
 
 /*-------------------------------------------------------------------------+
 | External Prototypes
 +--------------------------------------------------------------------------*/
 extern void timerisr(void);
        /* timer (int 08h) Interrupt Service Routine (defined in 'timerisr.s') */
-extern int x86_capability;
 
 /*
  * forward declarations
@@ -83,22 +84,6 @@ void Timer_exit(void);
 +--------------------------------------------------------------------------*/
 
 /*-------------------------------------------------------------------------+
-|         Function: rdtsc
-|      Description: Read the value of PENTIUM on-chip cycle counter.
-| Global Variables: None.
-|        Arguments: None.
-|          Returns: Value of PENTIUM on-chip cycle counter.
-+--------------------------------------------------------------------------*/
-static inline unsigned long long
-rdtsc(void)
-{
-  /* Return the value of the on-chip cycle counter. */
-  unsigned long long result;
-  asm volatile(".byte 0x0F, 0x31" : "=A" (result));
-  return result;
-} /* rdtsc */
-
-/*-------------------------------------------------------------------------+
 |         Function: Timer_exit
 |      Description: Timer cleanup routine at RTEMS exit. NOTE: This routine is
 |                   not really necessary, since there will be a reset at exit.
@@ -288,7 +273,7 @@ benchmark_timer_initialize(void)
     static bool First = true;
 
     if (First) {
-        if (x86_capability & (1 << 4) ) {
+        if (x86_has_tsc()) {
 #if defined(DEBUG)
             printk("TSC: timer initialization\n");
 #endif /* DEBUG */