From 8a9578f6c167e8bc50b5ee35bc637e4adc3b0c9d Mon Sep 17 00:00:00 2001
From: Alan Cox <alan@linux.intel.com>
Date: Sun, 25 Jan 2015 00:23:50 +0000
Subject: [PATCH] tgl6502: enable raw I/O on the rom driver

---
 Kernel/platform-tgl6502/devrd.c   | 14 +++++----
 Kernel/platform-tgl6502/tgl6502.s | 47 ++++++++++++++++++++++++-------
 2 files changed, 45 insertions(+), 16 deletions(-)

diff --git a/Kernel/platform-tgl6502/devrd.c b/Kernel/platform-tgl6502/devrd.c
index da49b940..b6dae434 100644
--- a/Kernel/platform-tgl6502/devrd.c
+++ b/Kernel/platform-tgl6502/devrd.c
@@ -7,9 +7,10 @@
 #include <printf.h>
 #include <devrd.h>
 
-uint16_t romd_roff;
-uint8_t romd_rmap;
-uint8_t romd_bank;
+extern uint16_t romd_roff;
+extern uint8_t romd_rmap;
+extern uint8_t romd_bank;
+extern uint8_t romd_mapu;
 
 extern void __fastcall__ rd_copyin(uint16_t addr);
 
@@ -28,19 +29,20 @@ static int rd_transfer(bool is_read, uint8_t rawflag)
     if(rawflag) {
         dlen = udata.u_count;
         dptr = (uint16_t)udata.u_base;
-        if (dptr & 0x1FF || 1 /* BROKEN */) {
+        /* Must be block aligned but otherwise we are happy */
+        if ((udata.u_offset|dlen) & 0x1FF) {
             udata.u_error = EIO;
             return -1;
         }
         block = udata.u_offset >> 9;
         block_xfer = dlen >> 9;
-        map = 1;
+        romd_mapu = 1;
     } else { /* rawflag == 0 */
         dlen = 512;
         dptr = (uint16_t)udata.u_buf->bf_data;
         block = udata.u_buf->bf_blk;
         block_xfer = 1;
-        map = 0;
+        romd_mapu = 0;
     }
 
     while (ct < block_xfer) {
diff --git a/Kernel/platform-tgl6502/tgl6502.s b/Kernel/platform-tgl6502/tgl6502.s
index 58400df1..eca9ec97 100644
--- a/Kernel/platform-tgl6502/tgl6502.s
+++ b/Kernel/platform-tgl6502/tgl6502.s
@@ -639,34 +639,61 @@ _platform_interrupt_i:
 ;	ROM disc copier (needs to be in common), call with ints off
 ;
 ;	AX = ptr, length always 512, src and page in globals
+;
+;	Uses ptr3/4 as 1/2 are reserved for the mappers
 ;
 
-	.import _romd_bank, _romd_roff, _romd_rmap;
+	.export _romd_bank, _romd_roff, _romd_rmap, _romd_mapu
 	.export _rd_copyin
 
 _rd_copyin:
-	sta ptr1
-	stx ptr1+1		; Save the target
+	sta ptr3
+	stx ptr3+1		; Save the target
+
+	;
+	;	We must flip banks before we play mmu pokery, or it will
+	; undo all our work. This means our variables must be commondata
+	;
+	lda _romd_mapu
+	beq rd_kmap
+	jsr map_process_always
+rd_kmap:
 	ldy _romd_bank		; 0 = A0, 1 = C0, pick based on target
 	lda $FF8F,y		;
-	pha
+	pha			; Save the old mapping
 	lda _romd_rmap
 	sta $FF8F,y
 	lda _romd_roff
-	sta ptr2
+	sta ptr4
 	lda _romd_roff+1
-	sta ptr2+1
+	sta ptr4+1
 	ldy #0
 	ldx #2
-rd_cl:	lda (ptr2),y
-	sta (ptr1),y
+rd_cl:	lda (ptr4),y
+	sta (ptr3),y
 	iny
 	bne rd_cl
-	inc ptr1+1
-	inc ptr2+1
+	inc ptr3+1
+	inc ptr4+1
 	dex
 	bne rd_cl
 	pla
+	ldx rd_kmap
+	bne rd_mapback
 	ldy _romd_bank
 	sta $FF8F,y
 	rts
+rd_mapback:
+	jsr map_kernel
+	rts
+
+	.segment "COMMONDATA"
+
+_romd_roff:
+	.res 2
+_romd_rmap:
+	.res 1
+_romd_bank:
+	.res 1
+_romd_mapu:
+	.res 1
-- 
2.34.1