cpu_freq: extra timing loop WIP

1 files changed, 44 insertions, 33 deletions

diff --git a/avr/cmd_cpu.c b/avr/cmd_cpu.c
index 6f50a2a..61e3ec3 100644
--- a/avr/cmd_cpu.c
+++ b/avr/cmd_cpu.c
@@ -32,26 +32,36 @@ static void	test_delay(uint32_t count)
 	while (get_timer(ts) <= count);
 }
 
-static int32_t z80_measure_phi(uint8_t cycles, bool input_clk, uint16_t wait_ms)
+static int32_t z80_measure_phi(uint8_t cycles, uint16_t wait_ms)
 {
 	uint16_t ref_stop;
 	uint16_t ref_ovfl;
 	uint32_t x_freq;
+	uint8_t eimsk_save,eicrb_save;
 
 
+	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;
 	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)
 			;
-		//ref_start = TCNT4;
 		OCR4B = TCNT4;
 		TCCR3B = 0b110<<CS30;	/* Count falling edges on T3 (==INT6) */
 		TIFR4 = _BV(OCF4B);		/* clear compare match flag */
@@ -102,14 +112,12 @@ static int32_t z80_measure_phi(uint8_t cycles, bool input_clk, uint16_t wait_ms)
 		if ((TIFR3 & _BV(TOV3)) != 0)
 			x_freq += 1UL << 16;
 		uint32_t x_cnt = x_freq;
-		if (input_clk)						/* compute input clock */
-			x_freq *= 2;
 		x_freq *= cycles;
 
 		x_freq = ((uint64_t) x_freq * F_CPU + (ref_cnt / 2))/ ref_cnt;
 
-		printf_P(PSTR("ref_start: %6u, ref_stop: %6u, ref_ovfl: %4u, ref_cnt: %9lu\n"
-					  "    TCNT3: %6u,    x_cnt: %6lu, cycles: %3u, xfreq: %9lu\n"),
+		debug("ref_start: %6u, ref_stop: %6u, ref_ovfl: %4u, ref_cnt: %9lu\n"
+					  "    TCNT3: %6u,    x_cnt: %6lu, cycles: %3u, xfreq: %9lu\n",
 					OCR4B, ref_stop, ref_ovfl, ref_cnt,
 					TCNT3, x_cnt, cycles, x_freq);
 
@@ -127,6 +135,19 @@ static int32_t z80_measure_phi(uint8_t cycles, bool input_clk, uint16_t wait_ms)
 	TCCR3B = 0;
 	PRR1 |= _BV(PRTIM3);
 
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		/* Restore INT6 */
+#if 0 /* wtf? */
+		eicrb_save = EICRB;
+		EICRB = (EICRB & ~(0b11 << ISC60)) | (eicrb_save & (0b11 << ISC60));
+#endif
+		EICRB = eicrb_save;
+		if ((eimsk_save & _BV(INT6)) != 0)
+			EIMSK |= _BV(INT6);
+		/* Reset pending int */
+		EIFR = _BV(INTF6);
+	}
+
 	return (int32_t) x_freq;
 }
 
@@ -315,6 +336,19 @@ command_ret_t do_busack_test(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED,
 	return CMD_RET_SUCCESS;
 }
 
+static const FLASH uint8_t loop_code[] = {
+	/* 0000 */  0x01,0x36,0x00,		/* ld	bc,00*256+RCR                           */
+	/* 0003 */  0xAF,	         	/* xor	a                                       */
+	/* 0004 */  0xED,0x79,       	/* out	(c),a                                   */
+	/* 0006 */  0xD3,0x40,       	/* out	(040H),a                                */
+	/*      */  		         	/* 					;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[])
 {
 
@@ -327,9 +361,7 @@ command_ret_t do_cpu_freq(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int
 	uint8_t lcycles = 18;
 	uint16_t timeout = 1000;
 
-	uint8_t eicrb_save;
-	uint8_t eimsk_save;
-	uint8_t mem_save[cpuinfo_length];
+	uint8_t mem_save[ARRAY_SIZE(loop_code)];
 
 	/* reset getopt() */
 	optind = 0;
@@ -368,22 +400,11 @@ command_ret_t do_cpu_freq(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int
 		return CMD_RET_FAILURE;
 	}
 
-	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);
-	}
 
 	z80_bus_cmd(Request);
 	if (options & O_LOAD_LOOP) {
-		z80_read_block(mem_save, 0, cpuinfo_length);
-		z80_load_mem(0, cpuinfo,
-					&cpuinfo_sections,
-					cpuinfo_address,
-					cpuinfo_length_of_sections);
+		z80_read_block(mem_save, 0, ARRAY_SIZE(loop_code));
+		z80_write_block_P(loop_code, 0, ARRAY_SIZE(loop_code));
 	}
 	Stat &= ~S_IO_0X40;				/* Reset pending int */
 	z80_bus_cmd(Release);
@@ -408,19 +429,9 @@ command_ret_t do_cpu_freq(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int
 	z80_bus_cmd(Reset);
 	if (options & O_UNLOAD_LOOP) {
 		z80_bus_cmd(Request);
-		z80_write_block(mem_save, 0, cpuinfo_length);
+		z80_write_block(mem_save, 0, ARRAY_SIZE(loop_code));
 		z80_bus_cmd(Release);
 	}
-	ATOMIC_BLOCK(ATOMIC_FORCEON) {
-		/* Restore INT6 */
-		eicrb_save = EICRB;
-		EICRB = (EICRB & ~(0b11 << ISC60)) | (eicrb_save & (0b11 << ISC60));
-		if ((eimsk_save & _BV(INT6)) != 0)
-			EIMSK |= _BV(INT6);
-		/* Reset pending int */
-		EIFR = _BV(INTF6);
-	}
-
 	if (err)
 		cmd_error(CMD_RET_FAILURE, err, NULL);