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 cdc6e5e3b9ec22733ab5185e4b39b384bd4ba691..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 "z180-serv.h"
 #include "common.h"

-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>

 #include <util/atomic.h>
 
 #include "config.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 */
 
 /*
 #define CPM_DAY_OFFSET ((1978-1900) * 365 + 19)                /* 19 leap years */
 
 /*


@@ -417,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) {
        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'"),
                char msg[40];
                strncpy_P(msg, rc_messages[rc & 0x7f], sizeof msg -1);
                drv_debug(END, PSTR(" rc: %.02x/%d, '%s'"),


@@ -625,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},
        { 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},
        { 0xff,                         /* end mark */
          0, 0,
          0},


@@ -632,8 +671,6 @@ const FLASH struct msg_item z80_messages[] =
 };
 
 
 };
 
 

-
-

 void do_message(int len, uint8_t *msg)
 {
        uint8_t fct, sub_fct;
 void do_message(int len, uint8_t *msg)
 {
        uint8_t fct, sub_fct;