diff --git a/cemi_frame.cpp b/cemi_frame.cpp
index 808dd0b..4bac662 100644
--- a/cemi_frame.cpp
+++ b/cemi_frame.cpp
@@ -60,6 +60,42 @@ uint16_t CemiFrame::totalLenght() const
     return tmp;
 }
 
+uint16_t CemiFrame::telegramLengthtTP() const
+{
+    if (frameType() == StandardFrame)
+        return totalLenght() - 2; /*-AddInfo -MsgCode - only one CTRL + CRC, */
+    else
+        return totalLenght() - 1; /*-AddInfo -MsgCode + CRC, */
+}
+
+void CemiFrame::fillTelegramTP(uint8_t* data)
+{
+    uint16_t len = telegramLengthtTP();
+    if (frameType() == StandardFrame)
+    {
+        uint8_t octet5 = (_ctrl1[1] & 0xF0) | (_ctrl1[6] & 0x0F);
+        data[0] = _ctrl1[0]; //CTRL
+        memcpy(data + 1, _ctrl1 + 2, 4); // SA, DA
+        data[5] = octet5; // LEN; Hopcount, ..
+        memcpy(data + 6, _ctrl1 + 8, len - 7); // APDU
+    }
+    else
+    {
+        memcpy(data, _ctrl1, len - 1);
+    }
+    data[len - 1] = calcCRC(data, len - 1);
+}
+
+uint8_t CemiFrame::calcCRC(uint8_t * buffer, uint16_t len)
+{
+    uint8_t crc = 0xFF;
+    
+    for (uint16_t i = 0; i < len; i++)
+        crc ^= buffer[i];
+    
+    return crc;
+}
+
 FrameFormat CemiFrame::frameType() const
 {
     return (FrameFormat)(_ctrl1[0] & StandardFrame);
diff --git a/cemi_frame.h b/cemi_frame.h
index be6f51f..8a2ed60 100644
--- a/cemi_frame.h
+++ b/cemi_frame.h
@@ -24,6 +24,8 @@ public:
     MessageCode messageCode() const;
     void messageCode(MessageCode value);
     uint16_t totalLenght() const;
+    uint16_t telegramLengthtTP() const;
+    void fillTelegramTP(uint8_t* data);
     
     FrameFormat frameType() const;
     void frameType(FrameFormat value);
@@ -48,6 +50,7 @@ public:
     TPDU& tpdu();
     APDU& apdu();
     
+    uint8_t calcCRC(uint8_t* buffer, uint16_t len);
     bool valid() const;
 private:
     uint8_t buffer[0xff + NPDU_LPDU_DIFF]; //only valid of add info is zero
diff --git a/tpuart_data_link_layer.cpp b/tpuart_data_link_layer.cpp
index 94f48f1..25cbb2e 100644
--- a/tpuart_data_link_layer.cpp
+++ b/tpuart_data_link_layer.cpp
@@ -17,10 +17,9 @@ TpUartDataLinkLayer::TpUartDataLinkLayer(DeviceObject& devObj, AddressTableObjec
 
 bool TpUartDataLinkLayer::sendFrame(CemiFrame& frame)
 {
-    uint16_t length = frame.totalLenght();
+    uint16_t length = frame.telegramLengthtTP();
     uint8_t* buffer = new uint8_t[length];
-
-    //TODO: Create TP standard frame or extended frame from cemi frame into buffer
+    frame.fillTelegramTP(buffer);
     
     bool success = sendBytes(buffer, length);