Синхронизация по фронтам Опции

[Mayak]

Здравствуйте. Есть задача. На вход поступает сигнал определенной частоты. На выходе необходимо после вычисления частоты выдать такой же сигнал синхронизированный с входящим по нарастающему и убывающему фронтам.
Делаю так:
- по первому прерыванию int0 включаю счетчик
- по второму прерыванию int0 останавливаю счетчик и вычисляю полупериод
- вычисляю задержки на вход в прерывания и по прерыванию по сравнению таймера меняю выход с 1 на 0 и наоборот.
Прерывание по сравнению происходит раньше смены фронтов входящего сигнала на величину задержки.
По идее должно работать, но на осциллографе частота получается равная, а вот фронты не совпадают.
Подскажите, что не так. Может вообще как-то по другому надо все делать?

Код

;***************************************************************************

;***************************************************************************
.include "m16def.inc"
.def temp=r16;
.def timeL=r17;
.def timeH=r18;
.def counter=r21;

.CSEG          ;начало сегмента
.ORG 0              ;вектор сброса
    rjmp init;
.ORG INT0addr
    rjmp Ext_INT0          ; (INT0) External Interrupt Request 0
.ORG OC1Aaddr            ; Timer/Counter1 Compare Match A
    rjmp Ext_OC1A
.ORG OC1Baddr
    rjmp Ext_OC1B

;==========================
;        Прерывания
;==========================

Ext_INT0:
loop1:
    cpi counter,1;        Захватываем начало периода по 1-му нарастающему фронту
    brne loop2
    ldi r16,0b00000001
    out TCCR1B,r16
    inc counter
    rjmp exit
loop2:
    cpi counter,2;        Захватываем конец периода по 2-му нарастающему фронту
    brne loop3
    ldi r16,0b00000000
    out TCCR1B,r16
    in timeL,TCNT1L;            
    in timeH,TCNT1H
    subi timeL,3;        убираем задержку вычисления периода
    sbci timeH,0
    mov r23,timeH
    mov r22,timeL
    subi r22,51    ;устанавливаем задержку для совпадения фронта
    sbci r23,0    ; и заносим ее в OCR1B
    out OCR1BH,r23        
    out OCR1BL,r22
    lsr timeH    ;Вычисляем длительность импульса
    ror timeL        
    subi timeL,16    ; убираем задержку для смены фронтов
    sbci timeH,0
    out OCR1AH,timeH; длительность импульса в OCR1A
    out OCR1AL,timeL
    ldi r16,0
    out TCNT1H,r16
    out TCNT1L,r16
    inc counter
    rjmp exit
loop3:
    ldi r16,0    ;включаем таймер тс1в
    out GICR,r16
    ldi r16,0b00001000
    out TIMSK,r16
    ldi r16,0b00000001    
    out TCCR1B,r16
    ldi r16,0
    out TCNT1H,r16
    out TCNT1L,r16
exit:
reti

Ext_OC1B:
    ldi r16,0b00010000; запускаем задержку для совпадения фронтов
    out TIMSK,r16
    ldi r16,0
    out TCNT1H,r16
    out TCNT1L,r16
reti

Ext_OC1A:        ; запускаем задержку для смены фронтов
    in r16,PortD
    ldi r17,0b00000001
    eor r16,r17
    out PortD,r16
    ldi r16,0b00000000
    out TCNT1H,r16
    out TCNT1L,r16
reti


;=========================
;        Инициализация
;=========================
Init:
    ldi r31,LOW(RAMEND)
    out spl,r31
    ldi r31,HIGH(RAMEND)
    out sph,r31

    ldi r16,0b11111011; Настройка портов
    out DDRD,r16;
    ldi r16,0b00000100;
    out PortD,r16;
    ldi r16,0b00000011;
    out MCUCR,r16;
    ldi r16,0b01000000;
    out GICR,r16;
    ldi r18,100
    ldi r21,1
    ldi r16,0b00000101;
    sei;
    
reset:  ;RESET