1 files changed, 131 insertions, 76 deletions

diff --git a/kernel/hd.c b/kernel/hd.c
index 65ebfd7..eab82dc 100644
--- a/kernel/hd.c
+++ b/kernel/hd.c
@@ -1,7 +1,12 @@
 #include <asm/io.h>
 #include <kernel/con.h>
+#include <kernel/fs.h>
 #include <kernel/kernel.h>
 #include <stdint.h>
+#include <sys/types.h>
+
+#define SECTS(n) ((n) * (BLOCK_SIZE / 512))
+#define BLOCKS(n) ((n) / (BLOCK_SIZE / 512))
 
 #define ST_ERR 1
 #define ST_DRQ 8
@@ -10,53 +15,23 @@
 #define ST_RDY 64
 #define ST_BSY 128
 
-/* sector buffer */
+/* status buffer */
 static char sbuf[512];
 
-void hd_init(void) {
-  int i;
-  uint8_t s;
-  uint16_t *buf = (uint16_t*) sbuf;
-
-  /* select the master drive */
-  outb(0x1F6, 0xA0);
-  /* send the IDENTIFY command */
-  outb(0x1F7, 0xEC);
+static uint32_t nblocks;
 
-  while(1) {
-    s = inb(0x1F7);
-    if(!s) {
-      printk("[hd] Master drive not detected on primary bus!\n");
-      return;
-    }
-    if(s & ST_ERR) {
-      if(!inb(0x1F4) && !inb(0x1F5))
-        continue;
-      printk("[hd] IDENTIFY command error!\n");
-      return;
-    }
-    if(s & ST_DRQ && !(s & ST_ERR))
-      break;
-  }
-
-  printk("[hd] Master drive detected on primary bus! (status: 0x%02x)\n", s);
-
-  for(i = 0; i < 256; i++)
-    buf[i] = inw(0x1F0);
-}
-
-int hd_read(void *buf, uint32_t lba, uint8_t c) {
+static int read_sect(void *buf, uint32_t lba) {
   int n;
   uint8_t s;
   uint16_t *p = buf;
 
-  if(!c)
-    return 0;
+  if(lba > nblocks - 1)
+    return -1;
 
   /* select the master disk on the primary bus */
   outb(0x1F6, 0xE0 | ((lba >> 24) & 0xF));
   /* set the number of sectors to be read */
-  outb(0x1F2, c);
+  outb(0x1F2, 1);
   /* set the LBA address for the read operation */
   outb(0x1F3, lba);
   outb(0x1F4, lba >> 8);
@@ -64,42 +39,40 @@ int hd_read(void *buf, uint32_t lba, uint8_t c) {
   /* send the READ command */
   outb(0x1F7, 0x20);
 
-  while(c--) {
-    /* wait for the drive */
-    while(1) {
-      /* poll the drive's status */
-      s = inb(0x1F7);
-      if(s & ST_DF) {
-        printk("[hd] Drive failure!\n");
-        panic();
-      }
-      if(s & ST_ERR)
-        return -1;
-      if(!(s & ST_BSY) && s & ST_DRQ)
-        break;
+  /* wait for the drive */
+  while(1) {
+    /* poll the drive's status */
+    s = inb(0x1F7);
+    if(s & ST_DF) {
+      printk("[hd] Drive failure!\n");
+      panic();
     }
-
-    /* buffer is full, copy it's data */
-    n = 256;
-    while(n--)
-      *(p++) = inw(0x1F0);
+    if(s & ST_ERR)
+      return -1;
+    if(!(s & ST_BSY) && s & ST_DRQ)
+      break;
   }
 
+  /* buffer is full, copy it's data */
+  n = 256;
+  while(n--)
+    *(p++) = inw(0x1F0);
+
   return 0;
 }
 
-int hd_write(void *buf, uint32_t lba, uint8_t c) {
+static int write_sect(void *buf, uint32_t lba) {
   int n;
   uint8_t s;
   uint16_t *p = buf;
 
-  if(!c)
-    return 0;
+  if(lba > nblocks - 1)
+    return -1;
 
   /* select the master disk on the primary bus */
   outb(0x1F6, 0xE0 | ((lba >> 24) & 0xF));
   /* set the number of sectors to be written */
-  outb(0x1F2, c);
+  outb(0x1F2, 1);
   /* set the LBA address for the write operation */
   outb(0x1F3, lba);
   outb(0x1F4, lba >> 8);
@@ -107,26 +80,108 @@ int hd_write(void *buf, uint32_t lba, uint8_t c) {
   /* send the WRITE command */
   outb(0x1F7, 0x30);
 
-  while(c--) {
-    /* wait for the drive */
-    while(1) {
-      /* poll the drive's status */
-      s = inb(0x1F7);
-      if(s & ST_DF) {
-        printk("[hd] Drive failure!\n");
-        panic();
-      }
-      if(s & ST_ERR)
-        return -1;
-      if(!(s & ST_BSY) && s & ST_DRQ)
-        break;
+  /* wait for the drive */
+  while(1) {
+    /* poll the drive's status */
+    s = inb(0x1F7);
+    if(s & ST_DF) {
+      printk("[hd] Drive failure!\n");
+      panic();
     }
-
-    /* refill the drive's buffer */
-    n = 256;
-    while(n--)
-      outw(0x1F0, *(p++));
+    if(s & ST_ERR)
+      return -1;
+    if(!(s & ST_BSY) && s & ST_DRQ)
+      break;
   }
 
+  /* refill the drive's buffer */
+  n = 256;
+  while(n--)
+    outw(0x1F0, *(p++));
+
   return 0;
 }
+
+size_t hd_read_block(void *buf, size_t block, size_t len) {
+  int ret;
+  size_t i;
+  uint32_t s = SECTS(block);
+  uint32_t e = SECTS(block + len);
+
+  if(s > nblocks - 1)
+    s = nblocks - 1;
+  if(e > nblocks - 1)
+    e = nblocks - 1;
+
+  for(i = 0; i < (e - s); i++) {
+    ret = read_sect(buf, i + s);
+
+    if(ret < 0)
+      return BLOCKS(i);
+
+    buf = ((char*) buf + 512);
+  }
+
+  return len;
+}
+
+size_t hd_write_block(void *buf, size_t block, size_t len) {
+  int ret;
+  size_t i;
+  uint32_t s = SECTS(block);
+  uint32_t e = SECTS(block + len);
+
+  if(s > nblocks - 1)
+    s = nblocks - 1;
+  if(e > nblocks - 1)
+    e = nblocks - 1;
+
+  for(i = 0; i < (e - s); i++) {
+    ret = write_sect(buf, i + s);
+
+    if(ret < 0)
+      return BLOCKS(i);
+
+    buf = ((char*) buf + 512);
+  }
+
+  return len;
+}
+
+void hd_init(void) {
+  int i;
+  uint8_t s;
+  uint16_t *buf = (uint16_t*) sbuf;
+
+  /* select the master drive */
+  outb(0x1F6, 0xA0);
+  /* send the IDENTIFY command */
+  outb(0x1F7, 0xEC);
+
+  while(1) {
+    s = inb(0x1F7);
+    if(!s) {
+      printk("[hd] Master drive not detected on primary bus!\n");
+      return;
+    }
+    if(s & ST_ERR) {
+      if(!inb(0x1F4) && !inb(0x1F5))
+        continue;
+      printk("[hd] IDENTIFY command error!\n");
+      return;
+    }
+    if(s & ST_DRQ && !(s & ST_ERR))
+      break;
+  }
+
+  for(i = 0; i < 256; i++)
+    buf[i] = inw(0x1F0);
+
+  nblocks = *((uint32_t*) &buf[60]);
+  if(!nblocks) {
+    printk("[hd] Drive does not support LBA28 addressing!\n");
+    return;
+  }
+
+  printk("[hd] Primary bus master drive initialized! (status: 0x%02x, nblocks: 0x%01x)\n", s, nblocks);
+}