This is the header file:

#ifndef FreqCounter_h
#define FreqCounter_h

#include <avr/interrupt.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif


namespace FreqCounter
{
   extern volatile unsigned long f_freq;
   extern volatile unsigned char f_ready;
   extern volatile unsigned char f_mlt;
   extern volatile unsigned int f_tics;
   extern volatile unsigned int f_period;

   void start(int ms);
}

#endif


This is the source file:

#include <FreqCounter.h>

volatile unsigned long FreqCounter::f_freq;
volatile unsigned char FreqCounter::f_ready;
volatile unsigned char FreqCounter::f_mlt;
volatile unsigned int FreqCounter::f_tics;
volatile unsigned int FreqCounter::f_period;

void FreqCounter::start(int ms) {
   f_period = ms;
   GTCCR = (1<<TSM) | (1<<PSRASY) | (1<<PSRSYNC); //halt all timers

   //set TC1 to normal mode => TOV1 flag set at TOP = 0xFFFF
   TCCR1A &= 0x00; //reset TC1 control register A
   TCCR1B &= 0x00; //reset TC1 control register B

   //set TC1 to use external clock source, clocked on rising edge
   TCCR1B |= (1<<CS10);
   TCCR1B |= (1<<CS11);
   TCCR1B |= (1<<CS12);

   //reset TC2
   TCCR2A &= 0x00; //reset TC2 control register A
   TCCR2B &= 0x00; //reset TC2 control register B

   //set TC2 to CTC Mode with TOP = OCR2A => counter cleared on reaching TOP
   TCCR2A |= (1<<WGM21);

   //TC2 is in CTC mode => set value of OCR2A = TOP
   OCR2A = 124; //TOP is reached every 125 counts (starting from 0)

   //set TC2 to use a prescaler of 1 => 16MHz / 128 = 125kHz
   TCCR2B |= (1<<CS20);
   TCCR2B |= (1<<CS22);

   f_ready = 0; //reset ready flag
   f_tics = 0; //reset TC2 interrupt counter

   //synchronize the value of both timers
   TCNT2 = 0;
   TCNT1 = 0;

   //clear interrupt flags
   TIFR2 |= (1<<OCF2A);
   TIFR1 |= (1<<TOV1);

   TIMSK2 |= (1<<OCIE2A); //enable TC2 Output Compare A interrupt => triggered on reaching TOP = OCR2A
   TIMSK1 |= (1<<TOIE1); //enable TC1 overflow interrupt => triggered on reaching TOP = MAX = 0xFFFF

   GTCCR &= 0x00; //restart all timers => start counting
}

/*
* This ISR is triggered by TC2 every 1ms because
* TC2 increments with a frequency of 16MHz / 128 = 125KHz
* and reaches TOP every 125 counts
* => trigger frequency = 1kHz
* => trigger period = 1ms
*
* This ISR handles the Gate Time generation
*/
ISR(TIMER2_COMPA_vect)
{
   //increment number of TC2 interrupts (triggers when reaching TOP = OCR2A)
   FreqCounter::f_tics++;

   /*
    * If branch evaluates as true if given gate time (f_period) equals
    * amount of milliseconds TC2 has counted (= f_tics)
    */
   if (FreqCounter::f_tics >= FreqCounter::f_period)
   {
       //end of gate time => measurement ready
       TCCR1B &= ~0x07; //set TC1 to no clk source => TC1 stops counting
       TIMSK2 &= ~(1<<OCIE2A); //disable TC2 Output Compare A interrupt
       TIMSK1 &= ~(1<<TOIE1); //disable TC1 overflow interrupt
       FreqCounter::f_ready = 1; //set software flag for end of gating period

       //increment on overflow of TCNT1
       if (bit_is_set(TIFR1, TOV1))
           FreqCounter::f_mlt++;

       //calculate frequency
       FreqCounter::f_freq = 65536 * FreqCounter::f_mlt;
       FreqCounter::f_freq += TCNT1; //add value of TC1
       FreqCounter::f_freq = (long)(FreqCounter::f_freq * 1000.0 / (double)FreqCounter::f_tics);
       FreqCounter::f_mlt = 0;
   }
}

ISR(TIMER1_OVF_vect)
{
   FreqCounter::f_mlt++;
}


This is the test program:

#include <FreqCounter.h>

long frq;


void setup()
{
   //generate a 5 kHz square wave on OC0B = digital pin 5 = T1
   DDRD |= _BV(PD5); //PD5 = OC0B as output
   TCCR0B = 0; //stop timer
   TIMSK0 = 0; //disable overflow interrupt
   OCR0A = 50 - 1; //period = 50 * 64 cycles
   OCR0B = 50 / 2 - 1; //50% duty cycle
   TCCR0A = _BV(COM0B1) //non-inverting PWM on OC0B
       | _BV(WGM00) //fast PWM, TOP = OCR0A
       | _BV(WGM01); //...ditto
   TCCR0B = _BV(WGM02) //...ditto
       | _BV(CS00) //clock at F_CPU / 64
       | _BV(CS01); //...ditto


   Serial.begin(57600);
   Serial.println("Frequency Counter");
}

void loop()
{
   FreqCounter::start(1000);
   while (FreqCounter::f_ready == 0);
   frq = FreqCounter::f_freq;
   Serial.println(frq);
}