From c82e9b3342cb1971d2d6e66d4537f2ae3ba6c72c Mon Sep 17 00:00:00 2001
From: Leo C.
Date: Thu, 4 Jul 2024 13:45:45 +0200
Subject: z180-serv: measure cpu frequency

---
 avr/cmd_cpu.c   |  97 -----------------------------------------------------
 avr/z180-serv.c |  44 ++++++++++++++++++++++--
 avr/z80-if.c    | 101 +++++++++++++++++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 142 insertions(+), 100 deletions(-)

(limited to 'avr')

diff --git a/avr/cmd_cpu.c b/avr/cmd_cpu.c
index 07d2774..af9ded4 100644
--- a/avr/cmd_cpu.c
+++ b/avr/cmd_cpu.c
@@ -25,21 +25,6 @@
 #define debug_cpu(fmt, args...)		                                              \
 	debug_cond(DEBUG_CPU, fmt, ##args)
 
-static
-char * ulltoa (uint64_t val, char *s)
-{
-	char *p = s;
-
-	while (val >= 10) {
-		*p++ = (val % 10) + '0';
-		val = val / 10;
-	}
-	*p++ = val + '0';
-	*p = '\0';
-
-	return strrev(s);
-}
-
 /*
  * delay for <count> ms...
  */
@@ -50,88 +35,6 @@ static void	test_delay(uint32_t count)
 	while (get_timer(ts) <= count);
 }
 
-static uint32_t z80_measure_phi(uint_fast8_t cycles)
-{
-	uint16_t ref_stop;
-	uint16_t ref_ovfl;
-	uint8_t x_ovfl;
-	uint32_t x_freq;
-
-
-	PRR1 &= ~_BV(PRTIM3);
-	TCCR3A = 0;
-	TCCR3B = 0b000<<CS30;			/* stop counter */
-	TCNT3 = 0;
-	x_ovfl = 0;
-	TIFR3 = _BV(TOV3);
-	ref_ovfl = 0;
-
-	ATOMIC_BLOCK(ATOMIC_FORCEON) {
-		EIFR = _BV(INTF6);					/* Reset pending int */
-		while ((EIFR & _BV(INTF6)) == 0)	/* Wait for falling edge */
-			;
-		OCR4B = TCNT4;
-		TCCR3B = 0b110<<CS30;		/* Count falling edges on T3 (==INT6) */
-		TIFR4 = _BV(OCF4B);			/* clear compare match flag */
-
-		while (ref_ovfl < 60) {
-			if ((TIFR4 & _BV(OCF4B)) != 0) {
-				TIFR4 = _BV(OCF4B);
-				++ref_ovfl;
-			}
-			if ((TIFR3 & _BV(TOV3)) != 0) {
-				TIFR3 = _BV(TOV3);
-				++x_ovfl;
-			}
-		}
-
-		EIFR = _BV(INTF6);
-		for (;;) {
-			if (EIFR & _BV(INTF6)) {
-				TCCR3B = 0b000<<CS30;	/* stop counter */
-				ref_stop = TCNT4;
-				break;
-			}
-			if ((TIFR4 & _BV(OCF4B)) != 0) {
-				TIFR4 = _BV(OCF4B);
-				++ref_ovfl;
-			}
-		}
-	}
-
-	if ((TIFR3 & _BV(TOV3)) != 0) {
-		TIFR3 = _BV(TOV3);
-		x_ovfl++;
-	}
-
-	uint32_t ref_cnt = (ref_stop - OCR4B) + ((uint32_t)ref_ovfl << 16);
-	uint32_t x_cnt = TCNT3 + ((uint32_t) x_ovfl << 16);
-	uint64_t x_tmp = (uint64_t) 100000 * (x_cnt * cycles);
-
-	/* Stop Timer */
-	TCCR3B = 0;
-	PRR1 |= _BV(PRTIM3);
-
-//	char x_tmp_str[21];
-//
-//	debug_cpu("TCNT3: %6u, ref_cnt: %9lu\n", TCNT3, ref_cnt);
-//	ulltoa(x_tmp, x_tmp_str);
-//	debug_cpu("x_tmp: %s\n", x_tmp_str);
-
-	x_tmp = (x_tmp * getenv_ulong(PSTR(ENV_FMON), 10, F_CPU) + (ref_cnt / 2)) / ref_cnt;
-
-//	ulltoa(x_tmp, x_tmp_str);
-//	debug_cpu("x_tmp: %s\n", x_tmp_str);
-
-	/* round to 5 decimal digits */
-	int_fast8_t sc = 5;
-	for ( ; sc > 0 || x_tmp >= 100000; sc--) x_tmp = (x_tmp + 5)/10;
-	x_freq = x_tmp;
-	for ( ; sc < 0; sc++) x_freq *= 10;
-
-	return x_freq;
-}
-
 static const FLASH char * const FLASH cpu_strings[] = {
 	FSTR("Unknown"),
 	FSTR("8080"),
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;
diff --git a/avr/z80-if.c b/avr/z80-if.c
index 5a4104d..f36466c 100644
--- a/avr/z80-if.c
+++ b/avr/z80-if.c
@@ -163,7 +163,6 @@ ISR(TIMER4_COMPB_vect)
 
 ISR(TIMER5_COMPA_vect)
 {
-
 	uint8_t i = timer;
 
 	if (i)
@@ -518,6 +517,106 @@ zstate_t z80_bus_cmd(bus_cmd_t cmd)
 	return zstate;
 }
 
+/*--------------------------------------------------------------------------*/
+
+#define DEBUG_FREQ	0	/* set to 1 to debug */
+
+#define debug_cpu(fmt, args...)		                                              \
+	debug_cond(DEBUG_FREQ, fmt, ##args)
+
+#if 0
+static
+char * ulltoa (uint64_t val, char *s)
+{
+	char *p = s;
+
+	while (val >= 10) {
+		*p++ = (val % 10) + '0';
+		val = val / 10;
+	}
+	*p++ = val + '0';
+	*p = '\0';
+
+	return strrev(s);
+}
+#endif
+
+uint32_t z80_measure_phi(uint_fast8_t cycles)
+{
+	uint16_t ref_stop;
+	uint16_t ref_ovfl;
+	uint8_t x_ovfl;
+	uint32_t x_freq;
+
+
+	PRR1 &= ~_BV(PRTIM3);
+	TCCR3A = 0;
+	TCCR3B = 0b000<<CS30;			/* stop counter */
+	TCNT3 = 0;
+	x_ovfl = 0;
+	TIFR3 = _BV(TOV3);
+	ref_ovfl = 0;
+
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		EIFR = _BV(INTF6);					/* Reset pending int */
+		while ((EIFR & _BV(INTF6)) == 0)	/* Wait for falling edge */
+			;
+		OCR4B = TCNT4;
+		TCCR3B = 0b110<<CS30;		/* Count falling edges on T3 (==INT6) */
+		TIFR4 = _BV(OCF4B);			/* clear compare match flag */
+
+		while (ref_ovfl < 60) {
+			if ((TIFR4 & _BV(OCF4B)) != 0) {
+				TIFR4 = _BV(OCF4B);
+				++ref_ovfl;
+			}
+			if ((TIFR3 & _BV(TOV3)) != 0) {
+				TIFR3 = _BV(TOV3);
+				++x_ovfl;
+			}
+		}
+
+		EIFR = _BV(INTF6);
+		for (;;) {
+			if (EIFR & _BV(INTF6)) {
+				TCCR3B = 0b000<<CS30;	/* stop counter */
+				ref_stop = TCNT4;
+				break;
+			}
+			if ((TIFR4 & _BV(OCF4B)) != 0) {
+				TIFR4 = _BV(OCF4B);
+				++ref_ovfl;
+			}
+		}
+	}
+
+	if ((TIFR3 & _BV(TOV3)) != 0) {
+		TIFR3 = _BV(TOV3);
+		x_ovfl++;
+	}
+
+	uint32_t ref_cnt = (ref_stop - OCR4B) + ((uint32_t)ref_ovfl << 16);
+	uint32_t x_cnt = TCNT3 + ((uint32_t) x_ovfl << 16);
+	uint64_t x_tmp = (uint64_t) 100000 * (x_cnt * cycles);
+
+	/* Stop Timer */
+	TCCR3B = 0;
+	PRR1 |= _BV(PRTIM3);
+
+	debug_cpu("\nx_ovfl: %6u, TCNT3: %6u, cycles: %3u\n", x_ovfl, TCNT3, cycles);
+	debug_cpu("ref_ovfl: %6u, ref_...: %6u\n", ref_ovfl, ref_stop-OCR4B);
+	debug_cpu("x_cnt: %9lu, ref_cnt: %9lu\n", x_cnt, ref_cnt);
+
+	x_tmp = (x_tmp * getenv_ulong(PSTR(ENV_FMON), 10, F_CPU) + (ref_cnt / 2)) / ref_cnt;
+
+	/* round to 5 decimal digits */
+	int_fast8_t sc = 5;
+	for ( ; sc > 0 || x_tmp >= 100000; sc--) x_tmp = (x_tmp + 5)/10;
+	x_freq = x_tmp;
+	for ( ; sc < 0; sc++) x_freq *= 10;
+
+	return x_freq;
+}
 
 /*--------------------------------------------------------------------------*/
 
-- 
cgit v1.2.3