X-Git-Url: http://cloudbase.mooo.com/gitweb/z180-stamp.git/blobdiff_plain/5e8ac5e02a09140e97d56a9d5313504dfa31282d..18d57acbf4ccd7220e59fe1f2ac1556837c5da83:/avr/z180-serv.c

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index cdc6e5e..6973908 100644
--- 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 */
 
 /*
@@ -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) {
-#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'"),
@@ -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},
+	{ 4,
+	  1, 1,
+	  do_msg_m_fcpu},
 	{ 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;