dsk debug option (unfinished)

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index d0d4cfcebc35d163d51f6b252c4d14d0f4014af6..a49c9f9752535a732fd7a7e258167926377ba355 100644 (file)
--- 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;
        }