+#define DEBUG_CPU 1 /* set to 1 to debug */
+
+#define debug_cpu(fmt, args...) \
+ debug_cond(DEBUG_CPU, fmt, ##args)
+
+
+/*
+ * delay for <count> ms...
+ */
+static void test_delay(uint32_t count)
+{
+ uint32_t ts = get_timer(0);
+
+ 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) {
+ /* 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++;
+ }
+
+ 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_cpu("TCNT3: %6u, ref_cnt: %9lu\n", TCNT3, ref_cnt);
+ debug_cpu("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_cpu("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 (sc > 0 || x_tmp >= 100000) {
+ x_tmp = (x_tmp + 5)/10;
+ sc--;
+ }
+ x_freq = x_tmp;
+ while (sc < 0) {
+ x_freq *= 10;
+ sc++;
+ }
+
+ /* Stop Timer */
+ TCCR3B = 0;
+ PRR1 |= _BV(PRTIM3);
+
+ return x_freq;
+}
+
+static const FLASH uint8_t loop_code[] = {
+/* 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[])
+{
+
+#define O_SILENT (1<<0)
+#define O_WENV (1<<1)
+#define O_LOAD_LOOP (1<<2)
+#define O_UNLOAD_LOOP (1<<3)
+
+ uint_fast8_t options = O_LOAD_LOOP | O_UNLOAD_LOOP;
+ uint_fast8_t lcycles = 19;
+ uint16_t timeout = 1000;
+ uint8_t eimsk_save;
+
+ uint8_t mem_save[ARRAY_SIZE(loop_code)];
+
+ int opt;
+ while ((opt = getopt(argc, argv, PSTR("swnuc:t:"))) != -1) {
+ switch (opt) {
+ case 's':
+ options |= O_SILENT;
+ break;
+ case 'w':
+ options |= O_WENV;
+ break;
+ case 'n':
+ options &= ~O_LOAD_LOOP;
+ break;
+ case 'u':
+ options &= ~O_UNLOAD_LOOP;
+ break;
+ case 'c':
+ lcycles = eval_arg(optarg, NULL);
+ break;
+ case 't':
+ timeout = eval_arg(optarg, NULL);
+ break;
+ default: /* '?' */
+ return CMD_RET_USAGE;
+ }
+ }
+ if (argc - optind != 0)
+ return CMD_RET_USAGE;
+
+ if (z80_bus_state() & ZST_RUNNING) {
+ if (!(options & O_SILENT))
+ printf_P(PSTR("Frequency measuring failed. CPU allready running!\n"));
+ return CMD_RET_FAILURE;
+ }
+
+ ATOMIC_BLOCK(ATOMIC_FORCEON) {
+ /* Save state and disable INT5/INT6 */
+ eimsk_save = EIMSK;
+ EIMSK &= ~_BV(INT6);
+ EIMSK &= ~_BV(INT5);
+ }
+
+ z80_bus_cmd(Request);
+ if (options & O_LOAD_LOOP) {
+ z80_read_block(mem_save, 0, ARRAY_SIZE(loop_code));
+ z80_write_block_P(loop_code, 0, ARRAY_SIZE(loop_code));
+ }
+ EIFR = _BV(INTF5); /* Reset pending int */
+ z80_bus_cmd(Release);
+ z80_bus_cmd(Run);
+
+ clear_ctrlc(); /* forget any previous Control C */
+ ERRNUM err = 0;
+
+ /* Wait for falling edge */
+ do {
+ /* check for ctrl-c to abort... */
+ if (had_ctrlc() || ctrlc()) {
+ err = EINTR;
+ break;
+ }
+ } while ((EIFR & _BV(INTF5)) == 0);
+
+ uint32_t cpu_freq = 0;
+ if (!err)
+ cpu_freq = z80_measure_phi(lcycles);
+
+ z80_bus_cmd(Reset);
+ if (options & O_UNLOAD_LOOP) {
+ z80_bus_cmd(Request);
+ z80_write_block(mem_save, 0, ARRAY_SIZE(loop_code));
+ z80_bus_cmd(Release);
+ }
+ ATOMIC_BLOCK(ATOMIC_FORCEON) {
+ /* Restore INT5/INT6 */
+ 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);
+ }
+
+ Stat &= ~S_MSG_PENDING;
+ Stat &= ~S_CON_PENDING;
+
+ if (err)
+ cmd_error(CMD_RET_FAILURE, err, NULL);
+
+ if (!(options & O_SILENT)) {
+ uint8_t sc = cpu_freq >> 28;
+ printf_P(PSTR("%lu %3u\n"), cpu_freq & 0x0fffffff, sc);
+ }
+#if 0
+ if (options & O_WENV) {
+ if (setenv_ulong(PSTR(ENV_CPU_FREQ), cpu_freq)) {
+ if (!(options & O_SILENT))
+ printf_P(PSTR("'SETENV (%S, %lu)' failed!\n"), PSTR(ENV_CPU_FREQ), cpu_freq);
+ return CMD_RET_FAILURE;
+ }
+ }
+#endif
+ return CMD_RET_SUCCESS;
+}