diff --git a/cMIPS/tests/handlerUART.s b/cMIPS/tests/handlerUART.s
index e88dce142ab9725cc49b3d62a3b538fae1fcc020..b142c05de888edd973dce2022cf0e226fcf3e9c0 100644
--- a/cMIPS/tests/handlerUART.s
+++ b/cMIPS/tests/handlerUART.s
@@ -36,6 +36,7 @@ RX:
     # sw  $k1, 0($a0)               # Print for debug
 
     lw    $k1, 0($a1)
+    nop
     j     RX
     nop
 
@@ -81,6 +82,7 @@ TX:
     # sw  $a1, 0($a0)               # Print for debug
 
     lw    $k1, 0($a0)
+    nop
     j     RX
     nop
 
diff --git a/cMIPS/tests/uart.c b/cMIPS/tests/uart.c
index 203d3ca9689dc98d7d6a42554ac13251af94b2dc..09d01e3be8eaa2f9caa190b3b2972d3167ff65bd 100644
--- a/cMIPS/tests/uart.c
+++ b/cMIPS/tests/uart.c
@@ -58,24 +58,20 @@ int iostat(void);          // returns integer with status at lsb
 void ioctl(int);           // write lsb in control register
 char getc(void);           // returns char in queue, decrements nrx
 void Putc(char);           // inserts char in queue, decrements ntx
-int ctoi(char);            // converts a character to an integer
-void itoc(int, char*);            // converts integer to char 
 
-void insertionSort(int v[], int size);
 void initUd();
+void insertionSort(int v[], int size);
+void writeFirstChar();     // get the first character on TX queue and write it in the UART
+int ctoi(char);            // converts a character to an integer
+void itoc(int, char*);     // converts integer to char 
 
 extern UARTDriver Ud;
 volatile Tserial *uart;
 
 int main(){
     uart = (void *)IO_UART_ADDR; // bottom of UART address range
-    Tcontrol ctrl;
-
-    ctrl.ign   = 0;
-    ctrl.intTX = 1;
-    ctrl.intRX = 1;
-    ctrl.speed = 2;        // operate at 1/2 of the highest data rate
-    uart->cs.ctl = ctrl;
+    int ictl = 0x1a; // 00011010: ign=0; intTX=1; intRX=1; speed=2;
+    ioctl(ictl);
 
     initUd();
     volatile char c;
@@ -101,7 +97,6 @@ int main(){
     insertionSort(v,i);
     char hex[9];
     for(;i>0;i--){
-        // print(v[i-1]);
         itoc(v[i-1], hex);
         n = 0;
         while(hex[n] != '\0') {
@@ -110,22 +105,13 @@ int main(){
         }
         Putc('\n');
     }
-    Putc('\n');
-    // for(i=0;i<16;i++) {
-    //     //TODO
-    //     c = convert(v[i]);
-    //     Putc(c);
-    // }
-    if(Ud.ntx < 16){
-        disableInterr();
-        uart->d.tx = Ud.tx_q[Ud.tx_hd];
-        Ud.tx_hd = (Ud.tx_hd+1)%16;
-        Ud.ntx++;
-        enableInterr();
+    
+    if(probetx() < 16){
+        writeFirstChar();
     }
 
-    int cont;
-    for(cont=0;cont<1000;cont++);  //Wait for the remote uart
+    int count;
+    for(count=0;count<1000;count++);  //Wait for the remote uart
     
     return 0;
 }
@@ -139,22 +125,30 @@ void initUd(){
     Ud.ntx = 16;
 }
 
-void insertionSort(int v[], int size){
-  int i, j, index;
-  for (i=1; i < size; i++) {
-    index = v[i];
-    j = i;
-    while ((j > 0) && (v[j-1] > index)) {
-      v[j] = v[j-1];
-      j = j - 1;
-    }
-    v[j] = index;
-  }
+int proberx(){
+    return Ud.nrx;
+}
+
+int probetx(){
+    return Ud.ntx;
+}
+
+int iostat(){
+    return uart->cs.stat.s;
+}
+
+void ioctl(int ictl){
+    Tcontrol ctrl;
+    ctrl.speed = ictl & 0x7; 
+    ctrl.intRX = (ictl>>3) & 0x1;
+    ctrl.intTX = (ictl>>4) & 0x1;
+    ctrl.ign   = (ictl>>5) & 0x7;
+    uart->cs.ctl = ctrl;
 }
 
 char getc(){
     char c = EOF;
-    if(Ud.nrx > 0){
+    if(proberx() > 0){
         disableInterr();
         c = Ud.rx_q[Ud.rx_hd];
         Ud.rx_hd = (Ud.rx_hd+1)%16;
@@ -165,29 +159,38 @@ char getc(){
 }
 
 void Putc(char c){
-    if(Ud.ntx > 0){
+    if(probetx() > 0){
         disableInterr();
         Ud.tx_q[Ud.tx_tl] = c;
         Ud.tx_tl = (Ud.tx_tl+1)%16;
         Ud.ntx--;
         enableInterr();
     }else{
-        while(!(TXempty&uart->cs.stat.s));
-        disableInterr();
-        uart->d.tx = Ud.tx_q[Ud.tx_hd];
-        Ud.tx_hd = (Ud.tx_hd+1)%16;
-        Ud.ntx++;
-        enableInterr();
+        writeFirstChar();
         Putc(c);
     }
 }
 
-int proberx(){
-    return Ud.nrx;
+void writeFirstChar(){
+    while(!(iostat()&TXempty));
+    disableInterr();
+    uart->d.tx = Ud.tx_q[Ud.tx_hd];
+    Ud.tx_hd = (Ud.tx_hd+1)%16;
+    Ud.ntx++;
+    enableInterr();
 }
 
-int probetx(){
-    return Ud.ntx;
+void insertionSort(int v[], int size){
+  int i, j, index;
+  for (i=1; i < size; i++) {
+    index = v[i];
+    j = i;
+    while ((j > 0) && (v[j-1] > index)) {
+      v[j] = v[j-1];
+      j = j - 1;
+    }
+    v[j] = index;
+  }
 }
 
 int ctoi(char c) {