new command: dissassemle - Disassemble Z180 code from memory

[z180-stamp.git] / avr / z180-serv.c

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index d1f52ddbac19f42e46b43e75bcd4ff679c66458f..697390853e373ab443b86b127deb6e4abab0e002 100644 (file)
--- a/avr/z180-serv.c
+++ b/avr/z180-serv.c
@@ -6,9 +6,6 @@
 
 #include "z180-serv.h"
 #include "common.h"
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
 #include <util/atomic.h>
 
 #include "config.h"
@@ -109,6 +106,45 @@ void do_msg_get_timer(uint8_t subf, int len, uint8_t * msg)
 
 /* ---------------------------------------------------------------------------*/
 
+/* measure f_cpu */
+void do_msg_m_fcpu(uint8_t subf, int len UNUSED, uint8_t * msg)
+{
+       uint32_t freq;
+       uint8_t eimsk_save;
+
+       /* Save state and disable INT5/INT6 */
+       ATOMIC_BLOCK(ATOMIC_FORCEON) {
+               eimsk_save = EIMSK;
+               EIMSK &= ~_BV(INT6);
+               EIMSK &= ~_BV(INT5);
+       }
+       EIFR = _BV(INTF5);                              /* Reset pending int */
+       z80_bus_cmd(Request);
+       z80_write(0x3f, 0xff);
+       z80_bus_cmd(Release);
+
+       freq = z80_measure_phi(*msg);
+
+       z80_bus_cmd(Request);
+       z80_write(0x3f, 0xff);
+       z80_bus_cmd(Release);
+
+       /* Restore INT5/INT6 */
+       ATOMIC_BLOCK(ATOMIC_FORCEON) {
+               if ((eimsk_save & _BV(INT5)) != 0)
+                       EIMSK |= _BV(INT5);
+               if ((eimsk_save & _BV(INT6)) != 0)
+                       EIMSK |= _BV(INT6);
+               /* Reset pending int */
+               EIFR = _BV(INTF5);
+               EIFR = _BV(INTF6);
+       }
+
+       msg_xmit(4, subf, sizeof(freq), (uint8_t *) &freq);
+}
+
+/* ---------------------------------------------------------------------------*/
+
 #define CPM_DAY_OFFSET ((1978-1900) * 365 + 19)                /* 19 leap years */
 
 /*
@@ -240,8 +276,9 @@ int drv_list(void)
        for (uint8_t i = 0; i < CONFIG_CPM_MAX_DRIVE; i++) {
                struct cpm_drive_s * p = &drv_table[i];
                if (p->img_name) {
-                       printf_P(PSTR("  dsk%d: %2s %3s attached to %s\n"), i,
-                                       p->opt&DRV_OPT_RO ? "RO":"RW", p->opt&DRV_OPT_DEBUG ? "DBG":"",
+                       printf_P(PSTR("  dsk%d: %2S %3S attached to %s\n"), i,
+                                       p->opt&DRV_OPT_RO ? PSTR("RO") : PSTR("RW"),
+                                       p->opt&DRV_OPT_DEBUG ? PSTR("DBG") : PSTR(""),
                                        p->img_name);
                }
        }
@@ -291,17 +328,17 @@ int drv_attach(uint8_t unit, const char *filename, drv_opt_t options)
 
        drv = unit;
        if (drv >= CONFIG_CPM_MAX_DRIVE)
-               return AT_RANGE;
+               return EATRANGE;
 
        struct cpm_drive_s *p = &drv_table[drv];
 
        if (options & DRV_OPT_REATTATCH) {
                if (filename) {
-                       return AT_ERROR;
+                       return EUNEXPARG;
                }
 
                if (!p->img_name) {
-                       return AT_NOT;
+                       return EATNOT;
                }
 
                /* change options */
@@ -316,16 +353,16 @@ int drv_attach(uint8_t unit, const char *filename, drv_opt_t options)
        } else {
 
                if (p->img_name)
-                       return AT_ALREADY;
+                       return EATALRDY;
                if (drv_find_file_attached(filename) >= 0)
-                       return AT_OTHER;
+                       return EATOTHER;
 
                p->opt = options;
 
                /* new attachment */
 
                if ((p->img_name = strdup(filename)) == NULL)
-                       return AT_NOMEM;
+                       return ENOMEM;
 
                res = f_open(&p->fd, p->img_name,
                                FA_READ | (options&DRV_OPT_RO ? 0 : FA_WRITE));
@@ -354,11 +391,11 @@ int drv_attach(uint8_t unit, const char *filename, drv_opt_t options)
                }
                if (res) {
                        drv_detach(drv);
-                       return AT_OPEN;
+                       return EATOPEN;
                }
        }
 
-       return AT_OK;
+       return ESUCCESS;
 }
 
 
@@ -416,7 +453,7 @@ void msg_cpm_result(uint8_t subf, uint8_t rc, int res)
        msg_xmit(2, subf, sizeof(result_msg), result_msg);
 
        if (rc) {
-#if GCC_BUG_61443
+#if defined(GCC_BUG_61443)
                char msg[40];
                strncpy_P(msg, rc_messages[rc & 0x7f], sizeof msg -1);
                drv_debug(END, PSTR(" rc: %.02x/%d, '%s'"),
@@ -624,6 +661,9 @@ const FLASH struct msg_item z80_messages[] =
        { 3,
          2, 3,                         /* 2: get, 3: set time and date */
          do_msg_get_set_time},
+       { 4,
+         1, 1,
+         do_msg_m_fcpu},
        { 0xff,                         /* end mark */
          0, 0,
          0},
@@ -631,8 +671,6 @@ const FLASH struct msg_item z80_messages[] =
 };
 
 
-
-
 void do_message(int len, uint8_t *msg)
 {
        uint8_t fct, sub_fct;