trs80m1: Add model 3 floppy driver initial attempt

diff --git a/Kernel/platform-trs80m1/devfd3.c b/Kernel/platform-trs80m1/devfd3.c
new file mode 100644 (file)
index 0000000..6dd7520
--- /dev/null
+++ b/Kernel/platform-trs80m1/devfd3.c
@@ -0,0 +1,107 @@
+#include <kernel.h>
+#include <kdata.h>
+#include <printf.h>
+#include <devfd3.h>
+
+#define MAX_FD 4
+
+#define OPDIR_NONE     0
+#define OPDIR_READ     1
+#define OPDIR_WRITE    2
+
+#define FD_READ                0x80    /* 2797 needs 0x88, 1797 needs 0x80 */
+#define FD_WRITE       0xA0    /* Likewise A8 v A0 */
+
+static uint8_t motorct;
+
+/* Extern as they live in common */
+extern uint8_t fd_map, fd_tab[MAX_FD];
+extern uint8_t fd_selected;
+extern uint8_t fd_cmd[7];
+
+/*
+ *     We only support normal block I/O for the moment. We do need to
+ *     add swapping!
+ */
+static uint8_t selmap[4] = { 0x01, 0x02, 0x04, 0x08 };
+
+static int fd3_transfer(uint8_t minor, bool is_read, uint8_t rawflag)
+{
+    int ct = 0;
+    int tries;
+    uint8_t err = 0;
+    uint8_t *driveptr = fd_tab + minor;
+
+    if(rawflag == 2)
+        goto bad2;
+
+    if (fd_selected != minor) {
+        uint8_t err;
+        /* FIXME: We force DD for now */
+        err = fd3_motor_on(selmap[minor]|0x80);
+        if (err)
+            goto bad;
+    }
+
+    if (*driveptr == 0xFF)
+        fd3_reset(driveptr);
+
+    fd_map = rawflag;
+    if (rawflag && d_blkoff(BLKSHIFT))
+            return -1;
+
+    udata.u_nblock *= 2;
+
+    fd_cmd[0] = is_read ? FD_READ : FD_WRITE;
+    fd_cmd[1] = udata.u_block / 9;             /* 2 sectors per block */
+    fd_cmd[2] = ((udata.u_block % 9) << 1) + 1;        /*eww.. */
+    fd_cmd[3] = is_read ? OPDIR_READ: OPDIR_WRITE;
+    fd_cmd[4] = ((uint16_t)udata.u_dptr) & 0xFF;
+    fd_cmd[5] = ((uint16_t)udata.u_dptr) >> 8;
+    fd_cmd[6] = 1;                             /* For now . FIXME */
+
+    while (ct < udata.u_nblock) {
+        for (tries = 0; tries < 4 ; tries++) {
+            err = fd3_operation(driveptr);
+            if (err == 0)
+                break;
+            if (tries > 1)
+                fd3_reset(driveptr);
+        }
+        /* FIXME: should we try the other half and then bale out ? */
+        if (tries == 4)
+            goto bad;
+        fd_cmd[5]++;   /* Move on 256 bytes in the buffer */
+        fd_cmd[2]++;   /* Next sector for 2nd block */
+        ct++;
+    }
+    return udata.u_nblock << 8;
+bad:
+    kprintf("fd%d: error %x\n", minor, err);
+bad2:
+    udata.u_error = EIO;
+    return -1;
+}
+
+int fd3_open(uint8_t minor, uint16_t flag)
+{
+    flag;
+    if(minor >= MAX_FD) {
+        udata.u_error = ENODEV;
+        return -1;
+    }
+    return 0;
+}
+
+int fd3_read(uint8_t minor, uint8_t rawflag, uint8_t flag)
+{
+    flag;
+    return fd3_transfer(minor, true, rawflag);
+}
+
+int fd3_write(uint8_t minor, uint8_t rawflag, uint8_t flag)
+{
+    flag;rawflag;minor;
+//    return 0;
+    return fd3_transfer(minor, false, rawflag);
+}

diff --git a/Kernel/platform-trs80m1/devfd3.h b/Kernel/platform-trs80m1/devfd3.h
new file mode 100644 (file)
index 0000000..1a02e27
--- /dev/null
+++ b/Kernel/platform-trs80m1/devfd3.h
@@ -0,0 +1,17 @@
+#ifndef __DEVFD3_DOT_H__
+#define __DEVFD3_DOT_H__
+
+/* public interface */
+int fd3_read(uint8_t minor, uint8_t rawflag, uint8_t flag);
+int fd3_write(uint8_t minor, uint8_t rawflag, uint8_t flag);
+int fd3_open(uint8_t minor, uint16_t flag);
+
+/* low level interface */
+uint16_t fd3_reset(uint8_t *driveptr);
+uint16_t fd3_operation(uint8_t *driveptr);
+uint16_t fd3_motor_on(uint16_t drivesel);
+uint16_t fd3_motor_off(uint16_t driveptr);
+
+extern void floppy_setup(void);
+
+#endif /* __DEVFD3_DOT_H__ */