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

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index ec3db7d..6973908 100644
--- a/avr/z180-serv.c
+++ b/avr/z180-serv.c
@@ -106,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 */
 
 /*
@@ -622,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},
@@ -629,8 +671,6 @@ const FLASH struct msg_item z80_messages[] =
 };
 
 
-
-
 void do_message(int len, uint8_t *msg)
 {
 	uint8_t fct, sub_fct;