Reorganize ata code; reduce stack usage.

This fixes an issue with freedos lbacache - the bios was overrunning
the stack on disk requests.
The code has been simplified by extracting common code.
Some handlers moved to inline code to reduce overall stack usage.

4 files changed, 405 insertions, 355 deletions

diff --git a/src/ata.c b/src/ata.c
index 78701f6..3aea3ad 100644
--- a/src/ata.c
+++ b/src/ata.c
@@ -28,7 +28,7 @@
 static int
 await_ide(u8 when_done, u16 base, u16 timeout)
 {
-    u32 time=0,last=0;
+    u32 time=0, last=0;
     // for the times you're supposed to throw one away
     u16 status = inb(base + ATA_CB_STAT);
     for (;;) {
@@ -138,90 +138,97 @@ ata_reset(u16 device)
       // 5 : more sectors to read/verify
       // 6 : no sectors left to write
       // 7 : more sectors to write
-u16
-ata_cmd_data(u16 device, u16 command, u16 count, u16 cylinder
-             , u16 head, u16 sector, u32 lba, u16 segment, u16 offset)
-{
-    DEBUGF("ata_cmd_data d=%d cmd=%d count=%d c=%d h=%d s=%d"
-           " lba=%d seg=%x off=%x\n"
-           , device, command, count, cylinder, head, sector
-           , lba, segment, offset);
-
-    u8 channel = device / 2;
-    u8 slave   = device % 2;
 
+static int
+send_cmd(struct ata_pio_command *cmd)
+{
+    u16 biosid = cmd->biosid;
+    u8 channel = biosid / 2;
     u16 iobase1 = GET_EBDA(ata.channels[channel].iobase1);
     u16 iobase2 = GET_EBDA(ata.channels[channel].iobase2);
-    u8 mode     = GET_EBDA(ata.devices[device].mode);
-
-    // Reset count of transferred data
-    SET_EBDA(ata.trsfsectors,0);
-    SET_EBDA(ata.trsfbytes,0L);
 
     u8 status = inb(iobase1 + ATA_CB_STAT);
     if (status & ATA_CB_STAT_BSY)
         return 1;
 
     outb(ATA_CB_DC_HD15 | ATA_CB_DC_NIEN, iobase2 + ATA_CB_DC);
-
-    // sector will be 0 only on lba access. Convert to lba-chs
-    if (sector == 0) {
-        if ((count >= 1 << 8) || (lba + count >= 1UL << 28)) {
-            outb(0x00, iobase1 + ATA_CB_FR);
-            outb((count >> 8) & 0xff, iobase1 + ATA_CB_SC);
-            outb(lba >> 24, iobase1 + ATA_CB_SN);
-            outb(0, iobase1 + ATA_CB_CL);
-            outb(0, iobase1 + ATA_CB_CH);
-            command |= 0x04;
-            count &= (1UL << 8) - 1;
-            lba &= (1UL << 24) - 1;
-        }
-        sector = (u16) (lba & 0x000000ffL);
-        cylinder = (u16) ((lba>>8) & 0x0000ffffL);
-        head = ((u16) ((lba>>24) & 0x0000000fL)) | ATA_CB_DH_LBA;
+    if (cmd->command & 0x04) {
+        outb(0x00, iobase1 + ATA_CB_FR);
+        outb(cmd->sector_count2, iobase1 + ATA_CB_SC);
+        outb(cmd->lba_low2, iobase1 + ATA_CB_SN);
+        outb(cmd->lba_mid2, iobase1 + ATA_CB_CL);
+        outb(cmd->lba_high2, iobase1 + ATA_CB_CH);
     }
-
-    outb(0x00, iobase1 + ATA_CB_FR);
-    outb(count, iobase1 + ATA_CB_SC);
-    outb(sector, iobase1 + ATA_CB_SN);
-    outb(cylinder & 0x00ff, iobase1 + ATA_CB_CL);
-    outb(cylinder >> 8, iobase1 + ATA_CB_CH);
-    outb((slave ? ATA_CB_DH_DEV1 : ATA_CB_DH_DEV0) | (u8) head
-         , iobase1 + ATA_CB_DH);
-    outb(command, iobase1 + ATA_CB_CMD);
+    outb(cmd->feature, iobase1 + ATA_CB_FR);
+    outb(cmd->sector_count, iobase1 + ATA_CB_SC);
+    outb(cmd->lba_low, iobase1 + ATA_CB_SN);
+    outb(cmd->lba_mid, iobase1 + ATA_CB_CL);
+    outb(cmd->lba_high, iobase1 + ATA_CB_CH);
+    outb(cmd->device, iobase1 + ATA_CB_DH);
+    outb(cmd->command, iobase1 + ATA_CB_CMD);
 
     await_ide(NOT_BSY_DRQ, iobase1, IDE_TIMEOUT);
-    status = inb(iobase1 + ATA_CB_STAT);
 
+    status = inb(iobase1 + ATA_CB_STAT);
     if (status & ATA_CB_STAT_ERR) {
-        DEBUGF("ata_cmd_data : read error\n");
+        DEBUGF("send_cmd : read error\n");
         return 2;
     }
     if (!(status & ATA_CB_STAT_DRQ)) {
-        DEBUGF("ata_cmd_data : DRQ not set (status %02x)\n"
+        DEBUGF("send_cmd : DRQ not set (status %02x)\n"
                , (unsigned) status);
         return 3;
     }
 
-    // FIXME : move seg/off translation here
+    return 0;
+}
+
+int
+ata_transfer(struct ata_pio_command *cmd)
+{
+    DEBUGF("ata_transfer id=%d cmd=%d lba=%d count=%d seg=%x off=%x\n"
+           , cmd->biosid, cmd->command
+           , (cmd->lba_high << 16) | (cmd->lba_mid << 8) | cmd->lba_low
+           , cmd->sector_count, cmd->segment, cmd->offset);
+
+    // Reset count of transferred data
+    SET_EBDA(ata.trsfsectors,0);
+    SET_EBDA(ata.trsfbytes,0L);
+
+    int ret = send_cmd(cmd);
+    if (ret)
+        return ret;
+
+    u16 biosid = cmd->biosid;
+    u8 channel  = biosid / 2;
+    u16 iobase1 = GET_EBDA(ata.channels[channel].iobase1);
+    u16 iobase2 = GET_EBDA(ata.channels[channel].iobase2);
+    u8 mode     = GET_EBDA(ata.devices[biosid].mode);
+    int iswrite = (cmd->command & ~0x40) == ATA_CMD_WRITE_SECTORS;
 
     irq_enable();
 
+    u16 segment = cmd->segment;
+    u16 offset = cmd->offset;
     u8 current = 0;
-    while (1) {
-        if (offset > 0xf800) {
+    u16 count = cmd->sector_count;
+    u8 status;
+    for (;;) {
+        if (offset >= 0xf800) {
             offset -= 0x800;
             segment += 0x80;
         }
 
-        if (command == ATA_CMD_WRITE_SECTORS) {
+        if (iswrite) {
             // Write data to controller
+            DEBUGF("Write sector id=%d dest=%x:%x\n", biosid, segment, offset);
             if (mode == ATA_MODE_PIO32)
                 outsl_seg(iobase1, segment, offset, 512 / 4);
             else
                 outsw_seg(iobase1, segment, offset, 512 / 2);
         } else {
             // Read data from controller
+            DEBUGF("Read sector id=%d dest=%x:%x\n", biosid, segment, offset);
             if (mode == ATA_MODE_PIO32)
                 insl_seg(iobase1, segment, offset, 512 / 4);
             else
@@ -239,7 +246,7 @@ ata_cmd_data(u16 device, u16 command, u16 count, u16 cylinder
         status &= (ATA_CB_STAT_BSY | ATA_CB_STAT_RDY | ATA_CB_STAT_DRQ
                    | ATA_CB_STAT_ERR);
         if (status != (ATA_CB_STAT_RDY | ATA_CB_STAT_DRQ)) {
-            DEBUGF("ata_cmd_data : more sectors left (status %02x)\n"
+            DEBUGF("ata_transfer : more sectors left (status %02x)\n"
                    , (unsigned) status);
             return 5;
         }
@@ -247,17 +254,18 @@ ata_cmd_data(u16 device, u16 command, u16 count, u16 cylinder
 
     status &= (ATA_CB_STAT_BSY | ATA_CB_STAT_RDY | ATA_CB_STAT_DF
                | ATA_CB_STAT_DRQ | ATA_CB_STAT_ERR);
-    if (command != ATA_CMD_WRITE_SECTORS)
+    if (!iswrite)
         status &= ~ATA_CB_STAT_DF;
     if (status != ATA_CB_STAT_RDY ) {
-        DEBUGF("ata_cmd_data : no sectors left (status %02x)\n"
+        DEBUGF("ata_transfer : no sectors left (status %02x)\n"
                , (unsigned) status);
         return 4;
     }
 
+    irq_disable();
+
     // Enable interrupts
     outb(ATA_CB_DC_HD15, iobase2+ATA_CB_DC);
-    irq_disable();
     return 0;
 }
 
@@ -270,23 +278,17 @@ ata_cmd_data(u16 device, u16 command, u16 count, u16 cylinder
       // 2 : BUSY bit set
       // 3 : error
       // 4 : not ready
-u16
-ata_cmd_packet(u16 device, u8 *cmdbuf, u8 cmdlen, u16 header
-               , u32 length, u8 inout, u16 bufseg, u16 bufoff)
+int
+ata_cmd_packet(u16 biosid, u8 *cmdbuf, u8 cmdlen
+               , u16 header, u32 length, u16 bufseg, u16 bufoff)
 {
-    DEBUGF("ata_cmd_packet d=%d cmdlen=%d h=%d l=%d inout=%d"
+    DEBUGF("ata_cmd_packet d=%d cmdlen=%d h=%d l=%d"
            " seg=%x off=%x\n"
-           , device, cmdlen, header, length, inout
+           , biosid, cmdlen, header, length
            , bufseg, bufoff);
 
-    u8 channel = device / 2;
-    u8 slave = device % 2;
-
-    // Data out is not supported yet
-    if (inout == ATA_DATA_OUT) {
-        BX_INFO("ata_cmd_packet: DATA_OUT not supported yet\n");
-        return 1;
-    }
+    u8 channel = biosid / 2;
+    u8 slave = biosid % 2;
 
     // The header length must be even
     if (header & 1) {
@@ -296,155 +298,133 @@ ata_cmd_packet(u16 device, u8 *cmdbuf, u8 cmdlen, u16 header
 
     u16 iobase1 = GET_EBDA(ata.channels[channel].iobase1);
     u16 iobase2 = GET_EBDA(ata.channels[channel].iobase2);
-    u8 mode     = GET_EBDA(ata.devices[device].mode);
-
-    if (cmdlen < 12)
-        cmdlen=12;
-    if (cmdlen > 12)
-        cmdlen=16;
-    cmdlen>>=1;
+    u8 mode     = GET_EBDA(ata.devices[biosid].mode);
+
+    struct ata_pio_command cmd;
+    cmd.sector_count = 0;
+    cmd.feature = 0;
+    cmd.lba_low = 0;
+    cmd.lba_mid = 0xf0;
+    cmd.lba_high = 0xff;
+    cmd.device = slave ? ATA_CB_DH_DEV1 : ATA_CB_DH_DEV0;
+    cmd.command = ATA_CMD_PACKET;
+
+    cmd.biosid = biosid;
+    int ret = send_cmd(&cmd);
+    if (ret)
+        return ret;
 
     // Reset count of transferred data
     SET_EBDA(ata.trsfsectors,0);
     SET_EBDA(ata.trsfbytes,0L);
 
-    u8 status = inb(iobase1 + ATA_CB_STAT);
-    if (status & ATA_CB_STAT_BSY)
-        return 2;
-
-    outb(ATA_CB_DC_HD15 | ATA_CB_DC_NIEN, iobase2 + ATA_CB_DC);
-    outb(0x00, iobase1 + ATA_CB_FR);
-    outb(0x00, iobase1 + ATA_CB_SC);
-    outb(0x00, iobase1 + ATA_CB_SN);
-    outb(0xfff0 & 0x00ff, iobase1 + ATA_CB_CL);
-    outb(0xfff0 >> 8, iobase1 + ATA_CB_CH);
-    outb(slave ? ATA_CB_DH_DEV1 : ATA_CB_DH_DEV0, iobase1 + ATA_CB_DH);
-    outb(ATA_CMD_PACKET, iobase1 + ATA_CB_CMD);
-
-    // Device should ok to receive command
-    await_ide(NOT_BSY_DRQ, iobase1, IDE_TIMEOUT);
-    status = inb(iobase1 + ATA_CB_STAT);
-
-    if (status & ATA_CB_STAT_ERR) {
-        DEBUGF("ata_cmd_packet : error, status is %02x\n", status);
-        return 3;
-    } else if ( !(status & ATA_CB_STAT_DRQ) ) {
-        DEBUGF("ata_cmd_packet : DRQ not set (status %02x)\n"
-               , (unsigned) status);
-        return 4;
-    }
+    irq_enable();
 
     // Send command to device
-    irq_enable();
+    outsw_seg(iobase1, GET_SEG(SS), (u32)cmdbuf, cmdlen / 2);
 
-    outsw_seg(iobase1, GET_SEG(SS), (u32)cmdbuf, cmdlen);
+    u8 status;
+    u16 loops = 0;
+    for (;;) {
+        if (loops == 0) {//first time through
+            status = inb(iobase2 + ATA_CB_ASTAT);
+            await_ide(NOT_BSY_DRQ, iobase1, IDE_TIMEOUT);
+        } else
+            await_ide(NOT_BSY, iobase1, IDE_TIMEOUT);
+        loops++;
 
-    if (inout == ATA_DATA_NO) {
-        await_ide(NOT_BSY, iobase1, IDE_TIMEOUT);
         status = inb(iobase1 + ATA_CB_STAT);
-    } else {
-        u16 loops = 0;
-        while (1) {
-            if (loops == 0) {//first time through
-                status = inb(iobase2 + ATA_CB_ASTAT);
-                await_ide(NOT_BSY_DRQ, iobase1, IDE_TIMEOUT);
-            } else
-                await_ide(NOT_BSY, iobase1, IDE_TIMEOUT);
-            loops++;
+        inb(iobase1 + ATA_CB_SC);
 
-            status = inb(iobase1 + ATA_CB_STAT);
-            inb(iobase1 + ATA_CB_SC);
+        // Check if command completed
+        if(((inb(iobase1 + ATA_CB_SC)&0x7)==0x3) &&
+           ((status & (ATA_CB_STAT_RDY | ATA_CB_STAT_ERR)) == ATA_CB_STAT_RDY))
+            break;
 
-            // Check if command completed
-            if(((inb(iobase1 + ATA_CB_SC)&0x7)==0x3) &&
-               ((status & (ATA_CB_STAT_RDY | ATA_CB_STAT_ERR)) == ATA_CB_STAT_RDY))
-                break;
+        if (status & ATA_CB_STAT_ERR) {
+            DEBUGF("ata_cmd_packet : error (status %02x)\n", status);
+            return 3;
+        }
 
-            if (status & ATA_CB_STAT_ERR) {
-                DEBUGF("ata_cmd_packet : error (status %02x)\n", status);
-                return 3;
-            }
+        // Normalize address
+        bufseg += (bufoff / 16);
+        bufoff %= 16;
 
-            // Normalize address
-            bufseg += (bufoff / 16);
-            bufoff %= 16;
-
-            // Get the byte count
-            u16 lcount =  (((u16)(inb(iobase1 + ATA_CB_CH))<<8)
-                           + inb(iobase1 + ATA_CB_CL));
-
-            // adjust to read what we want
-            u16 lbefore, lafter;
-            if (header > lcount) {
-                lbefore=lcount;
-                header-=lcount;
-                lcount=0;
-            } else {
-                lbefore=header;
-                header=0;
-                lcount-=lbefore;
-            }
+        // Get the byte count
+        u16 lcount =  (((u16)(inb(iobase1 + ATA_CB_CH))<<8)
+                       + inb(iobase1 + ATA_CB_CL));
 
-            if (lcount > length) {
-                lafter=lcount-length;
-                lcount=length;
-                length=0;
-            } else {
-                lafter=0;
-                length-=lcount;
-            }
+        // adjust to read what we want
+        u16 lbefore, lafter;
+        if (header > lcount) {
+            lbefore=lcount;
+            header-=lcount;
+            lcount=0;
+        } else {
+            lbefore=header;
+            header=0;
+            lcount-=lbefore;
+        }
 
-            // Save byte count
-            u16 count = lcount;
+        if (lcount > length) {
+            lafter=lcount-length;
+            lcount=length;
+            length=0;
+        } else {
+            lafter=0;
+            length-=lcount;
+        }
 
-            DEBUGF("Trying to read %04x bytes (%04x %04x %04x) "
-                   , lbefore+lcount+lafter, lbefore, lcount, lafter);
-            DEBUGF("to 0x%04x:0x%04x\n", bufseg, bufoff);
+        // Save byte count
+        u16 count = lcount;
 
-            // If counts not dividable by 4, use 16bits mode
-            u8 lmode = mode;
-            if (lbefore & 0x03) lmode=ATA_MODE_PIO16;
-            if (lcount  & 0x03) lmode=ATA_MODE_PIO16;
-            if (lafter  & 0x03) lmode=ATA_MODE_PIO16;
+        DEBUGF("Trying to read %04x bytes (%04x %04x %04x) "
+               , lbefore+lcount+lafter, lbefore, lcount, lafter);
+        DEBUGF("to 0x%04x:0x%04x\n", bufseg, bufoff);
 
-            // adds an extra byte if count are odd. before is always even
-            if (lcount & 0x01) {
-                lcount+=1;
-                if ((lafter > 0) && (lafter & 0x01)) {
-                    lafter-=1;
-                }
-            }
+        // If counts not dividable by 4, use 16bits mode
+        u8 lmode = mode;
+        if (lbefore & 0x03) lmode=ATA_MODE_PIO16;
+        if (lcount  & 0x03) lmode=ATA_MODE_PIO16;
+        if (lafter  & 0x03) lmode=ATA_MODE_PIO16;
 
-            if (lmode == ATA_MODE_PIO32) {
-                lcount>>=2; lbefore>>=2; lafter>>=2;
-            } else {
-                lcount>>=1; lbefore>>=1; lafter>>=1;
+        // adds an extra byte if count are odd. before is always even
+        if (lcount & 0x01) {
+            lcount+=1;
+            if ((lafter > 0) && (lafter & 0x01)) {
+                lafter-=1;
             }
+        }
 
-            int i;
-            for (i=0; i<lbefore; i++)
-                if (lmode == ATA_MODE_PIO32)
-                    inl(iobase1);
-                else
-                    inw(iobase1);
+        if (lmode == ATA_MODE_PIO32) {
+            lcount>>=2; lbefore>>=2; lafter>>=2;
+        } else {
+            lcount>>=1; lbefore>>=1; lafter>>=1;
+        }
 
+        int i;
+        for (i=0; i<lbefore; i++)
             if (lmode == ATA_MODE_PIO32)
-                insl_seg(iobase1, bufseg, bufoff, lcount);
+                inl(iobase1);
             else
-                insw_seg(iobase1, bufseg, bufoff, lcount);
+                inw(iobase1);
 
-            for (i=0; i<lafter; i++)
-                if (lmode == ATA_MODE_PIO32)
-                    inl(iobase1);
-                else
-                    inw(iobase1);
+        if (lmode == ATA_MODE_PIO32)
+            insl_seg(iobase1, bufseg, bufoff, lcount);
+        else
+            insw_seg(iobase1, bufseg, bufoff, lcount);
 
-            // Compute new buffer address
-            bufoff += count;
+        for (i=0; i<lafter; i++)
+            if (lmode == ATA_MODE_PIO32)
+                inl(iobase1);
+            else
+                inw(iobase1);
 
-            // Save transferred bytes count
-            SET_EBDA(ata.trsfsectors, loops);
-        }
+        // Compute new buffer address
+        bufoff += count;
+
+        // Save transferred bytes count
+        SET_EBDA(ata.trsfsectors, loops);
     }
 
     // Final check, device must be ready
@@ -461,8 +441,8 @@ ata_cmd_packet(u16 device, u8 *cmdbuf, u8 cmdlen, u16 header
     return 0;
 }
 
-u16
-cdrom_read(u16 device, u32 lba, u32 count, u16 segment, u16 offset, u16 skip)
+int
+cdrom_read(u16 biosid, u32 lba, u32 count, u16 segment, u16 offset, u16 skip)
 {
     u16 sectors = (count + 2048 - 1) / 2048;
 
@@ -476,9 +456,8 @@ cdrom_read(u16 device, u32 lba, u32 count, u16 segment, u16 offset, u16 skip)
     atacmd[4]=(lba & 0x0000ff00) >> 8;
     atacmd[5]=(lba & 0x000000ff);
 
-    return ata_cmd_packet(device, atacmd, sizeof(atacmd)
-                          , skip, count, ATA_DATA_IN
-                          , segment, offset);
+    return ata_cmd_packet(biosid, atacmd, sizeof(atacmd)
+                          , skip, count, segment, offset);
 }
 
 // ---------------------------------------------------------------------------
@@ -587,9 +566,9 @@ ata_detect()
             SET_EBDA(ata.devices[device].device,ATA_DEVICE_HD);
             SET_EBDA(ata.devices[device].mode, ATA_MODE_PIO16);
 
-            u16 ret = ata_cmd_data(device,ATA_CMD_IDENTIFY_DEVICE
-                                   , 1, 0, 0, 0, 0L
-                                   , GET_SEG(SS), (u32)buffer);
+            u16 ret = ata_cmd_data_chs(device, ATA_CMD_IDENTIFY_DEVICE
+                                       , 0, 0, 1, 1
+                                       , GET_SEG(SS), (u32)buffer);
             if (ret)
                 BX_PANIC("ata-detect: Failed to detect ATA device\n");
 
@@ -691,9 +670,9 @@ ata_detect()
             SET_EBDA(ata.devices[device].device,ATA_DEVICE_CDROM);
             SET_EBDA(ata.devices[device].mode, ATA_MODE_PIO16);
 
-            u16 ret = ata_cmd_data(device,ATA_CMD_IDENTIFY_DEVICE_PACKET
-                                   , 1, 0, 0, 0, 0L
-                                   , GET_SEG(SS), (u32)buffer);
+            u16 ret = ata_cmd_data_chs(device, ATA_CMD_IDENTIFY_DEVICE_PACKET
+                                       , 0, 0, 1, 1
+                                       , GET_SEG(SS), (u32)buffer);
             if (ret != 0)
                 BX_PANIC("ata-detect: Failed to detect ATAPI device\n");
 
diff --git a/src/ata.h b/src/ata.h
index d7d03f3..0ad3590 100644
--- a/src/ata.h
+++ b/src/ata.h
@@ -9,152 +9,87 @@
 #define __ATA_H
 
 #include "types.h" // u16
+#include "atabits.h"
+
+struct ata_pio_command {
+    u16 segment;
+    u16 offset;
+    u8 biosid;
+
+    u8 feature;
+    u8 sector_count;
+    u8 lba_low;
+    u8 lba_mid;
+    u8 lba_high;
+    u8 device;
+    u8 command;
+
+    u8 sector_count2;
+    u8 lba_low2;
+    u8 lba_mid2;
+    u8 lba_high2;
+};
 
 // Function definitions
 void ata_reset(u16 device);
-u16 ata_cmd_data(u16 device, u16 command, u16 count, u16 cylinder
-                 , u16 head, u16 sector, u32 lba, u16 segment, u16 offset);
-u16 ata_cmd_packet(u16 device, u8 *cmdbuf, u8 cmdlen, u16 header
-                   , u32 length, u8 inout, u16 bufseg, u16 bufoff);
-u16 cdrom_read(u16 device, u32 lba, u32 count
+int ata_transfer(struct ata_pio_command *cmd);
+int ata_cmd_packet(u16 device, u8 *cmdbuf, u8 cmdlen
+                   , u16 header, u32 length, u16 bufseg, u16 bufoff);
+int cdrom_read(u16 device, u32 lba, u32 count
                , u16 segment, u16 offset, u16 skip);
 void ata_detect();
 
-// Global defines -- ATA register and register bits.
-// command block & control block regs
-#define ATA_CB_DATA  0   // data reg         in/out pio_base_addr1+0
-#define ATA_CB_ERR   1   // error            in     pio_base_addr1+1
-#define ATA_CB_FR    1   // feature reg         out pio_base_addr1+1
-#define ATA_CB_SC    2   // sector count     in/out pio_base_addr1+2
-#define ATA_CB_SN    3   // sector number    in/out pio_base_addr1+3
-#define ATA_CB_CL    4   // cylinder low     in/out pio_base_addr1+4
-#define ATA_CB_CH    5   // cylinder high    in/out pio_base_addr1+5
-#define ATA_CB_DH    6   // device head      in/out pio_base_addr1+6
-#define ATA_CB_STAT  7   // primary status   in     pio_base_addr1+7
-#define ATA_CB_CMD   7   // command             out pio_base_addr1+7
-#define ATA_CB_ASTAT 6   // alternate status in     pio_base_addr2+6
-#define ATA_CB_DC    6   // device control      out pio_base_addr2+6
-#define ATA_CB_DA    7   // device address   in     pio_base_addr2+7
-
-#define ATA_CB_ER_ICRC 0x80    // ATA Ultra DMA bad CRC
-#define ATA_CB_ER_BBK  0x80    // ATA bad block
-#define ATA_CB_ER_UNC  0x40    // ATA uncorrected error
-#define ATA_CB_ER_MC   0x20    // ATA media change
-#define ATA_CB_ER_IDNF 0x10    // ATA id not found
-#define ATA_CB_ER_MCR  0x08    // ATA media change request
-#define ATA_CB_ER_ABRT 0x04    // ATA command aborted
-#define ATA_CB_ER_NTK0 0x02    // ATA track 0 not found
-#define ATA_CB_ER_NDAM 0x01    // ATA address mark not found
-
-#define ATA_CB_ER_P_SNSKEY 0xf0   // ATAPI sense key (mask)
-#define ATA_CB_ER_P_MCR    0x08   // ATAPI Media Change Request
-#define ATA_CB_ER_P_ABRT   0x04   // ATAPI command abort
-#define ATA_CB_ER_P_EOM    0x02   // ATAPI End of Media
-#define ATA_CB_ER_P_ILI    0x01   // ATAPI Illegal Length Indication
-
-// ATAPI Interrupt Reason bits in the Sector Count reg (CB_SC)
-#define ATA_CB_SC_P_TAG    0xf8   // ATAPI tag (mask)
-#define ATA_CB_SC_P_REL    0x04   // ATAPI release
-#define ATA_CB_SC_P_IO     0x02   // ATAPI I/O
-#define ATA_CB_SC_P_CD     0x01   // ATAPI C/D
-
-// bits 7-4 of the device/head (CB_DH) reg
-#define ATA_CB_DH_DEV0 0xa0    // select device 0
-#define ATA_CB_DH_DEV1 0xb0    // select device 1
-#define ATA_CB_DH_LBA 0x40    // use LBA
-
-// status reg (CB_STAT and CB_ASTAT) bits
-#define ATA_CB_STAT_BSY  0x80  // busy
-#define ATA_CB_STAT_RDY  0x40  // ready
-#define ATA_CB_STAT_DF   0x20  // device fault
-#define ATA_CB_STAT_WFT  0x20  // write fault (old name)
-#define ATA_CB_STAT_SKC  0x10  // seek complete
-#define ATA_CB_STAT_SERV 0x10  // service
-#define ATA_CB_STAT_DRQ  0x08  // data request
-#define ATA_CB_STAT_CORR 0x04  // corrected
-#define ATA_CB_STAT_IDX  0x02  // index
-#define ATA_CB_STAT_ERR  0x01  // error (ATA)
-#define ATA_CB_STAT_CHK  0x01  // check (ATAPI)
-
-// device control reg (CB_DC) bits
-#define ATA_CB_DC_HD15   0x08  // bit should always be set to one
-#define ATA_CB_DC_SRST   0x04  // soft reset
-#define ATA_CB_DC_NIEN   0x02  // disable interrupts
-
-// Most mandtory and optional ATA commands (from ATA-3),
-#define ATA_CMD_CFA_ERASE_SECTORS            0xC0
-#define ATA_CMD_CFA_REQUEST_EXT_ERR_CODE     0x03
-#define ATA_CMD_CFA_TRANSLATE_SECTOR         0x87
-#define ATA_CMD_CFA_WRITE_MULTIPLE_WO_ERASE  0xCD
-#define ATA_CMD_CFA_WRITE_SECTORS_WO_ERASE   0x38
-#define ATA_CMD_CHECK_POWER_MODE1            0xE5
-#define ATA_CMD_CHECK_POWER_MODE2            0x98
-#define ATA_CMD_DEVICE_RESET                 0x08
-#define ATA_CMD_EXECUTE_DEVICE_DIAGNOSTIC    0x90
-#define ATA_CMD_FLUSH_CACHE                  0xE7
-#define ATA_CMD_FORMAT_TRACK                 0x50
-#define ATA_CMD_IDENTIFY_DEVICE              0xEC
-#define ATA_CMD_IDENTIFY_DEVICE_PACKET       0xA1
-#define ATA_CMD_IDENTIFY_PACKET_DEVICE       0xA1
-#define ATA_CMD_IDLE1                        0xE3
-#define ATA_CMD_IDLE2                        0x97
-#define ATA_CMD_IDLE_IMMEDIATE1              0xE1
-#define ATA_CMD_IDLE_IMMEDIATE2              0x95
-#define ATA_CMD_INITIALIZE_DRIVE_PARAMETERS  0x91
-#define ATA_CMD_INITIALIZE_DEVICE_PARAMETERS 0x91
-#define ATA_CMD_NOP                          0x00
-#define ATA_CMD_PACKET                       0xA0
-#define ATA_CMD_READ_BUFFER                  0xE4
-#define ATA_CMD_READ_DMA                     0xC8
-#define ATA_CMD_READ_DMA_QUEUED              0xC7
-#define ATA_CMD_READ_MULTIPLE                0xC4
-#define ATA_CMD_READ_SECTORS                 0x20
-#define ATA_CMD_READ_VERIFY_SECTORS          0x40
-#define ATA_CMD_RECALIBRATE                  0x10
-#define ATA_CMD_REQUEST_SENSE                0x03
-#define ATA_CMD_SEEK                         0x70
-#define ATA_CMD_SET_FEATURES                 0xEF
-#define ATA_CMD_SET_MULTIPLE_MODE            0xC6
-#define ATA_CMD_SLEEP1                       0xE6
-#define ATA_CMD_SLEEP2                       0x99
-#define ATA_CMD_STANDBY1                     0xE2
-#define ATA_CMD_STANDBY2                     0x96
-#define ATA_CMD_STANDBY_IMMEDIATE1           0xE0
-#define ATA_CMD_STANDBY_IMMEDIATE2           0x94
-#define ATA_CMD_WRITE_BUFFER                 0xE8
-#define ATA_CMD_WRITE_DMA                    0xCA
-#define ATA_CMD_WRITE_DMA_QUEUED             0xCC
-#define ATA_CMD_WRITE_MULTIPLE               0xC5
-#define ATA_CMD_WRITE_SECTORS                0x30
-#define ATA_CMD_WRITE_VERIFY                 0x3C
-
-#define ATA_IFACE_NONE    0x00
-#define ATA_IFACE_ISA     0x00
-#define ATA_IFACE_PCI     0x01
-
-#define ATA_TYPE_NONE     0x00
-#define ATA_TYPE_UNKNOWN  0x01
-#define ATA_TYPE_ATA      0x02
-#define ATA_TYPE_ATAPI    0x03
-
-#define ATA_DEVICE_NONE  0x00
-#define ATA_DEVICE_HD    0xFF
-#define ATA_DEVICE_CDROM 0x05
-
-#define ATA_MODE_NONE    0x00
-#define ATA_MODE_PIO16   0x00
-#define ATA_MODE_PIO32   0x01
-#define ATA_MODE_ISADMA  0x02
-#define ATA_MODE_PCIDMA  0x03
-#define ATA_MODE_USEIRQ  0x10
-
-#define ATA_TRANSLATION_NONE  0
-#define ATA_TRANSLATION_LBA   1
-#define ATA_TRANSLATION_LARGE 2
-#define ATA_TRANSLATION_RECHS 3
-
-#define ATA_DATA_NO      0x00
-#define ATA_DATA_IN      0x01
-#define ATA_DATA_OUT     0x02
+static inline int
+ata_cmd_data(u16 biosid, u16 command, u32 lba, u16 count
+             , u16 segment, u16 offset)
+{
+    u8 slave   = biosid % 2;
+
+    struct ata_pio_command cmd;
+    cmd.biosid = biosid;
+    cmd.segment = segment;
+    cmd.offset = offset;
+
+    if (count >= (1<<8) || lba + count >= (1<<28)) {
+        cmd.sector_count2 = count >> 8;
+        cmd.lba_low2 = lba >> 24;
+        cmd.lba_mid2 = 0;
+        cmd.lba_high2 = 0;
+
+        command |= 0x04;
+        lba &= 0xffffff;
+    }
+
+    cmd.feature = 0;
+    cmd.sector_count = count;
+    cmd.lba_low = lba;
+    cmd.lba_mid = lba >> 8;
+    cmd.lba_high = lba >> 16;
+    cmd.device = ((slave ? ATA_CB_DH_DEV1 : ATA_CB_DH_DEV0)
+                  | ((lba >> 24) & 0xf) | ATA_CB_DH_LBA);
+    cmd.command = command;
+    return ata_transfer(&cmd);
+}
+
+static inline int
+ata_cmd_data_chs(u16 biosid, u16 command, u16 cyl, u16 head, u16 sect, u16 count
+                 , u16 segment, u16 offset)
+{
+    u8 slave   = biosid % 2;
+
+    struct ata_pio_command cmd;
+    cmd.biosid = biosid;
+    cmd.segment = segment;
+    cmd.offset = offset;
+
+    cmd.sector_count = count & 0xff;
+    cmd.feature = 0;
+    cmd.lba_low = sect;
+    cmd.lba_mid = cyl;
+    cmd.lba_high = cyl >> 8;
+    cmd.device = (slave ? ATA_CB_DH_DEV1 : ATA_CB_DH_DEV0) | (head & 0xff);
+    cmd.command = command;
+    return ata_transfer(&cmd);
+}
 
 #endif /* __ATA_H */
diff --git a/src/atabits.h b/src/atabits.h
new file mode 100644
index 0000000..d4ddc15
--- /dev/null
+++ b/src/atabits.h
@@ -0,0 +1,136 @@
+// Global defines -- ATA register and register bits.
+// command block & control block regs
+#define ATA_CB_DATA  0   // data reg         in/out pio_base_addr1+0
+#define ATA_CB_ERR   1   // error            in     pio_base_addr1+1
+#define ATA_CB_FR    1   // feature reg         out pio_base_addr1+1
+#define ATA_CB_SC    2   // sector count     in/out pio_base_addr1+2
+#define ATA_CB_SN    3   // sector number    in/out pio_base_addr1+3
+#define ATA_CB_CL    4   // cylinder low     in/out pio_base_addr1+4
+#define ATA_CB_CH    5   // cylinder high    in/out pio_base_addr1+5
+#define ATA_CB_DH    6   // device head      in/out pio_base_addr1+6
+#define ATA_CB_STAT  7   // primary status   in     pio_base_addr1+7
+#define ATA_CB_CMD   7   // command             out pio_base_addr1+7
+#define ATA_CB_ASTAT 6   // alternate status in     pio_base_addr2+6
+#define ATA_CB_DC    6   // device control      out pio_base_addr2+6
+#define ATA_CB_DA    7   // device address   in     pio_base_addr2+7
+
+#define ATA_CB_ER_ICRC 0x80    // ATA Ultra DMA bad CRC
+#define ATA_CB_ER_BBK  0x80    // ATA bad block
+#define ATA_CB_ER_UNC  0x40    // ATA uncorrected error
+#define ATA_CB_ER_MC   0x20    // ATA media change
+#define ATA_CB_ER_IDNF 0x10    // ATA id not found
+#define ATA_CB_ER_MCR  0x08    // ATA media change request
+#define ATA_CB_ER_ABRT 0x04    // ATA command aborted
+#define ATA_CB_ER_NTK0 0x02    // ATA track 0 not found
+#define ATA_CB_ER_NDAM 0x01    // ATA address mark not found
+
+#define ATA_CB_ER_P_SNSKEY 0xf0   // ATAPI sense key (mask)
+#define ATA_CB_ER_P_MCR    0x08   // ATAPI Media Change Request
+#define ATA_CB_ER_P_ABRT   0x04   // ATAPI command abort
+#define ATA_CB_ER_P_EOM    0x02   // ATAPI End of Media
+#define ATA_CB_ER_P_ILI    0x01   // ATAPI Illegal Length Indication
+
+// ATAPI Interrupt Reason bits in the Sector Count reg (CB_SC)
+#define ATA_CB_SC_P_TAG    0xf8   // ATAPI tag (mask)
+#define ATA_CB_SC_P_REL    0x04   // ATAPI release
+#define ATA_CB_SC_P_IO     0x02   // ATAPI I/O
+#define ATA_CB_SC_P_CD     0x01   // ATAPI C/D
+
+// bits 7-4 of the device/head (CB_DH) reg
+#define ATA_CB_DH_DEV0 0xa0    // select device 0
+#define ATA_CB_DH_DEV1 0xb0    // select device 1
+#define ATA_CB_DH_LBA 0x40    // use LBA
+
+// status reg (CB_STAT and CB_ASTAT) bits
+#define ATA_CB_STAT_BSY  0x80  // busy
+#define ATA_CB_STAT_RDY  0x40  // ready
+#define ATA_CB_STAT_DF   0x20  // device fault
+#define ATA_CB_STAT_WFT  0x20  // write fault (old name)
+#define ATA_CB_STAT_SKC  0x10  // seek complete
+#define ATA_CB_STAT_SERV 0x10  // service
+#define ATA_CB_STAT_DRQ  0x08  // data request
+#define ATA_CB_STAT_CORR 0x04  // corrected
+#define ATA_CB_STAT_IDX  0x02  // index
+#define ATA_CB_STAT_ERR  0x01  // error (ATA)
+#define ATA_CB_STAT_CHK  0x01  // check (ATAPI)
+
+// device control reg (CB_DC) bits
+#define ATA_CB_DC_HD15   0x08  // bit should always be set to one
+#define ATA_CB_DC_SRST   0x04  // soft reset
+#define ATA_CB_DC_NIEN   0x02  // disable interrupts
+
+// Most mandtory and optional ATA commands (from ATA-3),
+#define ATA_CMD_CFA_ERASE_SECTORS            0xC0
+#define ATA_CMD_CFA_REQUEST_EXT_ERR_CODE     0x03
+#define ATA_CMD_CFA_TRANSLATE_SECTOR         0x87
+#define ATA_CMD_CFA_WRITE_MULTIPLE_WO_ERASE  0xCD
+#define ATA_CMD_CFA_WRITE_SECTORS_WO_ERASE   0x38
+#define ATA_CMD_CHECK_POWER_MODE1            0xE5
+#define ATA_CMD_CHECK_POWER_MODE2            0x98
+#define ATA_CMD_DEVICE_RESET                 0x08
+#define ATA_CMD_EXECUTE_DEVICE_DIAGNOSTIC    0x90
+#define ATA_CMD_FLUSH_CACHE                  0xE7
+#define ATA_CMD_FORMAT_TRACK                 0x50
+#define ATA_CMD_IDENTIFY_DEVICE              0xEC
+#define ATA_CMD_IDENTIFY_DEVICE_PACKET       0xA1
+#define ATA_CMD_IDENTIFY_PACKET_DEVICE       0xA1
+#define ATA_CMD_IDLE1                        0xE3
+#define ATA_CMD_IDLE2                        0x97
+#define ATA_CMD_IDLE_IMMEDIATE1              0xE1
+#define ATA_CMD_IDLE_IMMEDIATE2              0x95
+#define ATA_CMD_INITIALIZE_DRIVE_PARAMETERS  0x91
+#define ATA_CMD_INITIALIZE_DEVICE_PARAMETERS 0x91
+#define ATA_CMD_NOP                          0x00
+#define ATA_CMD_PACKET                       0xA0
+#define ATA_CMD_READ_BUFFER                  0xE4
+#define ATA_CMD_READ_DMA                     0xC8
+#define ATA_CMD_READ_DMA_QUEUED              0xC7
+#define ATA_CMD_READ_MULTIPLE                0xC4
+#define ATA_CMD_READ_SECTORS                 0x20
+#define ATA_CMD_READ_VERIFY_SECTORS          0x40
+#define ATA_CMD_RECALIBRATE                  0x10
+#define ATA_CMD_REQUEST_SENSE                0x03
+#define ATA_CMD_SEEK                         0x70
+#define ATA_CMD_SET_FEATURES                 0xEF
+#define ATA_CMD_SET_MULTIPLE_MODE            0xC6
+#define ATA_CMD_SLEEP1                       0xE6
+#define ATA_CMD_SLEEP2                       0x99
+#define ATA_CMD_STANDBY1                     0xE2
+#define ATA_CMD_STANDBY2                     0x96
+#define ATA_CMD_STANDBY_IMMEDIATE1           0xE0
+#define ATA_CMD_STANDBY_IMMEDIATE2           0x94
+#define ATA_CMD_WRITE_BUFFER                 0xE8
+#define ATA_CMD_WRITE_DMA                    0xCA
+#define ATA_CMD_WRITE_DMA_QUEUED             0xCC
+#define ATA_CMD_WRITE_MULTIPLE               0xC5
+#define ATA_CMD_WRITE_SECTORS                0x30
+#define ATA_CMD_WRITE_VERIFY                 0x3C
+
+#define ATA_IFACE_NONE    0x00
+#define ATA_IFACE_ISA     0x00
+#define ATA_IFACE_PCI     0x01
+
+#define ATA_TYPE_NONE     0x00
+#define ATA_TYPE_UNKNOWN  0x01
+#define ATA_TYPE_ATA      0x02
+#define ATA_TYPE_ATAPI    0x03
+
+#define ATA_DEVICE_NONE  0x00
+#define ATA_DEVICE_HD    0xFF
+#define ATA_DEVICE_CDROM 0x05
+
+#define ATA_MODE_NONE    0x00
+#define ATA_MODE_PIO16   0x00
+#define ATA_MODE_PIO32   0x01
+#define ATA_MODE_ISADMA  0x02
+#define ATA_MODE_PCIDMA  0x03
+#define ATA_MODE_USEIRQ  0x10
+
+#define ATA_TRANSLATION_NONE  0
+#define ATA_TRANSLATION_LBA   1
+#define ATA_TRANSLATION_LARGE 2
+#define ATA_TRANSLATION_RECHS 3
+
+#define ATA_DATA_NO      0x00
+#define ATA_DATA_IN      0x01
+#define ATA_DATA_OUT     0x02
diff --git a/src/disk.c b/src/disk.c
index 676914f..1d5fd27 100644
--- a/src/disk.c
+++ b/src/disk.c
@@ -44,7 +44,7 @@ basic_access(struct bregs *regs, u8 device, u16 command)
     u16 sector      = regs->cl & 0x3f;
     u16 head        = regs->dh;
 
-    if ((count > 128) || (count == 0) || (sector == 0)) {
+    if (count > 128 || count == 0 || sector == 0) {
         BX_INFO("int13_harddisk: function %02x, parameter out of range!\n"
                 , regs->ah);
         disk_ret(regs, DISK_RET_EPARAM);
@@ -58,7 +58,7 @@ basic_access(struct bregs *regs, u8 device, u16 command)
     u16 npspt = GET_EBDA(ata.devices[device].pchs.spt);
 
     // sanity check on cyl heads, sec
-    if ( (cylinder >= nlc) || (head >= nlh) || (sector > nlspt )) {
+    if (cylinder >= nlc || head >= nlh || sector > nlspt) {
         BX_INFO("int13_harddisk: function %02x, parameters out of"
                 " range %04x/%04x/%04x!\n"
                 , regs->ah, cylinder, head, sector);
@@ -66,14 +66,6 @@ basic_access(struct bregs *regs, u8 device, u16 command)
         return;
     }
 
-    u32 lba = 0;
-    // if needed, translate lchs to lba, and execute command
-    if ( (nph != nlh) || (npspt != nlspt)) {
-        lba = (((((u32)cylinder * (u32)nlh) + (u32)head) * (u32)nlspt)
-               + (u32)sector - 1);
-        sector = 0; // this forces the command to be lba
-    }
-
     if (!command) {
         // If verify or seek
         disk_ret(regs, DISK_RET_SUCCESS);
@@ -83,9 +75,19 @@ basic_access(struct bregs *regs, u8 device, u16 command)
     u16 segment = regs->es;
     u16 offset  = regs->bx;
 
-    u8 status = ata_cmd_data(device, command
-                             , count, cylinder, head, sector
-                             , lba, segment, offset);
+    u8 status;
+    u32 lba;
+    if (nph != nlh || npspt != nlspt) {
+        // translate lchs to lba
+        lba = (((((u32)cylinder * (u32)nlh) + (u32)head) * (u32)nlspt)
+               + (u32)sector - 1);
+        status = ata_cmd_data(device, command, lba, count, segment, offset);
+    } else {
+        // XXX - see if lba access can always be used.
+        status = ata_cmd_data_chs(device, command
+                                  , cylinder, head, sector
+                                  , count, segment, offset);
+    }
 
     // Set nb of sector transferred
     regs->al = GET_EBDA(ata.trsfsectors);
@@ -188,9 +190,7 @@ extended_access(struct bregs *regs, u8 device, u16 command)
 
     u8 status;
     if (type == ATA_TYPE_ATA)
-        status = ata_cmd_data(device, command
-                              , count, 0, 0, 0
-                              , lba, segment, offset);
+        status = ata_cmd_data(device, command, lba, count, segment, offset);
     else
         status = cdrom_read(device, lba, count*2048, segment, offset, 0);