z80pack: "implement" the new fd API

diff --git a/Kernel/dev/z80pack/devfd.c b/Kernel/dev/z80pack/devfd.c
index b045ed0..c5d8970 100644 (file)
--- a/Kernel/dev/z80pack/devfd.c
+++ b/Kernel/dev/z80pack/devfd.c
@@ -7,6 +7,7 @@
 #include <kdata.h>
 #include <printf.h>
 #include <devfd.h>
+#include <fdc.h>
 
 __sfr __at 10 fd_drive;
 __sfr __at 11 fd_track;
@@ -51,6 +52,49 @@ int hd_write(uint8_t minor, uint8_t rawflag, uint8_t flag)
     return fd_transfer(false, minor + 8, rawflag);
 }
 
+/* Floppies are fixed to classic IBM 8" SD format */
+static struct fdcinfo fdcap = {
+    FDF_SD|FDF_8INCH|FDF_SEC128,
+    26,                        /* 26 sectors per track */
+    80,
+    1,
+    1,
+    3,
+    0,
+    FDC_FMT_AUTO,
+    0,
+};
+
+static uint8_t skewtab[4][32] = {
+    { 1,7,13,19,25,5,11,17,23,3,9,15,21,2,8,14,20,26,6,12,18,24,4,10,16,22, },
+    { 1,7,13,19,25,5,11,17,23,3,9,15,21,2,8,14,20,26,6,12,18,24,4,10,16,22, },
+    { 1,7,13,19,25,5,11,17,23,3,9,15,21,2,8,14,20,26,6,12,18,24,4,10,16,22, },
+    { 1,7,13,19,25,5,11,17,23,3,9,15,21,2,8,14,20,26,6,12,18,24,4,10,16,22, }
+};
+
+int fd_ioctl(uint8_t minor, uarg_t request, char *buffer)
+{
+    switch(request) {
+    case FDIO_GETCAP:
+        return uput(&fdcap, buffer, sizeof(struct fdcinfo));
+    case FDIO_GETINFO:
+        return uput(&fdcap, buffer, sizeof(struct fdcinfo));
+    case FDIO_SETINFO:
+        /* Whatever - when they query to check they'll get no changes */
+        return 0;
+    case FDIO_FMTTRK:
+        /* Virtual fd has no format mechanism */
+        return 0;
+    case FDIO_RESTORE:
+        /* Virtual restore is meaningless */
+        return 0;
+    case FDIO_SETSKEW:
+        return uput(skewtab + minor, buffer, 32);
+    default:
+        return -1;
+    }
+}
+
 /* We will wrap on big disks if we ever try and support the Z80Pack P:
    that wants different logic */
 static void fd_geom(int minor, blkno_t block)
@@ -59,9 +103,17 @@ static void fd_geom(int minor, blkno_t block)
        and write to the controller.
        Forced to do real / and % */
     int track = block / sectrack[minor];
-    int sector = block % sectrack[minor] + 1;
-    fd_sectorl = sector & 0xFF;
-    fd_sectorh = sector >> 8;
+    int sector = block % sectrack[minor];
+    if (minor >= 4) {
+        /* Hard disk */
+        sector++;
+        fd_sectorl = sector & 0xFF;
+        fd_sectorh = sector >> 8;
+    } else {
+        /* Floppy */
+        fd_sectorl = skewtab[minor][sector];
+        fd_sectorh = 0;
+    }
     fd_track = track;
 }
 
@@ -134,6 +186,7 @@ static int fd_transfer(bool is_read, uint8_t minor, uint8_t rawflag)
     return ct << 7;
 }
 
+/* Media is fixed at start up time */
 int fd_open(uint8_t minor, uint16_t flag)
 {
     flag;