2000-12-05 Eric Valette <valette@crf.canon.fr>

	* console/inch.c, console/keyboard.c, console/pc_keyb.c,
	console/vt.c, include/bsp.h: Correct incorrect interrupt level
	handling in new keyboard management code.  Correct
	BSP_poll_char initialization routine.
	* start/start.S, startup/bspstart.c: Correct when the video is
	initialized.
	* timer/timer.c (Calibrate_1ms_loop): Address problem where this
	did not work correctly on all PC speeds. The new calibrate routine
	has been tested on Pentium 166, pentium II 200, pentium III
	300 Mhz and does work as expected.

9 files changed, 470 insertions, 128 deletions

diff --git a/c/src/lib/libbsp/i386/pc386/ChangeLog b/c/src/lib/libbsp/i386/pc386/ChangeLog
index 0b75b293a2..956cb7e3a4 100644
--- a/c/src/lib/libbsp/i386/pc386/ChangeLog
+++ b/c/src/lib/libbsp/i386/pc386/ChangeLog
@@ -1,3 +1,16 @@
+2000-12-05	 Eric Valette <valette@crf.canon.fr>
+
+	* console/inch.c, console/keyboard.c, console/pc_keyb.c,
+	console/vt.c, include/bsp.h: Correct incorrect interrupt level
+	handling in new keyboard management code.  Correct
+	BSP_poll_char initialization routine.
+	* start/start.S, startup/bspstart.c: Correct when the video is
+	initialized.
+	* timer/timer.c (Calibrate_1ms_loop): Address problem where this
+	did not work correctly on all PC speeds. The new calibrate routine
+	has been tested on Pentium 166, pentium II 200, pentium III
+	300 Mhz and does work as expected.
+
 2000-12-05	Joel Sherrill <joel@OARcorp.com>
 
 	* pc386/console/console.c, pc386/console/serial_mouse.c,
diff --git a/c/src/lib/libbsp/i386/pc386/console/inch.c b/c/src/lib/libbsp/i386/pc386/console/inch.c
index 3aa3ad706d..ac6cec61dd 100644
--- a/c/src/lib/libbsp/i386/pc386/console/inch.c
+++ b/c/src/lib/libbsp/i386/pc386/console/inch.c
@@ -36,7 +36,37 @@
 /*-------------------------------------------------------------------------+
 | Constants
 +--------------------------------------------------------------------------*/
-#define KBD_BUF_SIZE    256
+#define KBD_CTL      0x61  /* -------------------------------- */
+#define KBD_DATA     0x60  /* Ports for PC keyboard controller */
+#define KBD_STATUS   0x64  /* -------------------------------- */
+
+#define KBD_BUF_SIZE 256
+
+/*-------------------------------------------------------------------------+
+| Global Variables
++--------------------------------------------------------------------------*/
+static char key_map[] =
+{
+  0,033,'1','2','3','4','5','6','7','8','9','0','-','=','\b','\t',
+  'q','w','e','r','t','y','u','i','o','p','[',']',015,0x80,
+  'a','s','d','f','g','h','j','k','l',';',047,0140,0x80,
+  0134,'z','x','c','v','b','n','m',',','.','/',0x80,
+  '*',0x80,' ',0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,
+  0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,
+  0x80,0x80,0x80,'0',0177
+}; /* Keyboard scancode -> character map with no modifiers.       */
+
+static char shift_map[] =
+{
+  0,033,'!','@','#','$','%','^','&','*','(',')','_','+','\b','\t',
+  'Q','W','E','R','T','Y','U','I','O','P','{','}',015,0x80,
+  'A','S','D','F','G','H','J','K','L',':',042,'~',0x80,
+  '|','Z','X','C','V','B','N','M','<','>','?',0x80,
+  '*',0x80,' ',0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,
+  0x80,0x80,0x80,0x80,'7','8','9',0x80,'4','5','6',0x80,
+  '1','2','3','0',177
+}; /* Keyboard scancode -> character map with SHIFT key modifier. */ 
+
 
 static unsigned short   kbd_buffer[KBD_BUF_SIZE];
 static rtems_unsigned16 kbd_first = 0;
@@ -57,25 +87,212 @@ void rtemsReboot(void)
 } /* rtemsReboot */
 
 
+/*-------------------------------------------------------------------------+
+|         Function: _IBMPC_scankey
+|      Description: This function can be called during a poll for input, or by
+|                   an ISR. Basically any time you want to process a keypress.
+| Global Variables: key_map, shift_map.
+|        Arguments: outChar - character read in case of a valid reading,
+|                   otherwise unchanged.
+|          Returns: TRUE in case a valid character has been read,
+|                   FALSE otherwise.
++--------------------------------------------------------------------------*/
+static rtems_boolean
+_IBMPC_scankey(char *outChar)
+{
+  unsigned char inChar;
+  static int alt_pressed   = 0;
+  static int ctrl_pressed  = 0;
+  static int shift_pressed = 0;
+  static int caps_pressed  = 0;
+  static int extended      = 0;
+
+  *outChar = '\0'; /* default value if we return FALSE */
+
+  /* Read keyboard controller, toggle enable */
+  inport_byte(KBD_CTL, inChar);
+  outport_byte(KBD_CTL, inChar & ~0x80);
+  outport_byte(KBD_CTL, inChar | 0x80);
+  outport_byte(KBD_CTL, inChar & ~0x80);
+
+  /* See if it has data */
+  inport_byte(KBD_STATUS, inChar);
+  if ((inChar & 0x01) == 0)
+    return FALSE;
+
+  /* Read the data.  Handle nonsense with shift, control, etc. */
+  inport_byte(KBD_DATA, inChar);
+
+  if (extended)
+    extended--;
+
+  switch (inChar)
+  {
+    case 0xe0:
+      extended = 2;
+      return FALSE;
+      break;
+
+    case 0x38:
+      alt_pressed = 1;
+      return FALSE;
+      break;
+    case 0xb8:
+      alt_pressed = 0;
+      return FALSE;
+      break;
+
+    case 0x1d:
+      ctrl_pressed = 1;
+      return FALSE;
+      break;
+    case 0x9d:
+      ctrl_pressed = 0;
+      return FALSE;
+      break;
+
+    case 0x2a:
+      if (extended)
+        return FALSE;
+    case 0x36:
+      shift_pressed = 1;
+      return FALSE;
+      break;
+    case 0xaa:
+      if (extended)
+        return FALSE;
+    case 0xb6:
+      shift_pressed = 0;
+      return FALSE;
+      break;
+
+    case 0x3a:
+      caps_pressed = 1;
+      return FALSE;
+      break;
+    case 0xba:
+      caps_pressed = 0;
+      return FALSE;
+      break;
+
+    case 0x53:
+      if (ctrl_pressed && alt_pressed) 
+        rtemsReboot(); /* ctrl+alt+del -> reboot */
+      break;
+
+    /*
+     * Ignore unrecognized keys--usually arrow and such
+     */
+    default:
+      if ((inChar & 0x80) || (inChar > 0x39))
+      /* High-bit on means key is being released, not pressed */
+        return FALSE;
+      break;
+  } /* switch */
+
+  /* Strip high bit, look up in our map */
+  inChar &= 0x7f;
+  if (ctrl_pressed)
+  {
+    *outChar = key_map[inChar];
+    *outChar &= 037;
+  }
+  else
+  {
+    *outChar = shift_pressed ? shift_map[inChar] : key_map[inChar];
+    if (caps_pressed)
+    {
+      if (*outChar >= 'A' && *outChar <= 'Z')
+        *outChar += 'a' - 'A';
+      else if (*outChar >= 'a' && *outChar <= 'z')
+        *outChar -= 'a' - 'A';
+    }
+  }
+
+  return TRUE;
+} /* _IBMPC_scankey */
+
+/*-------------------------------------------------------------------------+
+|         Function: _IBMPC_chrdy
+|      Description: Check keyboard ISR buffer and return character if not empty.
+| Global Variables: kbd_buffer, kbd_first, kbd_last.
+|        Arguments: c - character read if keyboard buffer not empty, otherwise
+|                   unchanged.
+|          Returns: TRUE if keyboard buffer not empty, FALSE otherwise.
++--------------------------------------------------------------------------*/
+static rtems_boolean
+_IBMPC_chrdy(char *c)
+{
+  /* FIX ME!!! It doesn't work without something like the following line.
+     Find out why! */
+  printk("");
+
+  /* Check buffer our ISR builds */
+  if (kbd_first != kbd_last)
+  {
+    *c = kbd_buffer[kbd_first];
+
+    kbd_first = (kbd_first + 1) % KBD_BUF_SIZE;
+    return TRUE;
+  }
+  else
+    return FALSE;
+} /* _IBMPC_chrdy */
+
+
+/*-------------------------------------------------------------------------+
+|         Function: _IBMPC_inch
+|      Description: Poll keyboard until a character is ready and return it.
+| Global Variables: None.
+|        Arguments: None.
+|          Returns: character read from keyboard.
++--------------------------------------------------------------------------*/
+char
+_IBMPC_inch(void)
+{
+    char c;
+    while (!_IBMPC_chrdy(&c))
+      continue;
+
+    return c;
+} /* _IBMPC_inch */
+
+ 
+ /*
+  * Routine that can be used before interrupt management is initialized.
+  */
+ 
+char
+BSP_wait_polled_input(void)
+{
+  char c;
+  while (!_IBMPC_scankey(&c))
+    continue;
+
+  return c;
+}
 
-#define disable __asm__ __volatile__("cli")
-#define enable  __asm__ __volatile__("sti");
 /*
  * Check if a key has been pressed. This is a non-destructive
  * call, meaning, it keeps the key in the buffer.
  */
 int rtems_kbpoll( void )
 {
-  int rc;
-  disable;
+  int rc,level;
+
+  _CPU_ISR_Disable(level);
+
   rc = ( kbd_first != kbd_last ) ? TRUE : FALSE;
-  enable;
+
+  _CPU_ISR_Enable (level);
+
   return rc;
 }
 
 int getch( void )
 {
   int c;
+
   while( kbd_first == kbd_last )
   {
      rtems_task_wake_after( 10 );
diff --git a/c/src/lib/libbsp/i386/pc386/console/keyboard.c b/c/src/lib/libbsp/i386/pc386/console/keyboard.c
index 5520ae7354..db9e0b6f1a 100644
--- a/c/src/lib/libbsp/i386/pc386/console/keyboard.c
+++ b/c/src/lib/libbsp/i386/pc386/console/keyboard.c
@@ -60,27 +60,27 @@ extern void rtemsReboot( void );
 
 int set_bit(int nr, unsigned long * addr)
 {
-	int	mask, retval;
+	int	mask, retval,level;
 
 	addr += nr >> 5;
 	mask = 1 << (nr & 0x1f);
-	cli();
+	_CPU_ISR_Disable(level)
 	retval = (mask & *addr) != 0;
 	*addr |= mask;
-	sti();
+	_CPU_ISR_Enable (level);
 	return retval;
 }
 
 int clear_bit(int nr, unsigned long * addr)
 {
-	int	mask, retval;
+	int	mask, retval,level;
 
 	addr += nr >> 5;
 	mask = 1 << (nr & 0x1f);
-	cli();
+	_CPU_ISR_Disable(level)
 	retval = (mask & *addr) != 0;
 	*addr &= ~mask;
-	sti();
+	_CPU_ISR_Enable (level);
 	return retval;
 }
 
diff --git a/c/src/lib/libbsp/i386/pc386/console/pc_keyb.c b/c/src/lib/libbsp/i386/pc386/console/pc_keyb.c
index 0b94666504..d9feaca1b4 100644
--- a/c/src/lib/libbsp/i386/pc386/console/pc_keyb.c
+++ b/c/src/lib/libbsp/i386/pc386/console/pc_keyb.c
@@ -636,13 +636,15 @@ void pckbd_init_hw(void)
 
 }
 
-
+/*
 char BSP_wait_polled_input( void )
 {
-  int c;
-  cli();
+  int c,level;
+
+  _CPU_ISR_Disable(level);
   while ( ( c= kbd_wait_for_input() ) < 0 )
       continue;
-  sti();
+  _CPU_ISR_Enable (level);
   return c;
 }
+*/
diff --git a/c/src/lib/libbsp/i386/pc386/console/vt.c b/c/src/lib/libbsp/i386/pc386/console/vt.c
index eb8ec64c76..218a907c92 100644
--- a/c/src/lib/libbsp/i386/pc386/console/vt.c
+++ b/c/src/lib/libbsp/i386/pc386/console/vt.c
@@ -66,12 +66,13 @@ kd_nosound(unsigned long ignored)
 void
 _kd_mksound(unsigned int hz, unsigned int ticks)
 {
-	unsigned int count = 0;
+  unsigned int count = 0;
+  int level;
 
 	if (hz > 20 && hz < 32767)
 		count = 1193180 / hz;
 	
-	cli();
+	 _CPU_ISR_Disable(level);
 /*	del_timer(&sound_timer);  */
 	if (count) {
 		/* enable counter 2 */
@@ -91,7 +92,7 @@ _kd_mksound(unsigned int hz, unsigned int ticks)
 	} else
 		kd_nosound(0);
 
-	sti();
+	 _CPU_ISR_Enable (level);
 	return;
 }
 
diff --git a/c/src/lib/libbsp/i386/pc386/include/bsp.h b/c/src/lib/libbsp/i386/pc386/include/bsp.h
index c152152fb7..7431059314 100644
--- a/c/src/lib/libbsp/i386/pc386/include/bsp.h
+++ b/c/src/lib/libbsp/i386/pc386/include/bsp.h
@@ -122,6 +122,14 @@ extern int rtems_dec21140_driver_attach(struct rtems_bsdnet_ifconfig *, int);
 #define TIMER_MSB      0x20            /* r/w counter MSB                */
 #define TIMER_16BIT    0x30            /* r/w counter 16 bits, LSB first */
 #define TIMER_BCD      0x01            /* count in BCD                   */
+#define TIMER_RD_BACK  0xc0	       /* Read Back Command 		 */
+		/* READ BACK command layout in the Command Register 	 */
+#define RB_NOT_COUNT	0x40	       /* Don't select counter latch 	 */
+#define RB_NOT_STATUS	0x20	       /* Don't select status latch 	 */
+#define	RB_COUNT_0	0x02	       /* Counter 0 latch 		 */
+#define RB_COUNT_1	0x04	       /* Counter 1 latch 		 */
+#define RB_COUNT_2	0x08	       /* Counter 2 latch 		 */
+#define RB_OUTPUT	0x80	       /* Output of the counter is 1 	 */
 
 #define	TIMER_TICK     1193182  /* The internal tick rate in ticks per second */
 
diff --git a/c/src/lib/libbsp/i386/pc386/start/start.S b/c/src/lib/libbsp/i386/pc386/start/start.S
index 33af08e17c..09aa589807 100644
--- a/c/src/lib/libbsp/i386/pc386/start/start.S
+++ b/c/src/lib/libbsp/i386/pc386/start/start.S
@@ -145,12 +145,21 @@ SYM (zero_bss):
 	shrl	ecx
 	xorl	eax, eax		# value to clear out memory
 	repne				# while ecx != 0
-	stosl				#   clear a long in the bss
-
+	stosl
+					#   clear a long in the bss
+
+/*-------------------------------------------------------------------+
+| Initialize the video because zero_bss has cleared initVideo parameters
+| if it was called earlier
+| So from now we can use printk
++-------------------------------------------------------------------*/
+	call _IBMPC_initVideo
+		
 /*---------------------------------------------------------------------+
 | Check CPU type. Enable Cache and init coprocessor if needed.
 +---------------------------------------------------------------------*/
 	call checkCPUtypeSetCr0
+	
 /*---------------------------------------------------------------------+
 | Transfer control to User's Board Support Package
 +---------------------------------------------------------------------*/
diff --git a/c/src/lib/libbsp/i386/pc386/startup/bspstart.c b/c/src/lib/libbsp/i386/pc386/startup/bspstart.c
index 96091455a9..e4f051dc07 100644
--- a/c/src/lib/libbsp/i386/pc386/startup/bspstart.c
+++ b/c/src/lib/libbsp/i386/pc386/startup/bspstart.c
@@ -138,18 +138,12 @@ void bsp_pretasking_hook(void)
 void bsp_start_default( void )
 {
   void Calibrate_loop_1ms(void);
-
+  
   /*
    * Calibrate variable for 1ms-loop (see timer.c)
    */
   Calibrate_loop_1ms();
 
-  /*
-   * Initialize printk channel
-   */
-  
-  _IBMPC_initVideo();
-
   rtemsFreeMemStart = (rtems_unsigned32)&_end + _stack_size;
                                     /* set the value of start of free memory. */
 
diff --git a/c/src/lib/libbsp/i386/pc386/timer/timer.c b/c/src/lib/libbsp/i386/pc386/timer/timer.c
index 1095061bea..8d5a36055e 100644
--- a/c/src/lib/libbsp/i386/pc386/timer/timer.c
+++ b/c/src/lib/libbsp/i386/pc386/timer/timer.c
@@ -60,8 +60,8 @@
 | Global Variables
 +--------------------------------------------------------------------------*/
 volatile rtems_unsigned32 Ttimer_val;
-         rtems_boolean    Timer_driver_Find_average_overhead = TRUE;
-         unsigned int     loop1ms;
+rtems_boolean    	  Timer_driver_Find_average_overhead = TRUE;
+volatile unsigned int     fastLoop1ms, slowLoop1ms;
 
 /*-------------------------------------------------------------------------+
 | External Prototypes
@@ -305,137 +305,233 @@ Set_find_average_overhead(rtems_boolean find_flag)
   Timer_driver_Find_average_overhead = find_flag;
 } /* Set_find_average_overhead */
 
-
+static unsigned short lastLoadedValue;
 
 /*-------------------------------------------------------------------------+
 | Description: Loads timer 0 with value passed as arguemnt.
-| Returns: Nothing. 
+| Returns: Nothing. Loaded value must be a number of clock bits...
 +--------------------------------------------------------------------------*/
-inline void loadTimerValue( unsigned short loadedValue )
+void loadTimerValue( unsigned short loadedValue )
 {
-  outport_byte(TIMER_MODE, TIMER_SEL0|TIMER_16BIT|TIMER_RATEGEN);
-  outport_byte(TIMER_CNTR0, loadedValue >> 0 & 0xff);
-  outport_byte(TIMER_CNTR0, loadedValue >> 8 & 0xff);
-}
-
-
-/*-------------------------------------------------------------------------+
-| Description: Waits until the counter on timer 0 reaches 0.
-| Returns: Nothing. 
-+--------------------------------------------------------------------------*/
-inline void waitTimerStatus( void )
-{
-  unsigned char status;
-  outport_byte(TIMER_MODE, CMD_READ_BACK_STATUS); /* read Status counter 0 */
-  inport_byte(TIMER_CNTR0, status);
-  while (status & MSK_NULL_COUNT){ 	/* wait for counter ready */ 	
-    outport_byte(TIMER_MODE, CMD_READ_BACK_STATUS);
-    inport_byte(TIMER_CNTR0, status);
-  }
+  lastLoadedValue = loadedValue;
+  outport_byte(TIMER_MODE, TIMER_SEL0|TIMER_16BIT|TIMER_SQWAVE);
+  outport_byte(TIMER_CNTR0, loadedValue & 0xff);
+  outport_byte(TIMER_CNTR0, (loadedValue >> 8) & 0xff);
 }
 
 
 /*-------------------------------------------------------------------------+
 | Description: Reads the current value of the timer, and converts the 
 |			   number of ticks to micro-seconds.	
-| Returns: current number of microseconds since last value loaded.. 
+| Returns: number of clock bits elapsed since last load. 
 +--------------------------------------------------------------------------*/
-inline unsigned short readCurrentTimer()
+unsigned int readTimer0()
 {
   unsigned short lsb, msb;
-  outport_byte(TIMER_MODE, TIMER_SEL0|TIMER_LATCH);
+  unsigned char  status;
+  unsigned int	 count;
+
+  outport_byte(TIMER_MODE, (TIMER_RD_BACK | (RB_COUNT_0 & ~(RB_NOT_STATUS | RB_NOT_COUNT))));
+  inport_byte(TIMER_CNTR0, status);
   inport_byte(TIMER_CNTR0, lsb);
   inport_byte(TIMER_CNTR0, msb);
-  return TICK_TO_US( ( msb << 8 ) | lsb );
+  count = ( msb << 8 ) | lsb ;
+  if (status & RB_OUTPUT )
+  	count += lastLoadedValue;
+  
+  return (2*lastLoadedValue - count);
 }
 
+void Timer0Reset()
+{
+  loadTimerValue(0xffff);
+  readTimer0();
+}
 
-/*-------------------------------------------------------------------------+
- * clockbits - Read low order bits of timer 0 (the TOD clock)
- * This works only for the 8254 chips used in ATs and 386s.
- *
- * The timer runs in mode 3 (square wave mode), counting down
- * by twos, twice for each cycle. So it is necessary to read back the
- * OUTPUT pin to see which half of the cycle we're in. I.e., the OUTPUT
- * pin forms the most significant bit of the count. Unfortunately,
- * the 8253 in the PC/XT lacks a command to read the OUTPUT pin...
- *
- * The PC's clock design is soooo brain damaged...
- *
- * Rosimildo - I've got this routine from the KA9Q32 distribution and
- *			   have updated it for the RTEMS environment.
- +--------------------------------------------------------------------------*/
-unsigned int clockbits(void)
+void fastLoop (unsigned int loopCount)
 {
-	unsigned int stat,count1, count;
-
-	do 
-	{
-	  outport_byte( 0x43, 0xc2 );	 /* latch timer 0 count and status for reading */
-	  inport_byte( 0x40, stat );	 /* get status of timer 0 */
-	  inport_byte( 0x40, count1 ); /* lsb of count */
-	  inport_byte( 0x40, count ); /* msb of count */
-	  count = count1 | ( count << 8 );
-	} while(stat & 0x40);		 /* reread if NULL COUNT bit set */
-	stat = (stat & 0x80) << 8;	 /* Shift OUTPUT to msb of 16-bit word */
-	if(count == 0)
-		return stat ^ 0x8000;	/* return complement of OUTPUT bit */
-	else
-		return count | stat;	/* Combine OUTPUT with counter */
+  unsigned int i;
+  for( i=0; i < loopCount; i++ )outport_byte( SLOW_DOWN_IO, 0 );
 }
 
+void slowLoop (unsigned int loopCount)
+{
+  unsigned int j;
+  for (j=0; j <100 ;  j++) {
+    fastLoop (loopCount);
+  }
+}
 
-/*-------------------------------------------------------------------------+
-|         Function: Calibrate_loop_1ms
-|      Description: Set loop variable to calibrate a 1ms loop
-| Global Variables: loop1ms
-|        Arguments: none
-|          Returns: Nothing. 
-+--------------------------------------------------------------------------*/
+/*
+ * #define DEBUG_CALIBRATE
+ */
 void
 Calibrate_loop_1ms(void)
 {
-  unsigned int i;
-  unsigned short loadedValue, offset;
-  unsigned int timerValue, t1_ref, t2_ref=0;
+  unsigned int offset, offsetTmp, emptyCall, emptyCallTmp, res, i, j;
+  unsigned int targetClockBits, currentClockBits;
+  unsigned int slowLoopGranularity, fastLoopGranularity;
   rtems_interrupt_level  level;
   
+#ifdef DEBUG_CALIBRATE
+  printk( "Calibrate_loop_1ms is starting,  please wait ( but not too loooong. )\n" );
+#endif  
+  targetClockBits = US_TO_TICK(1000);
+  
+  rtems_interrupt_disable(level);
+  /*
+   * Fill up the cache to get a correct offset
+   */
+  Timer0Reset();
+  readTimer0();
+  /* 
+   * Compute the minimal offset to apply due to read counter register.
+   */
+  offset = 0xffffffff;
+  for (i=0; i <1000; i++) {
+    Timer0Reset();
+    offsetTmp = readTimer0();
+    offset += offsetTmp;
+  }
+  offset = offset / 1000; /* compute average */
+  /*
+   * calibrate empty call
+   */
+  fastLoop (0);
+  emptyCall = 0;
+  j = 0;
+  for (i=0; i <10; i++) {
+    Timer0Reset();
+    fastLoop (0);
+    res =  readTimer0();
+    /* res may be inferior to offset on fast
+     * machine because we took an average for offset
+     */
+    if (res >  offset) {
+      ++j;
+      emptyCallTmp = res - offset;
+      emptyCall += emptyCallTmp;
+    }
+  }
+  if (j == 0) emptyCall = 0;
+  else emptyCall = emptyCall / j; /* compute average */
+  /*
+   * calibrate fast loop
+   */
+  Timer0Reset();
+  fastLoop (10000);
+  res = readTimer0() - offset;
+  if (res < emptyCall) {
+     printk("Problem #1 in offset computation in Calibrate_loop_1ms in file libbsp/i386/pc386/timer/timer.c\n");
+    while (1);
+  }
+  fastLoopGranularity = (res - emptyCall) / 10000;
+  /*
+   * calibrate slow loop
+   */
+  Timer0Reset();
+  slowLoop(10);
+  res = readTimer0();
+  if (res < offset + emptyCall) {
+     printk("Problem #2 in offset computation in Calibrate_loop_1ms in file libbsp/i386/pc386/timer/timer.c\n");
+    while (1);
+  }
+  slowLoopGranularity = (res - offset - emptyCall)/ 10;
+  
+  if (slowLoopGranularity == 0) {
+    printk("Problem #3 in Calibrate_loop_1ms in file libbsp/i386/pc386/timer/timer.c\n");
+    while (1);
+  }
 
-  printk( "Calibrate_loop_1ms is starting,  please wait ( but not too loooong. )\n" ); 
- 
-  loop1ms = 200;
-  timerValue = 0;
+  targetClockBits += offset;
+#ifdef DEBUG_CALIBRATE  
+  printk("offset = %u, emptyCall = %u, targetClockBits = %u\n",
+	 offset, emptyCall, targetClockBits);
+  printk("slowLoopGranularity = %u fastLoopGranularity =  %u\n",
+	 slowLoopGranularity, fastLoopGranularity);
+#endif  
+  slowLoop1ms = (targetClockBits - emptyCall) / slowLoopGranularity;
+  if (slowLoop1ms != 0) {
+    fastLoop1ms = targetClockBits % slowLoopGranularity;
+    if (fastLoop1ms > emptyCall) fastLoop1ms -= emptyCall;
+  }    
+  else
+    fastLoop1ms = targetClockBits - emptyCall / fastLoopGranularity;
 
-  /* Let's load the timer with 2ms, initially */
-  loadedValue = US_TO_TICK( 2000 );
+  if (slowLoop1ms != 0) {
+    /*
+     * calibrate slow loop
+     */
   
-  rtems_interrupt_disable(level);
+    while(1)
+      {
+	int previousSign = 0; /* 0 = unset, 1 = incrementing,  2 = decrementing */
+	Timer0Reset();
+	slowLoop(slowLoop1ms);
+	currentClockBits = readTimer0();
+	if (currentClockBits > targetClockBits) {
+	  if ((currentClockBits - targetClockBits) < slowLoopGranularity) {
+	    /* decrement loop counter anyway to be sure slowLoop(slowLoop1ms) < targetClockBits */
+	    --slowLoop1ms;
+	    break;
+	  }
+	  else {
+	    --slowLoop1ms;
+	    if (slowLoop1ms == 0) break;
+	    if (previousSign == 0) previousSign = 2;
+	    if (previousSign == 1) break;
+	  }
+	}
+	else {
+	  if ((targetClockBits - currentClockBits) < slowLoopGranularity) {
+	    break;
+	  }
+	  else {
+	    ++slowLoop1ms;
+	    if (previousSign == 0) previousSign = 1;
+	    if (previousSign == 2) break;
+	  }
+	}
+      }
+  }
   /*
-   * Compute the offset to apply due to read counter register
+   * calibrate fast loop
    */
-  offset = 0;
-  loadTimerValue( loadedValue + offset );
-  waitTimerStatus();
-  t1_ref = clockbits();
-  offset = loadedValue - readCurrentTimer();
-  while( timerValue < 1000 )
-  {
-    loop1ms++;
-    loadTimerValue( loadedValue + offset );
-    waitTimerStatus();
-    t1_ref = clockbits();
-    for( i=0; i < loop1ms; i++ )
-       outport_byte( SLOW_DOWN_IO, 0 );	/* write is # 1us */
-    t2_ref = clockbits();
-    timerValue = TICK_TO_US( t1_ref - t2_ref );  /* timer0 is decrementing number of ticks  */
+  
+  if (fastLoopGranularity != 0 ) {
+    while(1) {
+      int previousSign = 0; /* 0 = unset, 1 = incrementing,  2 = decrementing */
+      Timer0Reset();
+      if (slowLoop1ms != 0) slowLoop(slowLoop1ms);
+      fastLoop(fastLoop1ms);
+      currentClockBits = readTimer0();
+      if (currentClockBits > targetClockBits) {
+	if ((currentClockBits - targetClockBits) < fastLoopGranularity)
+	  break;
+	else {
+	  --fastLoop1ms;
+	  if (previousSign == 0) previousSign = 2;
+	  if (previousSign == 1) break;
+	}
+      }
+      else {
+	if ((targetClockBits - currentClockBits) < fastLoopGranularity)
+	  break;
+	else {
+	  ++fastLoop1ms;
+	  if (previousSign == 0) previousSign = 1;
+	  if (previousSign == 2) break;
+	}
+      }
+    }
   }
-  printk( "Calibrate_loop_1ms timerValue=%d, loop1ms=%d, t1_ref=%x, clockbits=%x, delta=%d\n", 
-		   timerValue, loop1ms, t1_ref, t2_ref, t1_ref - t2_ref );
+#ifdef DEBUG_CALIBRATE  
+  printk("slowLoop1ms = %u, fastLoop1ms = %u\n", slowLoop1ms, fastLoop1ms);
+#endif  
   rtems_interrupt_enable(level);
+  
 }
 
-
-
 /*-------------------------------------------------------------------------+
 |         Function: Wait_X_1ms
 |      Description: loop which waits at least timeToWait ms
@@ -446,11 +542,13 @@ Calibrate_loop_1ms(void)
 void
 Wait_X_ms( unsigned int timeToWait){
 
-  unsigned int i, j;
+  unsigned int j;
+
+  for (j=0; j<timeToWait ; j++) {
+    if (slowLoop1ms != 0) slowLoop(slowLoop1ms);
+    fastLoop(fastLoop1ms);
+  }
 
-  for (j=0; j<timeToWait ; j++)
-    for (i=0; i<loop1ms; i++)
-      outport_byte(SLOW_DOWN_IO, 0);	/* write is # 1us */
 }