diff --git a/cMIPS/tests/handlerUART.s b/cMIPS/tests/handlerUART.s
index eefcde4d3ccd48084d12770b3404635287d3c772..66edd915a42d5fd624a2b71128437a1e6ac038a2 100644
--- a/cMIPS/tests/handlerUART.s
+++ b/cMIPS/tests/handlerUART.s
@@ -4,6 +4,14 @@
 RX:
     andi  $a0, $k1, UART_rx_irq # Is this reception?
     beq   $a0, $zero, TX        #   no, test if it's transmission
+
+    lui   $a0, %hi(nrx)    
+    ori   $a0, $a0, %lo(nrx)
+    lw    $a1, 0($a0)           # Read nrx
+    
+    addiu $a0, $zero, 16
+    slt   $a0, $a1, $a0         # If nrx >= 16 the queue is full
+    beq   $a0, $zero, END 
     
     lui   $a0, %hi(HW_uart_addr)
     ori   $a0, $a0, %lo(HW_uart_addr)
@@ -17,8 +25,9 @@ RX:
     ori   $a0, $a0, %lo(rx_tl)
     lw    $a1, 0($a0)           # Read rx_tl
     nop
-    addiu $a1, $a1, 1           # Increment rx_tl (shouldn't it be (rx_tl+1)%16 ?) 
-    sw    $a1, 0($a0)           # Save rx_tl
+    addiu $a1, $a1, 1           # Increment RX_tail
+    andi  $a1, $a1, 15          # It's a circular queue so: (rx_tl+1)%16 
+    sw    $a1, 0($a0)           # Save new rx_tl
 
     lui   $a0, %hi(nrx)    
     ori   $a0, $a0, %lo(nrx)
@@ -30,16 +39,14 @@ RX:
 TX:
     andi  $a0, $k1, UART_tx_irq # Is this transmission?
     beq   $a0, $zero, END       #   no, end handler
-    nop
 
     lui   $a0, %hi(ntx)
     ori   $a0, $a0, %lo(ntx)
     lw    $a1, 0($a0)           # Read ntx
 
-    li    $a0, 16
+    addiu $a0, $zero, 16
     slt   $a0, $a1, $a0         # If ntx < 16 there's something on the queue
-    beq   $a0, $zero, END       
-    nop
+    beq   $a0, $zero, END      
 
     lui   $a0, %hi(tx_queue)
     ori   $a0, $a0, %lo(tx_queue)
@@ -54,6 +61,7 @@ TX:
     lw    $a1, 0($a0)           # Read tx_hd
     nop
     addiu $a1, $a1, 1           # Increment tx_hd: we've transmitted, there's space on the queue
+    andi  $a1, $a1, 15          # It's a circular queue so: (tx_hd+1)%16
     sw    $a1, 0($a0)           # Save tx_hd
 
     lui   $a0, %hi(ntx)