diff --git a/src/knx/data_link_layer.cpp b/src/knx/data_link_layer.cpp
index 1503e14..1690eb4 100644
--- a/src/knx/data_link_layer.cpp
+++ b/src/knx/data_link_layer.cpp
@@ -14,16 +14,31 @@ DataLinkLayer::DataLinkLayer(DeviceObject& devObj, AddressTableObject& addrTab,
 
 void DataLinkLayer::dataRequest(AckType ack, AddressType addrType, uint16_t destinationAddr, FrameFormat format, Priority priority, NPDU& npdu)
 {
-    bool success = sendTelegram(npdu, ack, destinationAddr, addrType, format, priority);
-    _networkLayer.dataConfirm(ack, addrType, destinationAddr, format, priority, npdu.frame().sourceAddress(), npdu, success);
+    sendTelegram(npdu, ack, destinationAddr, addrType, format, priority);
 }
 
 void DataLinkLayer::systemBroadcastRequest(AckType ack, FrameFormat format, Priority priority, NPDU& npdu)
 {
-    bool success = sendTelegram(npdu, ack, 0, GroupAddress, format, priority);
-    _networkLayer.systemBroadcastConfirm(ack, format, priority, npdu.frame().sourceAddress(), npdu, success);
+    sendTelegram(npdu, ack, 0, GroupAddress, format, priority);
 }
 
+void DataLinkLayer::dataConReceived(CemiFrame& frame,bool success)
+{
+    AckType ack = frame.ack();
+    AddressType addrType = frame.addressType();
+    uint16_t destination = frame.destinationAddress();
+    uint16_t source = frame.sourceAddress();
+    FrameFormat type = frame.frameType();
+    Priority priority = frame.priority();
+    NPDU& npdu = frame.npdu();
+
+    if (addrType == GroupAddress && destination == 0)
+        _networkLayer.systemBroadcastConfirm(ack, type, priority, source, npdu, success);
+    else
+        _networkLayer.dataConfirm(ack, addrType, destination, type, priority, source, npdu, success);
+
+
+}
 void DataLinkLayer::frameRecieved(CemiFrame& frame)
 {
     AckType ack = frame.ack();
@@ -93,3 +108,5 @@ uint8_t* DataLinkLayer::frameData(CemiFrame& frame)
 {
     return frame._data;
 }
+
+
diff --git a/src/knx/data_link_layer.h b/src/knx/data_link_layer.h
index 6dfc389..74a3b50 100644
--- a/src/knx/data_link_layer.h
+++ b/src/knx/data_link_layer.h
@@ -21,6 +21,7 @@ public:
     virtual bool enabled() const = 0;
 protected:
     void frameRecieved(CemiFrame& frame);
+    void dataConReceived(CemiFrame& frame, bool success);
     bool sendTelegram(NPDU &npdu, AckType ack, uint16_t destinationAddr, AddressType addrType, FrameFormat format, Priority priority);
     virtual bool sendFrame(CemiFrame& frame) = 0;
     uint8_t* frameData(CemiFrame& frame);
@@ -28,4 +29,4 @@ protected:
     AddressTableObject& _groupAddressTable;
     NetworkLayer& _networkLayer;
     Platform& _platform;
-};
\ No newline at end of file
+};
diff --git a/src/knx/ip_data_link_layer.cpp b/src/knx/ip_data_link_layer.cpp
index 6723e1b..9467e55 100644
--- a/src/knx/ip_data_link_layer.cpp
+++ b/src/knx/ip_data_link_layer.cpp
@@ -36,6 +36,7 @@ bool IpDataLinkLayer::sendFrame(CemiFrame& frame)
     bool success = sendBytes(buffer, length);
     // only send 50 packet per second: see KNX 3.2.6 p.6
     _platform.mdelay(20);
+    dataConReceived(frame, success);
     delete[] buffer;
     return success;
 }
@@ -101,4 +102,4 @@ bool IpDataLinkLayer::sendBytes(uint8_t* bytes, uint16_t length)
         return false;
 
     return _platform.sendBytes(bytes, length);
-}
\ No newline at end of file
+}
diff --git a/src/knx/tpuart_data_link_layer.cpp b/src/knx/tpuart_data_link_layer.cpp
index b799721..498ce17 100644
--- a/src/knx/tpuart_data_link_layer.cpp
+++ b/src/knx/tpuart_data_link_layer.cpp
@@ -73,6 +73,252 @@
 #define U_STOP_MODE_IND     0x2B
 #define U_SYSTEM_STAT_IND   0x4B
 
+//loop states
+#define IDLE                     0
+#define RX_FIRST_BYTE            1
+#define RX_L_DATA                2
+#define RX_WAIT_DATA_CON         3
+#define TX_FRAME                 4
+
+
+void TpUartDataLinkLayer::loop(){
+
+    _receiveBuffer[0] = 0x29;
+    _receiveBuffer[1] = 0;
+    uint8_t* buffer = _receiveBuffer + 2;
+    uint8_t rxByte;
+
+    if (!_enabled)
+        return;
+
+    switch(_loopState){
+    case IDLE:
+        if(_platform.uartAvailable()){
+            _loopState = RX_FIRST_BYTE;
+        }
+        else{
+            if(!_waitConfirm && !isTxQueueEmpty()){
+                loadNextTxFrame();
+                _waitConfirm = true;
+                _waitConfirmStartTime = _platform.millis();
+                _loopState = TX_FRAME;
+            }
+        }
+        break;
+    case TX_FRAME:
+        if(sendSingleFrameByte() == false){
+            _loopState = IDLE;
+        }
+        break;
+    case RX_FIRST_BYTE:
+        rxByte =_platform.readUart();
+        _lastByteRxTime = _platform.millis();
+        _RxByteCnt = 0;
+        _xorSum = 0;
+        if ((rxByte & L_DATA_MASK) == L_DATA_STANDARD_IND){
+            buffer[_RxByteCnt++]=rxByte;
+            _xorSum ^= rxByte;
+            _RxByteCnt++;                                    //convert to L_DATA_EXTENDED
+            _convert = true;
+            _loopState=RX_L_DATA;
+            break;
+        }
+        else if ((rxByte & L_DATA_MASK) == L_DATA_EXTENDED_IND){
+            buffer[_RxByteCnt++]=rxByte;
+            _xorSum ^= rxByte;
+            _convert = false;
+            _loopState=RX_L_DATA;
+            break;
+        }
+        else if ((rxByte & L_DATA_CON_MASK) == L_DATA_CON){
+            println("got unexpected L_DATA_CON");
+        }
+        else if (rxByte == L_POLL_DATA_IND){
+            // not sure if this can happen
+            println("got L_POLL_DATA_IND");
+        }
+        else if ((rxByte & L_ACKN_MASK) == L_ACKN_IND){
+
+            // this can only happen in bus monitor mode
+            println("got L_ACKN_IND");
+        }
+        else if (rxByte == U_RESET_IND){
+            println("got U_RESET_IND");
+        }
+        else if((rxByte & U_STATE_IND) == U_STATE_IND){
+            print("got U_STATE_IND: 0x");
+            print(rxByte, HEX);
+            println();
+        }
+        else if((rxByte & U_FRAME_STATE_MASK) == U_FRAME_STATE_IND){
+            print("got U_FRAME_STATE_IND: 0x");
+            print(rxByte, HEX);
+            println();
+        }
+        else if((rxByte & U_CONFIGURE_MASK) == U_CONFIGURE_IND){
+            print("got U_CONFIGURE_IND: 0x");
+            print(rxByte, HEX);
+            println();
+        }
+        else if(rxByte == U_FRAME_END_IND){
+            println("got U_FRAME_END_IND");
+        }
+        else if(rxByte == U_STOP_MODE_IND){
+            println("got U_STOP_MODE_IND");
+        }
+        else if(rxByte == U_SYSTEM_STAT_IND){
+            print("got U_SYSTEM_STAT_IND: 0x");
+            while (true){
+                int tmp = _platform.readUart();
+                if (tmp < 0)
+                    continue;
+
+                print(tmp, HEX);
+                break;
+            }
+            println();
+        }
+        else{
+            print("got UNEXPECTED: 0x");
+            print(rxByte, HEX);
+            println();
+        }
+        _loopState = IDLE;
+        break;
+    case RX_L_DATA:
+        if (_platform.millis() - _lastByteRxTime > BYTE_TIMEOUT){
+            _RxByteCnt = 0;
+            _loopState = IDLE;
+            println("Timeout during RX_L_DATA");
+            break;
+        }
+        if(!_platform.uartAvailable()) break;
+        _lastByteRxTime = _platform.millis();
+        rxByte =_platform.readUart();
+
+        if(_RxByteCnt == MAX_KNX_TELEGRAM_SIZE){
+            _loopState = IDLE;
+            println("invalid telegram size");
+        }
+        else{
+            buffer[_RxByteCnt++]=rxByte;
+        }
+
+        if(_RxByteCnt==7){                 //Destination Address + payload available
+            _xorSum ^= rxByte;
+            //check if echo
+            if (!((buffer[0]^_sendBuffer[0])&~0x20) && !memcmp(buffer+2,_sendBuffer+1,5)){          //ignore repeated bit of control byte
+                _isEcho = true;
+            }
+            else{
+                _isEcho = false;
+            }
+
+            //convert into Extended.ind
+            if(_convert){
+                uint8_t payloadLength = buffer[6] & 0x0F;
+                buffer[1] = buffer[6] & 0xF0;
+                buffer[6] = payloadLength;
+            }
+
+            if(!_isEcho){
+                uint8_t c = 0x10;
+                //ceck if individual or group address
+                if ((buffer[6] & 0x80) == 0){                                                          //individual
+                    if(_deviceObject.induvidualAddress() == getWord(buffer + 4)){
+                        c |= 0x01;
+                    }
+                }
+                else{                                                                                  //group
+                    if(_groupAddressTable.contains(getWord(buffer + 4)) || getWord(buffer + 4) == 0){
+                        c |= 0x01;
+                    }
+                }
+                _platform.writeUart(c);
+            }
+        }
+        else if(_RxByteCnt == buffer[6]+7+2){                           //complete Frame received, payloadLength+1 for TCPI +1 for CRC
+            if(rxByte == (uint8_t)(~_xorSum)){                            //check if crc is correct
+                if(_isEcho && _sendBuffer != NULL){                        //check if it is realy an echo, rx_crc = tx_crc
+                    if(rxByte == _sendBuffer[_sendBufferLength-1])
+                        _isEcho = true;
+                    else
+                        _isEcho = false;
+                }
+                if(_isEcho){
+                    _loopState = RX_WAIT_DATA_CON;
+                }
+                else{
+                    frameBytesReceived(_receiveBuffer, _RxByteCnt+2);
+                    _loopState=IDLE;
+                }
+            }
+            else{
+                println("frame with invalid crc ignored");
+                _loopState=IDLE;
+            }
+        }
+        else{
+            _xorSum ^= rxByte;
+        }
+        break;
+    case RX_WAIT_DATA_CON:
+        if(!_platform.uartAvailable()) break;
+        rxByte =_platform.readUart();
+        _lastByteRxTime = _platform.millis();
+        if ((rxByte & L_DATA_CON_MASK) == L_DATA_CON){
+            //println("L_DATA_CON received");
+            dataConBytesReceived(_receiveBuffer, _RxByteCnt+2, ((rxByte & SUCCESS) > 0));
+            _waitConfirm = false;
+            delete[] _sendBuffer;
+            _sendBuffer = 0;
+            _sendBufferLength = 0;
+            _loopState=IDLE;
+        }
+        else{
+            //should not happen
+            println("expected L_DATA_CON not received");
+            dataConBytesReceived(_receiveBuffer, _RxByteCnt+2, false);
+            _waitConfirm = false;
+            delete[] _sendBuffer;
+            _sendBuffer = 0;
+            _sendBufferLength = 0;
+            _loopState=IDLE;
+        }
+        break;
+    default:
+        break;
+    }
+
+    if(_waitConfirm){
+        if (_platform.millis() - _waitConfirmStartTime > CONFIRM_TIMEOUT){
+            println("L_DATA_CON not received within expected time");
+            uint8_t cemiBuffer[MAX_KNX_TELEGRAM_SIZE];
+            cemiBuffer[0] = 0x29;
+            cemiBuffer[1] = 0;
+            memcpy((cemiBuffer+2),_sendBuffer,_sendBufferLength);
+            dataConBytesReceived(cemiBuffer, _sendBufferLength+2, false);
+            _waitConfirm = false;
+            delete[] _sendBuffer;
+            _sendBuffer = 0;
+            _sendBufferLength = 0;
+            if(_loopState == RX_WAIT_DATA_CON)
+                _loopState=IDLE;
+        }
+    }
+}
+
+
+bool TpUartDataLinkLayer::sendFrame(CemiFrame& frame)
+{
+    if (!_enabled)
+        return false;
+
+    addFrameTxQueue(frame);
+    return true;
+}
+
+
 void TpUartDataLinkLayer::resetChip()
 {
     uint8_t cmd = U_RESET_REQ;
@@ -106,160 +352,6 @@ TpUartDataLinkLayer::TpUartDataLinkLayer(DeviceObject& devObj, AddressTableObjec
 }
 
 
-bool TpUartDataLinkLayer::sendFrame(CemiFrame& frame)
-{
-    if (!_enabled)
-        return false;
-
-    uint16_t length = frame.telegramLengthtTP();
-    uint8_t* buffer = new uint8_t[length];
-    frame.fillTelegramTP(buffer);
-
-    _sendBuffer = buffer;
-    _sendResult = false;
-    _sendBufferLength = length;
-    sendBytes(buffer, length);
-
-    while (_sendBuffer != 0)
-        loop();
-
-    delete[] buffer;
-    return _sendResult;
-}
-
-void TpUartDataLinkLayer::loop()
-{
-    if (!_enabled)
-        return;
-
-    if (_platform.uartAvailable() == 0)
-        return;
-
-    uint8_t firstByte = _platform.readUart();
-
-    if (checkDataInd(firstByte))
-        return;
-
-    if (checkDataCon(firstByte))
-        return;
-
-    if (checkPollDataInd(firstByte))
-        return;
-
-    if (checkAckNackInd(firstByte))
-        return;
-
-    if (checkResetInd(firstByte))
-        return;
-
-    if (checkStateInd(firstByte))
-        return;
-
-    if (checkFrameStateInd(firstByte))
-        return;
-
-    if (checkConfigureInd(firstByte))
-        return;
-
-    if (checkFrameEndInd(firstByte))
-        return;
-
-    if (checkStopModeInd(firstByte))
-        return;
-
-    if (checkSystemStatInd(firstByte))
-        return;
-
-    handleUnexpected(firstByte);
-}
-
-bool TpUartDataLinkLayer::checkDataInd(uint8_t firstByte)
-{
-    const size_t bufferSize = 512;
-
-    uint8_t tmp = firstByte & L_DATA_MASK;
-    if (tmp != L_DATA_STANDARD_IND && tmp != L_DATA_EXTENDED_IND)
-        return false;
-
-    int len = 0;
-    uint8_t cemiBuffer[bufferSize + 2];
-    cemiBuffer[0] = 0x29;
-    cemiBuffer[1] = 0;
-    uint8_t* buffer = cemiBuffer + 2;
-    buffer[0] = firstByte;
-
-    uint8_t payloadLength = 0;
-    if (tmp == L_DATA_STANDARD_IND)
-    {
-        //convert to extended frame format
-        _platform.readBytesUart(buffer + 2, 5);
-        sendAck((AddressType)(buffer[6] & GroupAddress), getWord(buffer + 4));
-        payloadLength = buffer[6] & 0xF;
-        _platform.readBytesUart(buffer + 7, payloadLength + 2); //+1 for TCPI +1 for CRC
-        buffer[1] = buffer[6] & 0xF0;
-        buffer[6] = payloadLength;
-    }
-    else
-    {
-        //extended frame
-        _platform.readBytesUart(buffer + 1, 6);
-        sendAck((AddressType)(buffer[1] & GroupAddress), getWord(buffer + 4));
-        payloadLength = buffer[6];
-        _platform.readBytesUart(buffer + 7, payloadLength + 2); //+1 for TCPI +1 for CRC
-    }
-    len = payloadLength + 9;
-    
-    
-    const uint8_t queueLength = 5;
-    static uint8_t buffers[queueLength][bufferSize + 2];
-    static uint16_t bufferLengths[queueLength];
-
-    if (_sendBuffer != 0)
-    {
-        // we are trying to send a telegram queue received telegrams until we get our sent telegram
-
-        if (len == _sendBufferLength && memcmp(_sendBuffer, buffer, len) == 0)
-        {
-            //we got the send telegramm back next byte is L_Data.con byte
-            uint8_t confirm = _platform.readUart();
-            confirm &= L_DATA_CON_MASK;
-            _sendResult = (confirm > 0);
-            _sendBuffer = 0;
-            _sendBufferLength = 0;
-
-            return true;
-        }
-
-        // queue telegram, if we get more the queueLength before send succeeds 
-        // ignore the telegram
-        for (int i = 0; i < queueLength; i++)
-        {
-            if (bufferLengths[i] != 0)
-                continue;
-
-            bufferLengths[i] = len;
-            memcpy(&buffers[i][0], cemiBuffer, len + 2);
-            break;
-        }
-    }
-    else
-    {
-        // process all previously queued telegramms first
-        for (int i = 0; i < queueLength; i++)
-        {
-            if (bufferLengths[i] == 0)
-                break;
-
-            frameBytesReceived(&buffers[i][0], bufferLengths[i]+2);
-            bufferLengths[i] = 0;
-        }
-
-        frameBytesReceived(cemiBuffer, len+2);
-    }
-
-    return true;
-}
-
 void TpUartDataLinkLayer::frameBytesReceived(uint8_t* buffer, uint16_t length)
 {
     //printHex("=>", buffer, length);
@@ -268,133 +360,12 @@ void TpUartDataLinkLayer::frameBytesReceived(uint8_t* buffer, uint16_t length)
     frameRecieved(frame);
 }
 
-bool TpUartDataLinkLayer::checkDataCon(uint8_t firstByte)
+void TpUartDataLinkLayer::dataConBytesReceived(uint8_t* buffer, uint16_t length, bool success)
 {
-    uint8_t tmp = firstByte & L_DATA_CON_MASK;
-    if (tmp != L_DATA_CON)
-        return false;
+    //printHex("=>", buffer, length);
+    CemiFrame frame(buffer, length);
+    dataConReceived(frame,success);
 
-    if (_sendBuffer == 0)
-    {
-        println("got unexpected L_DATA_CON");
-        return true;
-    }
-
-    _sendResult = (firstByte & SUCCESS) > 0;
-    _sendBuffer = 0;
-    _sendBufferLength = 0;
-
-    return true;
-}
-
-bool TpUartDataLinkLayer::checkPollDataInd(uint8_t firstByte)
-{
-    if (firstByte != L_POLL_DATA_IND)
-        return false;
-
-    // not sure if this can happen
-    println("got L_POLL_DATA_IND");
-    return true;
-}
-
-bool TpUartDataLinkLayer::checkAckNackInd(uint8_t firstByte)
-{
-    uint8_t tmp = firstByte & L_ACKN_MASK;
-    if (tmp != L_ACKN_IND)
-        return false;
-
-    // this can only happen in bus monitor mode
-    println("got L_ACKN_IND");
-    return true;
-}
-
-bool TpUartDataLinkLayer::checkResetInd(uint8_t firstByte)
-{
-    if (firstByte != U_RESET_IND)
-        return false;
-
-    println("got U_RESET_IND");
-    return true;
-}
-
-bool TpUartDataLinkLayer::checkStateInd(uint8_t firstByte)
-{
-    uint8_t tmp = firstByte & U_STATE_IND;
-    if (tmp != U_STATE_IND)
-        return false;
-
-    print("got U_STATE_IND: 0x");
-    print(firstByte, HEX);
-    println();
-    return true;
-}
-
-bool TpUartDataLinkLayer::checkFrameStateInd(uint8_t firstByte)
-{
-    uint8_t tmp = firstByte & U_FRAME_STATE_MASK;
-    if (tmp != U_FRAME_STATE_IND)
-        return false;
-
-    print("got U_FRAME_STATE_IND: 0x");
-    print(firstByte, HEX);
-    println();
-    return true;
-}
-
-bool TpUartDataLinkLayer::checkConfigureInd(uint8_t firstByte)
-{
-    uint8_t tmp = firstByte & U_CONFIGURE_MASK;
-    if (tmp != U_CONFIGURE_IND)
-        return false;
-
-    print("got U_CONFIGURE_IND: 0x");
-    print(firstByte, HEX);
-    println();
-    return true;
-}
-
-bool TpUartDataLinkLayer::checkFrameEndInd(uint8_t firstByte)
-{
-    if (firstByte != U_FRAME_END_IND)
-        return false;
-
-    println("got U_FRAME_END_IND");
-    return true;
-}
-
-bool TpUartDataLinkLayer::checkStopModeInd(uint8_t firstByte)
-{
-    if (firstByte != U_STOP_MODE_IND)
-        return false;
-
-    println("got U_STOP_MODE_IND");
-    return true;
-}
-
-bool TpUartDataLinkLayer::checkSystemStatInd(uint8_t firstByte)
-{
-    if (firstByte != U_SYSTEM_STAT_IND)
-        return false;
-
-    print("got U_SYSTEM_STAT_IND: 0x");
-    while (true)
-    {
-        int tmp = _platform.readUart();
-        if (tmp < 0)
-            continue;
-
-        print(tmp, HEX);
-        break;
-    }
-    println();
-    return true;
-}
-
-void TpUartDataLinkLayer::handleUnexpected(uint8_t firstByte)
-{
-    print("got UNEXPECTED: 0x");
-    print(firstByte, HEX);
-    println();
 }
 
 void TpUartDataLinkLayer::enabled(bool value)
@@ -424,45 +395,74 @@ bool TpUartDataLinkLayer::enabled() const
 }
 
 
-void TpUartDataLinkLayer::sendBytes(uint8_t* bytes, uint16_t length)
-{
-    //printHex("<=", bytes, length);
+bool TpUartDataLinkLayer::sendSingleFrameByte(){
     uint8_t cmd[2];
+    uint8_t idx = _TxByteCnt / 64;
 
-    uint8_t oldIdx = 0;
-        
-    for (int i = 0; i < length; i++)
-    {
-        uint8_t idx = length / 64;
-        if (idx != oldIdx)
+    if(_sendBuffer == NULL) return false;
+
+    if(_TxByteCnt < _sendBufferLength){
+        if (idx != _oldIdx)
         {
-            oldIdx = idx;
+            _oldIdx = idx;
             cmd[0] = U_L_DATA_OFFSET_REQ | idx;
             _platform.writeUart(cmd, 1);
         }
 
-        if (i != length - 1)
-            cmd[0] = U_L_DATA_START_CONT_REQ | i;
+        if (_TxByteCnt != _sendBufferLength - 1)
+            cmd[0] = U_L_DATA_START_CONT_REQ | _TxByteCnt;
         else
-            cmd[0] = U_L_DATA_END_REQ | i;
+            cmd[0] = U_L_DATA_END_REQ | _TxByteCnt;
 
-        cmd[1] = bytes[i];
+        cmd[1] = _sendBuffer[_TxByteCnt];
 
         _platform.writeUart(cmd, 2);
+        _TxByteCnt++;
+        return true;
+    }
+    else{
+        _TxByteCnt=0;
+        return false;
     }
 }
 
-void TpUartDataLinkLayer::sendAck(AddressType type, uint16_t address)
-{
-    if ( (type == InduvidualAddress &&  _deviceObject.induvidualAddress() == address) 
-      || (type == GroupAddress &&  (_groupAddressTable.contains(address) || address == 0)))
-    {
-        //send ack. 
-        _platform.writeUart(U_ACK_REQ + 1);
+void TpUartDataLinkLayer::addFrameTxQueue(CemiFrame& frame){
+
+    _tx_queue_frame_t* tx_frame = new _tx_queue_frame_t;
+    tx_frame->length = frame.telegramLengthtTP();
+    tx_frame->data = new uint8_t[tx_frame->length];
+    tx_frame->next = NULL;
+    frame.fillTelegramTP(tx_frame->data);
+
+    if (_tx_queue.back == NULL) {
+        _tx_queue.front = _tx_queue.back = tx_frame;
     }
-    else
-    {
-        // send not addressed
-        _platform.writeUart(U_ACK_REQ);
+    else{
+        _tx_queue.back->next = tx_frame;
+        _tx_queue.back = tx_frame;
     }
 }
+
+bool TpUartDataLinkLayer::isTxQueueEmpty(){
+    if (_tx_queue.front == NULL){
+      return true;
+    }
+    return false;
+}
+
+
+void TpUartDataLinkLayer::loadNextTxFrame(){
+  if (_tx_queue.front == NULL) {
+    return;
+  }
+  _tx_queue_frame_t* tx_frame = _tx_queue.front;
+  _sendBuffer = tx_frame->data;
+  _sendBufferLength = tx_frame->length;
+  _tx_queue.front =  tx_frame->next;
+
+  if (_tx_queue.front == NULL){
+      _tx_queue.back = NULL;
+  }
+  delete tx_frame;
+}
+
diff --git a/src/knx/tpuart_data_link_layer.h b/src/knx/tpuart_data_link_layer.h
index 8ea40f1..fe1ccfe 100644
--- a/src/knx/tpuart_data_link_layer.h
+++ b/src/knx/tpuart_data_link_layer.h
@@ -3,6 +3,11 @@
 #include <stdint.h>
 #include "data_link_layer.h"
 
+#define MAX_KNX_TELEGRAM_SIZE            263
+#define BYTE_TIMEOUT                     3           //milli seconds
+#define CONFIRM_TIMEOUT                  500         //milli seconds
+
+
 class TpUartDataLinkLayer: public DataLinkLayer
 {
     using DataLinkLayer::_deviceObject;
@@ -17,25 +22,40 @@ public:
     bool enabled() const;
 private:
     bool _enabled = false;
+    bool _waitConfirm = false;
     uint8_t* _sendBuffer = 0;
-    bool _sendResult = false;
     uint16_t _sendBufferLength = 0;
+    uint8_t _receiveBuffer[MAX_KNX_TELEGRAM_SIZE];
+    uint8_t _loopState = 0;
+    uint16_t _RxByteCnt = 0;
+    uint16_t _TxByteCnt = 0;
+    uint8_t _oldIdx = 0;
+    bool _isEcho = false;
+    bool _isAddressed = false;
+    bool _convert = false;
+    uint8_t _xorSum=0;
+    uint32_t _lastByteRxTime;
+    uint32_t _waitConfirmStartTime;
+
+    struct _tx_queue_frame_t{
+        uint8_t *data;
+        uint16_t length;
+        _tx_queue_frame_t *next;
+    };
+
+    struct _tx_queue_t {
+      _tx_queue_frame_t *front=NULL;
+      _tx_queue_frame_t *back=NULL;
+    }_tx_queue;
+
+    void addFrameTxQueue(CemiFrame& frame);
+    bool isTxQueueEmpty();
+    void loadNextTxFrame();
+    bool sendSingleFrameByte();
     bool sendFrame(CemiFrame& frame);
-    bool checkDataInd(uint8_t firstByte);
-    bool checkDataCon(uint8_t firstByte);
-    bool checkPollDataInd(uint8_t firstByte);
-    bool checkAckNackInd(uint8_t firstByte);
-    bool checkResetInd(uint8_t firstByte);
-    bool checkStateInd(uint8_t firstByte);
-    bool checkFrameStateInd(uint8_t firstByte);
-    bool checkConfigureInd(uint8_t firstByte);
-    bool checkFrameEndInd(uint8_t firstByte);
-    bool checkStopModeInd(uint8_t firstByte);
-    bool checkSystemStatInd(uint8_t firstByte);
-    void handleUnexpected(uint8_t firstByte);
-    void sendBytes(uint8_t* buffer, uint16_t length);
     void frameBytesReceived(uint8_t* buffer, uint16_t length);
+    void dataConBytesReceived(uint8_t* buffer, uint16_t length, bool success);
     void resetChip();
     void stopChip();
-    void sendAck(AddressType type, uint16_t address);
-};
\ No newline at end of file
+};
+