X-Git-Url: http://cloudbase.mooo.com/gitweb/z180-stamp.git/blobdiff_plain/2c60e1dc26e1f424d9673541aebe778601070838..24ba732a4e11bae37bd42f3c9317a5d54597b11a:/avr/cmd_cpu.c

diff --git a/avr/cmd_cpu.c b/avr/cmd_cpu.c
index 745c68a..fd72b21 100644
--- a/avr/cmd_cpu.c
+++ b/avr/cmd_cpu.c
@@ -32,10 +32,225 @@ static void	test_delay(uint32_t count)
 	while (get_timer(ts) <= count);
 }
 
-static int32_t z80_measure_phi(uint8_t cycles, uint16_t wait_ms)
+uint32_t z80_measure_phi(uint_fast8_t cycles)
 {
 	uint16_t ref_stop;
 	uint16_t ref_ovfl;
+	uint8_t x_ovfl;
+	uint8_t eimsk_save,eicrb_save;
+	uint32_t x_freq;
+
+
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		/* Save state and disable INT6 */
+		eimsk_save = EIMSK;
+		EIMSK &= ~_BV(INT6);
+		/* Save state and set INT6 for falling edge */
+		eicrb_save = EICRB;
+		EICRB = (eicrb_save & ~(0b11 << ISC60)) | (0b10 << ISC60);
+	}
+
+	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) {
+		/* Reset pending int */
+		EIFR = _BV(INTF6);
+		/* Wait for falling edge */
+		while ((EIFR & _BV(INTF6)) == 0)
+			;
+		TCCR3B = 0b110<<CS30;	/* Count falling edges on T3 (==INT6) */
+		OCR4B = TCNT4;
+		TIFR4 = _BV(OCF4B);		/* clear compare match flag */
+	}
+	while (ref_ovfl < 60) {
+		ATOMIC_BLOCK(ATOMIC_FORCEON) {
+			if ((TIFR4 & _BV(OCF4B)) != 0) {
+				TIFR4 = _BV(OCF4B);
+				ref_ovfl++;
+			}
+			if ((TIFR3 & _BV(TOV3)) != 0) {
+				TIFR3 = _BV(TOV3);
+				x_ovfl++;
+			}
+		}
+	}
+
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		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++;
+	}
+
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		/* Restore INT6 */
+		EICRB = eicrb_save;
+		if ((eimsk_save & _BV(INT6)) != 0)
+			EIMSK |= _BV(INT6);
+		/* Reset pending int */
+		EIFR = _BV(INTF6);
+	}
+
+	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);
+
+	debug("TCNT3: %6u, ref_cnt: %9lu\n", TCNT3, ref_cnt);
+	debug("x_tmp: %lu %lu\n", (uint32_t) (x_tmp >> 32), (uint32_t) (x_tmp & 0xffffffff));
+
+	x_tmp = (x_tmp * getenv_ulong(PSTR(ENV_FMON), 10, F_CPU) + (ref_cnt / 2)) / ref_cnt;
+
+	debug("x_tmp: %lu %lu\n", (uint32_t) (x_tmp >> 32), (uint32_t) (x_tmp & 0xffffffff));
+
+	/* round to 5 decimal digits */
+	int_fast8_t sc = 5;
+	while (x_tmp >= 100000) {
+		x_tmp = (x_tmp + 5)/10;
+	    sc--;
+	}
+	x_freq = x_tmp;
+	while (sc < 0) {
+		x_freq *= 10;
+	    sc++;
+	}
+	x_freq += (uint32_t) sc << 28;
+
+
+	/* Stop Timer */
+	TCCR3B = 0;
+	PRR1 |= _BV(PRTIM3);
+
+	return x_freq;
+}
+
+#if 0
+float z80_measure_phi(uint_fast8_t cycles, uint16_t wait_ms)
+{
+	uint16_t ref_stop;
+	uint16_t ref_ovfl;
+	uint8_t x_ovfl;
+	uint8_t eimsk_save,eicrb_save;
+	float x_freq;
+
+
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		/* Save state and disable INT6 */
+		eimsk_save = EIMSK;
+		EIMSK &= ~_BV(INT6);
+		/* Save state and set INT6 for falling edge */
+		eicrb_save = EICRB;
+		EICRB = (eicrb_save & ~(0b11 << ISC60)) | (0b10 << ISC60);
+	}
+
+	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) {
+		/* Reset pending int */
+		EIFR = _BV(INTF6);
+		/* Wait for falling edge */
+		while ((EIFR & _BV(INTF6)) == 0)
+			;
+		TCCR3B = 0b110<<CS30;	/* Count falling edges on T3 (==INT6) */
+		OCR4B = TCNT4;
+		TIFR4 = _BV(OCF4B);		/* clear compare match flag */
+	}
+	while (ref_ovfl < 60) {
+		ATOMIC_BLOCK(ATOMIC_FORCEON) {
+			if ((TIFR4 & _BV(OCF4B)) != 0) {
+				TIFR4 = _BV(OCF4B);
+				ref_ovfl++;
+			}
+			if ((TIFR3 & _BV(TOV3)) != 0) {
+				TIFR3 = _BV(TOV3);
+				x_ovfl++;
+			}
+		}
+	}
+
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		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++;
+	}
+
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		/* Restore INT6 */
+		EICRB = eicrb_save;
+		if ((eimsk_save & _BV(INT6)) != 0)
+			EIMSK |= _BV(INT6);
+		/* Reset pending int */
+		EIFR = _BV(INTF6);
+	}
+
+	uint32_t ref_cnt = (ref_stop - OCR4B) + ((uint32_t)ref_ovfl << 16);
+
+	uint32_t x_cnt = TCNT3 + ((uint32_t) x_ovfl << 16);
+	x_freq = x_cnt * cycles;
+
+	x_freq = (x_freq * getenv_ulong(PSTR(ENV_FMON), 10, F_CPU)) / ref_cnt;
+
+	debug("TCNT3: %6u, ref_cnt: %9lu\n", TCNT3, ref_cnt);
+#if 0
+	debug("ref_start: %6u, ref_stop: %6u, ref_ovfl: %4u, ref_cnt: %9lu\n"
+				  "    TCNT3: %6u, x_cnt: %6lu, xfreq: %f\n",
+				OCR4B, ref_stop, ref_ovfl, ref_cnt,
+				TCNT3, x_cnt, x_freq);
+#endif
+
+
+	/* Stop Timer */
+	TCCR3B = 0;
+	PRR1 |= _BV(PRTIM3);
+
+	return x_freq;
+}
+#endif
+
+#if 0
+int32_t z80_measure_phi(uint_fast8_t cycles, uint16_t wait_ms)
+{
+	uint16_t ref_stop;
+	uint16_t ref_ovfl;
+	uint8_t x_ovfl;
 	uint32_t x_freq;
 	uint8_t eimsk_save,eicrb_save;
 
@@ -53,6 +268,7 @@ static int32_t z80_measure_phi(uint8_t cycles, uint16_t wait_ms)
 	TCCR3A = 0;
 	TCCR3B = 0b000<<CS30;	/* stop counter */
 	TCNT3 = 0;
+	x_ovfl = 0;
 	TIFR3 = _BV(TOV3);
 	ref_ovfl = 0;
 
@@ -72,9 +288,30 @@ static int32_t z80_measure_phi(uint8_t cycles, uint16_t wait_ms)
 				TIFR4 = _BV(OCF4B);
 				ref_ovfl++;
 			}
+			if ((TIFR3 & _BV(TOV3)) != 0) {
+				TIFR3 = _BV(TOV3);
+				x_ovfl++;
+			}
+		}
+	}
+
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		EIFR = _BV(INTF6);
+		for (;;) {
+			if (EIFR & _BV(INTF6)) {
+				ref_stop = TCNT4;
+				TCCR3B = 0b000<<CS30;	/* stop counter */
+				break;
+			}
+			if ((TIFR4 & _BV(OCF4B)) != 0) {
+				TIFR4 = _BV(OCF4B);
+				if (ref_ovfl)
+					ref_ovfl++;
+			}
 		}
 	}
 
+#if 0
 	ATOMIC_BLOCK(ATOMIC_FORCEON) {
 		EIFR = _BV(INTF6);
 		for (;;) {
@@ -101,6 +338,11 @@ static int32_t z80_measure_phi(uint8_t cycles, uint16_t wait_ms)
 				ref_ovfl++;
 		}
 	}
+#endif
+	if ((TIFR3 & _BV(TOV3)) != 0) {
+		TIFR3 = _BV(TOV3);
+		x_ovfl++;
+	}
 
 	if (ref_ovfl == 0)
 		x_freq = 0xFFFFFFFE;
@@ -108,21 +350,19 @@ static int32_t z80_measure_phi(uint8_t cycles, uint16_t wait_ms)
 	{
 		uint32_t ref_cnt = (ref_stop - OCR4B) + ((uint32_t)ref_ovfl << 16);
 
-		x_freq = TCNT3;						/* x_cnt (17 bit) */
-		if ((TIFR3 & _BV(TOV3)) != 0)
-			x_freq += 1UL << 16;
+		x_freq = TCNT3 + ((uint32_t) x_ovfl << 16);
 		uint32_t x_cnt = x_freq;
 		x_freq *= cycles;
 
 		x_freq = ((uint64_t) x_freq * getenv_ulong(PSTR(ENV_FMON), 10, F_CPU) + (ref_cnt / 2))/ ref_cnt;
 
 		debug("ref_start: %6u, ref_stop: %6u, ref_ovfl: %4u, ref_cnt: %9lu\n"
-					  "    TCNT3: %6u,    x_cnt: %6lu, cycles: %3u, xfreq: %9lu\n",
+					  "    TCNT3: %6u, x_cnt: %6lu, xfreq: %9lu\n",
 					OCR4B, ref_stop, ref_ovfl, ref_cnt,
-					TCNT3, x_cnt, cycles, x_freq);
+					TCNT3, x_cnt, x_freq);
 
 		/* round to 5 decimal digits */
-		uint8_t sc = 0;
+		uint_fast8_t sc = 0;
 		while (x_freq >= 100000UL) {
 			x_freq = (x_freq + 5)/10;
 			++sc;
@@ -150,7 +390,7 @@ static int32_t z80_measure_phi(uint8_t cycles, uint16_t wait_ms)
 
 	return (int32_t) x_freq;
 }
-
+#endif
 
 static const FLASH char * const FLASH cpu_strings[] = {
 	FSTR("Unknown CPU"),
@@ -165,8 +405,7 @@ static const FLASH char * const FLASH cpu_strings[] = {
 
 command_ret_t do_cpuchk(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int argc UNUSED, char * const argv[] UNUSED)
 {
-	uint8_t done = 0;
-	uint8_t cputype;
+	uint_fast8_t cputype;
 	ERRNUM err = ESUCCESS;
 	uint8_t ram_save[cpuinfo_length];
 
@@ -187,6 +426,7 @@ command_ret_t do_cpuchk(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int a
 		z80_bus_cmd(Run);
 
 		clear_ctrlc();		/* forget any previous Control C */
+		uint_fast8_t done = 0;
 		while (done != 0xFF) {
 			_delay_ms(8);
 			/* check for ctrl-c to abort... */
@@ -211,11 +451,9 @@ donot:
 	if (err)
 		cmd_error(CMD_RET_FAILURE, err, NULL);
 
-	if (done == 0xFF) {
-		if (cputype >= ARRAY_SIZE(cpu_strings))
-			cputype = 0;
-		printf_P(PSTR("Detected CPU: %S\n"), cpu_strings[cputype]);
-	}
+	if (cputype >= ARRAY_SIZE(cpu_strings))
+		cputype = 0;
+	printf_P(PSTR("Detected CPU: %S\n"), cpu_strings[cputype]);
 
 	return CMD_RET_SUCCESS;
 }
@@ -332,17 +570,19 @@ command_ret_t do_busack_test(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED,
 }
 
 static const FLASH uint8_t loop_code[] = {
-	/* 0000 */  0x01,0x36,0x00,		/* ld	bc,00*256+RCR ; no refresh cycles       */
-	/* 0003 */  0xAF,	         	/* xor	a                                       */
-	/* 0004 */  0xED,0x79,       	/* out	(c),a                                   */
-	/* 0006 */  0xD3,0x40,       	/* out	(040H),a      ; ready                   */
-	/*      */  		         	/* 					 							*/
-	/*      */  		         	/* 					;Z80	Z180(0W) Z180(MaxW) */
-	/* 0008 */  		         	/* loop:			;-------------------------- */
-	/* 0008 */  0xDB,0x50,       	/* in	a,(050h)	;11       10     +3*3  19   */
-	/* 000A */  0xC3,0x08,0x00     	/* jp	loop		;10	   	   9	 +3*3  18   */
-									/* 					;-------------------------- */
-									/* 					;21       19           37   */
+/* 0000 */  0x00,             /* nop                                        */
+/* 0001 */  0xAF,             /* xor  a                                     */
+/* 0005 */  0xD3,0x32,        /* out  (032h),a  ;DCNTL                      */
+/* 0002 */  0xD3,0x36,        /* out  (036h),a  ;RCR                        */
+/*      */                    /*                                            */
+/* 0006 */  0xD3,0x40,        /* out  (040H),a  ;Ready                      */
+/*      */                    /*                                            */
+/*      */                    /*                ;Z80    Z180(0W) Z180(MaxW) */
+/* 0008 */                    /* loop:          ;-------------------------- */
+/* 0008 */  0xDB,0x50,        /* in   a,(050h)  ;11       10     +3*3  19   */
+/* 000A */  0xC3,0x08,0x00    /* jp   loop      ;10        9     +3*3  18   */
+                              /*                ;-------------------------- */
+                              /*                ;21       19           37   */
 };
 
 command_ret_t do_cpu_freq(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int argc, char * const argv[])
@@ -354,7 +594,7 @@ command_ret_t do_cpu_freq(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int
 #define O_UNLOAD_LOOP   (1<<3)
 
 	uint_fast8_t options = O_LOAD_LOOP | O_UNLOAD_LOOP;
-	uint8_t lcycles = 18;
+	uint_fast8_t lcycles = 19;
 	uint16_t timeout = 1000;
 
 	uint8_t mem_save[ARRAY_SIZE(loop_code)];
@@ -415,9 +655,9 @@ command_ret_t do_cpu_freq(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int
 		}
 	} while ((Stat & S_IO_0X40) == 0);
 
-	int32_t cpu_freq;
+	uint32_t cpu_freq;
 	if (!err)
-		cpu_freq = z80_measure_phi(lcycles, timeout);
+		cpu_freq = z80_measure_phi(lcycles);
 
 	z80_bus_cmd(Reset);
 	if (options & O_UNLOAD_LOOP) {
@@ -429,7 +669,9 @@ command_ret_t do_cpu_freq(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int
 		cmd_error(CMD_RET_FAILURE, err, NULL);
 
 	if (!(options & O_SILENT)) {
-		printf_P(PSTR("%ld%S\n"), cpu_freq, cpu_freq < 0 ? PSTR("") : PSTR("Hz"));
+		printf_P(PSTR("%lu %3u\n"), cpu_freq & 0x0fffffff, cpu_freq >> 28);
+
+//		printf_P(PSTR("%f%S\n"), cpu_freq, cpu_freq < 0 ? PSTR("") : PSTR("Hz"));
 //		if (cpu_freq != 0)
 //		else
 //			printf_P(PSTR("No CPU clock or input frequency to low!\n"));