[mubios] Add CHS disk services and multi-sector PIO reads

3 files changed, 167 insertions, 23 deletions

diff --git a/mubios/drivers/ata.c b/mubios/drivers/ata.c
index a7c6d8d..86bab21 100644
--- a/mubios/drivers/ata.c
+++ b/mubios/drivers/ata.c
@@ -30,6 +30,7 @@
 #define ATA_DRIVE 0x1F6u
 #define ATA_STATUS 0x1F7u
 #define ATA_COMMAND 0x1F7u
+#define ATA_CONTROL 0x3F6u
 
 #define ATA_STATUS_ERR 0x01u
 #define ATA_STATUS_DRQ 0x08u
@@ -37,6 +38,7 @@
 
 #define ATA_CMD_READ_SECTORS 0x20u
 #define ATA_CMD_IDENTIFY 0xECu
+#define ATA_CONTROL_SRST 0x04u
 
 static u8
 ata_wait_not_busy (void)
@@ -61,10 +63,21 @@ ata_delay_400ns (void)
   io_in8 (ATA_STATUS);
 }
 
+u8
+ata_reset (void)
+{
+  io_out8 (ATA_CONTROL, ATA_CONTROL_SRST);
+  ata_delay_400ns ();
+  io_out8 (ATA_CONTROL, 0);
+  ata_delay_400ns ();
+  return 0;
+}
+
 static u8
-ata_read_one_sector28 (u32 lba, u16 *buffer)
+ata_read_transfer28 (u32 lba, u16 *buffer, u16 count)
 {
   u8 status;
+  u16 sector;
   u16 index;
 
   status = ata_wait_not_busy ();
@@ -74,21 +87,25 @@ ata_read_one_sector28 (u32 lba, u16 *buffer)
   io_out8 (ATA_DRIVE, (u8)(0xE0u | ((lba >> 24) & 0x0Fu)));
   ata_delay_400ns ();
 
-  io_out8 (ATA_SECTOR_COUNT, 1);
+  io_out8 (ATA_SECTOR_COUNT, (u8)count);
   io_out8 (ATA_LBA_LOW, (u8)(lba & 0xFFu));
   io_out8 (ATA_LBA_MID, (u8)((lba >> 8) & 0xFFu));
   io_out8 (ATA_LBA_HIGH, (u8)((lba >> 16) & 0xFFu));
   io_out8 (ATA_COMMAND, ATA_CMD_READ_SECTORS);
 
-  status = ata_wait_not_busy ();
-  if ((status & ATA_STATUS_ERR) != 0)
-    return 1;
+  for (sector = 0; sector < count; ++sector)
+    {
+      status = ata_wait_not_busy ();
+      if ((status & ATA_STATUS_ERR) != 0
+          || (status & ATA_STATUS_DRQ) == 0)
+        return 1;
 
-  if ((status & ATA_STATUS_DRQ) == 0)
-    return 1;
+      for (index = 0; index < 256; ++index)
+        buffer[index] = io_in16 (ATA_DATA);
 
-  for (index = 0; index < 256; ++index)
-    buffer[index] = io_in16 (ATA_DATA);
+      buffer += 256;
+      ata_delay_400ns ();
+    }
 
   return 0;
 }
@@ -96,21 +113,10 @@ ata_read_one_sector28 (u32 lba, u16 *buffer)
 u8
 ata_read_sectors28 (u32 lba, u16 *buffer, u16 count)
 {
-  u16 sector;
-
   if (count == 0u)
     return 1;
 
-  for (sector = 0; sector < count; ++sector)
-    {
-      if (ata_read_one_sector28 (lba, buffer) != 0)
-        return 1;
-
-      lba++;
-      buffer += 256;
-    }
-
-  return 0;
+  return ata_read_transfer28 (lba, buffer, count);
 }
 
 u8
@@ -141,13 +147,22 @@ ata_identify (u16 *cylinders, u16 *heads, u16 *sectors_per_track,
   for (index = 0; index < 256; ++index)
     identify_data[index] = io_in16 (ATA_DATA);
 
+  ata_delay_400ns ();
+
   *cylinders = identify_data[1];
   *heads = identify_data[3];
   *sectors_per_track = identify_data[6];
   *total_sectors_low
-      = (u32)identify_data[60] | ((u32)identify_data[61] << 16);
-  *total_sectors_high
       = (u32)identify_data[100] | ((u32)identify_data[101] << 16);
+  *total_sectors_high
+      = (u32)identify_data[102] | ((u32)identify_data[103] << 16);
+
+  if (*total_sectors_low == 0u && *total_sectors_high == 0u)
+    {
+      *total_sectors_low
+          = (u32)identify_data[60] | ((u32)identify_data[61] << 16);
+      *total_sectors_high = 0;
+    }
 
   return 0;
 }
diff --git a/mubios/include/bios.h b/mubios/include/bios.h
index 7ec2198..85906d6 100644
--- a/mubios/include/bios.h
+++ b/mubios/include/bios.h
@@ -51,6 +51,7 @@ void ioapic_init (void);
 void ivt_init (void);
 
 u8 ata_read_sectors28 (u32 lba, u16 *buffer, u16 count);
+u8 ata_reset (void);
 u8 ata_identify (u16 *cylinders, u16 *heads, u16 *sectors_per_track,
                  u32 *total_sectors_low, u32 *total_sectors_high);
 
diff --git a/mubios/services/int13.c b/mubios/services/int13.c
index 73d553b..033a045 100644
--- a/mubios/services/int13.c
+++ b/mubios/services/int13.c
@@ -23,6 +23,8 @@
 #include "regs.h"
 
 #define BIOS_CF 0x0001u
+#define BIOS_HARD_DISK 0x0080u
+#define CHS_MAX_CYLINDER 0x03FFu
 
 struct edd_packet
 {
@@ -48,6 +50,13 @@ struct edd_parameters
   u16 bytes_per_sector;
 } __attribute__ ((packed));
 
+struct ata_geometry
+{
+  u16 cylinders;
+  u16 heads;
+  u16 sectors_per_track;
+};
+
 static void
 int13_fail (struct bios_regs *regs)
 {
@@ -73,6 +82,17 @@ get_edd_parameters (struct bios_regs *regs)
   return (struct edd_parameters *)address;
 }
 
+static u8
+get_ata_geometry (struct ata_geometry *geometry)
+{
+  u32 total_sectors_low;
+  u32 total_sectors_high;
+
+  return ata_identify (&geometry->cylinders, &geometry->heads,
+                       &geometry->sectors_per_track, &total_sectors_low,
+                       &total_sectors_high);
+}
+
 void
 int13_service (struct bios_regs *regs)
 {
@@ -82,6 +102,114 @@ int13_service (struct bios_regs *regs)
 
   function = (u8)(regs->eax >> 8);
 
+  if (function == 0x00u)
+    {
+      if ((regs->edx & 0x00FFu) != BIOS_HARD_DISK)
+        {
+          int13_fail (regs);
+          return;
+        }
+
+      if (ata_reset () != 0)
+        {
+          int13_fail (regs);
+          return;
+        }
+
+      regs->eax &= 0xFFFF0000u;
+      regs->flags &= (u16)~BIOS_CF;
+      return;
+    }
+
+  if (function == 0x02u)
+    {
+      struct ata_geometry geometry;
+      u8 count;
+      u8 sector;
+      u8 head;
+      u16 cylinder;
+      u32 lba;
+
+      count = (u8)regs->eax;
+      sector = (u8)regs->ecx & 0x3Fu;
+      cylinder = (u16)(regs->ecx >> 8)
+                 | (u16)(((u8)(regs->ecx >> 14) & 0x03u) << 8);
+      head = (u8)(regs->edx >> 8);
+
+      if ((regs->edx & 0x00FFu) != BIOS_HARD_DISK || count == 0u
+          || get_ata_geometry (&geometry) != 0
+          || sector == 0u || sector > geometry.sectors_per_track
+          || head >= geometry.heads || cylinder >= geometry.cylinders)
+        {
+          int13_fail (regs);
+          return;
+        }
+
+      if (cylinder > CHS_MAX_CYLINDER)
+        {
+          int13_fail (regs);
+          return;
+        }
+
+      lba = (((u32)cylinder * geometry.heads) + head)
+            * geometry.sectors_per_track + (u32)sector - 1u;
+      buffer_address
+          = ((u32)regs->es << 4) + (u16)regs->ebx;
+
+      if (ata_read_sectors28 (lba, (u16 *)buffer_address, count) != 0)
+        {
+          int13_fail (regs);
+          return;
+        }
+
+      regs->eax = (regs->eax & 0xFFFF0000u) | (u32)count;
+      regs->flags &= (u16)~BIOS_CF;
+      return;
+    }
+
+  if (function == 0x08u)
+    {
+      struct ata_geometry geometry;
+      u16 max_cylinder;
+      u8 max_head;
+      u8 sectors_per_track;
+
+      if ((regs->edx & 0x00FFu) != BIOS_HARD_DISK
+          || get_ata_geometry (&geometry) != 0)
+        {
+          int13_fail (regs);
+          return;
+        }
+
+      max_cylinder = geometry.cylinders - 1u;
+      if (max_cylinder > CHS_MAX_CYLINDER)
+        max_cylinder = CHS_MAX_CYLINDER;
+
+      max_head = (u8)(geometry.heads - 1u);
+      sectors_per_track = (u8)geometry.sectors_per_track;
+      regs->eax &= 0xFFFF0000u;
+      regs->ecx = (regs->ecx & 0xFFFF0000u)
+                  | (u32)(sectors_per_track & 0x3Fu)
+                  | ((u32)(max_cylinder & 0x00FFu) << 8)
+                  | ((u32)(max_cylinder >> 8) << 14);
+      regs->edx = (regs->edx & 0xFFFF0000u) | ((u32)max_head << 8) | 1u;
+      regs->flags &= (u16)~BIOS_CF;
+      return;
+    }
+
+  if (function == 0x15u)
+    {
+      if ((regs->edx & 0x00FFu) != BIOS_HARD_DISK)
+        {
+          int13_fail (regs);
+          return;
+        }
+
+      regs->eax = (regs->eax & 0xFFFF0000u) | 0x0300u;
+      regs->flags &= (u16)~BIOS_CF;
+      return;
+    }
+
   if (function == 0x41u)
     {
       if (regs->ebx != 0x55AAu)