Эхо-код: перейти от печати к возврату

Question

Я хочу написать функцию в ассемблере, которая имеет поведение, аналогичное эхо, но вместо того, чтобы печатать данный символ, я хочу изменить его на функцию, которая будет вызываться, поэтому вместо печати она возвращает заданный ввод. Приведенный ниже код отражает поведение эха, которое я хочу изменить

!
! main - Repeat the following in an infinite loop:  Wait (in a busy loop)
!        until an input character is available.  Read it in.  Then wait
!        (in a busy loop) until the output is ready and write the character
!        to the output.
!
! r2 = character
! r3 = ptr to SERIAL_STAT word in the memory-mapped area.
! r4 = ptr to SERIAL_DATA word in the memory-mapped area.
! r5 = serial status word
!
main:
        set STACK_START,r15             ! Initialize the stack reg
        set SERIAL_STAT,r3              ! Initialize ptr to SERIAL_STAT word
        set SERIAL_DATA,r4              ! Initialize ptr to SERIAL_DATA word
loop:                                   ! LOOP:
wait1:                                  !   WAIT1:
        load    [r3],r5                 !     r5 := serial status word
        btst    0x00000001,r5           !     if status[charAvail] == 0 then
        be  wait1                       !     .    goto WAIT1
        load    [r4],r2                 !   Get the character
                cmp r2,'\n'             !   if char != \n
        be  charOK                      !   .
                cmp r2,'\r'             !   .  and char != \r
        be  charOK                      !   .
                cmp r2,' '              !   .  and char < ' ' char then
                bge charOK              !   .
                mov '?',r2              !     char := '?'
charOK:                                 !   endIf
                cmp r2,0x7e             !   if char > 7e char then
                ble charOK2             !   .
                mov '?',r2              !     char := '?'
charOK2:                                !   endIf
wait2:                                  !   WAIT2:
        load    [r3],r5                 !     r5 := serial status word
        btst    0x00000002,r5           !     if status[outputReady] == 0 then
        be  wait2                       !     .    goto WAIT2
        store   r2,[r4]                 !   send char in r2 to serial output
                cmp     r2,'q'          !   if char == 'q' then
                bne     cont            !   .
                debug                   !     debug
cont:                                   !   endIf
        jmp loop                        ! ENDLOOP       

STACK_START =   0x00ffff00
SERIAL_STAT =   0x00ffff00
SERIAL_DATA =   0x00ffff04

Проблема, с которой я столкнулся, заключается в том, что я не очень хорош в чтении ассемблера, поэтому я не совсем понимаю, как стереть l * 1005. * не нарушая код или куда вставить инструкцию возврата.