Echo code: change from printing to returning

I want to write a function in assembly that has a similar behavior to echo, but instead of printing the given char, I want to change it into a function that will be called upon so instead of printing it returns the input given. The code below is the echo behavior I want to modify

!
! 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

The trouble I'm having is that I am not very good at reading assembly language so I'm not entirely how to erase the loop without breaking the code, or where to insert the return instruction.