1 files changed, 58 insertions, 20 deletions

diff --git a/src/kernel/ide.c b/src/kernel/ide.c
index 39525ad..1f7df10 100644
--- a/src/kernel/ide.c
+++ b/src/kernel/ide.c
@@ -4,6 +4,7 @@
 #include "panic.h"
 #include "util.h"
 #include "ata.h"
+#include "log.h"
 #include "pci.h"
 
 #define MAX_IDE_DRIVES 8
@@ -17,7 +18,7 @@ struct ide_drive_info {
 static struct ide_drive_info ide_drives[MAX_IDE_DRIVES];
 static drive_id_t n_ide_drives;
 
-typedef uint16_t spinner_t;
+typedef uint32_t spinner_t;
 
 //returns the status after waiting
 static uint8_t wait_for_ready(uint16_t base_port) {
@@ -31,6 +32,17 @@ static uint8_t wait_for_ready(uint16_t base_port) {
   PANIC("Spun out in IDE driver.");
 }
 
+static void wait_for_not_busy(uint16_t base_port) {
+  for (spinner_t n = -1; n; --n) {
+    uint8_t s = inb(base_port | ATA_REG_STATUS);
+    if (s & ATA_STATUS_ERROR)
+      PANIC("Error status in IDE driver.");
+    if (!(s & ATA_STATUS_BUSY))
+      return;
+  }
+  PANIC("Spun out in IDE driver.");
+}
+
 //returns the status after waiting
 static uint8_t wait_for_error_or_ready(uint16_t base_port) {
   for (spinner_t n = -1; n; --n) {
@@ -53,6 +65,21 @@ static uint8_t wait_for_data_ready_not_busy(uint16_t base_port) {
   PANIC("Spun out in IDE driver.");
 }
 
+static void prepare_ata_access(uint16_t port, bool slave, uint32_t lba, uint32_t count) {
+  if (lba & 0xf0000000)
+    PANIC("IDE ATA driver does not support reads starting past 128GiB currently.");
+  if (count & 0xffffff00)
+    PANIC("IDE ATA driver does not support reads over 128kiB in length currently.");
+
+  outb(port | ATA_REG_SELECT, (slave ? 0xf0 : 0xe0) | (lba >> 24));
+  //spin?
+
+  outb(port | ATA_REG_COUNT, count);
+  outb(port | ATA_REG_LBA0, lba);
+  outb(port | ATA_REG_LBA1, lba >> 8);
+  outb(port | ATA_REG_LBA2, lba >> 16);
+}
+
 static uint32_t ide_ata_rs(const struct drive *d, uint32_t start, uint32_t count, void *buffer) {
   if (start >= d->n_sectors)
     return 0;
@@ -61,38 +88,49 @@ static uint32_t ide_ata_rs(const struct drive *d, uint32_t start, uint32_t count
   if (!count)
     return 0;
 
-  if (start & 0xf0000000)
-    PANIC("IDE ATA driver does not support reads starting past 256MiB currently.");
-  if (count & 0xffffff00)
-    PANIC("IDE ATA driver does not support reads over 128kiB in length currently.");
-
-  uint32_t lba = start & 0x00ffffff;
-
-  struct ide_drive_info *info = ide_drives + d->drive_id;
-
-  outb(info->base_port | ATA_REG_SELECT, (info->slave ? 0xf0 : 0xe0) | (start >> 24));
-  //spin?
-
-  outb(info->base_port | ATA_REG_COUNT, count);
-  outb(info->base_port | ATA_REG_LBA0, lba);
-  outb(info->base_port | ATA_REG_LBA1, lba >> 8);
-  outb(info->base_port | ATA_REG_LBA2, lba >> 16);
+  const struct ide_drive_info *const info = ide_drives + d->drive_id;
 
+  prepare_ata_access(info->base_port, info->slave, start, count);
   outb(info->base_port | ATA_REG_CMD, ATA_CMD_READ);
 
   uint16_t *buf16 = (uint16_t *)buffer;
 
-  for (uint16_t i = 0; i < count; ++i) {
+  for (uint32_t i = 0; i < count; ++i) {
     wait_for_data_ready_not_busy(info->base_port);
     for (uint16_t j = 0; j < 256; ++j)
-      *buf16++ = inw(info->base_port | ATA_REG_DATA);
+      *(buf16++) = inw(info->base_port | ATA_REG_DATA);
   }
 
   return count;
 }
 
 static uint32_t ide_ata_ws(const struct drive *d, uint32_t start, uint32_t count, const void *buffer) {
-  PANIC("IDE ATA writing not implemented yet.");
+  if (start >= d->n_sectors)
+    return 0;
+  if (start + count > d->n_sectors)
+    count = d->n_sectors - start;
+  if (!count)
+    return 0;
+
+//logf(LOG_INFO, "writing IDE sectors: 0x%h - 0x%h", start * 512, start * 512 + count * 512 - 1);
+
+  const struct ide_drive_info *const info = ide_drives + d->drive_id;
+
+  prepare_ata_access(info->base_port, info->slave, start, count);
+  outb(info->base_port | ATA_REG_CMD, ATA_CMD_WRITE);
+
+  const uint16_t *buf16 = (const uint16_t *)buffer;
+
+  for (uint32_t i = 0; i < count; ++i) {
+    wait_for_not_busy(info->base_port);
+    for (uint16_t j = 0; j < 256; ++j)
+      outw(info->base_port | ATA_REG_DATA, *(buf16++));
+  }
+
+  outb(info->base_port | ATA_REG_CMD, ATA_CMD_FLUSH);
+  wait_for_not_busy(info->base_port);
+
+  return count;
 }
 
 static uint32_t ide_atapi_rs(const struct drive *d, uint32_t start, uint32_t count, void *buffer) {