+ 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;
+
+
+ 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)
+ ;
+ OCR4B = TCNT4;
+ TCCR3B = 0b110<<CS30; /* Count falling edges on T3 (==INT6) */
+ 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)) {
+ ref_stop = TCNT4;
+ TCCR3B = 0b000<<CS30; /* stop counter */
+ break;
+ }
+ if ((TIFR4 & _BV(OCF4B)) != 0) {
+ TIFR4 = _BV(OCF4B);
+ if (ref_ovfl)
+ ref_ovfl++;
+ }
+ }
+ }
+
+#if 0