#define  FREQ_PIN (5)  // Arduino D5 pin (T1 pin)

volatile uint16_t ovf_count  = 0;
volatile uint32_t pulse_count = 0;
volatile uint16_t interval_tick_count = 0;
volatile boolean  done = false;

void start_timers( uint16_t msec ) {
  uint8_t SREG_tmp = SREG; // Save the SREG value
  done = false;
  ovf_count = 0;
  interval_tick_count = msec; 

  cli(); // Disable global interrupt  
  // Disable Timer1
  TCCR1A = 0;  // Reset Timer1 control register A
  TCCR1B = 0;  // Reset Timer1 control register B 
  TCNT1  = 0;  // Reset Timer1 counter value to 0
  TIFR1 |= (1<<TOV1);    // Clear Timer/Counter 1 overflow flag
  TIMSK1 &= ~(1<<TOIE1); // Disable Timer1 Overflow Interrupt 

  // Disable Timer2
  TCCR2A = 0;  // Reset Timer2 control register A
  TCCR2B = 0;  // Reset Timer2 control register B  
  TCNT2 = 0;   // Reset Timer2 counter value to 0

  // Setup Timer2
  // Set Timer2 prescaler = 128 -> 16MHz/128 = 125kHz
  TCCR2B |= (1<<CS22) | (1<<CS20); 
  // Use Timer2 in CTC (Clear Timer on Compare Match) mode 
  // -> WGM22=0, WGM21=1, WGM20=0
  // Note that in CTC mode the counter is automatically cleared to zero
  // when the counter value (TCNT2) matches the OCR2A.
  OCR2A  = 125-1; // -> 16MHz/128/125 = 1kHz or 1msec timing interval
  TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Output Compare Match A Interrupt

  // Setup Timer1 and use external clock source (rising edge) on T1 pin
  // Timer1 will be used to count events on the T1 pin (Arduino D5 pin).
  // It will operate in normal mode (WGM13=0, WGM12=0, WGM11=0, WGM10=0), no interrupt.
  GTCCR = (1<<PSRASY); // Reset Timer1 prescaler counting
  TCCR1B |= (1<<CS12) | (1<<CS11) | (1<<CS10); // Start Timer1
  TCCR2A |= (1<<WGM21);  // Start Timer2 in CTC mode
  SREG = SREG_tmp;       // Restore the SREG value
}

ISR(TIMER2_COMPA_vect) { // for Timer2 Output Compare Match A Interrupt
  pulse_count = TCNT1;
  if ( TIFR1 & (1<<TOV1) ) { // Check overflow
     TIFR1 |= (1<<TOV1);     // Clear Timer/Counter 1 overflow flag
     ovf_count++;            // Increment Timer1 overflow count
  }
  if (interval_tick_count-- == 0) {  // Timeout
     TCCR1A = 0;  // Reset Timer1 control register A
     TCCR1B = 0;  // Reset Timer1 control register B 
     TCCR2A = 0;  // Reset Timer2 control register A
     TCCR2B = 0;  // Reset Timer2 control register B 
     pulse_count = ((uint32_t)ovf_count << 16) + pulse_count; 
     done = true;
  }
}

uint32_t measure() {
  start_timers( 1000 /*msec*/ ); // Start Timer1/Timer2
  while( !done ) {} // Wait until the done flag is set
  return pulse_count; // pulses per second (Hz)
}

void setup() {
  Serial.begin( 115200 );
  Serial.println( "Arduino Frequency Measurement" );
   // Create a PWM signal (50% duty cycle) on Arduino D6 pin
   // Default PWM frequency = 16 Mhz/64/256 = 976.56Hz
  analogWrite( 6, 127 );
  delay(100);
}

void loop() {
  static char sbuf[40];
  uint32_t freq = measure();
  sprintf( sbuf, "Freq. = %lu Hz", freq );
  Serial.println( sbuf );
  delay(10);
}