diff --git a/platform/msb430/dev/msb430-uart1.c b/platform/msb430/dev/msb430-uart1.c
index 1588764f5..43e32a937 100644
--- a/platform/msb430/dev/msb430-uart1.c
+++ b/platform/msb430/dev/msb430-uart1.c
@@ -116,13 +116,14 @@ uart_set_speed(unsigned mode, unsigned ubr0,
 		unsigned ubr1, unsigned umctl)
 {
   // store setting
-  uart_speed_br0[mode] = ubr0;		// baudrate
-  uart_speed_br1[mode] = ubr1;		// baudrate
+  uart_speed_br0[mode] = ubr0;	// baudrate
+  uart_speed_br1[mode] = ubr1;	// baudrate
   uart_speed_bmn[mode] = umctl;	// modulation
 
   // reconfigure, if mode active
-  if (uart_mode == mode)
+  if (uart_mode == mode) {
     uart_configure(mode);
+  }
 }
 /*---------------------------------------------------------------------------*/
 void
@@ -143,7 +144,7 @@ int
 uart_lock(unsigned mode)
 {
   // already locked?
-  if (uart_lockcnt > 0) {
+  if (uart_mode != mode && uart_lockcnt > 0) {
     return 0;
   }
 
@@ -155,16 +156,26 @@ uart_lock(unsigned mode)
 }
 /*---------------------------------------------------------------------------*/
 int
+uart_lock_wait(unsigned mode)
+{
+  while (UART_WAIT_LOCK(mode)) {
+    _NOP();
+  }
+  return uart_lock(mode);
+}
+/*---------------------------------------------------------------------------*/
+int
 uart_unlock(unsigned mode)
 {
   if ((uart_lockcnt == 0) || (mode != uart_mode)) {
+    uart_lockcnt = 0;
+    uart_set_mode(UART_MODE_DEFAULT);
     return 0;
   }
 
   // decrement lock
   if (uart_lockcnt > 0) {
     uart_lockcnt--;
-
     // if no more locks, switch back to default mode
     if (uart_lockcnt == 0) {
       uart_set_mode(UART_MODE_DEFAULT);
@@ -178,15 +189,17 @@ void
 uart_set_mode(unsigned mode)
 {
   // do nothing if mode already set
-  if (mode == uart_mode)
+  if (mode == uart_mode) {
     return;
+  }
 
   IE2 &= ~(URXIE1 | UTXIE1);		// disable irq
   uart_configure(mode);			// configure uart parameters
   uart_mode = mode;
 	
-  if (uart_handler[mode] != NULL)
+  if (uart_handler[mode] != NULL) {
     IE2 |= URXIE1;			// Enable USART1 RX interrupt
+  }
 }
 /*---------------------------------------------------------------------------*/
 int
diff --git a/platform/msb430/dev/msb430-uart1.h b/platform/msb430/dev/msb430-uart1.h
index c2bf5cd77..eaf573173 100644
--- a/platform/msb430/dev/msb430-uart1.h
+++ b/platform/msb430/dev/msb430-uart1.h
@@ -99,10 +99,11 @@ typedef unsigned int(*fp_uart_handler)(unsigned char);
  */
 void uart_init(void);
 
-void uart_set_speed(unsigned mode, unsigned ubr0, unsigned ubr1, unsigned umctl);
-void uart_set_handler(unsigned mode, fp_uart_handler fpHandler);
-int uart_lock(unsigned mode);
-int uart_unlock(unsigned mode);
+void uart_set_speed(unsigned, unsigned, unsigned, unsigned);
+void uart_set_handler(unsigned, fp_uart_handler);
+int uart_lock(unsigned);
+int uart_lock_wait(unsigned);
+int uart_unlock(unsigned);
 void uart_set_mode(unsigned);
 int uart_get_mode(void);