workaround for bank/common border crossing problem. Initial multi sector transfer.

1 files changed, 65 insertions, 34 deletions

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index dfdb022..d6ab106 100644
--- a/avr/z180-serv.c
+++ b/avr/z180-serv.c
@@ -18,6 +18,7 @@
 #include "debug.h"
 #include "print-utils.h"
 #include "z180-serv.h"
+#include "timer.h"
 
 /*--------------------------------------------------------------------------*/
 
@@ -106,7 +107,8 @@ void do_msg_echo(uint8_t subf, int len, uint8_t * msg)
 
 #define MAX_DRIVE	4
 #define BLOCK_SIZE	512
-
+#define TPA_BASE	0x10000
+#define COMMON_BASE	0xC000
 
 struct cpm_drive_s {
 	uint8_t drv;
@@ -142,7 +144,7 @@ void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
 		goto out;
 	}
 
-	debug("\n##  login: %c:\n", msg[0]+'A');
+	debug("\n## %7lu  login: %c:\n", get_timer(0), msg[0]+'A');
 
 
 	drv = msg[0];
@@ -156,7 +158,7 @@ void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
 */
 
 	if (drv_table[drv].img_name != NULL) {
-		debug("##  close: '%s'\n", drv_table[drv].img_name);
+		debug("## %7lu  close: '%s'\n", get_timer(0), drv_table[drv].img_name);
 		f_close(&drv_table[drv].fd);
 		free(drv_table[drv].img_name);
 		drv_table[drv].img_name = NULL;
@@ -174,7 +176,7 @@ void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
 	res = f_open(&drv_table[drv].fd, drv_table[drv].img_name,
 			FA_WRITE | FA_READ);
 
-    debug("##   open: '%s', (env: '%s'), res: %d\n",
+    debug("## %7lu   open: '%s', (env: '%s'), res: %d\n", get_timer(0),
 			drv_table[drv].img_name, disk_buffer, res);
 
 out:
@@ -187,7 +189,7 @@ out:
 	result_msg[2] = res >> 8;
 
 	if (rc) {
-		debug("##   error    rc: %.02x, res: %d\n", rc, res);
+		debug("## %7lu   error    rc: %.02x, res: %d\n", get_timer(0), rc, res);
 	};
 
 	/* send  result*/
@@ -198,21 +200,29 @@ out:
 /*
  	db	2	; disk command
 	ds	1	; subcommand (login/read/write)
-	ds	1	; @adrv (8 bits)	+0
-	ds	1	; @rdrv (8 bits)	+1
-	ds	1	; @cnt  (8 bits)	+2
-	ds	2	; @trk (16 bits)	+3
-	ds	2	; @sect(16 bits)	+5
+	ds	1	; @adrv (8 bits)		+0
+	ds	1	; @rdrv (8 bits)		+1
+	ds	2	; @trk (16 bits)		+2
+	ds	2	; @sect(16 bits)		+4
+	ds	1	; @cnt  (8 bits)		+6
 	ds	3	; phys. transfer addr	+7
 */
 
+#define ADRV	0
+#define RDRV	1
+#define TRK		2
+#define SEC		4
+#define CNT		6
+#define ADDR	7
+
 void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 {
 	uint8_t drv;
 	uint32_t addr;
 	uint32_t pos;
-
+	unsigned int bytes;
 	bool dowrite = (subf == 2);
+	bool hightpa = false;
 	FRESULT res = 0;
 	uint8_t rc = 0;
 	bool buserr = 0;
@@ -223,48 +233,59 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 		goto out;
 	}
 
-	drv = msg[0];
+	drv = msg[ADRV];
 	if ( drv>= MAX_DRIVE) {
 		rc = 0x02;
 		goto out;
 	}
 
-	addr = ((uint32_t)msg[9] << 16) + ((uint16_t)msg[8] << 8) + msg[7];
+	bytes = msg[CNT] * BLOCK_SIZE;
+	addr = ((uint32_t)msg[ADDR+2] << 16) + ((uint16_t)msg[ADDR+1] << 8) + msg[ADDR];
 
-	/* bytes = BLOCK_SIZE; */			/* TODO: multi sector count */
-	pos = (((uint16_t)(msg[4] << 8) + msg[3]) * 8
-			+ ((uint32_t)(msg[6] << 8) + msg[5])) * BLOCK_SIZE;
 
+	/* TODO: tracks per sector from dpb */
+	pos = (((uint16_t)(msg[TRK+1] << 8) + msg[TRK]) * 8
+			+ ((uint32_t)(msg[SEC+1] << 8) + msg[SEC])) * BLOCK_SIZE;
 
-	debug("## cpm_rw: %s %c: trk: %4d, sec: %d, pos: 0x%.5lx, addr: 0x%.5lx\n",
-			dowrite ? "write" : " read", msg[0]+'A',
-			((uint16_t)(msg[4] << 8) + msg[3]), msg[5], pos, addr);
+	debug("## %7lu cpm_rw: %s %c: trk: %4d, sec: %d, pos: 0x%.8lx, secs: %2d\n",
+			get_timer(0), dowrite ? "write" : " read", msg[ADRV]+'A',
+			((uint16_t)(msg[TRK+1] << 8) + msg[TRK]), msg[SEC], pos, msg[CNT]);
 
 
 
 	/* TODO: check bank boundary crossing */
-	/*
-	   if (addr + BLOCK_SIZE > MAX_MEMORY)
-		... = MAX_MEMORY - addr;
-	*/
-
 
 	res = f_lseek(&drv_table[drv].fd, pos);
-	if (!res) {
-		unsigned int br;
+	while (!res && bytes) {
+		unsigned int cnt, br;
+
+		if (bytes >= BLOCK_SIZE)
+			cnt = BLOCK_SIZE;
+		else
+			cnt = bytes;
+
+		if (addr < (TPA_BASE + COMMON_BASE) &&
+					(addr + cnt) > (TPA_BASE + COMMON_BASE)) {
+			cnt = (TPA_BASE + COMMON_BASE) - addr;
+			hightpa = true;
+		}
+
+		bytes -= cnt;
+
+		debug("## %7lu                                                      "
+				" addr: 0x%.5lx, cnt: %3d\n",
+				get_timer(0), addr, cnt);
 
 		if (dowrite) {
 			if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
 				buserr = 1;
 			} else {
-				z80_read_block(disk_buffer, addr, BLOCK_SIZE);
+				z80_read_block(disk_buffer, addr, cnt);
 				z80_bus_cmd(Release);
-				res = f_write(&drv_table[drv].fd, disk_buffer, BLOCK_SIZE, &br);
-				if (!res)
-					res = f_sync(&drv_table[drv].fd);
+				res = f_write(&drv_table[drv].fd, disk_buffer, cnt, &br);
 			}
 		} else {
-			res = f_read(&drv_table[drv].fd, disk_buffer, BLOCK_SIZE, &br);
+			res = f_read(&drv_table[drv].fd, disk_buffer, cnt, &br);
 			if (res == FR_OK) {
 				if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
 					buserr = 1;
@@ -275,13 +296,23 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 			}
 		}
 
-		if (br != BLOCK_SIZE) {
-			debug("##  f_read res: %d, bytes rd/wr: %u\n", res, br);
+		addr += br;
+		if (hightpa) {
+			addr -= TPA_BASE;
+			hightpa = false;
+		}
+
+		if (br != cnt) {
+			debug("## %7lu f_read res: %d, bytes rd/wr: %u\n", get_timer(0), res, br);
 			dump_ram(disk_buffer, 0, 64, "Read Data");
 			res = -1;
 		}
+
 	}
 
+	if (dowrite && !res)
+		res = f_sync(&drv_table[drv].fd);
+
 out:
 	if (buserr) {
 		debug("Bus timeout\n");
@@ -295,7 +326,7 @@ out:
 	result_msg[2] = res >> 8;
 
 	if (rc) {
-		debug("#### error rc: %.02x, res: %d\n", rc, res);
+		debug("###%7lu  error rc: %.02x, res: %d\n", get_timer(0), rc, res);
 	}
 
 	/* send  result*/