diff --git a/libraries/SoftwareSerial/src/SoftwareSerial.cpp b/libraries/SoftwareSerial/src/SoftwareSerial.cpp
index aa15f0f..b1ab6fb 100644
--- a/libraries/SoftwareSerial/src/SoftwareSerial.cpp
+++ b/libraries/SoftwareSerial/src/SoftwareSerial.cpp
@@ -32,8 +32,9 @@ http://arduiniana.org.
 // #include <avr/pgmspace.h>
 #include <Arduino.h>
 #include <SoftwareSerial.h>
-// #include "mik32_hal_irq.h" 
+#include "mik32_hal_irq.h" 
 #include "mik32_hal_gpio.h"
+#include "mik32_hal_scr1_timer.h"
 
 //
 // Statics
@@ -68,11 +69,11 @@ volatile uint8_t SoftwareSerial::_receive_buffer_head = 0;
 // Private methods
 //
 
-/* static */ 
-#include "mik32_hal_scr1_timer.h"
+// get ticks from systick
 #define CURRENT_TICKS   ((uint64_t)(SCR1_TIMER->MTIMEH)<<32 | SCR1_TIMER->MTIME)
 
-void SoftwareSerial::tunedDelay(uint32_t delayTicks)
+// function placed in ram. Should not call other function placed in flash
+void SoftwareSerial::tunedDelay(uint32_t delayTicks) 
 { 
   uint64_t startTicks = CURRENT_TICKS;
   while ((CURRENT_TICKS - startTicks) < delayTicks)
@@ -359,47 +360,46 @@ void SoftwareSerial::end()
   stopListening();
 }
 
+// function placed in ram. Should not call other function placed in flash
 size_t SoftwareSerial::write(uint8_t byte)
 {
   if (_tx_delay == 0) 
-  {
-    setWriteError();
     return 0;
-  }
 
   if (_inverse_logic)
     byte = ~byte;
 
-  noInterrupts();  // turn off interrupts for a clean txmit
+  clear_csr(mie, MIE_MEIE);  // turn off interrupts for a clean txmit
 
   // Write the start bit
   if (_inverse_logic)
-    HAL_GPIO_WritePin((GPIO_TypeDef *)_transmitPortRegister, (HAL_PinsTypeDef)_transmitBitMask, GPIO_PIN_HIGH);
+    ((GPIO_TypeDef *)_transmitPortRegister)->SET = (HAL_PinsTypeDef)_transmitBitMask;
   else
-    HAL_GPIO_WritePin((GPIO_TypeDef *)_transmitPortRegister, (HAL_PinsTypeDef)_transmitBitMask, GPIO_PIN_LOW);
+    ((GPIO_TypeDef *)_transmitPortRegister)->CLEAR = (HAL_PinsTypeDef)_transmitBitMask;
+  tunedDelay(_tx_delay); // wait start bit
 
-  tunedDelay(_tx_delay);
-
-  // Write each of the 8 bits
+  // Write each of the 8 bits in LSB mode
   for (uint8_t i = 8; i > 0; --i)
   {
     if (byte & 1) // choose bit
-      HAL_GPIO_WritePin((GPIO_TypeDef *)_transmitPortRegister, (HAL_PinsTypeDef)_transmitBitMask, GPIO_PIN_HIGH);
+      ((GPIO_TypeDef *)_transmitPortRegister)->SET = (HAL_PinsTypeDef)_transmitBitMask;
     else
-      HAL_GPIO_WritePin((GPIO_TypeDef *)_transmitPortRegister, (HAL_PinsTypeDef)_transmitBitMask, GPIO_PIN_LOW);
-
+      ((GPIO_TypeDef *)_transmitPortRegister)->CLEAR = (HAL_PinsTypeDef)_transmitBitMask;
     tunedDelay(_tx_delay);
     byte >>= 1;
   }
 
-  // restore pin to natural state
+  // restore pin to natural state - stop bit
   if (_inverse_logic)
-    HAL_GPIO_WritePin((GPIO_TypeDef *)_transmitPortRegister, (HAL_PinsTypeDef)_transmitBitMask, GPIO_PIN_LOW);
+    ((GPIO_TypeDef *)_transmitPortRegister)->CLEAR = (HAL_PinsTypeDef)_transmitBitMask;
   else
-    HAL_GPIO_WritePin((GPIO_TypeDef *)_transmitPortRegister, (HAL_PinsTypeDef)_transmitBitMask, GPIO_PIN_HIGH);
+    ((GPIO_TypeDef *)_transmitPortRegister)->SET = (HAL_PinsTypeDef)_transmitBitMask;
 
-  interrupts(); // enable interrupts
-  tunedDelay(_tx_delay);
+  // enable interrupts
+  set_csr(mstatus, MSTATUS_MIE);
+  set_csr(mie, MIE_MEIE);
+
+  tunedDelay(_tx_delay); // wait stop bit
   
   return 1;
 }
diff --git a/libraries/SoftwareSerial/src/SoftwareSerial.h b/libraries/SoftwareSerial/src/SoftwareSerial.h
index fa9970f..e3bca24 100644
--- a/libraries/SoftwareSerial/src/SoftwareSerial.h
+++ b/libraries/SoftwareSerial/src/SoftwareSerial.h
@@ -76,7 +76,7 @@ private:
   static uint32_t subtract_cap(uint32_t num, uint16_t sub);
 
   // private static method for timing
-  static void __attribute__( ( noinline, section(".data") ) ) tunedDelay(uint32_t delayTicks);
+  static void tunedDelay(uint32_t delayTicks) __attribute__((noinline, section(".data"))) ;
 
 public:
   // public methods
@@ -96,7 +96,7 @@ public:
   }
   int peek();
 
-  virtual size_t write(uint8_t byte);
+  virtual size_t write(uint8_t byte) __attribute__((noinline, section(".data")));
   virtual int read();
   virtual int available();
   virtual void flush();