dsk debug option (unfinished)

1 files changed, 36 insertions, 18 deletions

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index d0d4cfc..a49c9f9 100644
--- a/avr/z180-serv.c
+++ b/avr/z180-serv.c
@@ -212,6 +212,24 @@ static struct cpm_drive_s drv_table[CONFIG_CPM_MAX_DRIVE];
 static int handle_cpm_drv_to;
 
 
+void drv_debug(uint8_t drv, const FLASH char *const fmt, ...)		                                            \
+{
+	struct cpm_drive_s *dp = &drv_table[drv];
+
+	if (dp->opt & DRV_OPT_DEBUG) {
+
+		va_list ap;
+
+		printf_P(PSTR("#  %7lu  dsk%d: "), get_timer(0), drv);
+
+		va_start (ap, fmt);
+		vfprintf_P (stdout, fmt, ap);
+		va_end (ap);
+
+		putc('\n', stdout);
+	}
+}
+
 int drv_list(void)
 {
 	for (uint8_t i = 0; i < CONFIG_CPM_MAX_DRIVE; i++) {
@@ -230,8 +248,7 @@ int drv_detach(uint8_t drv)
 	if (drv < CONFIG_CPM_MAX_DRIVE) {
 		struct cpm_drive_s *p = &drv_table[drv];
 
-		debug_cpmsd("##  detach dsk%d: %s\n", drv,
-				p->img_name ? p->img_name : "-");
+		drv_debug(drv, PSTR("detach from '%s'"), p->img_name ? p->img_name : "-");
 
 		if (p->img_name) {
 			f_close(&p->fd);
@@ -316,7 +333,7 @@ int drv_attach(uint8_t drv, const char *filename, drv_opt_t options)
 				bg_setstat(handle_cpm_drv_to, 1);
 			}
 #else
-			debug_cpmsd("            wrong image file zize: %ld, should be %ld\n",
+			drv_debug(drv, PSTR("wrong image file size: %ld, should be %ld"),
 					f_size(&p->fd), CONFIG_CPM_DISKSIZE);
 			res = 64;
 #endif
@@ -348,9 +365,9 @@ int cpm_drv_to(int state)
 		if (get_timer(ts) > 1000) {
 			for (uint_fast8_t i=0; i < CONFIG_CPM_MAX_DRIVE; i++) {
 				if (drv_table[i].flags & DRV_FLG_DIRTY) {
-					f_sync(&drv_table[i].fd);
 					drv_table[i].flags &= ~DRV_FLG_DIRTY;
-					debug_cpmsd("## %7lu f_sync: %c:\n", get_timer(0), i + CONFIG_CPM_BASE_DRIVE);
+					f_sync(&drv_table[i].fd);
+					drv_debug(i, PSTR("f_sync"));
 				}
 			}
 			state = 0;
@@ -398,15 +415,15 @@ void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
 		return msg_cpm_result(subf, 0x01, res);
 	}
 
-	debug_cpmsd("\n## %7lu  login: %c:\n", get_timer(0), msg[1]+CONFIG_CPM_BASE_DRIVE);
-
 	/* Get relative drive number */
 	drv = msg[1];
 	if ( drv >= CONFIG_CPM_MAX_DRIVE) {
-		debug_cpmsd("##           failed: invalid relative drive number '%d'\n", drv);
+		debug_cpmsd("##           login failed: invalid relative drive number '%d'\n", drv);
 		return msg_cpm_result(subf, 0x02, res);
 	}
 
+	drv_debug(drv, PSTR("login"));
+
 	dp = &drv_table[drv];
 	dp->flags &= ~DRV_FLG_OPEN;
 	dp->dph = ((uint32_t)msg[4] << 16) + ((uint16_t)msg[3] << 8) + msg[2];
@@ -414,7 +431,7 @@ void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
 
 	if (dp->img_name == NULL) {
 		/* no file attached */
-		debug_cpmsd("##           failed: no file attached:\n");
+		drv_debug(drv, PSTR("login failed: no file attached"));
 		return msg_cpm_result(subf, 0x03, res);
 	}
 
@@ -449,6 +466,8 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 	struct cpm_drive_s *dp;
 	uint32_t addr;
 	uint32_t pos;
+	uint16_t track;
+	uint16_t sec;
 	uint8_t secs;
 	bool dowrite;
 	FRESULT res = 0;
@@ -480,21 +499,20 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 		dowrite = false;
 	}
 
+	track = (uint16_t)(msg[TRK+1] << 8) + msg[TRK];
+	sec  = (uint16_t)(msg[SEC+1] << 8) + msg[SEC];
 	secs = msg[CNT];
 	addr = ((uint32_t)msg[ADDR+2] << 16) + ((uint16_t)msg[ADDR+1] << 8) + msg[ADDR];
 
 
 	/* TODO: tracks per sector from dpb */
-	pos = (((uint16_t)(msg[TRK+1] << 8) + msg[TRK]) * 8
-			+ ((uint32_t)(msg[SEC+1] << 8) + msg[SEC])) * CONFIG_CPM_BLOCK_SIZE;
+	pos = (track * 8UL + sec) * CONFIG_CPM_BLOCK_SIZE;
 
-	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[RDRV]+CONFIG_CPM_BASE_DRIVE,
-			((uint16_t)(msg[TRK+1] << 8) + msg[TRK]), msg[SEC],	pos, msg[CNT], addr);
+	drv_debug(drv, PSTR("%5s trk:%4d, sec: %d, pos: %.8lx, secs: %2d, addr: %.5lx"),
+			dowrite ? "write" : " read", track, sec, pos, secs, addr);
 
 	if (pos + secs * CONFIG_CPM_BLOCK_SIZE > CONFIG_CPM_DISKSIZE) {
-		debug_cpmsd("                   access > DISKSIZE (%.8lx > %.8lx) aborted!\n",
+		drv_debug(drv, PSTR("access > DISKSIZE (%.8lx > %.8lx) aborted!"),
 						pos + secs * CONFIG_CPM_BLOCK_SIZE, CONFIG_CPM_DISKSIZE);
 		return msg_cpm_result(subf, 0x04, res);
 	}
@@ -525,7 +543,7 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 			}
 		}
 		if (brw != CONFIG_CPM_BLOCK_SIZE) {
-			debug_cpmsd("           short read or write: res: %d, bytes rd/wr: %u\n", res, brw);
+			drv_debug(drv, PSTR("short read or write: res: %d, bytes rd/wr: %u"), res, brw);
 			res = 64;
 		}
 		addr += CONFIG_CPM_BLOCK_SIZE;
@@ -537,7 +555,7 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 	}
 
 	if (buserr) {
-		debug_cpmsd("Bus timeout\n");
+		drv_debug(drv, PSTR("Bus timeout"));
 		rc = 0x03;
 	}