Работа с матричной клавиатурой, ассемблер

[igor321]

по нажатии на кнопку 2 должен прозвучать сигнал с частотой 1 кгц
кнопка 5 - сигнал с частотой 2 кгц.
с выявлением номера нажатой кнопки проблем нет. но сигнал почему-то пищит постоянно, хотя должен прекратиться по истечении задержки ncircle и дальше должен запуститься снова опрос клавиатуры.

.device ATmega128
.include	"m128def.inc"

.def temp			=r16
.def counter		=r17
.def strok			=r18
.def mask1		=r19
.def schet_strok		=r20
.def schet_stolb		=r21
.def mask2		=r22
.def number		=r30

.EQU	ncircle1 = 1000
.EQU	ncircle2 = 2000


rjmp MAIN

delay1:			; задержка для сигнала с частотой 1 кГц
	ldi r23, 8
P1:	ldi temp, 250
del1: 
	dec temp
	brne del1
	dec r23
	brne P1
	ret;

delay2:			; задержка для сигнала с частотой 2 кГц
	ldi r24, 4
N1:	ldi temp, 250
del2: 
	dec temp
	brne del2
	dec r24
	brne N1
	ret;
MAIN:
	
	ldi	temp,High(RAMEND)	;инициализация стека
	out	SPH,temp			;
	ldi	temp,Low(RAMEND)	;
	out	SPL,temp       		;


Keybrd:
	in temp, ddrd			; загрузили данные с порта D в temp
	sbr temp, 0b10001111	; PD0-PD3, PD7 настроили на выход строки клавиатуры и динамик
	cbr temp, 0b01110000	; PD4-PD6 настроили на вход столбцы
	out ddrd, temp			; в порт D



	ldi schet_strok, 4		; создаем счетчик строк
	ldi mask1, 0b11111110	; записываем константу 

	clr counter				; обнуляем счетчик номера кнопки
Vnesh:
	mov strok, mask1		; копируем значение из mask1 в  strok
	sbr strok, 0b01110000	; промаскировали strok единицами
	cbr strok, 0b10000000
	out portd, strok		; выводим в порт D

	sec 					; устанавливаем в единицу флаг переноса
	rol mask1				; сдвигаем влево 
	ldi mask2, 0b00010000	; содаем mask2
	ldi schet_stolb, 3		; создаем счеткик столбцов						
Vnutr:
	
	in temp, pind			; прочитали данные с порта ввода-вывода
	inc counter				; инкремент счетчика
	And temp, mask2			; выявляем наличие нажатой кнопки
	brne Ne_najato			; если нет нажатия -> метка
	mov number, counter		; копируем номер кнопки
	rjmp signal
Ne_najato:
	lsl mask2				; сдвигаем влево mask2
	dec schet_stolb			; декремент счетчик столбцов
	brne vnutr				; пока не ноль
	dec schet_strok			; декремент счетчика строк
	brne vnesh    			; пока не ноль
	
rjmp Keybrd

Signal:
	ldi r25, low(ncircle1)	; запись числа 1000
	ldi r26, high(ncircle1)
	ldi r27, low(ncircle2)	; запись числа 2000
	ldi r28, high(ncircle2)
	mov temp, number			;

	cpi temp, 2			; выявляем  факт нажатия клавиши 2
	brne Ne2				; если это не двойка
	ldi r29, 0b10000000		; 

ton1:
	out portd, r29			; единицу в PD7, динамик запищал
	call delay1				; удерживается пищание  1 кГц
	swap r29			;
	sbiw r26, 1				;
	brne ton1				;
ldi temp, 0b00000000
out portd, temp                           ; выключить динамик
	rjmp Keybrd
Ne2:
	cpi  temp, 5			; выявляем факт нажатия клавиши 5
	brne Keybrd			; если не нажата
	ldi r29, 0b10000000
ton2:
	out portd, r29			; удерживается пищание 2 кГц
	call delay2				;
	swap r29				;
	sbiw r28, 1				;
	brne ton2				;
ldi temp, 0b00000000
out portd, temp                           ; выключить динамик
	rjmp Keybrd