diff --git a/firmwares/MuxTO/MuxTO.ino b/firmwares/MuxTO/MuxTO.ino
index 8560307..bddfb92 100644
--- a/firmwares/MuxTO/MuxTO.ino
+++ b/firmwares/MuxTO/MuxTO.ino
@@ -23,25 +23,16 @@
 #include "JTAG2.h"
 #include "UPDI_hi_lvl.h"
 
-volatile bool updi_mode = false;
-unsigned long baudrate = 115200;
-unsigned long updi_mode_start = 0;
-unsigned long updi_mode_end = 0;
-uint8_t stopbits = 1;
-uint8_t paritytype = 0;
-uint8_t numbits = 8;
-int8_t dtr = -1;
-int8_t rts = -1;
-uint16_t serial_mode = SERIAL_8N1;
-bool serialNeedReconfiguration = false;
 
-char support_buffer[64];
+
+uint16_t serial_mode = SERIAL_8N1;
+unsigned long baudrate = 115200;
 
 struct lock q;
 
 void setup() {
   /* Initialize MCU */
-  pinMode(LED_BUILTIN, OUTPUT);
+//  pinMode(LED_BUILTIN, OUTPUT);
 
   /* Initialize serial links */
   JICE_io::init();
@@ -49,6 +40,7 @@ void setup() {
   Serial1.begin(baudrate, serial_mode);
   lock_init(&q);
   JTAG2::sign_on();
+
 }
 
 //#define DEBUG;
@@ -57,7 +49,27 @@ void setup() {
 //long blink_delay = 1000;
 
 void loop() {
+volatile bool updi_mode = false;
+unsigned long updi_mode_start = 0;
+unsigned long updi_mode_end = 0;
+unsigned long tnow = 0;
+uint8_t stopbits_new = 1;
+uint8_t paritytype_new = 0;
+uint8_t numbits_new = 8;
+unsigned long baudrate_new = 115200;
+
+uint8_t stopbits = 1;
+uint8_t paritytype = 0;
+uint8_t numbits = 8;
+int8_t dtr = -1;
+int8_t rts = -1;
+int8_t dtr_new = -1;
+int8_t rts_new = -1;
+int cav1=0;
+int cav=0;
+bool serialNeedReconfiguration = false;
 
+char support_buffer[64];
 #ifdef DEBUG
   if (millis() - blink_timer > blink_delay) {
     SYS::toggleLED();
@@ -65,90 +77,107 @@ void loop() {
   }
 #endif
 
-  if (updi_mode == false) {
+  if (!updi_mode){ // == false) {
 
     //blink_delay = 1000;
 
-    if (int c = Serial1.available()) {
+    if (int c1 = Serial1.available()) {
       lock(&q);
-      if (c > Serial.availableForWrite()) {
-        c = Serial.availableForWrite();
+      cav=Serial.availableForWrite();
+      if (c1 > cav) {
+        c1 = cav;
       }
       unlock(&q);
-      Serial1.readBytes(support_buffer, c);
-      Serial.write(support_buffer, c);
+      Serial1.readBytes(support_buffer, c1);
+      Serial.write(support_buffer, c1);
     }
 
     if (int c = Serial.available()) {
       lock(&q);
-      if (c > Serial1.availableForWrite()) {
-        c = Serial1.availableForWrite();
+      cav1=Serial1.availableForWrite();
+      if (c > cav1) {
+        c = cav1;
       }
       unlock(&q);
       Serial.readBytes(support_buffer, c);
       Serial1.write(support_buffer, c);
     }
-
-    if (Serial.stopbits() != stopbits) {
-      serial_mode &= ~HARDSER_STOP_BIT_MASK;
-      stopbits = Serial.stopbits();
+    static const uint16_t modemasks[3]={~HARDSER_STOP_BIT_MASK,~HARDSER_PARITY_MASK,~HARDSER_DATA_MASK};
+    stopbits_new=Serial.stopbits();
+    if (stopbits_new != stopbits) {
+      //serial_mode &= ~HARDSER_STOP_BIT_MASK;
+      stopbits = stopbits_new;
       serialNeedReconfiguration = true;
-      switch (stopbits) {
-        case 1:
-          serial_mode |= HARDSER_STOP_BIT_1;
-          break;
-        case 2:
-          serial_mode |= HARDSER_STOP_BIT_2;
-          break;
+      if (stopbits_new==1 || stopbits_new==2){
+          serial_mode =(serial_mode & modemasks[0])|( (stopbits_new==1) ? HARDSER_STOP_BIT_1 : HARDSER_STOP_BIT_2);
       }
+
+//      switch (stopbits) {
+//        case 1:
+//          serial_mode |= HARDSER_STOP_BIT_1;
+//          break;
+//        case 2:
+//          serial_mode |= HARDSER_STOP_BIT_2;
+//          break;
+//      }
     }
-    if (Serial.paritytype() != paritytype) {
-      serial_mode &= ~HARDSER_PARITY_MASK;
-      paritytype = Serial.paritytype();
+    static const uint16_t paritytypecode[3]={HARDSER_PARITY_NONE,HARDSER_PARITY_EVEN,HARDSER_PARITY_ODD};
+    paritytype_new=Serial.paritytype();
+    if (paritytype_new!= paritytype) {
+      //serial_mode &= ~HARDSER_PARITY_MASK;
+      paritytype = paritytype_new;
       serialNeedReconfiguration = true;
-      switch (paritytype) {
-        case 0:
-          serial_mode |= HARDSER_PARITY_NONE;
-          break;
-        case 1:
-          serial_mode |= HARDSER_PARITY_EVEN;
-          break;
-        case 2:
-          serial_mode |= HARDSER_PARITY_ODD;
-          break;
+      if (paritytype_new == 0 || paritytype_new==1 || paritytype_new==2){
+        serial_mode = (serial_mode & modemasks[1]) | ( paritytypecode[paritytype_new]);
       }
+//      switch (paritytype) {
+//        case 0:
+//          serial_mode |= HARDSER_PARITY_NONE;
+//          break;
+//        case 1:
+//          serial_mode |= HARDSER_PARITY_EVEN;
+//          break;
+//        case 2:
+//          serial_mode |= HARDSER_PARITY_ODD;
+//          break;
+//      }
     }
-    if (Serial.numbits() != numbits) {
-      serial_mode &= ~HARDSER_DATA_MASK;
-      numbits = Serial.numbits();
+    static const uint16_t numbitscode_5[4]={HARDSER_DATA_5,HARDSER_DATA_6,HARDSER_DATA_7,HARDSER_DATA_8};
+    numbits_new=Serial.numbits();
+    if (numbits_new != numbits) {
+      //serial_mode &= ~HARDSER_DATA_MASK;
+      numbits = numbits_new;
       serialNeedReconfiguration = true;
-      switch (numbits) {
-        case 5:
-          serial_mode |= HARDSER_DATA_5;
-          break;
-        case 6:
-          serial_mode |= HARDSER_DATA_6;
-          break;
-        case 7:
-          serial_mode |= HARDSER_DATA_7;
-          break;
-        case 8:
-          serial_mode |= HARDSER_DATA_8;
-          break;
+      if (numbits_new==5||numbits_new==6||numbits_new==7||numbits_new==8){
+        serial_mode =(serial_mode&modemasks[2])|numbitscode_5[numbits_new-5];
       }
+//      switch (numbits) {
+//        case 5:
+//          serial_mode |= HARDSER_DATA_5;
+//          break;
+//        case 6:
+//          serial_mode |= HARDSER_DATA_6;
+//          break;
+//        case 7:
+//          serial_mode |= HARDSER_DATA_7;
+//          break;
+//        case 8:
+//          serial_mode |= HARDSER_DATA_8;
+//          break;
+//      }
     }
-
-    if (baudrate == 1200 && Serial.dtr() == 0 && (millis() - updi_mode_end > 200)) {
+    dtr_new=Serial.dtr();
+    if (baudrate == 1200 &&  dtr_new== 0 && (millis() - updi_mode_end > 200)) {
       // don't reenter here if you just finished flashing
       updi_mode = true;
       updi_mode_start = millis();
       updi_mode_end = 0;
     }
-
-    if (Serial.baud() != baudrate || serialNeedReconfiguration || Serial.dtr() != dtr) {
-      dtr = Serial.dtr();
-      if (Serial.dtr() == 1) {
-        baudrate = Serial.baud();
+    baudrate_new=Serial.baud();
+    if (baudrate_new != baudrate || serialNeedReconfiguration || dtr_new != dtr) {
+      dtr = dtr_new;
+      if (dtr_new == 1) {
+        baudrate = baudrate_new;
         Serial1.end();
         if (baudrate != 1200) {
           Serial1.begin(baudrate, serial_mode);
@@ -163,10 +192,11 @@ void loop() {
     return;
   }
 
-  if (updi_mode == true) {
-
+  //if (updi_mode == true) {
+  else{
     // updi_mode cannot last more than 1 minute; in that case, break forcibly
-    if ((updi_mode_end != 0 && (millis() - updi_mode_end) > 500) || ((millis() - updi_mode_start) > 60000)) {
+    tnow=millis();
+    if ((updi_mode_end != 0 && (tnow - updi_mode_end) > 500) || ((tnow - updi_mode_start) > 60000)) {
       updi_mode = false;
       baudrate = -1;
       return;