From: Alan Cox <alan@linux.intel.com>
Date: Sun, 26 Apr 2015 21:34:33 +0000 (+0100)
Subject: mtx: Add floppy support for raw mode
X-Git-Url: https://git.ndcode.org/public/gitweb.cgi?a=commitdiff_plain;h=62ede2393e3937e954a348e0a861ebb8a2e839cf;p=FUZIX.git

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 415e60de..7dde96cc 100644
--- 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 dfda83e5..e4921a98 100644
--- 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 ad5a7c14..d6eebfb4 100644
--- 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