Index: c/src/lib/libbsp/i386/pc386/ChangeLog
===================================================================
--- c/src/lib/libbsp/i386/pc386/ChangeLog	(revision 5fe133566282cd62b96084b3e384cd817e1ac6f6)
+++ c/src/lib/libbsp/i386/pc386/ChangeLog	(revision dcbc0a89c2acfa108234229bfa4452737f9a6a9b)
@@ -1,2 +1,10 @@
+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-09-06	Ralf CorsÃ©pius <ralf.corsepius@rtems.org>
 
Index: c/src/lib/libbsp/i386/pc386/clock/ckinit.c
===================================================================
--- c/src/lib/libbsp/i386/pc386/clock/ckinit.c	(revision 5fe133566282cd62b96084b3e384cd817e1ac6f6)
+++ c/src/lib/libbsp/i386/pc386/clock/ckinit.c	(revision dcbc0a89c2acfa108234229bfa4452737f9a6a9b)
@@ -25,4 +25,5 @@
 #include <bsp/irq.h>
 #include <bspopts.h>
+#include <libcpu/cpuModel.h>
 
 #define CLOCK_VECTOR 0
@@ -32,10 +33,44 @@
 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;
+
+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 ) \
@@ -45,6 +80,39 @@
 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;
@@ -56,7 +124,5 @@
    */
   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);
@@ -86,8 +152,56 @@
   /* 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(
@@ -115,4 +229,11 @@
   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();
 }
 
@@ -143,11 +264,40 @@
 };
 
-#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() \
Index: c/src/lib/libbsp/i386/pc386/configure.ac
===================================================================
--- c/src/lib/libbsp/i386/pc386/configure.ac	(revision 5fe133566282cd62b96084b3e384cd817e1ac6f6)
+++ c/src/lib/libbsp/i386/pc386/configure.ac	(revision dcbc0a89c2acfa108234229bfa4452737f9a6a9b)
@@ -47,4 +47,39 @@
 ])
 
+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
Index: c/src/lib/libbsp/i386/pc386/timer/timer.c
===================================================================
--- c/src/lib/libbsp/i386/pc386/timer/timer.c	(revision 5fe133566282cd62b96084b3e384cd817e1ac6f6)
+++ c/src/lib/libbsp/i386/pc386/timer/timer.c	(revision dcbc0a89c2acfa108234229bfa4452737f9a6a9b)
@@ -43,4 +43,5 @@
 #include <bsp.h>
 #include <bsp/irq.h>
+#include <libcpu/cpuModel.h>
 
 /*-------------------------------------------------------------------------+
@@ -60,9 +61,10 @@
 +--------------------------------------------------------------------------*/
 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;
 
 /*-------------------------------------------------------------------------+
@@ -71,5 +73,4 @@
 extern void timerisr(void);
        /* timer (int 08h) Interrupt Service Routine (defined in 'timerisr.s') */
-extern int x86_capability;
 
 /*
@@ -82,20 +83,4 @@
 | Pentium optimized timer handling.
 +--------------------------------------------------------------------------*/
-
-/*-------------------------------------------------------------------------+
-|         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 */
 
 /*-------------------------------------------------------------------------+
@@ -289,5 +274,5 @@
 
     if (First) {
-        if (x86_capability & (1 << 4) ) {
+        if (x86_has_tsc()) {
 #if defined(DEBUG)
             printk("TSC: timer initialization\n");
