Changeset 207 for trunk/src/os2ahci/ata.c

Timestamp:

Feb 23, 2021, 10:10:11 PM (4 years ago)

Author:

David Azarewicz

Message:

Add 64/48 bit LBA support.

File:

• trunk/src/os2ahci/ata.c (modified) (35 diffs)

trunk/src/os2ahci/ata.c

@@ -26 +26 @@
  */
 
+#define INCL_LONGLONG
 #include "os2ahci.h"
 #include "ata.h"
 
-/* -------------------------- function prototypes -------------------------- */
-
-static int ata_cmd_read(IORBH *pIorb, AD_INFO *ai, int p, int d, int slot,
-                        ULONG sector, ULONG count, SCATGATENTRY *sg_list,
-                        ULONG sg_cnt);
-
-static int ata_cmd_write(IORBH *pIorb, AD_INFO *ai, int p, int d, int slot,
-                         ULONG sector, ULONG count, SCATGATENTRY *sg_list,
-                         ULONG sg_cnt, int write_through);
+#define MAKEULL(h,l) (((ULONGLONG)h<<32)|l)
 
 /******************************************************************************
@@ -256 +249 @@
     {
       u32 chunk = (sg_size > AHCI_MAX_SG_ELEMENT_LEN) ? AHCI_MAX_SG_ELEMENT_LEN : sg_size;
+
       if (n >= AHCI_MAX_SG)
       {
         /* couldn't store all S/G elements in our DMA buffer */
-        dprintf(0,"ata_cmd(): too many S/G elements\n");
+        if (sg_size)
+        {
+          dprintf(0, __func__": unaligned SG element\n");
+          return -1;
+        }
+        dprintf(0, __func__": too many S/G elements\n");
         return(i - 1);
       }
@@ -341 +340 @@
 
 /******************************************************************************
- * Get index in S/G list for the number of transferred sectors in the IORB.
- *
- * Returning io->cSGList indicates an error.
- *
- * NOTE: OS/2 makes sure S/G lists are set up such that entries at the HW
- *       limit will never cross sector boundaries. This means that splitting
- *       S/G lists into multiple commands can be done without editing the S/G
- *       lists.
- */
-u16 ata_get_sg_indx(IORB_EXECUTEIO *io)
-{
-  ULONG offset = io->BlocksXferred * io->BlockSize;
-  SCATGATENTRY *pSGList = (SCATGATENTRY*)Far16ToFlat(io->f16SGList);
-  USHORT i;
-
-  for (i = 0; i < io->cSGList && offset > 0; i++)
-  {
-    offset -= pSGList[i].XferBufLen;
-  }
-
-  return(i);
-}
-
-/******************************************************************************
  * Get max S/G count which will fit into our HW S/G buffers. This function is
  * called when the S/G list is too long and we need to split the IORB into
@@ -459 +434 @@
  * geometry to the last full cylinder.
  */
-int adjust_cylinders(GEOMETRY *geometry, ULONG TotalSectors)
+int adjust_cylinders(GEOMETRY64 *geometry, ULONG TotalSectors)
 {
   USHORT SecPerCyl;
@@ -487 +462 @@
 #define BIOS_MAX_NUMHEADS         255
 #define BIOS_MAX_SECTORSPERTRACK   63
-void log_geom_calculate_LBA_assist(GEOMETRY *geometry, ULONG TotalSectors)
+void log_geom_calculate_LBA_assist(GEOMETRY64 *geometry, ULONG TotalSectors)
 {
   UCHAR numSpT   = BIOS_MAX_SECTORSPERTRACK;
@@ -524 +499 @@
   DLA_Table_Sector *pDLA = (DLA_Table_Sector*)add_workspace(pIorb)->buf;
   AD_INFO *ai = ad_infos + iorb_unit_adapter(pIorb);
-  GEOMETRY *geometry = ((IORB_GEOMETRY*)pIorb)->f16Geometry;
+  GEOMETRY64 *geometry = (GEOMETRY64*)((IORB_GEOMETRY*)pIorb)->f16Geometry;
   int p = iorb_unit_port(pIorb);
   int rc;
@@ -544 +519 @@
     geometry->NumHeads = pDLA->Heads_Per_Cylinder;
     geometry->SectorsPerTrack = pDLA->Sectors_Per_Track;
-    geometry->TotalSectors = pDLA->Cylinders * pDLA->Heads_Per_Cylinder * pDLA->Sectors_Per_Track;
+    if (add_workspace(pIorb)->fIs64bit)
+      geometry->ullTotalSectors = pDLA->Cylinders * pDLA->Heads_Per_Cylinder * pDLA->Sectors_Per_Track;
+    else
+      geometry->TotalSectors = pDLA->Cylinders * pDLA->Heads_Per_Cylinder * pDLA->Sectors_Per_Track;
     return 1;
   }
@@ -589 +567 @@
 void ata_get_geometry_pp(IORBH FAR16DATA *vIorb, IORBH *pIorb)
 {
-  GEOMETRY *geometry = ((IORB_GEOMETRY*)pIorb)->f16Geometry;
+  GEOMETRY64 *geometry = (GEOMETRY64*)((IORB_GEOMETRY*)pIorb)->f16Geometry;
   USHORT geometry_len =  ((IORB_GEOMETRY *)pIorb)->GeometryLen;
+  ULONGLONG ullTotalSectors;
   u16 *id_buf = add_workspace(pIorb)->buf;
   int a = iorb_unit_adapter(pIorb);
@@ -627 +606 @@
    * can be unexpected and catastrophic.
    */
-  memset(geometry, 0x00, geometry_len);
+  memset(geometry, 0, geometry_len);
   geometry->BytesPerSector = ATA_SECTOR_SIZE;
 
@@ -634 +613 @@
   {
     /* 48-bit LBA supported */
-    if (ATA_CAPACITY48_H(id_buf) != 0)
+    if ((ATA_CAPACITY48_H(id_buf) != 0) && !add_workspace(pIorb)->fIs64bit)
     {
       /* more than 32 bits for number of sectors */
@@ -640 +619 @@
               iorb_unit_adapter(pIorb), iorb_unit_port(pIorb),
               iorb_unit_device(pIorb));
-      geometry->TotalSectors = 0xffffffffUL;
+      ullTotalSectors = 0xffffffff;
     }
     else
     {
-      geometry->TotalSectors = ATA_CAPACITY48_L(id_buf);
+      ullTotalSectors = MAKEULL(ATA_CAPACITY48_H(id_buf), ATA_CAPACITY48_L(id_buf));
     }
   }
@@ -650 +629 @@
   {
     /* 28-bit LBA */
-    geometry->TotalSectors = ATA_CAPACITY(id_buf) & 0x0fffffffUL;
+    ullTotalSectors = ATA_CAPACITY(id_buf) & 0x0fffffff;
   }
 
@@ -663 +642 @@
     geometry->NumHeads        = 255;
     geometry->SectorsPerTrack = track_size[a][p];
-    geometry->TotalCylinders  = geometry->TotalSectors / ((u32) geometry->NumHeads * (u32) geometry->SectorsPerTrack);
+    geometry->TotalCylinders  = ullTotalSectors / ((u32)geometry->NumHeads * (u32)geometry->SectorsPerTrack);
     Method = "Custom";
   }
@@ -688 +667 @@
     geometry->NumHeads        = 255;
     geometry->SectorsPerTrack = 63;
-    geometry->TotalCylinders  = geometry->TotalSectors / ((u32) geometry->NumHeads * (u32) geometry->SectorsPerTrack);
+    geometry->TotalCylinders  = ullTotalSectors / ((u32)geometry->NumHeads * (u32)geometry->SectorsPerTrack);
     Method = "SCSI";
   }
@@ -694 +673 @@
   DPRINTF(2,"Physical geometry: %d cylinders, %d heads, %d sectors per track (%dMB) (%s)\n",
       geometry->TotalCylinders, geometry->NumHeads, geometry->SectorsPerTrack,
-      (geometry->TotalSectors / 2048), Method);
-
-  /* Fixup the geometry in case the geometry reported by the BIOS is bad */
-  if (adjust_cylinders(geometry, geometry->TotalSectors))
-  { // cylinder overflow
-    log_geom_calculate_LBA_assist(geometry, geometry->TotalSectors);
-    geometry->TotalSectors = (USHORT)(geometry->NumHeads * geometry->SectorsPerTrack) * (ULONG)geometry->TotalCylinders;
-  }
-  adjust_cylinders(geometry, geometry->TotalSectors);
-
-  DPRINTF(2,"Logical geometry: %d cylinders, %d heads, %d sectors per track (%dMB) (%s)\n",
-      geometry->TotalCylinders, geometry->NumHeads, geometry->SectorsPerTrack,
-      (geometry->TotalSectors / 2048), Method);
-
-  if (is_lvm_geometry(pIorb)) Method = "LVM";
+      (ULONG)(ullTotalSectors / 2048), Method);
+
+  if (is_lvm_geometry(pIorb)) Method = "LVM";  /* Writes TotalSectors/ullTotalSectors */
+  else
+  {
+    if (add_workspace(pIorb)->fIs64bit)
+    {
+      geometry->NumHeads        = 255;
+      geometry->SectorsPerTrack = 255;
+      geometry->TotalCylinders  = ullTotalSectors / (255*255);
+      geometry->ullTotalSectors = ullTotalSectors;
+    }
+    else
+    {
+      /* Fixup the geometry in case the geometry reported by the BIOS is bad */
+      if (adjust_cylinders(geometry, ullTotalSectors))  /* Writes TotalSectors */
+      { // cylinder overflow
+        log_geom_calculate_LBA_assist(geometry, ullTotalSectors); /* Does not write TotalSectors */
+        ullTotalSectors = (USHORT)(geometry->NumHeads * geometry->SectorsPerTrack) * (ULONG)geometry->TotalCylinders;
+      }
+      adjust_cylinders(geometry, ullTotalSectors); /* Writes TotalSectors */
+    }
+  }
+  if (add_workspace(pIorb)->fIs64bit) ullTotalSectors = geometry->ullTotalSectors;
+  else ullTotalSectors = geometry->TotalSectors;
+
   ad_infos[a].ports[p].devs[0].dev_info.Cylinders = geometry->TotalCylinders;
   ad_infos[a].ports[p].devs[0].dev_info.HeadsPerCylinder = geometry->NumHeads;
   ad_infos[a].ports[p].devs[0].dev_info.SectorsPerTrack = geometry->SectorsPerTrack;
-  ad_infos[a].ports[p].devs[0].dev_info.TotalSectors = geometry->TotalSectors;
+  ad_infos[a].ports[p].devs[0].dev_info.TotalSectors = ullTotalSectors;
   ad_infos[a].ports[p].devs[0].dev_info.Method = Method;
 
-  DPRINTF(2,"Reported geometry: %d cylinders, %d heads, %d sectors per track (%dMB) (%s)\n",
+  //DAZ DPRINTF(2,"Reported geometry: %d cylinders, %d heads, %d sectors per track (%dMB) (%s)\n",
+  DPRINTF(0,"Reported geometry: %d cylinders, %d heads, %d sectors per track (%dMB) (%s)\n",
       geometry->TotalCylinders, geometry->NumHeads, geometry->SectorsPerTrack,
-      (geometry->TotalSectors / 2048), Method);
+      (ULONG)(ullTotalSectors / 2048), Method);
 
   /* tell interrupt handler that this IORB is complete */
@@ -737 +728 @@
 
 /******************************************************************************
+ * Fabricate ATA READ command based on the capabilities of the corresponding
+ * device and the paramters set from above (NCQ, etc).
+ */
+static int ata_cmd_read(IORBH *pIorb, AD_INFO *ai, int p, int d, int slot,
+                        ULONGLONG ullLba, ULONG count, SCATGATENTRY *sg_list,
+                        ULONG sg_cnt)
+{
+  int rc;
+
+  if (ullLba >= (1UL << 28) || count > 256 || add_workspace(pIorb)->is_ncq)
+  {
+    /* need LBA48 for this command */
+    if (!ai->ports[p].devs[d].lba48)
+    {
+      iorb_seterr(pIorb, IOERR_RBA_LIMIT);
+      return(-1);
+    }
+    if (add_workspace(pIorb)->is_ncq)
+    {
+      /* use NCQ read; count goes into feature register, tag into count! */
+      rc = ata_cmd(ai, p, d, slot, ATA_CMD_FPDMA_READ,
+                   AP_SECTOR_48, ullLba,
+                   AP_FEATURES,  count,
+                   AP_COUNT,     (slot << 3), /* tag == slot */
+                   AP_SGLIST,    sg_list, sg_cnt,
+                   AP_DEVICE,    0x40,
+                   AP_END);
+    }
+    else
+    {
+      rc = ata_cmd(ai, p, d, slot, ATA_CMD_READ_EXT,
+                   AP_SECTOR_48, ullLba,
+                   AP_COUNT,     count,
+                   AP_SGLIST,    sg_list, sg_cnt,
+                   AP_DEVICE,    0x40,
+                   AP_END);
+    }
+
+  }
+  else
+  {
+    rc = ata_cmd(ai, p, d, slot, ATA_CMD_READ,
+                 AP_SECTOR_28, (ULONG)ullLba,
+                 AP_COUNT,     count & 0xffU,
+                 AP_SGLIST,    sg_list, sg_cnt,
+                 AP_DEVICE,    0x40,
+                 AP_END);
+  }
+
+  return(rc);
+}
+
+/******************************************************************************
  * Read sectors from AHCI device.
  */
@@ -744 +788 @@
   SCATGATENTRY *pSGList = (SCATGATENTRY*)Far16ToFlat(io->f16SGList);
   AD_INFO *ai = ad_infos + iorb_unit_adapter(pIorb);
-  ULONG sector = io->RBA + io->BlocksXferred;
+  ULONGLONG ullLba = io->RBA + io->BlocksXferred;
   USHORT count = io->BlockCount - io->BlocksXferred;
   USHORT sg_indx;
@@ -762 +806 @@
     /* unaligned S/G addresses present; need to use double buffers */
     return(ata_read_unaligned(pIorb, slot));
+  }
+
+  if (add_workspace(pIorb)->fIs64bit)
+  {
+    ullLba = ((IORB_EXECUTEIO64*)pIorb)->ullRBA + io->BlocksXferred;
   }
 
@@ -778 +827 @@
   do
   {
-    sg_indx = ata_get_sg_indx(io);
+    sg_indx = SgIndexFromOffset(pSGList, io->cSGList, io->BlocksXferred * io->BlockSize);
     sg_cnt = io->cSGList - sg_indx;
-    if ((rc = ata_cmd_read(pIorb, ai, p, d, slot, sector, count,
-                           pSGList + sg_indx, sg_cnt)) > 0)
+    rc = ata_cmd_read(pIorb, ai, p, d, slot, ullLba, count, pSGList + sg_indx, sg_cnt);
+    if (rc > 0)
     {
       /* couldn't map all S/G elements */
@@ -802 +851 @@
     add_workspace(pIorb)->unaligned = 1;
     return(ata_read_unaligned(pIorb, slot));
-
   }
   else
@@ -823 +871 @@
   ADD_WORKSPACE *aws = add_workspace(pIorb);
   AD_INFO *ai = ad_infos + iorb_unit_adapter(pIorb);
-  ULONG sector = io->RBA + io->BlocksXferred;
+  ULONGLONG ullLba = io->RBA + io->BlocksXferred;
   SCATGATENTRY sg_single;
   int p = iorb_unit_port(pIorb);
@@ -829 +877 @@
   int rc;
 
-  DPRINTF(7,"ata_read_unaligned(%d.%d.%d, %d)\n", ad_no(ai), p, d, sector);
+  if (add_workspace(pIorb)->fIs64bit)
+  {
+    ullLba = ((IORB_EXECUTEIO64*)pIorb)->ullRBA + io->BlocksXferred;
+  }
+
+  DPRINTF(7,"ata_read_unaligned(%d.%d.%d, %lld)\n", ad_no(ai), p, d, ullLba);
   ai->ports[p].unaligned_read_count++;
 
@@ -842 +895 @@
   sg_single.ppXferBuf = MemPhysAdr(aws->buf);
   sg_single.XferBufLen = io->BlockSize;
-  rc = ata_cmd_read(pIorb, ai, p, d, slot, sector, 1, &sg_single, 1);
+  rc = ata_cmd_read(pIorb, ai, p, d, slot, ullLba, 1, &sg_single, 1);
 
   if (rc == 0) {
@@ -873 +926 @@
   {
     /* copy transfer buffer to corresponding physical address in S/G list */
-    sg_memcpy(pSGList, io->cSGList,
-              (ULONG) io->BlocksXferred * (ULONG) io->BlockSize,
-              aws->buf, io->BlockSize, BUF_TO_SG);
+    CopySgBuf(pSGList, io->cSGList, (ULONG)io->BlocksXferred * (ULONG)io->BlockSize,
+              aws->buf, io->BlockSize, BUF_TO_SGL);
   }
 
@@ -902 +954 @@
   int p = iorb_unit_port(pIorb);
   int d = iorb_unit_device(pIorb);
+  ULONGLONG ullLba = io->RBA;
   int rc;
 
@@ -910 +963 @@
   }
 
+  if (add_workspace(pIorb)->fIs64bit)
+  {
+    ullLba = ((IORB_EXECUTEIO64*)pIorb)->ullRBA;
+  }
+
   /* prepare verify command */
-  if (io->RBA >= (1UL << 28) || io->BlockCount > 256)
+  if (ullLba >= (1UL << 28) || io->BlockCount > 256)
   {
     /* need LBA48 for this command */
@@ -919 +977 @@
     }
     rc = ata_cmd(ai, p, d, slot, ATA_CMD_VERIFY_EXT,
-                 AP_SECTOR_48, io->RBA, 0,
+                 AP_SECTOR_48, ullLba,
                  AP_COUNT,     io->BlockCount,
                  AP_DEVICE,    0x40,
@@ -925 +983 @@
   } else {
     rc = ata_cmd(ai, p, d, slot, ATA_CMD_VERIFY,
-                 AP_SECTOR_28, io->RBA,
+                 AP_SECTOR_28, (ULONG)ullLba,
                  AP_COUNT,     io->BlockCount & 0xffU,
                  AP_DEVICE,    0x40,
@@ -935 +993 @@
 
 /******************************************************************************
+ * Fabricate ATA WRITE command based on the capabilities of the corresponding
+ * device and the paramters set from above (NCQ, etc)
+ */
+static int ata_cmd_write(IORBH *pIorb, AD_INFO *ai, int p, int d, int slot,
+                         ULONGLONG ullLba, ULONG count, SCATGATENTRY *sg_list,
+                         ULONG sg_cnt, int write_through)
+{
+  int rc;
+
+  if (ullLba >= (1UL << 28) || count > 256 || add_workspace(pIorb)->is_ncq)
+  {
+    /* need LBA48 for this command */
+    if (!ai->ports[p].devs[d].lba48)
+    {
+      iorb_seterr(pIorb, IOERR_RBA_LIMIT);
+      return(-1);
+    }
+    if (add_workspace(pIorb)->is_ncq)
+    {
+      /* use NCQ write; count goes into feature register, tag into count! */
+      rc = ata_cmd(ai, p, d, slot, ATA_CMD_FPDMA_WRITE,
+                   AP_SECTOR_48, ullLba,
+                   AP_FEATURES,  count,
+                   /* tag = slot */
+                   AP_COUNT,     (slot << 3),
+                   AP_SGLIST,    sg_list, sg_cnt,
+                   AP_DEVICE,    0x40,
+                   /* force unit access */
+                   AP_DEVICE,    (write_through && !force_write_cache) ? 0x80 : 0,
+                   AP_WRITE,     1,
+                   AP_END);
+    }
+    else
+    {
+      rc = ata_cmd(ai, p, d, slot, ATA_CMD_WRITE_EXT,
+                   AP_SECTOR_48, ullLba,
+                   AP_COUNT,     count,
+                   AP_SGLIST,    sg_list, sg_cnt,
+                   AP_DEVICE,    0x40,
+                   AP_WRITE,     1,
+                   AP_END);
+    }
+  }
+  else
+  {
+    rc = ata_cmd(ai, p, d, slot, ATA_CMD_WRITE,
+                 AP_SECTOR_28, (ULONG)ullLba,
+                 AP_COUNT,     count & 0xffU,
+                 AP_SGLIST,    sg_list, sg_cnt,
+                 AP_DEVICE,    0x40,
+                 AP_WRITE,     1,
+                 AP_END);
+  }
+
+  return(rc);
+}
+
+/******************************************************************************
  * Write sectors to AHCI device.
  */
@@ -942 +1058 @@
   SCATGATENTRY *pSGList = (SCATGATENTRY*)Far16ToFlat(io->f16SGList);
   AD_INFO *ai = ad_infos + iorb_unit_adapter(pIorb);
-  ULONG sector = io->RBA + io->BlocksXferred;
+  ULONGLONG ullLba = io->RBA + io->BlocksXferred;
   USHORT count = io->BlockCount - io->BlocksXferred;
   USHORT sg_indx;
@@ -962 +1078 @@
   }
 
+  if (add_workspace(pIorb)->fIs64bit)
+  {
+    ullLba = ((IORB_EXECUTEIO64*)pIorb)->ullRBA + io->BlocksXferred;
+  }
+
   /* prepare write command while keeping an eye on S/G count limitations */
   do {
-    sg_indx = ata_get_sg_indx(io);
+    sg_indx = SgIndexFromOffset(pSGList, io->cSGList, io->BlocksXferred * io->BlockSize);
     sg_cnt = io->cSGList - sg_indx;
-    if ((rc = ata_cmd_write(pIorb, ai, p, d, slot, sector, count,
+    if ((rc = ata_cmd_write(pIorb, ai, p, d, slot, ullLba, count,
                             pSGList + sg_indx, sg_cnt,
                             io->Flags & XIO_DISABLE_HW_WRITE_CACHE)) > 0)
     {
       /* couldn't map all S/G elements */
-      ata_max_sg_cnt(io, sg_indx, (USHORT) rc, &sg_cnt, &count);
+      ata_max_sg_cnt(io, sg_indx, (USHORT)rc, &sg_cnt, &count);
     }
   } while (rc > 0 && sg_cnt > 0);
@@ -1010 +1131 @@
   ADD_WORKSPACE *aws = add_workspace(pIorb);
   AD_INFO *ai = ad_infos + iorb_unit_adapter(pIorb);
-  ULONG sector = io->RBA + io->BlocksXferred;
+  ULONGLONG ullLba = io->RBA + io->BlocksXferred;
   SCATGATENTRY sg_single;
   int p = iorb_unit_port(pIorb);
@@ -1016 +1137 @@
   int rc;
 
-  DPRINTF(7,"ata_write_unaligned(%d.%d.%d, %d)\n", ad_no(ai), p, d, sector);
+  if (add_workspace(pIorb)->fIs64bit)
+  {
+    ullLba = ((IORB_EXECUTEIO64*)pIorb)->ullRBA + io->BlocksXferred;
+  }
+
+  DPRINTF(7,"ata_write_unaligned(%d.%d.%d, %lld)\n", ad_no(ai), p, d, ullLba);
 
   /* allocate transfer buffer */
@@ -1026 +1152 @@
 
   /* copy next sector from S/G list to transfer buffer */
-  sg_memcpy(pSGList, io->cSGList,
-            (ULONG) io->BlocksXferred * (ULONG) io->BlockSize,
-            aws->buf, io->BlockSize, SG_TO_BUF);
+  CopySgBuf(pSGList, io->cSGList, (ULONG)io->BlocksXferred * (ULONG)io->BlockSize,
+            aws->buf, io->BlockSize, SGL_TO_BUF);
 
   /* prepare write command using transfer buffer */
   sg_single.ppXferBuf = MemPhysAdr(aws->buf);
   sg_single.XferBufLen = io->BlockSize;
-  rc = ata_cmd_write(pIorb, ai, p, d, slot, sector, 1, &sg_single, 1,
+  rc = ata_cmd_write(pIorb, ai, p, d, slot, ullLba, 1, &sg_single, 1,
                      io->Flags & XIO_DISABLE_HW_WRITE_CACHE);
 
@@ -1250 +1375 @@
 
 /******************************************************************************
- * Fabricate ATA READ command based on the capabilities of the corresponding
- * device and the paramters set from above (NCQ, etc).
- */
-static int ata_cmd_read(IORBH *pIorb, AD_INFO *ai, int p, int d, int slot,
-                        ULONG sector, ULONG count, SCATGATENTRY *sg_list,
-                        ULONG sg_cnt)
-{
-  int rc;
-
-  if (sector >= (1UL << 28) || count > 256 || add_workspace(pIorb)->is_ncq)
-  {
-    /* need LBA48 for this command */
-    if (!ai->ports[p].devs[d].lba48)
-    {
-      iorb_seterr(pIorb, IOERR_RBA_LIMIT);
-      return(-1);
-    }
-    if (add_workspace(pIorb)->is_ncq)
-    {
-      /* use NCQ read; count goes into feature register, tag into count! */
-      rc = ata_cmd(ai, p, d, slot, ATA_CMD_FPDMA_READ,
-                   AP_SECTOR_48, sector, 0,
-                   AP_FEATURES,  count,
-                   AP_COUNT,     (slot << 3), /* tag == slot */
-                   AP_SGLIST,    sg_list, sg_cnt,
-                   AP_DEVICE,    0x40,
-                   AP_END);
-    }
-    else
-    {
-      rc = ata_cmd(ai, p, d, slot, ATA_CMD_READ_EXT,
-                   AP_SECTOR_48, sector, 0,
-                   AP_COUNT,     count,
-                   AP_SGLIST,    sg_list, sg_cnt,
-                   AP_DEVICE,    0x40,
-                   AP_END);
-    }
-
-  }
-  else
-  {
-    rc = ata_cmd(ai, p, d, slot, ATA_CMD_READ,
-                 AP_SECTOR_28, sector,
-                 AP_COUNT,     count & 0xffU,
-                 AP_SGLIST,    sg_list, sg_cnt,
-                 AP_DEVICE,    0x40,
-                 AP_END);
-  }
-
-  return(rc);
-}
-
-/******************************************************************************
- * Fabricate ATA WRITE command based on the capabilities of the corresponding
- * device and the paramters set from above (NCQ, etc)
- */
-static int ata_cmd_write(IORBH *pIorb, AD_INFO *ai, int p, int d, int slot,
-                         ULONG sector, ULONG count, SCATGATENTRY *sg_list,
-                         ULONG sg_cnt, int write_through)
-{
-  int rc;
-
-  if (sector >= (1UL << 28) || count > 256 || add_workspace(pIorb)->is_ncq)
-  {
-    /* need LBA48 for this command */
-    if (!ai->ports[p].devs[d].lba48)
-    {
-      iorb_seterr(pIorb, IOERR_RBA_LIMIT);
-      return(-1);
-    }
-    if (add_workspace(pIorb)->is_ncq)
-    {
-      /* use NCQ write; count goes into feature register, tag into count! */
-      rc = ata_cmd(ai, p, d, slot, ATA_CMD_FPDMA_WRITE,
-                   AP_SECTOR_48, sector, 0,
-                   AP_FEATURES,  count,
-                   /* tag = slot */
-                   AP_COUNT,     (slot << 3),
-                   AP_SGLIST,    sg_list, sg_cnt,
-                   AP_DEVICE,    0x40,
-                   /* force unit access */
-                   AP_DEVICE,    (write_through && !force_write_cache) ? 0x80 : 0,
-                   AP_WRITE,     1,
-                   AP_END);
-    }
-    else
-    {
-      rc = ata_cmd(ai, p, d, slot, ATA_CMD_WRITE_EXT,
-                   AP_SECTOR_48, sector, 0,
-                   AP_COUNT,     count,
-                   AP_SGLIST,    sg_list, sg_cnt,
-                   AP_DEVICE,    0x40,
-                   AP_WRITE,     1,
-                   AP_END);
-    }
-  }
-  else
-  {
-    rc = ata_cmd(ai, p, d, slot, ATA_CMD_WRITE,
-                 AP_SECTOR_28, sector,
-                 AP_COUNT,     count & 0xffU,
-                 AP_SGLIST,    sg_list, sg_cnt,
-                 AP_DEVICE,    0x40,
-                 AP_WRITE,     1,
-                 AP_END);
-  }
-
-  return(rc);
-}
-
-/******************************************************************************
- * Copy block from S/G list to virtual address or vice versa.
- */
-void sg_memcpy(SCATGATENTRY *sg_list, USHORT sg_cnt, ULONG sg_off,
-               void *buf, USHORT len, SG_MEMCPY_DIRECTION dir)
-{
-  USHORT i;
-  USHORT l;
-  ULONG phys_addr;
-  ULONG pos = 0;
-  char *p;
-
-  /* walk through S/G list to find the elements involved in the operation */
-  for (i = 0; i < sg_cnt && len > 0; i++)
-  {
-    if (pos <= sg_off && pos + sg_list[i].XferBufLen > sg_off)
-    {
-      /* this S/G element intersects with the block to be copied */
-      phys_addr = sg_list[i].ppXferBuf + (sg_off - pos);
-      if ((l = sg_list[i].XferBufLen - (sg_off - pos)) > len)
-      {
-        l = len;
-      }
-
-      if (Dev32Help_PhysToLin(phys_addr, l, (PVOID) &p))
-      {
-        panic(__func__": DevHelp_PhysToLin() failed");
-      }
-      if (dir == SG_TO_BUF)
-      {
-        memcpy(buf, p, l);
-      }
-      else
-      {
-        memcpy(p, buf, l);
-      }
-      sg_off += l;
-      buf = (char *) buf + l;
-      len -= l;
-    }
-
-    pos += sg_list[i].XferBufLen;
-  }
-}
-
-/******************************************************************************
  * Halt processing by submitting an internal error. This is a last resort and
  * should only be called when the system state is corrupt.