new command: dissassemle - Disassemble Z180 code from memory

[z180-stamp.git] / avr / z80-if.c

diff --git a/avr/z80-if.c b/avr/z80-if.c
index 9865208b7f0d9f3fcdf4119afb7efb1a780d925b..f36466c33cddb6e537dba3af38edcddc838fab41 100644 (file)
--- 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)
@@ -402,7 +401,7 @@ zstate_t z80_bus_cmd(bus_cmd_t cmd)
                                Z80_I_RST = 1;                                  /* Toggle RESET  --> inactive */
                                OCR4B = TCNT4;
                                TIFR4 = _BV(OCF4B);                             /* Clear compare match flag */
-/*test*/               TIMSK4 &= ~_BV(OCIE4A);         /* Disable Output Compare A interrupt */
+//                             TIMSK4 &= ~_BV(OCIE4A);         /* Disable Output Compare A interrupt */
                        }
                        TIMSK4 |= _BV(OCIE4B);                          /* Enable compare match interrupt */
 
@@ -414,7 +413,7 @@ zstate_t z80_bus_cmd(bus_cmd_t cmd)
                                ovl_cnt = busack_cycles_ovl;
                                ifr = TIFR4;
                                TIMSK4 &= ~_BV(OCIE4B);                 /* Disable compare match interrupt */
-/*test*/               TIMSK4 |= _BV(OCIE4A);          /* Enable Output Compare A interrupt */
+//                             TIMSK4 |= _BV(OCIE4A);          /* Enable Output Compare A interrupt */
                        }
                        if (Z80_I_BUSACK == 0) {
                                if ((ifr & _BV(OCF4B)) && !(tcnt & (1<<15)))
@@ -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;
+}
 
 /*--------------------------------------------------------------------------*/