diff --git a/P8000_WDC_Emulator/P8000_WDC_Emulator/wdc_drv_pata.c b/P8000_WDC_Emulator/P8000_WDC_Emulator/wdc_drv_pata.c
index fe949e7..f30a25a 100644
--- a/P8000_WDC_Emulator/P8000_WDC_Emulator/wdc_drv_pata.c
+++ b/P8000_WDC_Emulator/P8000_WDC_Emulator/wdc_drv_pata.c
@@ -26,7 +26,7 @@
  */
 
 /*
- * $Id: wdc_drv_pata.c,v 1.4 2012/06/15 22:42:42 olivleh1 Exp $
+ * $Id: wdc_drv_pata.c,v 1.5 2012/06/16 00:33:26 olivleh1 Exp $
  */
 
 #include "wdc_config.h"
@@ -39,173 +39,150 @@
 uint8_t pata_bsy();
 uint8_t pata_rdy();
 uint8_t pata_drq();
-void pata_cmd ( uint8_t cmd );
 void ata_identify();
+void pata_set_highbyte(uint8_t byte);
+void pata_set_lowbyte(uint8_t byte);
+uint8_t pata_get_highbyte();
+uint8_t pata_get_lowbyte();
+void set_io_register( uint8_t ioreg );
+uint8_t read_io_register( uint8_t ioreg );
+void write_io_register( uint8_t ioreg, uint8_t byte );
+uint16_t pata_read_single_word ();
+
+
+/*
+ *                                                 +----- CS0
+ *                                                 |+---- CS1
+ *                                                 ||+--- DA0
+ *                                                 |||+-- DA1
+ *                                                 ||||+- DA2
+ *                                                 |||||*/
+#define PATA_RD_STATUS_REGISTER         0x0F  /* 0b01111*/
+#define PATA_WR_COMMAND_REGISTER        0x0F  /* 0b01111*/
+#define PATA_WR_DEVICE_CONTROL_REGISTER 0x0B  /* 0b01011*/
+#define PATA_RD_DATA_REGISTER           0x08  /* 0b01000*/
 
 #define ata_ctl_high() ata_cs0_disable();ata_cs1_disable();ata_da0_disable();ata_da1_disable();ata_da2_disable();
 
+
+/*
+ * Private functions
+ */
+
+
+uint8_t pata_rdy()
+{
+    return (read_io_register ( PATA_RD_STATUS_REGISTER ) & ATA_STAT_RDY )?1:0;
+}
+
+uint8_t pata_bsy()
+{
+    return (read_io_register ( PATA_RD_STATUS_REGISTER ) & ATA_STAT_BSY )?1:0;
+}
+
+uint8_t pata_drq()
+{
+    return (read_io_register ( PATA_RD_STATUS_REGISTER ) & ATA_STAT_DRQ )?1:0;
+}
+
+void set_io_register ( uint8_t ioreg )
+{
+    if ( ! ( ioreg & ( 1 << 0 ) ) )
+        ata_da2_enable();
+    if ( ! ( ioreg & ( 1 << 1 ) ) )
+        ata_da1_enable();
+    if ( ! ( ioreg & ( 1 << 2 ) ) )
+        ata_da0_enable();
+    if ( ! ( ioreg & ( 1 << 3 ) ) )
+        ata_cs1_enable();
+    if ( ! ( ioreg & ( 1 << 4 ) ) )
+        ata_cs0_enable();
+}
+
+
+void pata_set_lowbyte ( uint8_t byte )
+{
+    port_data_set ( byte );
+    enable_rdwrtoata();
+    nop();
+    nop();
+    disable_rdwrtoata();
+}
+
 void pata_set_highbyte ( uint8_t byte )
 {
     port_data_set ( byte );
     enable_atalatch();
     nop();
     nop();
-    init_addressdecoder();
+    disable_atalatch();
 }
 
-void pata_set_lowbyte ( uint8_t byte )
-{
-    port_data_set ( byte );
-}
-
-void pata_send_data()
-{
-    enable_rdwrtoata();
-    _delay_us ( 10 );
-    init_addressdecoder();
-}
-
-
 uint8_t pata_get_lowbyte ()
 {
+    uint8_t byte;
+
+    enable_rdwrtoata();
     nop();
     nop();
-    return port_data_get();
+    byte = port_data_get();
+    disable_rdwrtoata();
+
+    return(byte);
 }
 
 uint8_t pata_get_highbyte ()
 {
     uint8_t byte;
+
     enable_atalatch();
     nop();
     nop();
-    nop();
     byte = port_data_get();
-    init_addressdecoder();
+    disable_atalatch();
+
     return byte;
 }
 
-uint8_t pata_read_byte()
+uint8_t read_io_register ( uint8_t ioreg )
 {
     uint8_t byte;
 
     configure_port_data_read();
+
+    set_io_register ( ioreg );
     ata_rd_enable();
-    enable_rdwrtoata();
-    _delay_us ( 1 );
     byte = pata_get_lowbyte();
-    init_addressdecoder();
     ata_rd_disable();
+    ata_ctl_high();
 
     return byte;
 }
 
-void pata_write_byte ( uint8_t byte )
+void write_io_register ( uint8_t ioreg, uint8_t byte )
 {
     configure_port_data_write();
-    pata_set_lowbyte ( byte );
-    enable_rdwrtoata();
+
+    set_io_register ( ioreg );
     ata_wr_enable();
-    _delay_us ( 1 );
+    pata_set_lowbyte ( byte );
     ata_wr_disable();
-    init_addressdecoder();
-
-}
-
-
-uint8_t ata_get_status_byte ( void )
-{
-    uint8_t byte;
-    ata_cs0_enable();
-    byte = pata_read_byte();
     ata_ctl_high();
-
-    return byte;
 }
 
-uint8_t ata_get_alt_status_byte ( void )
-{
-    uint8_t byte;
-    ata_cs1_enable();
-    ata_da0_enable();
-    byte = pata_read_byte();
-    ata_ctl_high();
-
-    return byte;
-}
-
-
-uint16_t pata_read_word ( uint8_t ofs, uint8_t count )
-{
-    uint16_t word;
-    uint8_t byte_l;
-    uint8_t byte_h;
-    uint8_t c = 0;
-
-    ata_da0_enable();
-    ata_da1_enable();
-    ata_da2_enable();
-    ata_cs0_enable();
-
-    do {
-
-        ata_rd_enable();
-        enable_rdwrtoata();
-        nop();
-        nop();
-        nop();
-        byte_l = port_data_get();
-        disable_rdwrtoata();
-        enable_atalatch();
-        nop();
-        nop();
-        nop();
-        byte_h = port_data_get();
-        disable_atalatch();
-        ata_rd_disable();
-
-        if ( count && ( c >= ofs ) ) {
-            uart_putc ( byte_h );
-            uart_putc ( byte_l );
-            count--;
-        }
-    } while ( ++c );
-
-    ata_ctl_high();
-
-    word = byte_h << 8;
-    word |= byte_l;
-
-    return ( word );
-}
-
-
 uint16_t pata_read_single_word ()
 {
     uint16_t word;
     uint8_t byte_l;
     uint8_t byte_h;
 
-    ata_da0_enable();
-    ata_da1_enable();
-    ata_da2_enable();
-    ata_cs0_enable();
+    configure_port_data_read();
 
+    set_io_register ( PATA_RD_DATA_REGISTER );
     ata_rd_enable();
-    enable_rdwrtoata();
-    nop();
-    nop();
-    nop();
-    byte_l = port_data_get();
-    disable_rdwrtoata();
-    enable_atalatch();
-    nop();
-    nop();
-    nop();
-    byte_h = port_data_get();
-    disable_atalatch();
+    byte_l = pata_get_lowbyte();
+    byte_h = pata_get_highbyte();
     ata_rd_disable();
-
     ata_ctl_high();
 
     word = byte_h << 8;
@@ -214,148 +191,67 @@ uint16_t pata_read_single_word ()
     return ( word );
 }
 
-uint8_t pata_init ()
-{
-    
-    // is needed for some disks (for example Maxtor 6L080J4)
-    _delay_ms ( 100 );
-
-    uart_puts_p ( PSTR ( "Init start\n" ) );
-
-    while ( ( !pata_rdy() ) & pata_bsy() );
-
-    uart_putc_hex ( ata_get_status_byte() );
-
-    /*device register*/
-    ata_da0_enable();
-    ata_cs0_enable();
-#ifdef PIO
-    pata_write_byte ( 0 );          // Drive 0, PIO
-#else
-    pata_write_byte ( 0xE0 );          // Drive 0, LBA
-#endif
-    ata_ctl_high();
-
-#ifdef PIO
-    while ( pata_bsy() );
-
-    //device control
-    ata_da0_enable();
-    ata_cs1_enable();
-    pata_write_byte ( 0x40 | 0x20 ); // reset + no interrupt
-    ata_ctl_high();
-
-    _delay_ms ( 20 );
-
-    ata_da0_enable();
-    ata_cs1_enable();
-    pata_write_byte ( 0x20 );     // no interrupt
-    ata_ctl_high();
-
-    _delay_ms ( 20 );
-
-    while ( ( !pata_rdy() ) & pata_bsy() );
-
-    //feature register
-    ata_da1_enable();
-    ata_da2_enable();
-    ata_cs0_enable();
-    pata_write_byte ( 3 );      // PIO
-    ata_ctl_high();
-
-    //sector count
-    ata_da0_enable();
-    ata_da2_enable();
-    ata_cs0_enable();
-    pata_write_byte ( 1 );
-    ata_ctl_high();
-
-    //command
-    ata_cs0_enable();
-    pata_write_byte ( 0xEF );   // Set Features
-    ata_ctl_high();
-#else
-
-    while ( ( !pata_rdy() ) & pata_bsy() );
-    /*command*/
-    ata_cs0_enable();
-    pata_write_byte ( 0x10 );   // Recalibrate
-    ata_ctl_high();
-#endif
-    while ( pata_bsy() );
-
-    uart_puts_p ( PSTR ( "disk is now ready\n" ) );
-
-    ata_identify();
-
-    return 1;
-}
-
-
 void ata_identify()
 {
     uint32_t dword;
     uint16_t word;
-    uint8_t byte_l,byte_h;
+    uint8_t byte_l, byte_h;
 
-    /*command*/
     while ( pata_bsy() );
-    ata_cs0_enable();
-    pata_write_byte ( 0xEc );   // Identify
-    ata_ctl_high();
+    write_io_register ( PATA_WR_COMMAND_REGISTER, CMD_IDENTIFY_DEVICE );
 
     /* Wait for BSY goes low and DRQ goes high */
-    while ( ( ata_get_status_byte() & ( ATA_STAT_BSY | ATA_STAT_DRQ ) ) != ATA_STAT_DRQ );
+    while ( pata_bsy() & !pata_drq() );
 
     word = pata_read_single_word(); // General configuration bit
     word = pata_read_single_word();
     uart_puts_p ( PSTR ( "Number of logical cylinders: " ) );
-    uart_putw_dec(word);
-    uart_putc('\n');
+    uart_putw_dec ( word );
+    uart_putc ( '\n' );
     word = pata_read_single_word(); // Specific configuration
     word = pata_read_single_word();
     uart_puts_p ( PSTR ( "Number of logical heads: " ) );
-    uart_putw_dec(word);
-    uart_putc('\n');
+    uart_putw_dec ( word );
+    uart_putc ( '\n' );
     word = pata_read_single_word();
     word = pata_read_single_word();
     word = pata_read_single_word();
     uart_puts_p ( PSTR ( "Number of logical sectors per logical track: " ) );
-    uart_putw_dec(word);
-    uart_putc('\n');
+    uart_putw_dec ( word );
+    uart_putc ( '\n' );
     word = pata_read_single_word(); // CompactFlash
     word = pata_read_single_word(); // CompactFlash
     word = pata_read_single_word(); // Retired
     uart_puts_p ( PSTR ( "Serial number: " ) );
-    for(uint8_t i=0;i<10;i++) {
+    for ( uint8_t i = 0; i < 10; i++ ) {
         word = pata_read_single_word();
         byte_h = word >> 8;
         byte_l = word & 0x00FF;
-        uart_putc(byte_h);
-        uart_putc(byte_l);
+        uart_putc ( byte_h );
+        uart_putc ( byte_l );
     }
-    uart_putc('\n');
+    uart_putc ( '\n' );
     word = pata_read_single_word(); // Retired
     word = pata_read_single_word(); // Retired
     word = pata_read_single_word(); // Obsolete
     uart_puts_p ( PSTR ( "Firmware revision: " ) );
-    for(uint8_t i=0;i<4;i++) {
+    for ( uint8_t i = 0; i < 4; i++ ) {
         word = pata_read_single_word();
         byte_h = word >> 8;
         byte_l = word & 0x00FF;
-        uart_putc(byte_h);
-        uart_putc(byte_l);
+        uart_putc ( byte_h );
+        uart_putc ( byte_l );
     }
-    uart_putc('\n');
+    uart_putc ( '\n' );
     uart_puts_p ( PSTR ( "Model number: " ) );
-    for(uint8_t i=0;i<20;i++) {
+    for ( uint8_t i = 0; i < 20; i++ ) {
         word = pata_read_single_word();
         byte_h = word >> 8;
         byte_l = word & 0x00FF;
-        uart_putc(byte_h);
-        uart_putc(byte_l);
+        uart_putc ( byte_h );
+        uart_putc ( byte_l );
     }
-    uart_putc('\n');
+    uart_putc ( '\n' );
     word = pata_read_single_word(); // no idea
     word = pata_read_single_word(); // Reserved
     word = pata_read_single_word(); // Capabilities
@@ -365,71 +261,64 @@ void ata_identify()
     word = pata_read_single_word(); // Reserved
     word = pata_read_single_word();
     uart_puts_p ( PSTR ( "Number of current logical cylinders: " ) );
-    uart_putw_dec(word);
-    uart_putc('\n');
+    uart_putw_dec ( word );
+    uart_putc ( '\n' );
     word = pata_read_single_word();
     uart_puts_p ( PSTR ( "Number of current logical heads: " ) );
-    uart_putw_dec(word);
-    uart_putc('\n');
+    uart_putw_dec ( word );
+    uart_putc ( '\n' );
     word = pata_read_single_word();
     uart_puts_p ( PSTR ( "Number of current logical sectors per track: " ) );
-    uart_putw_dec(word);
-    uart_putc('\n');
+    uart_putw_dec ( word );
+    uart_putc ( '\n' );
     word = pata_read_single_word();
     dword = word;
     word = pata_read_single_word();
-    dword |= (uint32_t)word<<16;
+    dword |= ( uint32_t ) word << 16;
     uart_puts_p ( PSTR ( "Current capacity in sectors: " ) );
-    uart_putdw_dec(dword);
-    uart_putc('\n');
+    uart_putdw_dec ( dword );
+    uart_putc ( '\n' );
     word = pata_read_single_word(); // Reserved
     word = pata_read_single_word();
     dword = word;
     word = pata_read_single_word();
-    dword |= (uint32_t)word <<16;
+    dword |= ( uint32_t ) word << 16;
     uart_puts_p ( PSTR ( "Total number of user addressable sectors (LBA mode only): " ) );
-    uart_putdw_dec(dword);
-    uart_putc('\n');
+    uart_putdw_dec ( dword );
+    uart_putc ( '\n' );
     word = pata_read_single_word(); // Obsolete
-    for(uint8_t i=0;i<193;i++)
-    word = pata_read_single_word(); // ignore it
+    for ( uint8_t i = 0; i < 193; i++ )
+        word = pata_read_single_word(); // ignore it
 }
 
-uint8_t pata_rdy()
-{
-    uint8_t i;
-    if ( ata_get_status_byte() & ATA_STAT_RDY )
-        i = 1;
-    else
-        i = 0;
-    return i;
-}
 
-uint8_t pata_bsy()
-{
-    uint8_t i;
-    if ( ata_get_status_byte() & ATA_STAT_BSY )
-        i = 1;
-    else
-        i = 0;
-    return i;
-}
 
-uint8_t pata_drq()
-{
-    uint8_t i;
-    if ( ata_get_status_byte() & ATA_STAT_DRQ )
-        i = 1;
-    else
-        i = 0;
-    return i;
-}
+/*
+ * Public functions
+ */
 
-void pata_cmd ( uint8_t cmd )
+uint8_t pata_init ()
 {
-    ata_cs0_enable();
-    pata_write_byte ( cmd );
-    ata_ctl_high();
+
+    // is needed for some disks (for example Maxtor 6L080J4)
+    _delay_ms ( 200 );
+
+    uart_puts_p ( PSTR ( "Init start\n" ) );
+    while ( ( !pata_rdy() ) & pata_bsy() );
+
+    /*set drive 0 to LBA mode*/
+    write_io_register ( PATA_WR_DEVICE_CONTROL_REGISTER, ATA_LBA_DRIVE_0 );
+    while ( ( !pata_rdy() ) & pata_bsy() );
+
+    /*recalibrate*/
+    write_io_register ( PATA_WR_COMMAND_REGISTER, CMD_RECALIBRATE );
+    while ( pata_bsy() );
+
+    uart_puts_p ( PSTR ( "disk is now ready\n" ) );
+
+    ata_identify();
+
+    return 1;
 }
 
 uint8_t pata_read_block ( uint32_t addr, uint8_t *buffer )
diff --git a/P8000_WDC_Emulator/P8000_WDC_Emulator/wdc_drv_pata.h b/P8000_WDC_Emulator/P8000_WDC_Emulator/wdc_drv_pata.h
index 84b39cb..a927a20 100644
--- a/P8000_WDC_Emulator/P8000_WDC_Emulator/wdc_drv_pata.h
+++ b/P8000_WDC_Emulator/P8000_WDC_Emulator/wdc_drv_pata.h
@@ -26,7 +26,7 @@
  */
 
 /*
- * $Id: wdc_drv_pata.h,v 1.4 2012/06/15 22:42:42 olivleh1 Exp $
+ * $Id: wdc_drv_pata.h,v 1.5 2012/06/16 00:33:26 olivleh1 Exp $
  */
 
 #ifndef WDC_DRV_PATA_H_
@@ -50,8 +50,8 @@ extern uint8_t pata_write_multiblock ( uint32_t addr, uint8_t *buffer, uint8_t n
 #define ATA_STAT_IDX  0x02  // ATA index
 #define ATA_STAT_ERR  0x01  // ATA error
 
-#define ATA_DH_DEV0 0xE0
-#define ATA_DH_DEV1 0xF0
+#define ATA_LBA_DRIVE_0 0xE0
+#define ATA_LBA_DRIVE_1 0xF0
 
 //ATA commands (from ATA-3),
 #define CMD_CFA_ERASE_SECTORS            0xC0