diff options
| -rw-r--r-- | c/src/lib/libbsp/i386/pc386/ChangeLog | 13 | ||||
| -rw-r--r-- | c/src/lib/libbsp/i386/pc386/console/inch.c | 229 | ||||
| -rw-r--r-- | c/src/lib/libbsp/i386/pc386/console/keyboard.c | 12 | ||||
| -rw-r--r-- | c/src/lib/libbsp/i386/pc386/console/pc_keyb.c | 10 | ||||
| -rw-r--r-- | c/src/lib/libbsp/i386/pc386/console/vt.c | 7 | ||||
| -rw-r--r-- | c/src/lib/libbsp/i386/pc386/include/bsp.h | 8 | ||||
| -rw-r--r-- | c/src/lib/libbsp/i386/pc386/start/start.S | 13 | ||||
| -rw-r--r-- | c/src/lib/libbsp/i386/pc386/startup/bspstart.c | 8 | ||||
| -rw-r--r-- | c/src/lib/libbsp/i386/pc386/timer/timer.c | 298 |
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 */ } |