diff --git a/cMIPS/include/handlers.s b/cMIPS/include/handlers.s
index c80f830c4dd43be5dda129e5ea9788bb173273d8..f236ac29c9bc6a4116f001f151d55683b53e22e8 100644
--- a/cMIPS/include/handlers.s
+++ b/cMIPS/include/handlers.s
@@ -106,6 +106,7 @@ UARTinterr:
 	
 	sw    $a0, 12($k0)	    # save registers $a0,$a1, others?
 	sw    $a1, 16($k0)
+	sw    $a2, 18($k0)
 
 	#----------------------------------
 	# while you are developing the complete handler,
@@ -125,6 +126,7 @@ UARTinterr:
 	# sw    $a1, 8($k0)           # Signal new arrival 
 		
 UARTret:
+	lw    $a2, 18($k0)
 	lw    $a1, 16($k0)          # restore registers $a0,$a1, others?
 	lw    $a0, 12($k0)
 
diff --git a/cMIPS/tests/handlerUART.s b/cMIPS/tests/handlerUART.s
index 66edd915a42d5fd624a2b71128437a1e6ac038a2..623963059ad4fc9b5b696a4c603c5c629e6bf4dd 100644
--- a/cMIPS/tests/handlerUART.s
+++ b/cMIPS/tests/handlerUART.s
@@ -9,32 +9,30 @@ RX:
     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 
+    addiu $a2, $zero, 16
+    slt   $a2, $a1, $a2         # If nrx >= 16 the queue is full
+    beq   $a2, $zero, END 
     
-    lui   $a0, %hi(HW_uart_addr)
-    ori   $a0, $a0, %lo(HW_uart_addr)
-    lw    $a1, 4($a0)           # Read data
-
-    lui   $a0, %hi(rx_queue)    
-    ori   $a0, $a0, %lo(rx_queue)
-    sw    $a1, 0($a0)           # Put data on RX_queue
+    addiu $a1, $a1, 1           # Increment nrx right now so we dont need to read it again
+    sw    $a1, 0($a0)           # Save incremented nrx
 
     lui   $a0, %hi(rx_tl)    
     ori   $a0, $a0, %lo(rx_tl)
     lw    $a1, 0($a0)           # Read rx_tl
     nop
-    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
+    addiu $a2, $a1, 1           # Increment RX_tail
+    andi  $a2, $a2, 15          # It's a circular queue so: (rx_tl+1)%16 
+    sw    $a2, 0($a0)           # Save new rx_tl
+
+    lui   $a0, %hi(HW_uart_addr)
+    ori   $a0, $a0, %lo(HW_uart_addr)
+    lw    $a2, 4($a0)           # Read data
+
+    lui   $a0, %hi(rx_queue)    
+    ori   $a0, $a0, %lo(rx_queue)
+    add   $a0, $a1, $a0         # Get queue tail address (before the increment)
+    sw    $a2, 0($a0)           # Put data on RX_queue tail
 
-    lui   $a0, %hi(nrx)    
-    ori   $a0, $a0, %lo(nrx)
-    lw    $a1, 0($a0)           # Read nrx
-    nop
-    addiu $a1, $a1, 1           # Increment nrx 
-    sw    $a1, 0($a0)           # Save incremented nrx
 
 TX:
     andi  $a0, $k1, UART_tx_irq # Is this transmission?
@@ -44,32 +42,29 @@ TX:
     ori   $a0, $a0, %lo(ntx)
     lw    $a1, 0($a0)           # Read ntx
 
-    addiu $a0, $zero, 16
-    slt   $a0, $a1, $a0         # If ntx < 16 there's something on the queue
-    beq   $a0, $zero, END      
+    addiu $a2, $zero, 16
+    slt   $a2, $a1, $a2         # If ntx < 16 there's something on the queue
+    beq   $a2, $zero, END      
 
-    lui   $a0, %hi(tx_queue)
-    ori   $a0, $a0, %lo(tx_queue)
-    lw    $a1, 0($a0)           # Read TX_queue
-
-    lui   $a0, %hi(HW_uart_addr)
-    ori   $a0, $a0, %lo(HW_uart_addr)
-    sw    $a1, 0($a0)           # Put data on UART
+    addiu $a1, $a1, 1           # Increment ntx
+    sw    $a1, 0($a0)           # Save incremented ntx
 
     lui   $a0, %hi(tx_hd)
     ori   $a0, $a0, %lo(tx_hd)
     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
+    addiu $a2, $a1, 1           # Increment tx_hd: we've transmitted, there's space on the queue
+    andi  $a2, $a2, 15          # It's a circular queue so: (tx_hd+1)%16
+    sw    $a2, 0($a0)           # Save tx_hd
 
-    lui   $a0, %hi(ntx)
-    ori   $a0, $a0, %lo(ntx)
-    lw    $a1, 0($a0)           # Read ntx
-    nop
-    addiu $a1, $a1, 1           # Increment ntx
-    sw    $a1, 0($a0)           # Save incremented ntx
+    lui   $a0, %hi(tx_queue)
+    ori   $a0, $a0, %lo(tx_queue)
+    add   $a0, $a1, $a0         # Get queue head address (before the increment)
+    lw    $a1, 0($a0)           # Read TX_queue head
+
+    lui   $a0, %hi(HW_uart_addr)
+    ori   $a0, $a0, %lo(HW_uart_addr)
+    sw    $a1, 0($a0)           # Put data on UART
 
 END: