diff --git a/src/dbg_print.h b/src/dbg_print.h
index 177422e..1f204ba 100644
--- a/src/dbg_print.h
+++ b/src/dbg_print.h
@@ -9,9 +9,9 @@
 //-------------------------------------------------------------------------------
 // Debug print
 
-#if OPTION_ENABLE_DBG_PRINTS
+#ifdef XCP_ENABLE_DBG_PRINTS
 
-extern unsigned int gDebugLevel;
+extern uint8_t gDebugLevel;
 #define DBG_LEVEL gDebugLevel
 
 /*
diff --git a/src/main.h b/src/main.h
index 1332275..dc5068a 100644
--- a/src/main.h
+++ b/src/main.h
@@ -71,7 +71,7 @@
 #include <vector>
 #endif
 
-#define BOOL int
+#define BOOL uint8_t
 #define FALSE 0
 #define TRUE 1
 
diff --git a/src/platform.c b/src/platform.c
index 5343eea..ea59aef 100644
--- a/src/platform.c
+++ b/src/platform.c
@@ -18,6 +18,10 @@
 #include "platform.h"
 #include "dbg_print.h"
 
+#ifndef __MAIN_CFG_H__
+#error "Include dependency error!"
+#endif
+
 #if defined(_WIN) // Windows // Windows needs to link with Ws2_32.lib
 
 #pragma comment(lib, "ws2_32.lib")
@@ -189,6 +193,7 @@ void mutexDestroy(MUTEX* m) {
 // Sockets
 /**************************************************************************/
 
+#if defined(XCPTL_ENABLE_UDP) || defined(XCPTL_ENABLE_TCP)
 
 #ifdef _LINUX
 
@@ -237,7 +242,8 @@ BOOL socketBind(SOCKET sock, uint8_t* addr, uint16_t port) {
     return TRUE;
 }
 
-
+// Shutdown socket
+// Block rx and tx direction
 BOOL socketShutdown(SOCKET sock) {
     if (sock != INVALID_SOCKET) {
         shutdown(sock, SHUT_RDWR);
@@ -245,6 +251,8 @@ BOOL socketShutdown(SOCKET sock) {
     return TRUE;
 }
 
+// Close socket
+// Make addr reusable
 BOOL socketClose(SOCKET *sp) {
     if (*sp != INVALID_SOCKET) {
         close(*sp);
@@ -257,10 +265,10 @@ BOOL socketClose(SOCKET *sp) {
 
 #ifdef PLATFORM_ENABLE_GET_LOCAL_ADDR
 
-#include <ifaddrs.h>
 #ifndef __APPLE__
 #include <linux/if_packet.h>
 #else
+#include <ifaddrs.h>
 #include <net/if_dl.h>
 #endif
 
@@ -458,7 +466,8 @@ BOOL socketBind(SOCKET sock, uint8_t *addr, uint16_t port) {
     return TRUE;
 }
 
-
+// Shutdown socket
+// Block rx and tx direction
 BOOL socketShutdown(SOCKET sock) {
 
     if (sock != INVALID_SOCKET) {
@@ -467,6 +476,8 @@ BOOL socketShutdown(SOCKET sock) {
     return TRUE;
 }
 
+// Close socket
+// Make addr reusable
 BOOL socketClose(SOCKET* sockp) {
 
     if (*sockp != INVALID_SOCKET) {
@@ -648,6 +659,7 @@ int16_t socketSend(SOCKET sock, const uint8_t* buffer, uint16_t size) {
     return (int16_t)send(sock, (const char *)buffer, size, 0);
 }
 
+#endif
 
 /**************************************************************************/
 // Clock
diff --git a/src/platform.h b/src/platform.h
index a51ce6e..5b2c8af 100644
--- a/src/platform.h
+++ b/src/platform.h
@@ -4,9 +4,9 @@
 | Code released into public domain, no attribution required
 */
 
-
-#define PLATFORM_ENABLE_GET_LOCAL_ADDR
-#define PLATFORM_ENABLE_KEYBOARD
+#ifndef __MAIN_CFG_H__
+#error "Include dependency error!"
+#endif
 
 //-------------------------------------------------------------------------------
 // Keyboard
@@ -76,14 +76,14 @@ void mutexDestroy(MUTEX* m);
 typedef HANDLE tXcpThread;
 #define create_thread(h,t) *h = CreateThread(0, 0, t, NULL, 0, NULL)
 #define join_thread(h) WaitForSingleObject(h, INFINITE);
-#define terminate_thread(h) { TerminateThread(h,0); WaitForSingleObject(h,1000); CloseHandle(h); }
+#define cancel_thread(h) { TerminateThread(h,0); WaitForSingleObject(h,1000); CloseHandle(h); }
 
 #elif defined(_LINUX) // Linux
 
 typedef pthread_t tXcpThread;
 #define create_thread(h,t) pthread_create(h, NULL, t, NULL);
 #define join_thread(h) pthread_join(h,NULL);
-#define detach_thread(h) { pthread_detach(h); pthread_cancel(h); }
+#define cancel_thread(h) { pthread_detach(h); pthread_cancel(h); }
 
 #endif
 
@@ -91,6 +91,8 @@ typedef pthread_t tXcpThread;
 //-------------------------------------------------------------------------------
 // Platform independant socket functions
 
+#if defined(XCPTL_ENABLE_UDP) || defined(XCPTL_ENABLE_TCP)
+
 #ifdef _LINUX // Linux sockets
 
 #define SOCKET int
@@ -153,6 +155,8 @@ extern BOOL socketClose(SOCKET* sp);
 extern BOOL socketGetLocalAddr(uint8_t* mac, uint8_t* addr);
 #endif
 
+#endif
+
 //-------------------------------------------------------------------------------
 // Clock
 
diff --git a/src/xcpEthServer.c b/src/xcpEthServer.c
index 2fc2148..9c3aed1 100644
--- a/src/xcpEthServer.c
+++ b/src/xcpEthServer.c
@@ -18,6 +18,11 @@
 #include "xcpLite.h"   
 #include "xcpEthServer.h"
 
+#ifndef __MAIN_CFG_H__
+#error "Include dependency error!"
+#endif
+
+#if defined(XCPTL_ENABLE_UDP) || defined(XCPTL_ENABLE_TCP)
 
 #if defined(_WIN) // Windows
 static DWORD WINAPI XcpServerReceiveThread(LPVOID lpParameter);
@@ -36,9 +41,9 @@ static struct {
     BOOL isInit; 
 
     // Threads
-    tXcpThread DAQThreadHandle;
+    tXcpThread TransmitThreadHandle;
     volatile BOOL TransmitThreadRunning;
-    tXcpThread CMDThreadHandle;
+    tXcpThread ReceiveThreadHandle;
     volatile BOOL ReceiveThreadRunning;
 
 } gXcpServer;
@@ -51,7 +56,7 @@ BOOL XcpEthServerStatus() {
 
 
 // XCP server init
-BOOL XcpEthServerInit(const uint8_t* addr, uint16_t port, BOOL useTCP, uint16_t segmentSize)
+BOOL XcpEthServerInit(const uint8_t* addr, uint16_t port, BOOL useTCP)
 {
     int r = 0;
 
@@ -61,22 +66,22 @@ BOOL XcpEthServerInit(const uint8_t* addr, uint16_t port, BOOL useTCP, uint16_t
     // Init network sockets
     if (!socketStartup()) return FALSE;
     
-    gXcpServer.TransmitThreadRunning = 0;
-    gXcpServer.ReceiveThreadRunning = 0;
+    gXcpServer.TransmitThreadRunning = FALSE;
+    gXcpServer.ReceiveThreadRunning = FALSE;
 
     // Initialize XCP protocol layer if not already done
     XcpInit();
 
     // Initialize XCP transport layer
-    r = XcpEthTlInit(addr, port, useTCP, segmentSize, TRUE /*blocking rx*/);
+    r = XcpEthTlInit(addr, port, useTCP, TRUE /*blocking rx*/);
     if (!r) return 0;
 
     // Start XCP protocol layer
     XcpStart();
 
     // Create threads
-    create_thread(&gXcpServer.DAQThreadHandle, XcpServerTransmitThread);
-    create_thread(&gXcpServer.CMDThreadHandle, XcpServerReceiveThread);
+    create_thread(&gXcpServer.TransmitThreadHandle, XcpServerTransmitThread);
+    create_thread(&gXcpServer.ReceiveThreadHandle, XcpServerReceiveThread);
 
     gXcpServer.isInit = TRUE;
     return TRUE;
@@ -84,16 +89,32 @@ BOOL XcpEthServerInit(const uint8_t* addr, uint16_t port, BOOL useTCP, uint16_t
 
 BOOL XcpEthServerShutdown() {
 
+#ifdef XCP_SERVER_FORCEFULL_TERMINATION
+    // Forcefull termination
+    if (gXcpServer.isInit) {
+        DBG_PRINT3("Disconnect, cancel threads and shutdown XCP!\n");
+        XcpDisconnect();
+        cancel_thread(gXcpServer.ReceiveThreadHandle);
+        cancel_thread(gXcpServer.TransmitThreadHandle);
+        XcpEthTlShutdown();
+        gXcpServer.isInit = FALSE;
+        socketCleanup();
+        XcpReset();
+    }
+#else
+    // Gracefull termination
     if (gXcpServer.isInit) {
         XcpDisconnect();
         gXcpServer.ReceiveThreadRunning = FALSE;
         gXcpServer.TransmitThreadRunning = FALSE;
         XcpEthTlShutdown();
-        join_thread(gXcpServer.CMDThreadHandle);
-        join_thread(gXcpServer.DAQThreadHandle);
+        join_thread(gXcpServer.ReceiveThreadHandle);
+        join_thread(gXcpServer.TransmitThreadHandle);
         gXcpServer.isInit = FALSE;
         socketCleanup();
+        XcpReset();
     }
+#endif
     return TRUE;
 }
 
@@ -156,3 +177,5 @@ extern void* XcpServerTransmitThread(void* par)
     return 0;
 }
 
+
+#endif
\ No newline at end of file
diff --git a/src/xcpEthServer.h b/src/xcpEthServer.h
index f33f653..27c2546 100644
--- a/src/xcpEthServer.h
+++ b/src/xcpEthServer.h
@@ -4,7 +4,10 @@
 /* Copyright(c) Vector Informatik GmbH.All rights reserved.
    Licensed under the MIT license.See LICENSE file in the project root for details. */
 
-extern BOOL XcpEthServerInit(const uint8_t *addr, uint16_t port, BOOL useTCP, uint16_t segmentSize);
+#if defined(XCPTL_ENABLE_UDP) || defined(XCPTL_ENABLE_TCP)
+
+extern BOOL XcpEthServerInit(const uint8_t *addr, uint16_t port, BOOL useTCP);
 extern BOOL XcpEthServerShutdown();
 extern BOOL XcpEthServerStatus();
 
+#endif
diff --git a/src/xcpEthTl.c b/src/xcpEthTl.c
index 5e33bbe..4144ea3 100644
--- a/src/xcpEthTl.c
+++ b/src/xcpEthTl.c
@@ -50,7 +50,7 @@ message = len + ctr + (protocol layer packet) + fill
 typedef struct {
     uint16_t uncommited;        // Number of uncommited messages in this segment
     uint16_t size;              // Number of overall bytes in this segment
-    uint8_t msg[XCPTL_MAX_SEGMENT_SIZE];  // Segment/MTU - concatenated transport layer messages
+    uint8_t msg[XCPTL_MAX_SEGMENT_SIZE];  // Segment (UDP MTU) - concatenated transport layer messages
 } tXcpMessageBuffer;
 
 
@@ -58,8 +58,6 @@ static struct {
 
     // Generic transport layer 
     int32_t lastError;
-    int (*sendCallback)(uint16_t msgLen, const uint8_t* msgBuf); // Callback to send a message to the application
-    uint16_t SegmentSize; // Maximum size of a XCP transportlayer segment
     uint16_t lastCroCtr; // Last CRO command receive object message message counter received
     uint16_t ctr; // next DAQ DTO data transmit message packet counter
 
@@ -74,14 +72,16 @@ static struct {
     tXcpMessageBuffer* msg_ptr; // current incomplete or not fully commited segment
     MUTEX Mutex_Queue;
     
+#if defined(XCPTL_ENABLE_UDP) || defined(XCPTL_ENABLE_TCP)
+
     // Ethernet
     SOCKET Sock;
-#ifdef XCPTL_ENABLE_TCP
-    SOCKET ListenSock;
-#endif
-#ifdef PLATFORM_ENABLE_GET_LOCAL_ADDR
-    uint8_t ServerMac[6];
-#endif
+    #ifdef XCPTL_ENABLE_TCP
+        SOCKET ListenSock;
+    #endif
+    #ifdef PLATFORM_ENABLE_GET_LOCAL_ADDR
+        uint8_t ServerMac[6];
+    #endif
     uint8_t ServerAddr[4];
     uint16_t ServerPort;
     BOOL ServerUseTCP;
@@ -91,15 +91,17 @@ static struct {
     BOOL MasterAddrValid;
 
     // Multicast
-#ifdef XCPTL_ENABLE_MULTICAST
-    tXcpThread MulticastThreadHandle;
-    SOCKET MulticastSock;
+    #ifdef XCPTL_ENABLE_MULTICAST
+        tXcpThread MulticastThreadHandle;
+        SOCKET MulticastSock;
+    #endif
+
 #endif
 
 } gXcpTl;
 
 
-#if defined XCPTL_ENABLE_TCP && defined XCPTL_ENABLE_UDP
+#if defined(XCPTL_ENABLE_TCP) && defined(XCPTL_ENABLE_UDP)
 #define isTCP() (gXcpTl.ListenSock != INVALID_SOCKET)
 #else
 #ifdef XCPTL_ENABLE_TCP
@@ -114,55 +116,105 @@ static struct {
 static int handleXcpMulticastCommand(int n, tXcpCtoMessage* p, uint8_t* dstAddr, uint16_t dstPort);
 #endif
 
-//------------------------------------------------------------------------------
 
-// Transmit a UDP datagramm or TCP segment (contains multiple XCP DTO messages or a single CRM message (len+ctr+packet+fill))
-// Must be thread safe, because it is called from CMD and from DAQ thread
-// Returns -1 on would block, 1 if ok, 0 on error
-static int sendEthDatagram(const uint8_t *data, uint16_t size, const uint8_t* addr, uint16_t port) {
 
-    int r;
+//-------------------------------------------------------------------------------------------------------
 
-#ifdef XCPTL_ENABLE_TCP
-    if (isTCP()) {
-      r = socketSend(gXcpTl.Sock, data, size);
+static void XcpTlInitTransmitQueue();
+
+
+//-------------------------------------------------------------------------------------------------------
+// Generic transport layer functions
+
+BOOL XcpTlInit() {
+
+    gXcpTl.lastCroCtr = 0;
+    gXcpTl.ctr = 0;
+    mutexInit(&gXcpTl.Mutex_Queue, FALSE, 1000);
+    XcpTlInitTransmitQueue();
+#if defined(_WIN) // Windows
+    gXcpTl.queue_event = CreateEvent(NULL, TRUE, FALSE, NULL);
+    assert(gXcpTl.queue_event!=NULL); 
+    gXcpTl.queue_event_time = 0;
+#endif
+    
+    DBG_PRINT3("\nInit XCP transport layer\n");
+    DBG_PRINTF3("  SEGMENT_SIZE=%u, MAX_CTO_SIZE=%u, QUEUE_SIZE=%u, ALIGNMENT=%u, %uKiB memory used\n", XCPTL_MAX_SEGMENT_SIZE, XCPTL_MAX_CTO_SIZE, XCPTL_QUEUE_SIZE, XCPTL_PACKET_ALIGNMENT, (unsigned int)sizeof(gXcpTl) / 1024);
+    DBG_PRINT3("  Note: These parameters in xcptl_cfg.h need to be configured for optimal memory consumption and performance!\n");
+#ifdef XCPTL_ENABLE_MULTICAST
+    DBG_PRINT3("        Option ENABLE_MULTICAST is not recommended\n");
+#endif
+#ifndef XCPTL_QUEUED_CRM
+    DBG_PRINT3("        Option QUEUED_CRM is disabled, enabled is recommended\n");
+#endif
+   
+    
+    return TRUE;
+}
+
+void XcpTlShutdown() {
+
+    mutexDestroy(&gXcpTl.Mutex_Queue);
+#if defined(_WIN) // Windows
+    CloseHandle(gXcpTl.queue_event);
+#endif
+}
+
+
+#if defined(XCPTL_QUEUED_CRM) && !defined(XCPTL_QUEUED_CRM_OPT)
+
+// Queue a response or event packet
+// Must be thread save, if XcpPrint used, XcpCommand is never called from multiple threads
+// If transmission fails, when queue is full, tool times out, retries or take appropriate action
+// Note: CANape cancels measurement, when answer to GET_DAQ_CLOCK times out
+void XcpTlSendCrm(const uint8_t* packet, uint16_t packet_size) {
+
+    void* handle = NULL;
+    uint8_t* p;
+
+    // Queue the response packet
+    if ((p = XcpTlGetTransmitBuffer(&handle, packet_size)) != NULL) {
+        memcpy(p, packet, packet_size);
+        XcpTlCommitTransmitBuffer(handle, TRUE /* flush */);
     }
-    else
+    else { // Buffer overflow
+        DBG_PRINT_WARNING("WARNING: queue overflow\n");
+        // Ignore, handled by tool
+    }
+}
+
 #endif
 
-#ifdef XCPTL_ENABLE_UDP
-    {
-      if (addr != NULL) { // Respond to given addr and port (used for multicast)
-        r = socketSendTo(gXcpTl.Sock, data, size, addr, port, NULL);
-      }
-      else { // Respond to active master
-        if (!gXcpTl.MasterAddrValid) {
-          DBG_PRINT_ERROR("ERROR: invalid master address!\n");
-          gXcpTl.lastError = XCPTL_ERROR_INVALID_MASTER;
-          return 0;
-        }
-        r = socketSendTo(gXcpTl.Sock, data, size, gXcpTl.MasterAddr, gXcpTl.MasterPort, NULL);
-      }
+// Execute XCP command
+// Returns XCP error code
+uint8_t XcpTlCommand( uint16_t msgLen, const uint8_t* msgBuf) {
+
+    BOOL connected = XcpIsConnected();
+    tXcpCtoMessage* p = (tXcpCtoMessage*)msgBuf;
+    assert(msgLen>=p->dlc+XCPTL_TRANSPORT_LAYER_HEADER_SIZE);
+
+    /* Connected */
+    if (connected) {
+        return XcpCommand((const uint32_t*)&p->packet[0], p->dlc); // Handle command
     }
-#endif // UDP
 
-    if (r != size) {
-        if (socketGetLastError()==SOCKET_ERROR_WBLOCK) {
-            gXcpTl.lastError = XCPTL_ERROR_WOULD_BLOCK;
-            return -1; // Would block
+    /* Not connected yet */
+    else {
+        /* Check for CONNECT command ? */
+        if (p->dlc == 2 && p->packet[0] == CC_CONNECT) {
+            XcpTlInitTransmitQueue();
+            return XcpCommand((const uint32_t*)&p->packet[0],p->dlc); // Handle CONNECT command
         }
         else {
-            DBG_PRINTF_ERROR("ERROR: sento failed (result=%d, errno=%d)!\n", r, socketGetLastError());
-            gXcpTl.lastError = XCPTL_ERROR_SEND_FAILED;
-            return 0; // Error
+            DBG_PRINTF_WARNING("WARNING: XcpTlCommand: no valid CONNECT command, dlc=%u, data=%02X\n", p->dlc, p->packet[0]);
+            return CRC_CMD_SYNTAX;
         }
-    }
 
-    return 1; // Ok
+    }
 }
 
 
-//------------------------------------------------------------------------------
+//-------------------------------------------------------------------------------------------------------
 // XCP (UDP or TCP) transport layer segment/message/packet queue (DTO buffers)
 
 // Notify transmit queue handler thread
@@ -220,66 +272,6 @@ static void XcpTlInitTransmitQueue() {
     assert(gXcpTl.msg_ptr);
 }
 
-// Transmit all completed and fully commited UDP frames
-// Returns number of bytes sent or -1 on error
-int32_t XcpTlHandleTransmitQueue( void ) {
-
-    const uint32_t max_loops = 20; // maximum number of packets to send without sleep(0) 
-
-    tXcpMessageBuffer* b = NULL;
-    int32_t n = 0;
-
-    for (;;) {
-      for (uint32_t i = 0; i < max_loops; i++) {
-
-        // Check
-        mutexLock(&gXcpTl.Mutex_Queue);
-        if (gXcpTl.queue_len > 1) {
-          b = &gXcpTl.queue[gXcpTl.queue_rp];
-          if (b->uncommited > 0) b = NULL; // return when reaching a not fully commited segment buffer 
-        }
-        else {
-          b = NULL;
-        }
-        mutexUnlock(&gXcpTl.Mutex_Queue);
-        if (b == NULL) break; // Ok, queue is empty or not fully commited
-        assert(b->size!=0); 
-
-        // Send this frame
-        int r;
-        if (gXcpTl.sendCallback != NULL) {
-            r = gXcpTl.sendCallback(b->size, &b->msg[0]);
-        }
-        else {
-            // Send the frame (UDP or TCP)
-            r = sendEthDatagram(&b->msg[0], b->size, NULL, 0);
-        }
-        if (r == (-1)) { // would block
-          b = NULL;
-          break; 
-        }
-        if (r == 0) { // error
-          return -1; 
-        }
-        n += b->size;
-
-        // Free this buffer when succesfully sent
-        mutexLock(&gXcpTl.Mutex_Queue);
-        if (++gXcpTl.queue_rp >= XCPTL_QUEUE_SIZE) gXcpTl.queue_rp = 0;
-        gXcpTl.queue_len--;
-        mutexUnlock(&gXcpTl.Mutex_Queue);
-
-      } // for (max_loops)
-
-      if (b == NULL) break; // queue is empty
-      sleepMs(0);
-
-    } // for (ever)
-
-    return n; // Ok, queue empty now
-}
-
-
 // Get transmit queue level
 int32_t XcpTlGetTransmitQueueLevel() {
   return gXcpTl.queue_len; 
@@ -301,14 +293,14 @@ uint8_t *XcpTlGetTransmitBuffer(void **handlep, uint16_t packet_size) {
     packet_size = (uint16_t)((packet_size + 3) & 0xFFFC); // Add fill %4
 #endif
     msg_size = (uint16_t)(packet_size + XCPTL_TRANSPORT_LAYER_HEADER_SIZE);
-    if (msg_size > gXcpTl.SegmentSize) {
+    if (msg_size > XCPTL_MAX_SEGMENT_SIZE) {
         return NULL; // Overflow, should never happen in correct DAQ setups
     }
 
     mutexLock(&gXcpTl.Mutex_Queue);
 
     // Get another message buffer from queue, when active buffer ist full
-    if (gXcpTl.msg_ptr==NULL || (uint16_t)(gXcpTl.msg_ptr->size + msg_size) > gXcpTl.SegmentSize) {
+    if (gXcpTl.msg_ptr==NULL || (uint16_t)(gXcpTl.msg_ptr->size + msg_size) > XCPTL_MAX_SEGMENT_SIZE) {
         getSegmentBuffer();
     }
 
@@ -373,7 +365,146 @@ void XcpTlWaitForTransmitQueueEmpty() {
 }
 
 
+// Check if there is a fully commited segment buffer in the transmit queue
+const uint8_t * XcpTlTransmitQueuePeek( uint16_t* msg_len) {
+
+    tXcpMessageBuffer* b = NULL;
+    
+    // Check there is a commited entry
+    mutexLock(&gXcpTl.Mutex_Queue);
+    if (gXcpTl.queue_len > 1) {
+        b = &gXcpTl.queue[gXcpTl.queue_rp];
+        if (b->uncommited > 0) b = NULL; // return when reaching a not fully commited segment buffer 
+    }
+    else {
+        b = NULL;
+    }
+    mutexUnlock(&gXcpTl.Mutex_Queue);
+    if (b == NULL) return NULL; // Qqueue is empty or not fully commited
+    assert(b->size!=0); 
+    *msg_len = b->size;
+    return &b->msg[0]; 
+}
+
+// Remove the next transmit queue entry
+void XcpTlTransmitQueueNext() {
+  
+    mutexLock(&gXcpTl.Mutex_Queue);
+    if (gXcpTl.queue_len > 1 && gXcpTl.queue[gXcpTl.queue_rp].uncommited == 0) {
+        if (++gXcpTl.queue_rp >= XCPTL_QUEUE_SIZE) gXcpTl.queue_rp = 0;
+        gXcpTl.queue_len--;
+    }
+    mutexUnlock(&gXcpTl.Mutex_Queue);
+}
+
+
+
 //------------------------------------------------------------------------------
+// Ethernet transport layer socket functions
+
+#if defined(XCPTL_ENABLE_UDP) || defined(XCPTL_ENABLE_TCP)
+
+// Transmit a UDP datagramm or TCP segment (contains multiple XCP DTO messages or a single CRM message (len+ctr+packet+fill))
+// Must be thread safe, because it is called from CMD and from DAQ thread
+// Returns -1 on would block, 1 if ok, 0 on error
+static int sendEthDatagram(const uint8_t *data, uint16_t size, const uint8_t* addr, uint16_t port) {
+
+    int r;
+
+#ifdef XCPTL_ENABLE_TCP
+    if (isTCP()) {
+      r = socketSend(gXcpTl.Sock, data, size);
+    }
+    else
+#endif
+
+#ifdef XCPTL_ENABLE_UDP
+    {
+      if (addr != NULL) { // Respond to given addr and port (used for multicast)
+        r = socketSendTo(gXcpTl.Sock, data, size, addr, port, NULL);
+      }
+      else { // Respond to active master
+        if (!gXcpTl.MasterAddrValid) {
+          DBG_PRINT_ERROR("ERROR: invalid master address!\n");
+          gXcpTl.lastError = XCPTL_ERROR_INVALID_MASTER;
+          return 0;
+        }
+        r = socketSendTo(gXcpTl.Sock, data, size, gXcpTl.MasterAddr, gXcpTl.MasterPort, NULL);
+      }
+    }
+#endif // UDP
+
+    if (r != size) {
+        if (socketGetLastError()==SOCKET_ERROR_WBLOCK) {
+            gXcpTl.lastError = XCPTL_ERROR_WOULD_BLOCK;
+            return -1; // Would block
+        }
+        else {
+            DBG_PRINTF_ERROR("ERROR: sendEthDatagram: send failed (result=%d, errno=%d)!\n", r, socketGetLastError());
+            gXcpTl.lastError = XCPTL_ERROR_SEND_FAILED;
+            return 0; // Error
+        }
+    }
+
+    return 1; // Ok
+}
+
+
+// Transmit all completed and fully commited UDP frames
+// Returns number of bytes sent or -1 on error
+int32_t XcpTlHandleTransmitQueue() {
+
+    const uint32_t max_loops = 20; // maximum number of packets to send without sleep(0) 
+
+    tXcpMessageBuffer* b = NULL;
+    int32_t n = 0;
+
+    for (;;) {
+      for (uint32_t i = 0; i < max_loops; i++) {
+
+        // Check
+        mutexLock(&gXcpTl.Mutex_Queue);
+        if (gXcpTl.queue_len > 1) {
+          b = &gXcpTl.queue[gXcpTl.queue_rp];
+          if (b->uncommited > 0) b = NULL; // return when reaching a not fully commited segment buffer 
+        }
+        else {
+          b = NULL;
+        }
+        mutexUnlock(&gXcpTl.Mutex_Queue);
+        if (b == NULL) break; // Ok, queue is empty or not fully commited
+        assert(b->size!=0); 
+
+        // Send this frame
+        int r = sendEthDatagram(&b->msg[0], b->size, NULL, 0);
+        
+        if (r == (-1)) { // would block
+          b = NULL;
+          break; 
+        }
+        if (r == 0) { // error
+          return -1; 
+        }
+        n += b->size;
+
+        // Free this buffer when succesfully sent
+        mutexLock(&gXcpTl.Mutex_Queue);
+        if (++gXcpTl.queue_rp >= XCPTL_QUEUE_SIZE) gXcpTl.queue_rp = 0;
+        gXcpTl.queue_len--;
+        mutexUnlock(&gXcpTl.Mutex_Queue);
+
+      } // for (max_loops)
+
+      if (b == NULL) break; // queue is empty
+      sleepMs(0);
+
+    } // for (ever)
+
+    return n; // Ok, queue empty now
+}
+
+
+#if !defined(XCPTL_QUEUED_CRM) || defined(XCPTL_QUEUED_CRM_OPT)
 
 // Transmit XCP response or event packet
 // No error handling in protocol layer
@@ -388,6 +519,7 @@ void XcpTlSendCrm(const uint8_t* packet, uint16_t packet_size) {
     uint8_t* p;
     int r = 0;
 
+#ifdef XCPTL_QUEUED_CRM_OPT
     // If transmit queue is empty, save the space and transmit instantly
     mutexLock(&gXcpTl.Mutex_Queue);
     if (gXcpTl.queue_len <= 1 && (gXcpTl.msg_ptr == NULL || gXcpTl.msg_ptr->size == 0)) {
@@ -401,15 +533,11 @@ void XcpTlSendCrm(const uint8_t* packet, uint16_t packet_size) {
         msg_size = packet_size;
         msg.dlc = (uint16_t)msg_size;
         msg_size = (uint16_t)(msg_size + XCPTL_TRANSPORT_LAYER_HEADER_SIZE);
-        if (gXcpTl.sendCallback != NULL) {
-            r = gXcpTl.sendCallback(msg_size, (uint8_t*)&msg);
-        }
-        else {
-            r = sendEthDatagram((uint8_t*)&msg.dlc, msg_size, NULL, 0);
-        }
+        r = sendEthDatagram((uint8_t*)&msg.dlc, msg_size, NULL, 0);
     }
     mutexUnlock(&gXcpTl.Mutex_Queue);
     if (r == 1) return; // ok
+#endif // XCPTL_QUEUED_CRM_OPT
 
     // Queue the response packet
     if ((p = XcpTlGetTransmitBuffer(&handle, packet_size)) != NULL) {
@@ -437,6 +565,12 @@ void XcpTlSendCrm(const uint8_t* packet, uint16_t packet_size) {
 #endif
 }
 
+#endif // !defined(XCPTL_QUEUED_CRM) || defined(XCPTL_QUEUED_CRM_OPT)
+
+
+//------------------------------------------------------------------------------
+
+
 // Transmit XCP multicast response
 void XcpEthTlSendMulticastCrm(const uint8_t* packet, uint16_t packet_size, const uint8_t* addr, uint16_t port) {
 
@@ -453,6 +587,10 @@ void XcpEthTlSendMulticastCrm(const uint8_t* packet, uint16_t packet_size, const
   }
 }
 
+
+//------------------------------------------------------------------------------
+
+
 static int handleXcpCommand(int n, tXcpCtoMessage *p, uint8_t *srcAddr, uint16_t srcPort) {
 
     int connected;
@@ -511,7 +649,7 @@ static int handleXcpCommand(int n, tXcpCtoMessage *p, uint8_t *srcAddr, uint16_t
             XcpCommand((const uint32_t*)&p->packet[0],p->dlc); // Handle CONNECT command
         }
         else {
-            DBG_PRINT_WARNING("WARNING: no valid CONNECT command\n");
+            DBG_PRINT_WARNING("WARNING: handleXcpCommand: no valid CONNECT command\n");
         }
 
     }
@@ -570,7 +708,7 @@ BOOL XcpEthTlHandleCommands(uint32_t timeout_ms) {
                     return handleXcpCommand(n, &msgBuf, NULL, 0);
                 }
                 else {
-                    socketShutdown(gXcpTl.Sock);
+                    socketShutdown(gXcpTl.Sock); // Let the receive thread terminate without error message
                     return FALSE;  // Should not happen
                 }
             }
@@ -579,7 +717,7 @@ BOOL XcpEthTlHandleCommands(uint32_t timeout_ms) {
             DBG_PRINT3("XCP Master closed TCP connection! XCP disconnected.\n");
             XcpDisconnect();
             sleepMs(100);
-            socketShutdown(gXcpTl.Sock);
+            socketShutdown(gXcpTl.Sock); // Let the receive thread terminate without error message
             socketClose(&gXcpTl.Sock);
             return TRUE; // Ok, TCP socket closed
         }
@@ -679,38 +817,9 @@ extern void* XcpTlMulticastThread(void* par)
 
 //-------------------------------------------------------------------------------------------------------
 
-BOOL XcpTlInit(uint16_t segmentSize, int (*cb)(uint16_t msgLen, const uint8_t* msgBuf)) {
+BOOL XcpEthTlInit(const uint8_t* addr, uint16_t port, BOOL useTCP, BOOL blockingRx) {
 
-    if (segmentSize > XCPTL_MAX_SEGMENT_SIZE) return FALSE;
-    gXcpTl.SegmentSize = segmentSize;
-    gXcpTl.sendCallback = cb; // Callback to send a message to the application return 1 if ok, 0 on error, -1 on would block
-    gXcpTl.lastCroCtr = 0;
-    gXcpTl.ctr = 0;
-    mutexInit(&gXcpTl.Mutex_Queue, FALSE, 1000);
-    XcpTlInitTransmitQueue();
-#if defined(_WIN) // Windows
-    gXcpTl.queue_event = CreateEvent(NULL, TRUE, FALSE, NULL);
-    assert(gXcpTl.queue_event!=NULL); 
-    gXcpTl.queue_event_time = 0;
-#endif
-    
-    DBG_PRINT3("\nInit XCP transport layer\n");
-    DBG_PRINTF3("  SEGMENT_SIZE=%u, MAX_CTO_SIZE=%u, QUEUE_SIZE=%u, ALIGNMENT=%u, %uKiB memory used\n", segmentSize, XCPTL_MAX_CTO_SIZE, XCPTL_QUEUE_SIZE, XCPTL_PACKET_ALIGNMENT, (unsigned int)sizeof(gXcpTl) / 1024);
-    DBG_PRINT3("  Options=("); // Print activated XCP transport layer options  
-#ifdef XCPTL_ENABLE_MULTICAST
-    DBG_PRINT3("ENABLE_MULTICAST,");
-#endif
-#ifdef XCPTL_QUEUED_CRM
-    DBG_PRINT3("QUEUED_CRM,");
-#endif
-    DBG_PRINT3(")\n");
-    
-    return TRUE;
-}
-
-BOOL XcpEthTlInit(const uint8_t* addr, uint16_t port, BOOL useTCP, uint16_t segmentSize, BOOL blockingRx) {
-
-    if (!XcpTlInit(segmentSize,NULL)) return FALSE;
+    if (!XcpTlInit()) return FALSE;
 
     if (addr != 0)  { // Bind to given addr 
         memcpy(gXcpTl.ServerAddr, addr, 4);
@@ -774,14 +883,6 @@ BOOL XcpEthTlInit(const uint8_t* addr, uint16_t port, BOOL useTCP, uint16_t segm
 }
 
 
-void XcpTlShutdown() {
-
-    mutexDestroy(&gXcpTl.Mutex_Queue);
-#if defined(_WIN) // Windows
-    CloseHandle(gXcpTl.queue_event);
-#endif
-}
-
 void XcpEthTlShutdown() {
 
     // Close all sockets to enable all threads to terminate
@@ -847,34 +948,8 @@ void XcpEthTlGetInfo(BOOL* isTcp, uint8_t* mac, uint8_t* addr, uint16_t *port) {
 }
 #endif
 
-//-------------------------------------------------------------------------------------------------------
-
-// Execute XCP command from procedure call
-// Returns FALSE on error
-uint8_t XcpTlCommand( uint16_t msgLen, const uint8_t* msgBuf) {
 
-    BOOL connected = XcpIsConnected();
-    tXcpCtoMessage* p = (tXcpCtoMessage*)msgBuf;
-    assert (msgLen>=p->dlc+XCPTL_TRANSPORT_LAYER_HEADER_SIZE);
+#endif  // defined(XCPTL_ENABLE_UDP) || defined(XCPTL_ENABLE_TCP)
 
-    /* Connected */
-    if (connected) {
-        return XcpCommand((const uint32_t*)&p->packet[0], p->dlc); // Handle command
-    }
-
-    /* Not connected yet */
-    else {
-        /* Check for CONNECT command ? */
-        if (p->dlc == 2 && p->packet[0] == CC_CONNECT) {
-            XcpTlInitTransmitQueue();
-            return XcpCommand((const uint32_t*)&p->packet[0],p->dlc); // Handle CONNECT command
-        }
-        else {
-            DBG_PRINT_WARNING("WARNING: no valid CONNECT command\n");
-            return CRC_CMD_SYNTAX;
-        }
-
-    }
-}
 
 
diff --git a/src/xcpEthTl.h b/src/xcpEthTl.h
index 789eb88..1e62476 100644
--- a/src/xcpEthTl.h
+++ b/src/xcpEthTl.h
@@ -4,17 +4,36 @@
 /* Copyright(c) Vector Informatik GmbH.All rights reserved.
    Licensed under the MIT license.See LICENSE file in the project root for details. */
 
-/* ETH transport Layer functions called by application */
-extern BOOL XcpEthTlInit(const uint8_t* addr, uint16_t port, BOOL useTCP, uint16_t segmentSize, BOOL blockingRx); // Start transport layer
+
+/* ETH transport Layer functions called by server */
+#if defined(XCPTL_ENABLE_UDP) || defined(XCPTL_ENABLE_TCP)
+
+extern BOOL XcpEthTlInit(const uint8_t* addr, uint16_t port, BOOL useTCP, BOOL blockingRx); // Start transport layer
 extern void XcpEthTlShutdown();
 #ifdef PLATFORM_ENABLE_GET_LOCAL_ADDR
 extern void XcpEthTlGetInfo(BOOL* isTCP, uint8_t* mac, uint8_t* addr, uint16_t* port);
 #endif
 
-/* ETH transport Layer functions called by XCPlite.c */
+/* ETH transport Layer functions called by server */
 extern BOOL XcpEthTlHandleCommands(uint32_t timeout_ms); // Handle all incoming XCP commands, (wait for at least timeout_ms)
+
+// All other network specific application functions functions are declared in xcpCanTl.h or xcpEthTl.h
+extern int32_t XcpTlHandleTransmitQueue(); // Send all outgoing packets in the transmit queue
+extern BOOL XcpTlWaitForTransmitData(uint32_t timeout_ms); // Wait for at least timeout_ms, until packets are pending in the transmit queue
+
+/* ETH transport Layer functions called by protocol layer */
+#ifdef PLATFORM_ENABLE_GET_LOCAL_ADDR
 extern void XcpEthTlSendMulticastCrm(const uint8_t* data, uint16_t n, const uint8_t* addr, uint16_t port); // Send multicast command response
+#endif
 #ifdef XCPTL_ENABLE_MULTICAST
 extern void XcpEthTlSetClusterId(uint16_t clusterId); // Set cluster id for GET_DAQ_CLOCK_MULTICAST reception
 #endif
 
+// Get last error code
+#define XCPTL_OK                   0
+#define XCPTL_ERROR_WOULD_BLOCK    1
+#define XCPTL_ERROR_SEND_FAILED    2
+#define XCPTL_ERROR_INVALID_MASTER 3
+extern int32_t XcpTlGetLastError();
+
+#endif
diff --git a/src/xcpLite.c b/src/xcpLite.c
index 6a91f20..4c4d0de 100644
--- a/src/xcpLite.c
+++ b/src/xcpLite.c
@@ -294,11 +294,13 @@ static uint8_t XcpAsyncCommand( BOOL async, const uint32_t* cmdBuf, uint16_t cmd
 #define error(e) { err=(e); goto negative_response; }
 #define check_error(e) { err=(e); if (err!=0) goto negative_response;  }
 
-#define isInitialized() (gXcp.SessionStatus & SS_INITIALIZED)
-#define isStarted() (gXcp.SessionStatus & SS_STARTED)
-#define isConnected() (gXcp.SessionStatus & SS_CONNECTED)
-#define isDaqRunning() (gXcp.SessionStatus & SS_DAQ)
-#define isLegacyMode() (gXcp.SessionStatus & SS_LEGACY_MODE)
+// BOOL type macros
+#define isInitialized() (0!=(gXcp.SessionStatus & SS_INITIALIZED))
+#define isStarted()     (0!=(gXcp.SessionStatus & SS_STARTED))
+#define isConnected()   (0!=(gXcp.SessionStatus & SS_CONNECTED))
+#define isDaqRunning()  (0!=(gXcp.SessionStatus & SS_DAQ))
+#define isLegacyMode()  (0!=(gXcp.SessionStatus & SS_LEGACY_MODE))
+#define isConnected()   (0!=(gXcp.SessionStatus & SS_CONNECTED))
 
 
 /****************************************************************************/
@@ -488,7 +490,7 @@ static uint8_t XcpSetMta( uint8_t ext, uint32_t addr ) {
 /****************************************************************************/
 
 // Free all dynamic DAQ lists
-static void  XcpFreeDaq( void ) {
+static void  XcpFreeDaq() {
 
   gXcp.SessionStatus &= ~SS_DAQ;
 
@@ -504,7 +506,7 @@ static void  XcpFreeDaq( void ) {
 }
 
 // Allocate Memory for daq,odt,odtEntries and Queue according to DaqCount, OdtCount and OdtEntryCount
-static uint8_t XcpAllocMemory( void ) {
+static uint8_t XcpAllocMemory() {
 
   uint32_t s;
 
@@ -566,7 +568,7 @@ static BOOL  XcpAdjustOdtSize(uint16_t daq, uint16_t odt, uint8_t size) {
     if (sc == 0) sc = 1;
     DaqListOdtSize(odt) = (uint16_t)(DaqListOdtSize(odt) + size*sc);
 #else
-    (void) daq;
+    (void)daq;
     DaqListOdtSize(odt) = (uint16_t)(DaqListOdtSize(odt) + size);
 #endif
 #ifdef XCP_ENABLE_TEST_CHECKS
@@ -771,7 +773,7 @@ static void XcpStopAllSelectedDaq() {
 }
 
 // Stop all DAQs
-static void XcpStopAllDaq( void ) {
+static void XcpStopAllDaq() {
 
   for (uint8_t daq=0; daq<gXcp.Daq.DaqCount; daq++) {
     DaqListState(daq) = DAQ_STATE_STOPPED_UNSELECTED;
@@ -938,7 +940,7 @@ uint8_t XcpEventExt(uint16_t event, const uint8_t* base, uint32_t len) {
     mutexLock(&gXcp.CmdPendingMutex);
 #endif
     BOOL cmdPending = FALSE;
-    if (gXcp.SessionStatus & SS_CMD_PENDING) {
+    if ((gXcp.SessionStatus & SS_CMD_PENDING) != 0) {
         if (gXcp.MtaExt == XCP_ADDR_EXT_DYN && (uint16_t)(gXcp.MtaAddr >> 16) == event) {
             gXcp.SessionStatus &= ~SS_CMD_PENDING;
             cmdPending = TRUE;
@@ -972,17 +974,21 @@ uint8_t XcpEventExt(uint16_t event, const uint8_t* base, uint32_t len) {
 /****************************************************************************/
 
 // Stops DAQ and goes to disconnected state
-void XcpDisconnect( void )
+void XcpDisconnect()
 {
   if (!isStarted()) return;
 
-  if (isDaqRunning()) {
-    ApplXcpStopDaq();
-    XcpStopAllDaq();
-    XcpTlWaitForTransmitQueueEmpty(); // Wait until transmit queue empty
-  }
+  if (isConnected()) {
 
-  gXcp.SessionStatus &= ~SS_CONNECTED;
+    if (isDaqRunning()) {
+      ApplXcpStopDaq();
+      XcpStopAllDaq();
+      XcpTlWaitForTransmitQueueEmpty(); // Wait until transmit queue empty
+    }
+    
+    gXcp.SessionStatus &= ~SS_CONNECTED;
+    ApplXcpDisconnect();
+  }
 }
 
 // Transmit command response
@@ -1017,7 +1023,7 @@ static uint8_t XcpPushCommand( const tXcpCto* cmdBuf, uint16_t cmdLen) {
 #endif
 
   // Set pending command flag
-  if (gXcp.SessionStatus & SS_CMD_PENDING) {
+  if ((gXcp.SessionStatus & SS_CMD_PENDING) != 0) {
 #if defined(XCP_ENABLE_MULTITHREAD_CAL_EVENTS) 
     mutexUnlock(&gXcp.CmdPendingMutex);
 #endif
@@ -1067,7 +1073,7 @@ static uint8_t XcpAsyncCommand( BOOL async, const uint32_t* cmdBuf, uint16_t cmd
   {
 #ifdef DBG_LEVEL
       DBG_PRINTF3("CONNECT mode=%u\n", CRO_CONNECT_MODE);
-      if (gXcp.SessionStatus & SS_CONNECTED) DBG_PRINT_WARNING("WARNING: Already connected! DAQ setup cleared! Legacy mode activated!\n");
+      if ((gXcp.SessionStatus & SS_CONNECTED) != 0) DBG_PRINT_WARNING("WARNING: Already connected! DAQ setup cleared! Legacy mode activated!\n");
 #endif
 
       // Check application is ready for XCP connect 
@@ -1359,7 +1365,7 @@ static uint8_t XcpAsyncCommand( BOOL async, const uint32_t* cmdBuf, uint16_t cmd
             {
               check_len(CRO_SET_REQUEST_LEN);
               CRM_LEN = CRM_SET_REQUEST_LEN;
-              if (CRO_SET_REQUEST_MODE & SET_REQUEST_MODE_STORE_CAL) check_error(ApplXcpFreezeCalPage(0));
+              if ((CRO_SET_REQUEST_MODE & SET_REQUEST_MODE_STORE_CAL) != 0) check_error(ApplXcpFreezeCalPage(0));
             }
             break;
   #endif // XCP_ENABLE_FREEZE_CAL_PAGE
@@ -1503,8 +1509,8 @@ static uint8_t XcpAsyncCommand( BOOL async, const uint32_t* cmdBuf, uint16_t cmd
             uint8_t mode = CRO_SET_DAQ_LIST_MODE_MODE;
             uint8_t prio = CRO_SET_DAQ_LIST_MODE_PRIORITY;
             if (daq >= gXcp.Daq.DaqCount) error(CRC_OUT_OF_RANGE);
-            if (mode & (DAQ_MODE_ALTERNATING | DAQ_MODE_DIRECTION | DAQ_MODE_DTO_CTR | DAQ_MODE_PID_OFF)) error(CRC_OUT_OF_RANGE);  // none of these modes implemented
-            if (0==(mode & (DAQ_MODE_TIMESTAMP))) error(CRC_CMD_SYNTAX);  // timestamp is fixed on
+            if ((mode & (DAQ_MODE_ALTERNATING | DAQ_MODE_DIRECTION | DAQ_MODE_DTO_CTR | DAQ_MODE_PID_OFF)) != 0) error(CRC_OUT_OF_RANGE);  // none of these modes implemented
+            if ((mode & DAQ_MODE_TIMESTAMP) == 0) error(CRC_CMD_SYNTAX);  // timestamp is fixed on
             if (CRO_SET_DAQ_LIST_MODE_PRESCALER > 1) error(CRC_OUT_OF_RANGE); // prescaler is not implemented
             check_error(XcpSetDaqListMode(daq, event, mode, prio));
             break;
@@ -1600,20 +1606,20 @@ static uint8_t XcpAsyncCommand( BOOL async, const uint32_t* cmdBuf, uint16_t cmd
               gXcp.SessionStatus &= ~SS_LEGACY_MODE;
             }
   #ifdef XCP_ENABLE_DAQ_CLOCK_MULTICAST
-            if (CRO_TIME_SYNCH_PROPERTIES_SET_PROPERTIES & TIME_SYNCH_SET_PROPERTIES_CLUSTER_ID) { // set cluster id
+            if ((CRO_TIME_SYNCH_PROPERTIES_SET_PROPERTIES & TIME_SYNCH_SET_PROPERTIES_CLUSTER_ID) != 0) { // set cluster id
               DBG_PRINTF4("  Cluster id set to %u\n", CRO_TIME_SYNCH_PROPERTIES_CLUSTER_ID);
               gXcp.ClusterId = CRO_TIME_SYNCH_PROPERTIES_CLUSTER_ID; // Set cluster id
               XcpEthTlSetClusterId(gXcp.ClusterId);
             }
             CRM_TIME_SYNCH_PROPERTIES_CLUSTER_ID = gXcp.ClusterId;
   #else
-            if (CRO_TIME_SYNCH_PROPERTIES_SET_PROPERTIES & TIME_SYNCH_SET_PROPERTIES_CLUSTER_ID) { // set cluster id
+            if ((CRO_TIME_SYNCH_PROPERTIES_SET_PROPERTIES & TIME_SYNCH_SET_PROPERTIES_CLUSTER_ID) != 0) { // set cluster id
                 //error(CRC_OUT_OF_RANGE); // CANape insists on setting a cluster id, even if Multicast is not enabled
                 DBG_PRINTF4("  Cluster id = %u setting ignored\n", CRO_TIME_SYNCH_PROPERTIES_CLUSTER_ID);
             }
             CRM_TIME_SYNCH_PROPERTIES_CLUSTER_ID = 0;
   #endif
-            if (CRO_TIME_SYNCH_PROPERTIES_SET_PROPERTIES & TIME_SYNCH_SET_PROPERTIES_TIME_SYNCH_BRIDGE) error(CRC_OUT_OF_RANGE); // set time sync bride is not supported -> error
+            if ((CRO_TIME_SYNCH_PROPERTIES_SET_PROPERTIES & TIME_SYNCH_SET_PROPERTIES_TIME_SYNCH_BRIDGE) != 0) error(CRC_OUT_OF_RANGE); // set time sync bride is not supported -> error
             CRM_TIME_SYNCH_PROPERTIES_SERVER_CONFIG = SERVER_CONFIG_RESPONSE_FMT_ADVANCED | SERVER_CONFIG_DAQ_TS_SERVER | SERVER_CONFIG_TIME_SYNCH_BRIDGE_NONE;  // SERVER_CONFIG_RESPONSE_FMT_LEGACY
             CRM_TIME_SYNCH_PROPERTIES_RESERVED = 0x0;
   #ifndef XCP_ENABLE_PTP
@@ -1634,7 +1640,7 @@ static uint8_t XcpAsyncCommand( BOOL async, const uint32_t* cmdBuf, uint16_t cmd
                 CRM_TIME_SYNCH_PROPERTIES_CLOCK_INFO = CLOCK_INFO_SERVER;
             }
   #endif // XCP_ENABLE_PTP
-            if (CRO_TIME_SYNCH_PROPERTIES_GET_PROPERTIES_REQUEST & TIME_SYNCH_GET_PROPERTIES_GET_CLK_INFO) { // check whether MTA based upload is requested
+            if ((CRO_TIME_SYNCH_PROPERTIES_GET_PROPERTIES_REQUEST & TIME_SYNCH_GET_PROPERTIES_GET_CLK_INFO) != 0) { // check whether MTA based upload is requested
                 gXcp.MtaPtr = (uint8_t*)&gXcp.ClockInfo.server;
                 gXcp.MtaExt = XCP_ADDR_EXT_PTR;
             }
@@ -1863,8 +1869,8 @@ void XcpPrint( const char *str ) {
   uint8_t i;
   uint16_t l = strlen(str);
   for (i = 0; i < l && i < XCPTL_MAX_CTO_SIZE-4; i++) crm.b[i+2] = str[i];
-  crm.b[i+3] = '\n';
-  crm.b[i+4] = 0;
+  crm.b[i+2] = '\n';
+  crm.b[i+3] = 0;
   XcpTlSendCrm((const uint8_t*)&crm, l+4);
   //XcpTlFlushTransmitBuffer(); // Don't do this, as it will decrease performance of the transmit process
 }
@@ -1876,7 +1882,8 @@ void XcpPrint( const char *str ) {
 | Initialization of the XCP Protocol Layer
 ******************************************************************************/
 
-void XcpInit( void )
+// Init XCP protocol layer
+void XcpInit()
 {
   if (gXcp.SessionStatus == 0) {
 
@@ -1903,7 +1910,8 @@ void XcpInit( void )
   }
 }
 
-void XcpStart(void)
+// Start XCP protocol layer
+void XcpStart()
 {
     if (!isInitialized()) return;
 
@@ -1915,11 +1923,12 @@ void XcpStart(void)
     #endif
     DBG_PRINTF3("  Version=%u.%u, MAXEV=%u, MAXCTO=%u, MAXDTO=%u, DAQMEM=%u, MAXDAQ=%u, MAXENTRY=%u, MAXENTRYSIZE=%u\n", XCP_PROTOCOL_LAYER_VERSION >> 8, XCP_PROTOCOL_LAYER_VERSION & 0xFF, XCP_MAX_EVENT, XCPTL_MAX_CTO_SIZE, XCPTL_MAX_DTO_SIZE, XCP_DAQ_MEM_SIZE, (1 << sizeof(uint16_t) * 8) - 1, (1 << sizeof(uint16_t) * 8) - 1, (1 << (sizeof(uint8_t) * 8)) - 1);
     DBG_PRINTF3("  %u KiB memory used\n", (unsigned int)sizeof(gXcp) / 1024);
+    DBG_PRINT3("  Note: These parameters in xcp_cfg.h need to be configured for optimal memory consumption and performance!\n");
     DBG_PRINT3("  Options=(");
 
     // Print activated XCP protocol options
   #ifdef XCP_ENABLE_DAQ_CLOCK_MULTICAST // Enable GET_DAQ_CLOCK_MULTICAST
-    DBG_PRINT3("DAQ_CLK_MULTICAST,");
+    DBG_PRINT3("DAQ_CLK_MULTICAST (not recomended),");
   #endif
   #ifdef XCP_DAQ_CLOCK_64BIT  // Use 64 Bit time stamps
     DBG_PRINT3("DAQ_CLK_64BIT,");
@@ -1997,6 +2006,12 @@ void XcpStart(void)
 }
 
 
+// Reset XCP protocol layer
+void XcpReset() {
+    memset(&gXcp, 0, sizeof(gXcp));
+}
+
+
 /**************************************************************************/
 // Eventlist
 /**************************************************************************/
diff --git a/src/xcpLite.h b/src/xcpLite.h
index 7b2bf8a..d60e998 100644
--- a/src/xcpLite.h
+++ b/src/xcpLite.h
@@ -70,8 +70,14 @@ typedef struct {
 /****************************************************************************/
 
 /* Initialization for the XCP Protocol Layer */
-extern void XcpInit(void);
-extern void XcpStart(void);
+extern void XcpInit();
+extern void XcpStart();
+extern void XcpReset();
+
+/* XCP command processor */
+extern uint8_t XcpCommand( const uint32_t* pCommand, uint16_t len );
+
+/* Disconnect, stop DAQ, flush queue */
 extern void XcpDisconnect();
 
 /* Trigger a XCP data acquisition or stimulation event */
@@ -79,9 +85,6 @@ extern void XcpEvent(uint16_t event);
 extern uint8_t XcpEventExt(uint16_t event, const uint8_t* base, uint32_t len);
 extern void XcpEventAt(uint16_t event, uint64_t clock);
 
-/* XCP command processor */
-extern uint8_t XcpCommand( const uint32_t* pCommand, uint16_t len );
-
 /* Send an XCP event message */
 extern void XcpSendEvent(uint8_t ev, uint8_t evc, const uint8_t* d, uint8_t l);
 
@@ -102,14 +105,12 @@ extern uint32_t XcpGetDaqOverflowCount();
 extern uint16_t XcpGetClusterId();
 #endif
 
-
-
 /* Time synchronisation */
 #ifdef XCP_ENABLE_DAQ_CLOCK_MULTICAST
 extern uint16_t XcpGetClusterId();
 #endif
 
-// Event list
+/* Event list */
 #ifdef XCP_ENABLE_DAQ_EVENT_LIST
 
 // Clear event list
@@ -131,8 +132,9 @@ extern tXcpEvent* XcpGetEvent(uint16_t event);
 // All callback functions supplied by the application
 // Must be thread save
 
-/* Callbacks on connect, measurement prepare, start and stop */
+/* Callbacks on connect, disconnect, measurement prepare, start and stop */
 extern BOOL ApplXcpConnect();
+extern void ApplXcpDisconnect();
 #if XCP_PROTOCOL_LAYER_VERSION >= 0x0104
 extern BOOL ApplXcpPrepareDaq();
 #endif
@@ -142,7 +144,7 @@ extern void ApplXcpStopDaq();
 /* Address conversions from A2L address to pointer and vice versa in absolute addressing mode */
 #ifdef XCP_ENABLE_ABS_ADDRESSING
 extern uint8_t* ApplXcpGetPointer(uint8_t xcpAddrExt, uint32_t xcpAddr); /* Create a pointer (uint8_t*) from xcpAddrExt and xcpAddr, returns NULL if no access */
-extern uint32_t ApplXcpGetAddr(uint8_t* p); // Calculate the xcpAddr address from a pointer
+extern uint32_t ApplXcpGetAddr(const uint8_t* p); // Calculate the xcpAddr address from a pointer
 extern uint8_t *ApplXcpGetBaseAddr(); // Get the base address for DAQ data access */
 #endif
 
diff --git a/src/xcpTl.h b/src/xcpTl.h
index 4f3efcd..af3cf92 100644
--- a/src/xcpTl.h
+++ b/src/xcpTl.h
@@ -6,30 +6,19 @@
 
 #define XCPTL_TIMEOUT_INFINITE 0xFFFFFFFF // Infinite timeout (blocking mode) for XcpTlHandleCommands, XcpTlWaitForTransmitData
 
-/* Transport Layer functions called by XCPlite.c */
+// Transport Layer functions called by protocol layer in XCPlite.c 
 extern void XcpTlSendCrm(const uint8_t* data, uint16_t n); // Send or queue (depending on XCPTL_QUEUED_CRM) a command response
 extern uint8_t* XcpTlGetTransmitBuffer(void** handle, uint16_t size); // Get a buffer for a message with size
 extern void XcpTlCommitTransmitBuffer(void* handle, BOOL flush); // Commit a buffer (by handle returned from XcpTlGetTransmitBuffer)
 extern void XcpTlFlushTransmitBuffer(); // Finalize the current transmit packet (ETH only)
 extern void XcpTlWaitForTransmitQueueEmpty(); // Wait (sleep) until transmit queue is empty 
 
-/* Transport Layer functions called by application */
-/* All other network specific application functions functions declared in xcpCanTl.h or xcpEthTl.h */
-extern int32_t XcpTlHandleTransmitQueue(); // Send all outgoing packets in the transmit queue
-extern BOOL XcpTlWaitForTransmitData(uint32_t timeout_ms); // Wait for at least timeout_ms, until packets are pending in the transmit queue
-
-// Generic transport layer functions
-// For testing 
-extern BOOL XcpTlInit(uint16_t segmentSize, int (*cb)(uint16_t msgLen, const uint8_t* msgBuf)); // Start generic transport layer, tx message callback
-extern void XcpTlShutdown(); // Stop transport layer
-extern uint8_t XcpTlCommand(uint16_t msgLen, const uint8_t* msgBuf); // rx message
-
-// Get last error code
-#define XCPTL_OK                   0
-#define XCPTL_ERROR_WOULD_BLOCK    1
-#define XCPTL_ERROR_SEND_FAILED    2
-#define XCPTL_ERROR_INVALID_MASTER 3
-extern int32_t XcpTlGetLastError();
+// Generic transport layer functions for XCP server
+extern BOOL XcpTlInit(); // Start generic transport layer
+extern void XcpTlShutdown(); // Stop generic transport layer
+extern uint8_t XcpTlCommand(uint16_t msgLen, const uint8_t* msgBuf); // Handle XCP message
+extern const uint8_t * XcpTlTransmitQueuePeek( uint16_t* msg_len);  // Check if there is a fully commited message segment buffer in the transmit queue
+extern void XcpTlTransmitQueueNext(); // Remove the next transmit queue entry
 
 // Get transmit queue level
 extern int32_t XcpTlGetTransmitQueueLevel();