From 46b9e8f25c2f2c0652dced023b9fab4de21f2e9b Mon Sep 17 00:00:00 2001
From: Alan Cox <alan@linux.intel.com>
Date: Thu, 14 May 2015 00:26:35 +0100
Subject: [PATCH] dragon-nx32: initial patch for floppy support of user/kernel

---
 Kernel/platform-dragon-nx32/devfd.c  | 44 ++++++++++++++++++----------
 Kernel/platform-dragon-nx32/floppy.s | 28 ++++++++++++++----
 2 files changed, 51 insertions(+), 21 deletions(-)

diff --git a/Kernel/platform-dragon-nx32/devfd.c b/Kernel/platform-dragon-nx32/devfd.c
index c4754a56..5bd22196 100644
--- a/Kernel/platform-dragon-nx32/devfd.c
+++ b/Kernel/platform-dragon-nx32/devfd.c
@@ -14,7 +14,7 @@
 
 static uint8_t motorct;
 static uint8_t fd_selected = 0xFF;
-static uint8_t fd_tab[MAX_FD];
+extern uint8_t *fd_tab;
 
 static void fd_motor_busy(void)
 {
@@ -49,11 +49,11 @@ static int fd_transfer(uint8_t minor, bool is_read, uint8_t rawflag)
 {
     blkno_t block;
     uint16_t dptr;
-    int ct = 0;
+    uint8_t nblock;
     int tries;
     uint8_t err;
     uint8_t *driveptr = fd_tab + minor;
-    uint8_t cmd[6];
+    uint8_t cmd[7];
 
     if(rawflag)
         goto bad2;
@@ -66,18 +66,28 @@ static int fd_transfer(uint8_t minor, bool is_read, uint8_t rawflag)
             goto bad;
     }
 
-    dptr = (uint16_t)udata.u_buf->bf_data;
-    block = udata.u_buf->bf_blk;
+    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;
+    }
 
 //    kprintf("Issue command: drive %d\n", minor);
-    cmd[0] = is_read ? FD_READ : FD_WRITE;
-    cmd[1] = block / 9;		/* 2 sectors per block */
-    cmd[2] = ((block % 9) << 1) + 1;	/*eww.. */
-    cmd[3] = is_read ? OPDIR_READ: OPDIR_WRITE;
-    cmd[4] = dptr >> 8;
-    cmd[5] = dptr & 0xFF;
+    cmd[0] = rawflag;
+    cmd[1] = is_read ? FD_READ : FD_WRITE;
+    cmd[2] = block / 9;		/* 2 sectors per block */
+    cmd[3] = ((block % 9) << 1) + 1;	/*eww.. */
+    cmd[4] = is_read ? OPDIR_READ: OPDIR_WRITE;
+    cmd[5] = dptr >> 8;
+    cmd[6] = dptr & 0xFF;
         
-    while (ct < 2) {
+    while (nblock) {
         for (tries = 0; tries < 4 ; tries++) {
             err = fd_operation(cmd, driveptr);
             if (err == 0)
@@ -88,9 +98,13 @@ 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[4]++;	/* Move on 256 bytes in the buffer */
-        cmd[2]++;	/* Next sector for 2nd block */
-        ct++;
+        cmd[5]++;	/* Move on 256 bytes in the buffer */
+        cmd[3]++;	/* Next sector for next block */
+        if (cmd[3] == 10) {
+            cmd[3] = 1;	/* Track on */
+            cmd[2]++;
+        }
+        nblock--;
     }
     fd_motor_idle();
     return 1;
diff --git a/Kernel/platform-dragon-nx32/floppy.s b/Kernel/platform-dragon-nx32/floppy.s
index b039889e..7c980151 100644
--- a/Kernel/platform-dragon-nx32/floppy.s
+++ b/Kernel/platform-dragon-nx32/floppy.s
@@ -1,5 +1,8 @@
 ;
 ;	Core floppy routines for the Dragon
+;
+;	Must live in common space as they are called from interrupt
+;	contexts and also map user space about
 ;
 
 	.globl	fd_nmi_handler
@@ -9,6 +12,8 @@
 	.globl _fd_operation
 	.globl _fd_motor_on
 	.globl _fd_motor_off
+
+	.globl _fd_tab
 ;
 ;	MMIO for the floppy controller
 ;
@@ -44,7 +49,7 @@ SECTOR	EQU	2
 DIRECT	EQU	3		; 0 = read 2 = write 1 = status
 DATA	EQU	4
 
-	.area	.text
+	.area	.common
 ;
 ;	NMI handling for the floppy drive
 ;
@@ -315,8 +320,13 @@ _fd_operation:
 	orcc	#0x40		; Make sure FIR is off
 	jsr	piasave
 	ldy	6,s		; Drive struct
+	tst	,y+		; User or kernel ?
+	beq	fd_op_k
+	jsr	map_process_always
+fd_op_k:
 	jsr	fdsetup		; Set up for a command
 	tfr	a,b		; Status code or 0xFF for total failure
+	jsr	map_kernel
 	bsr	piaload
 	puls	y,cc,dp,pc	; Restore IRQ state etc
 ;
@@ -380,14 +390,18 @@ _fd_motor_off:
 no_work_motor:
 	puls y,dp,pc
 
-	.area .data
-
+;
+;	We need these mapped during interrupts so must live in common
+;
+	.area .common
 nmivector:
 	.word	nmi_handler
 curdrive:
 	.byte	0xff
-
-	.area .bss
+;
+;	BSS but used with user mapping so keep common
+;
+	.area .common
 motor_running:
 	.byte	0
 fdcctrl:
@@ -396,4 +410,6 @@ pia_stash:
 	.byte	0
 	.byte	0
 	.byte	0
-	.byte	0
\ No newline at end of file
+	.byte	0
+_fd_tab:
+	.byte	0,0,0,0
-- 
2.34.1