From 395c26c932d910cb1a0d18731fad4c80dd6a8b67 Mon Sep 17 00:00:00 2001
From: H. Peter Anvin <hpa@zytor.com>
Date: Mon, 15 Jun 2009 22:21:02 -0700
Subject: [PATCH] core: new diskio library

Beginnings of a diskio library capable of reading and writing, and
with proper handling of bounce buffers and segment boundaries.

Signed-off-by: H. Peter Anvin <hpa@zytor.com>
---
 core/diskio.c       |  268 +++++++++++++++++++++++++++++++++++++++++++++++++++
 core/fs.c           |    3 -
 core/include/disk.h |    6 +-
 core/include/fs.h   |   13 ++-
 4 files changed, 281 insertions(+), 9 deletions(-)
 create mode 100644 core/diskio.c

diff --git a/core/diskio.c b/core/diskio.c
new file mode 100644
index 0000000..24a8f89
--- /dev/null
+++ b/core/diskio.c
@@ -0,0 +1,268 @@
+#include <string.h>
+#include <stdbool.h>
+#include <klibc/compiler.h>
+#include <ilog2.h>
+#include "core.h"
+#include "fs.h"
+#include "disk.h"
+
+#define RETRY_COUNT 6
+
+extern uint16_t MaxTransfer, bsSecPerTrack, bsHeads;
+
+static int chs_rdwr_sectors(struct device *dev, void *buf,
+			    sector_t lba, size_t count, bool is_write)
+{
+    char *ptr = buf;
+    char *tptr;
+    size_t chunk, freeseg;
+    int sector_size  = dev->sector_size;
+    int sector_shift = dev->sector_shift;
+    uint32_t xlba = lba;	/* Truncated LBA (CHS is << 2 TB) */
+    uint32_t t;
+    uint16_t c, h, s;
+    com32sys_t ireg, oreg;
+    size_t done = 0;
+    size_t bytes;
+    int retry;
+
+    memset(&ireg, 0, sizeof ireg);
+
+    ireg.eax.b[1] = 0x02 + is_write;
+    ireg.edx.b[0] = dev->device_number;
+
+    while (count) {
+	chunk = count;
+	if (chunk > MaxTransfer)
+	    chunk = MaxTransfer;
+
+	freeseg = (0x10000 - ((size_t)ptr & 0xffff)) >> sector_shift;
+
+	if ((size_t)buf <= 0xf0000 && freeseg) {
+	    /* Can do a direct load */
+	    tptr = ptr;
+	} else {
+	    /* Either accessing high memory or we're crossing a 64K line */
+	    tptr = core_xfer_buf;
+	    freeseg = (0x10000 - ((size_t)tptr & 0xffff)) >> sector_shift;
+	}
+	if (chunk > freeseg)
+	    chunk = freeseg;
+
+	bytes = chunk << sector_shift;
+
+	if (tptr != ptr && is_write)
+	    memcpy(tptr, ptr, bytes);
+
+	s = xlba % dev->s;
+	t = xlba / dev->s;
+	h = t % dev->h;
+	c = t / dev->h;
+
+	ireg.eax.b[0] = chunk;
+	ireg.ecx.b[1] = c;
+	ireg.ecx.b[0] = ((c & 0x300) >> 2) | (s+1);
+	ireg.edx.b[1] = h;
+	ireg.ebx.w[0] = OFFS(tptr);
+	ireg.es       = SEG(tptr);
+
+	retry = RETRY_COUNT;
+
+	for (;;) {
+	    __intcall(0x13, &ireg, &oreg);
+	    if (!(oreg.eflags.l & EFLAGS_CF))
+		break;
+	    if (retry--)
+		continue;
+	    chunk >>= 1;
+	    if (chunk) {
+		MaxTransfer = chunk;
+		retry = RETRY_COUNT;
+		continue;
+	    }
+	    return done;	/* Failure */
+	}
+
+	bytes = chunk << sector_shift;
+
+	if (tptr != ptr && !is_write)
+	    memcpy(ptr, tptr, bytes);
+
+	ptr   += bytes;
+	xlba  += chunk;
+	count -= chunk;
+	done  += chunk;
+    }
+    return done;
+}
+
+struct edd_rdwr_packet {
+    uint16_t size;
+    uint16_t blocks;
+    far_ptr_t buf;
+    uint64_t lba;
+};
+
+static int edd_rdwr_sectors(struct device *dev, void *buf,
+			    sector_t lba, size_t count, bool is_write)
+{
+    static __lowmem struct edd_rdwr_packet pkt;
+    char *ptr = buf;
+    char *tptr;
+    size_t chunk, freeseg;
+    int sector_size  = dev->sector_size;
+    int sector_shift = dev->sector_shift;
+    com32sys_t ireg, oreg;
+    size_t done = 0;
+    size_t bytes;
+    int retry;
+
+    memset(&ireg, 0, sizeof ireg);
+
+    ireg.eax.b[1] = 0x42 + is_write;
+    ireg.edx.b[0] = dev->device_number;
+    ireg.ds       = SEG(&pkt);
+    ireg.esi.w[0] = OFFS(&pkt);
+
+    while (count) {
+	chunk = count;
+	if (chunk > MaxTransfer)
+	    chunk = MaxTransfer;
+
+	freeseg = (0x10000 - ((size_t)ptr & 0xffff)) >> sector_shift;
+
+	if ((size_t)buf <= 0xf0000 && freeseg) {
+	    /* Can do a direct load */
+	    tptr = ptr;
+	} else {
+	    /* Either accessing high memory or we're crossing a 64K line */
+	    tptr = core_xfer_buf;
+	    freeseg = (0x10000 - ((size_t)tptr & 0xffff)) >> sector_shift;
+	}
+	if (chunk > freeseg)
+	    chunk = freeseg;
+
+	bytes = chunk << sector_shift;
+
+	if (tptr != ptr && is_write)
+	    memcpy(tptr, ptr, bytes);
+
+	pkt.size   = sizeof pkt;
+	pkt.blocks = chunk;
+	pkt.buf    = FAR_PTR(tptr);
+	pkt.lba    = lba;
+
+	retry = RETRY_COUNT;
+
+	for (;;) {
+	    __intcall(0x13, &ireg, &oreg);
+	    if (!(oreg.eflags.l & EFLAGS_CF))
+		break;
+	    if (retry--)
+		continue;
+	    chunk >>= 1;
+	    if (chunk) {
+		MaxTransfer = chunk;
+		retry = RETRY_COUNT;
+		continue;
+	    }
+	    /*** XXX: Consider falling back to CHS here?! ***/
+	    return done;	/* Failure */
+	}
+
+	bytes = chunk << sector_shift;
+
+	if (tptr != ptr && !is_write)
+	    memcpy(ptr, tptr, bytes);
+
+	ptr   += bytes;
+	lba   += chunk;
+	count -= chunk;
+	done  += chunk;
+    }
+    return done;
+}
+struct edd_disk_params {
+    uint16_t  len;
+    uint16_t  flags;
+    uint32_t  phys_c;
+    uint32_t  phys_h;
+    uint32_t  phys_s;
+    uint64_t  sectors;
+    uint16_t  sector_size;
+    far_ptr_t dpte;
+    uint16_t  devpath_key;
+    uint8_t   devpath_len;
+    uint8_t   _pad1[3];
+    char      bus_type[4];
+    char      if_type[8];
+    uint8_t   if_path[8];
+    uint8_t   dev_path[8];
+    uint8_t   _pad2;
+    uint8_t   devpath_csum;
+} __attribute__((packed));
+
+static inline bool is_power_of_2(uint32_t x)
+{
+    return !(x & (x-1));
+}
+
+struct device *disk_init(uint8_t devno, bool cdrom, sector_t part_start)
+{
+    static struct device dev;
+    static __lowmem struct edd_disk_params edd_params;
+    com32sys_t ireg, oreg;
+    bool ebios = cdrom;
+    int sector_size = cdrom ? 2048 : 512;
+
+    memset(&ireg, 0, sizeof ireg);
+
+    /* Get EBIOS support */
+    ireg.eax.b[1] = 0x41;
+    ireg.ebx.w[0] = 0x55aa;
+    ireg.edx.b[0] = devno;
+    ireg.eflags.b[0] = 0x3;	/* CF set */
+
+    __intcall(0x13, &ireg, &oreg);
+
+    if (cdrom || (!(oreg.eflags.l & EFLAGS_CF) &&
+		  oreg.ebx.w[0] == 0xaa55 && (oreg.ecx.b[0] & 1))) {
+	/* Query EBIOS parameters */
+	ireg.eax.b[1] = 0x48;
+	ireg.ds = SEG(&edd_params);
+	ireg.esi.w[0] = OFFS(&edd_params);
+	__intcall(0x13, &ireg, &oreg);
+
+	if (!(oreg.eflags.l & EFLAGS_CF) && oreg.eax.b[1] == 0) {
+	    ebios = true;
+	    if (edd_params.sector_size >= 512 &&
+		is_power_of_2(edd_params.sector_size))
+		sector_size = edd_params.sector_size;
+	}
+    }
+
+    /* CBIOS parameters */
+    dev.h = bsHeads;
+    dev.s = bsSecPerTrack;
+
+    if ((int8_t)devno < 0) {
+	/* Get hard disk geometry from BIOS */
+
+	ireg.eax.b[1] = 0x08;
+	__intcall(0x13, &ireg, &oreg);
+
+	if (!(oreg.eflags.l & EFLAGS_CF)) {
+	    dev.h = oreg.edx.b[1] + 1;
+	    dev.s = oreg.ecx.b[0] & 63;
+	}
+    }
+
+    dev.device_number = devno;
+    dev.type          = ebios;
+    dev.sector_size   = sector_size;
+    dev.sector_shift  = ilog2(sector_size);
+    dev.part_start    = part_start;
+    dev.rdwr_sectors  = ebios ? edd_rdwr_sectors : chs_rdwr_sectors;
+
+    return &dev;
+}
diff --git a/core/fs.c b/core/fs.c
index 617e3e5..d2a82c3 100644
--- a/core/fs.c
+++ b/core/fs.c
@@ -126,9 +126,6 @@ void device_init(struct device *dev, uint8_t device_num, sector_t offset)
         dev->cache_size = sizeof core_cache_buf;
     } else 
         dev->cache_data = NULL;
-
-    /* I just considered the floppy and disk now */
-    dev->read_sectors = read_sectors;
 }
 
 
diff --git a/core/include/disk.h b/core/include/disk.h
index 44a425f..1eb9f03 100644
--- a/core/include/disk.h
+++ b/core/include/disk.h
@@ -1,11 +1,13 @@
 #ifndef DISK_H
 #define DISK_H
 
+#include <stdint.h>
+
 #define SECTOR_SHIFT     9
 #define SECTOR_SIZE      (1 << SECTOR_SHIFT)
 
-typedef unsigned int sector_t;
-typedef unsigned int block_t;
+typedef uint64_t sector_t;
+typedef uint32_t block_t;
 
 extern void read_sectors(char *, sector_t, int);
 extern void getoneblk(char *, block_t, int);
diff --git a/core/include/fs.h b/core/include/fs.h
index 0cb1bfc..0c837bd 100644
--- a/core/include/fs.h
+++ b/core/include/fs.h
@@ -1,6 +1,8 @@
 #ifndef FS_H
 #define FS_H
 
+#include <stddef.h>
+#include <stdbool.h>
 #include <com32.h>
 #include "core.h"
 #include "disk.h"
@@ -15,7 +17,7 @@ struct fs_info {
     struct device *fs_dev;
 };
 
-struct file{
+struct file {
     void*    open_file; /* points to the fs-specific open_file_t */
     struct   fs_info *fs;
     uint32_t file_len;
@@ -58,11 +60,16 @@ struct device {
 
     /* the sector size, 512B for disk and floppy, 2048B for CD */
     uint16_t sector_size;
+    uint8_t sector_shift;
+
+    /* CHS geometry */
+    uint8_t h, s;
+    uint8_t pad1;
 
     /* the start address of this partition(in sectors) */
     sector_t part_start;
        
-    void (*read_sectors)(char *, sector_t, int );
+    int (*rdwr_sectors)(struct device *, void *, sector_t, size_t, bool);
 
     /* 
      * I think we still need the cache_data filed here, 'cause hpa said 
@@ -76,6 +83,4 @@ struct device {
     uint32_t cache_size;
 };
 
-
-
 #endif /* FS_H */
-- 
1.6.0.6

