Jak optymalnie zmierzyć czas trwania impulsu na Atmega16?

#include <avr/io.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include <avr/interrupt.h>

//ATMega16 Q=4MHz

#define tau0 	87;	//stała czasowa timera0, dla 87 wystepuje przepelnienie licznika po ok 20mS
short			flPomiar=0;
bool			flMinCzasTrwania=false;
bool			flKoniecPomiaru=false;

void czekaj (unsigned long pt);

void czekaj (unsigned long pt) //procedura wytracania czasu
{
 unsigned char tp1;

 for(;pt>0;pt--)
 {
  for(tp1=255;tp1!=0;tp1--);
 }
}

int main (void)
{
 DDRD=0x00;        	//PORTD - we
 PORTD=0xFF;       	//podciąganie linii

 DDRB=0xFF;        	//PORTB - wy
 PORTB=0x00;      	//zapal LED

 DDRC=0xFF;        	//PORTB - wy
 PORTC=0xFF;      	//zgaś LED

 DDRA=0xFF;        	//PORTA - wy
 PORTA=0xFF;      	//zgaś LED

 // Konfiguracja zewnętrznego przerwania od INT0
 GICR=1<<INT0;		//włącz przerwanie
 MCUCR=1<<ISC00;	//przerwanie wyzwalane zboczem narastającym __|--
 MCUCR=1<<ISC01;

 sei();            	//włącz przerwania

 while (1)
  {
PORTC^=0xFF;		//zmień stan LED DEBUG!!!

   //Jeśli impuls trwa min. 25mS to coś tam wykonuje
   if (flMinCzasTrwania==true)
   	{
   	 PORTA=0x00;
	 flMinCzasTrwania=false;
	}
	else
   		PORTA=0xFF;

   czekaj (32);//32
   asm ("nop");
  }
}

//Procedura obsługi przerwanie od licznika TC0
SIGNAL (SIG_OVERFLOW0)
{
 TCNT0=tau0;			//odśwież stałą czasowa w TCNT0

 TIMSK=0<<TOIE0;		//nie zezwalam na przerwanie od TC0
 TCCR0=0;				//wyłączam TC0 kiedy wystąpi przepełnienie

 flMinCzasTrwania=true;	//Ustawiam flage która mówi ze impuls trwał min. 25mS
}

//Procedura obsługi przerwania zewnętrznego od INT0
SIGNAL(SIG_INTERRUPT0)
{
 if (flPomiar==0)
 {
  //1. ustawiam przerwanie wyzwalane zboczem opadającym --|__
  MCUCR=0<<ISC00;
  MCUCR=1<<ISC01;

  //2. uruchamiam timer
  TIMSK=1<<TOIE0;	//zezwolenie na przerwanie od TC0
  TCNT0=tau0;		//wpisuje stałą czasowa do TCNT0
  TCCR0=5;			//włączam TC0 i ustawiam preskaler na XTAL/1024

  //3. ustawiam flage pomiar trwa
  flPomiar=1;
 } else if (flPomiar==1)
 {
  TIMSK=0<<TOIE0;	//nie zezwalam na przerwanie od TC0
  TCCR0=0;			//wyłączam TC0

  MCUCR=1<<ISC00;	//przerwanie wyzwalane zboczem narastającym __|--
  MCUCR=1<<ISC01;

  flPomiar=0;
  flKoniecPomiaru=true;
 }
}

 TIMSK=0<<TOIE0

 TIMSK=0

 TIMSK&=~(1<<TOIE0)