/* Write data and read response */
static status_t ps2_write_read_command( uint8 nCmd, uint8* pnData )
{
uint32 nFlags;
int nError;
spinlock_cli( &g_sLock, nFlags );
nError = ps2_wait_write();
if( nError < 0 ) {
spinunlock_restore( &g_sLock, nFlags );
return( -EIO );
}
outb( nCmd, PS2_COMMAND_REG );
nError = ps2_wait_write();
if( nError < 0 ) {
spinunlock_restore( &g_sLock, nFlags );
return( -EIO );
}
outb( *pnData, PS2_DATA_REG );
nError = ps2_wait_read();
if( nError < 0 ) {
spinunlock_restore( &g_sLock, nFlags );
return( -EIO );
}
*pnData = inb( PS2_DATA_REG );
spinunlock_restore( &g_sLock, nFlags );
return( 0 );
}
void ps2_init( )
{
uint8 nCommand;
//Disable auxilliary devices
ps2_wait_write( );
outb( PS2_COMMAND, PS2_COM_DISABLE_AUX );
//Disable keyboard
ps2_wait_write( );
outb( PS2_COMMAND, PS2_COM_DISABLE );
//Get current keyboard command byte
ps2_wait_write( );
outb( PS2_COMMAND, PS2_COM_READ_COMMAND );
ps2_wait_read( );
nCommand = inb( PS2_DATA );
//Check if auxiliary devices are available
if (nCommand & 0x10)
bPS2AuxAvailable = true;
//Start keyboard self-test
ps2_wait_write( );
outb( PS2_COMMAND, PS2_COM_SELFTEST );
ps2_wait_read( );
if ( inb( PS2_DATA ) != PS2_TEST_SUCCESS )
kpanic( "Failed PS/2 Self Test." );
//Play around with the command byte
nCommand |= 0x1; //Enable interrupts
if (nCommand & 0x40)
nCommand ^= 0x40; //Disable scancode translation
ps2_wait_write( );
outb( PS2_COMMAND, PS2_COM_WRITE_COMMAND );
ps2_wait_write( );
outb( PS2_DATA, nCommand ); //Upload byte back to ps2
//Load the IRQ Handler and keymap
irq_loadHandler( 1, (uint32) &ps2_irq );
pKeymap = PS2_SCANCODE_US;
//Re-enable auxiliary devices (if available)
if ( ps2_auxAvailable( ) )
{
ps2_wait_write( );
outb( PS2_COMMAND, PS2_COM_ENABLE_AUX );
}
//Re-enable the keyboard
ps2_wait_write( );
outb( PS2_COMMAND, PS2_COM_ENABLE );
}