mtx: Add floppy support for raw mode

We need this in order to stamp out users of CONFIG_LEGACY_RAW

diff --git a/Kernel/platform-mtx/devfd.c b/Kernel/platform-mtx/devfd.c
index 415e60d..7dde96c 100644 (file)
--- a/Kernel/platform-mtx/devfd.c
+++ b/Kernel/platform-mtx/devfd.c
@@ -41,14 +41,24 @@ static int fd_transfer(uint8_t minor, bool is_read, uint8_t rawflag)
 {
     blkno_t block;
     uint16_t dptr;
-    int ct = 0;
     int tries;
     uint8_t err = 0;
     uint8_t *driveptr = &fd_tab[minor & 1];
-    uint8_t cmd[6];
     irqflags_t irq;
+    uint8_t nblock;
 
-    if(rawflag)
+    if(rawflag == 0) {
+        dptr = (uint16_t)udata.u_buf->bf_data;
+        block = udata.u_buf->bf_blk;
+        nblock = 2;
+    } else if (rawflag == 1) {
+        if (((uint16_t)udata.u_offset|udata.u_count) & BLKMASK)
+            goto bad2;
+        dptr = (uint16_t)udata.u_base;
+        block = udata.u_offset >> 9;
+        nblock = udata.u_count >> 8;
+    }
+    else
         goto bad2;
 
     irq = di();
@@ -60,21 +70,20 @@ static int fd_transfer(uint8_t minor, bool is_read, uint8_t rawflag)
     }
     irqrestore(irq);
 
-    dptr = (uint16_t)udata.u_buf->bf_data;
-    block = udata.u_buf->bf_blk;
 
 //    kprintf("Issue command: drive %d block %d\n", minor, block);
-    cmd[0] = is_read ? FD_READ : FD_WRITE;
-    cmd[1] = block / 16;               /* 2 sectors per block */
-    cmd[2] = ((block & 15) << 1); /* 0 - 1 base is corrected in asm */
-    cmd[3] = is_read ? OPDIR_READ: OPDIR_WRITE;
-    cmd[4] = dptr & 0xFF;
-    cmd[5] = dptr >> 8;
-
-    while (ct < 2) {
+    fd_cmd[0] = rawflag;
+    fd_cmd[1] = is_read ? FD_READ : FD_WRITE;
+    fd_cmd[2] = block / 16;            /* 2 sectors per block */
+    fd_cmd[3] = ((block & 15) << 1); /* 0 - 1 base is corrected in asm */
+    fd_cmd[4] = is_read ? OPDIR_READ: OPDIR_WRITE;
+
+    fd_data = dptr;
+
+    while (nblock--) {
         for (tries = 0; tries < 4 ; tries++) {
 //            kprintf("Sector: %d Track %d\n", cmd[2]+1, cmd[1]);
-            err = fd_operation(cmd, driveptr);
+            err = fd_operation(driveptr);
             if (err == 0)
                 break;
             if (tries > 1)
@@ -83,9 +92,12 @@ static int fd_transfer(uint8_t minor, bool is_read, uint8_t rawflag)
         /* FIXME: should we try the other half and then bale out ? */
         if (tries == 3)
             goto bad;
-        cmd[5]++;
-        cmd[2]++;      /* Next sector for next block */
-        ct++;
+        fd_data += 256;
+        fd_cmd[3]++;   /* Next sector for next block */
+        if (fd_cmd[3] == 16) { /* Next track */
+            fd_cmd[3] = 0;
+            fd_cmd[2]++;
+        }
     }
     return 1;
 bad:

diff --git a/Kernel/platform-mtx/devfd.h b/Kernel/platform-mtx/devfd.h
index dfda83e..e4921a9 100644 (file)
--- a/Kernel/platform-mtx/devfd.h
+++ b/Kernel/platform-mtx/devfd.h
@@ -9,8 +9,12 @@ void fd_motor_timer(void);
 
 /* low level interface */
 uint16_t fd_reset(uint8_t *driveptr);
-uint16_t fd_operation(uint8_t *cmd, uint8_t *driveptr);
+uint16_t fd_operation(uint8_t *driveptr);
 uint16_t fd_motor_on(uint16_t drivesel);
 uint16_t fd_motor_off(void);
 
+/* In common */
+extern uint8_t fd_cmd[8];
+extern uint16_t fd_data;
+
 #endif /* __DEVFD_DOT_H__ */

diff --git a/Kernel/platform-mtx/floppy.s b/Kernel/platform-mtx/floppy.s
index ad5a7c1..d6eebfb 100644 (file)
--- a/Kernel/platform-mtx/floppy.s
+++ b/Kernel/platform-mtx/floppy.s
@@ -14,6 +14,11 @@
        .globl _fd_operation
        .globl _fd_motor_on
        .globl _fd_motor_off
+       .globl _fd_cmd
+       .globl _fd_data
+
+       .globl map_process_always
+       .globl map_kernel
 
 FDCREG .equ    0x10
 FDCTRK .equ    0x11
@@ -275,19 +280,23 @@ _fd_reset:
 ;
 _fd_operation:
        pop     bc              ; return address
-       pop     hl              ; command
        pop     de              ; drive track ptr
        push    de
-       push    hl
        push    bc
-       push    ix
-       push    hl
-       pop     ix
-       ld      l, DATA(ix)
-       ld      h, DATA+1(ix)
+
+       push    ix              ; save IX (caller save in SDCC)
+
+       ld      hl, (_fd_data)   ; data ptr
+       ld      a, (_fd_cmd)     ; command type
+       ld      ix, #_fd_cmd + 1 ; command
+       or      a
+       push    af
+       call    nz, map_process_always
        call    fdsetup         ; Set up for a command
        ld      l, a
        ld      h, #0
+       pop     af
+       call    nz, map_kernel
        pop     ix
        ret
 ;
@@ -379,4 +388,7 @@ motor_running:
        .db     0
 fdcctrl:
        .db     0
-  
\ No newline at end of file
+_fd_cmd:
+       .ds     5
+_fd_data:
+       .dw     0