debug_cpmsd macro

1 files changed, 46 insertions, 42 deletions

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index d6ab106..9b4228a 100644
--- a/avr/z180-serv.c
+++ b/avr/z180-serv.c
@@ -20,6 +20,14 @@
 #include "z180-serv.h"
 #include "timer.h"
 
+
+#define DEBUG_CPM_SDIO	0	/* set to 1 to debug */
+
+#define debug_cpmsd(fmt, args...)		                                            \
+	debug_cond(DEBUG_CPM_SDIO, fmt, ##args)
+
+
+
 /*--------------------------------------------------------------------------*/
 
 
@@ -144,7 +152,7 @@ void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
 		goto out;
 	}
 
-	debug("\n## %7lu  login: %c:\n", get_timer(0), msg[0]+'A');
+	debug_cpmsd("\n## %7lu  login: %c:\n", get_timer(0), msg[0]+'A');
 
 
 	drv = msg[0];
@@ -158,7 +166,7 @@ void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
 */
 
 	if (drv_table[drv].img_name != NULL) {
-		debug("## %7lu  close: '%s'\n", get_timer(0), drv_table[drv].img_name);
+		debug_cpmsd("## %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;
@@ -176,7 +184,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("## %7lu   open: '%s', (env: '%s'), res: %d\n", get_timer(0),
+    debug_cpmsd("## %7lu   open: '%s', (env: '%s'), res: %d\n", get_timer(0),
 			drv_table[drv].img_name, disk_buffer, res);
 
 out:
@@ -189,7 +197,7 @@ out:
 	result_msg[2] = res >> 8;
 
 	if (rc) {
-		debug("## %7lu   error    rc: %.02x, res: %d\n", get_timer(0), rc, res);
+		debug_cpmsd("## %7lu   error    rc: %.02x, res: %d\n", get_timer(0), rc, res);
 	};
 
 	/* send  result*/
@@ -220,9 +228,8 @@ 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;
+	uint8_t secs;
 	bool dowrite = (subf == 2);
-	bool hightpa = false;
 	FRESULT res = 0;
 	uint8_t rc = 0;
 	bool buserr = 0;
@@ -239,7 +246,7 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 		goto out;
 	}
 
-	bytes = msg[CNT] * BLOCK_SIZE;
+	secs = msg[CNT];
 	addr = ((uint32_t)msg[ADDR+2] << 16) + ((uint16_t)msg[ADDR+1] << 8) + msg[ADDR];
 
 
@@ -247,67 +254,64 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 	pos = (((uint16_t)(msg[TRK+1] << 8) + msg[TRK]) * 8
 			+ ((uint32_t)(msg[SEC+1] << 8) + msg[SEC])) * BLOCK_SIZE;
 
-	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 */
+	debug_cpmsd("## %7lu cpm_rw: %s %c: trk:%4d, sec: %d, pos: %.8lx, secs: %2d, "
+			"addr: %.5lx\n", get_timer(0), dowrite ? "write" : " read",
+			msg[ADRV]+'A', ((uint16_t)(msg[TRK+1] << 8) + msg[TRK]), msg[SEC],
+			pos, msg[CNT], addr);
 
 	res = f_lseek(&drv_table[drv].fd, pos);
-	while (!res && bytes) {
+	while (!res && secs--) {
 		unsigned int cnt, br;
 
-		if (bytes >= BLOCK_SIZE)
-			cnt = BLOCK_SIZE;
-		else
-			cnt = bytes;
-
+		/* check bank boundary crossing */
+		cnt = 0;
 		if (addr < (TPA_BASE + COMMON_BASE) &&
-					(addr + cnt) > (TPA_BASE + COMMON_BASE)) {
+					(addr + BLOCK_SIZE) > (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 (cnt) {
+			debug_cpmsd("## %67c addr: %.5lx, cnt: %3d\n", ' ', addr, cnt);
+			debug_cpmsd("## %67c addr: %.5lx, cnt: %3d\n", ' ', addr+cnt-TPA_BASE, BLOCK_SIZE-cnt);
+		}
 
 		if (dowrite) {
 			if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
 				buserr = 1;
+				break;
 			} else {
-				z80_read_block(disk_buffer, addr, cnt);
+				if (cnt) {
+					z80_read_block(disk_buffer, addr, cnt);
+					addr = addr + cnt - TPA_BASE;
+				}
+				z80_read_block(disk_buffer+cnt, addr, BLOCK_SIZE - cnt);
 				z80_bus_cmd(Release);
-				res = f_write(&drv_table[drv].fd, disk_buffer, cnt, &br);
 			}
+			res = f_write(&drv_table[drv].fd, disk_buffer, BLOCK_SIZE, &br);
 		} else {
-			res = f_read(&drv_table[drv].fd, disk_buffer, cnt, &br);
-			if (res == FR_OK) {
+			res = f_read(&drv_table[drv].fd, disk_buffer, BLOCK_SIZE, &br);
+			if (res == FR_OK && br == BLOCK_SIZE) {
 				if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
 					buserr = 1;
+					break;
 				} else {
-					z80_write_block(disk_buffer, addr, br);
+					if (cnt) {
+						z80_write_block(disk_buffer, addr, cnt);
+						addr = addr + cnt - TPA_BASE;
+					}
+					z80_write_block(disk_buffer+cnt, addr, BLOCK_SIZE - cnt);
 					z80_bus_cmd(Release);
 				}
 			}
 		}
 
-		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);
+		if (br != BLOCK_SIZE) {
+			debug_cpmsd("## %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;
 		}
 
+		addr += BLOCK_SIZE;
 	}
 
 	if (dowrite && !res)
@@ -315,7 +319,7 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 
 out:
 	if (buserr) {
-		debug("Bus timeout\n");
+		debug_cpmsd("Bus timeout\n");
 		rc = 0x03;
 	}
 	if (res)
@@ -326,7 +330,7 @@ out:
 	result_msg[2] = res >> 8;
 
 	if (rc) {
-		debug("###%7lu  error rc: %.02x, res: %d\n", get_timer(0), rc, res);
+		debug_cpmsd("###%7lu  error rc: %.02x, res: %d\n", get_timer(0), rc, res);
 	}
 
 	/* send  result*/