diff --git a/src/CustomSoftwareSerial.cpp b/src/CustomSoftwareSerial.cpp
index 623fc50..19c624a 100644
--- a/src/CustomSoftwareSerial.cpp
+++ b/src/CustomSoftwareSerial.cpp
@@ -188,6 +188,8 @@ bool CustomSoftwareSerial::listen()
     _receive_buffer_head = _receive_buffer_tail = 0;
     active_object = this;
     SREG = oldSREG;
+
+    setRxIntMsk(true);
     return true;
   }
 
@@ -221,7 +223,10 @@ void CustomSoftwareSerial::recv()
   // If RX line is high, then we don't see any start bit
   // so interrupt is probably not for us
   if (_inverse_logic ? rx_pin_read() : !rx_pin_read())
-  {
+  { 
+    
+    setRxIntMsk(false);
+
     // Wait approximately 1/2 of a bit width to "center" the sample
     tunedDelay(_rx_delay_centering);
     DebugPulse(_DEBUG_PIN2, 1);
@@ -240,12 +245,6 @@ void CustomSoftwareSerial::recv()
 
     // skip the parity bit
     if(this->_parityBit != NONE) {
-        tunedDelay(_rx_delay_stopbit);
-        DebugPulse(_DEBUG_PIN2, 1);
-    }
-
-    // skip the stop bit
-    for(uint8_t i = 0; i < this->_numberOfStopBit; i ++) {
         tunedDelay(_rx_delay_stopbit);
         DebugPulse(_DEBUG_PIN2, 1);
     }
@@ -267,6 +266,15 @@ void CustomSoftwareSerial::recv()
 #endif
       _buffer_overflow = true;
     }
+
+  // skip the stop bit
+    for(uint8_t i = 0; i < this->_numberOfStopBit; i ++) {
+        tunedDelay(_rx_delay_stopbit);
+        DebugPulse(_DEBUG_PIN2, 1);
+    }
+    
+    // Re-enable interrupts when we're sure to be inside the stop bit
+    setRxIntMsk(true);
   }
 
 #if GCC_VERSION < 40302
@@ -409,8 +417,11 @@ void CustomSoftwareSerial::begin(long speed)
   {
     if (digitalPinToPCICR(_receivePin))
     {
+      //*digitalPinToPCMSK(_receivePin) |= _BV(digitalPinToPCMSKbit(_receivePin));
       *digitalPinToPCICR(_receivePin) |= _BV(digitalPinToPCICRbit(_receivePin));
-      *digitalPinToPCMSK(_receivePin) |= _BV(digitalPinToPCMSKbit(_receivePin));
+
+      _pcint_maskreg = digitalPinToPCMSK(_receivePin);
+      _pcint_maskvalue = _BV(digitalPinToPCMSKbit(_receivePin));
     }
     tunedDelay(_tx_delay); // if we were low this establishes the end
   }
@@ -423,6 +434,14 @@ void CustomSoftwareSerial::begin(long speed)
   listen();
 }
 
+void CustomSoftwareSerial::setRxIntMsk(bool enable)
+{
+    if (enable)
+      *_pcint_maskreg |= _pcint_maskvalue;
+    else
+      *_pcint_maskreg &= ~_pcint_maskvalue;
+}
+
 void CustomSoftwareSerial::begin(long speed, uint16_t configuration) {
     this->setPort(configuration);
     this->begin(speed);
@@ -430,8 +449,14 @@ void CustomSoftwareSerial::begin(long speed, uint16_t configuration) {
 
 void CustomSoftwareSerial::end()
 {
-  if (digitalPinToPCMSK(_receivePin))
-    *digitalPinToPCMSK(_receivePin) &= ~_BV(digitalPinToPCMSKbit(_receivePin));
+
+  if (active_object == this)
+  {
+    setRxIntMsk(false);
+    active_object = NULL;
+    //return true;
+  }
+  //return false;  
 }
 
 
@@ -603,9 +628,9 @@ void CustomSoftwareSerial::writeStopBits() {
 }
 
 uint8_t CustomSoftwareSerial::calculateNumberOfBits1(uint8_t sentData) {
-    uint8_t numberOfBit1 = 0;
+    uint8_t numberOfBit1 = 0;
     uint8_t index;
-    for (index = 0x80; index; index >>= 1) {
+    for (index = 0x80; index; index >>= 1) {
         if (sentData & index) numberOfBit1++;
     }
 
diff --git a/src/CustomSoftwareSerial.h b/src/CustomSoftwareSerial.h
index 2629ed9..516c55e 100644
--- a/src/CustomSoftwareSerial.h
+++ b/src/CustomSoftwareSerial.h
@@ -89,6 +89,8 @@ class CustomSoftwareSerial : public Stream
   volatile uint8_t *_receivePortRegister;
   uint8_t _transmitBitMask;
   volatile uint8_t *_transmitPortRegister;
+  volatile uint8_t *_pcint_maskreg;
+  uint8_t _pcint_maskvalue;
 
   uint16_t _rx_delay_centering;
   uint16_t _rx_delay_intrabit;
@@ -116,6 +118,7 @@ class CustomSoftwareSerial : public Stream
   void tx_pin_write(uint8_t pin_state);
   void setTX(uint8_t transmitPin);
   void setRX(uint8_t receivePin);
+  inline void setRxIntMsk(bool enable);
 
   void setPort(uint16_t configuration);