diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..e767f23
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,12 @@
+*.o
+*.lo
+*.so
+*.a
+*.orig
+*.rej
+*.log
+
+Makefile
+config.*
+configure
+
diff --git a/mod_openzap/mod_openzap.c b/mod_openzap/mod_openzap.c
index d28a754..b993a0a 100644
--- a/mod_openzap/mod_openzap.c
+++ b/mod_openzap/mod_openzap.c
@@ -673,6 +673,17 @@ static switch_status_t channel_write_frame(switch_core_session_t *session, switc
 
 }
 
+#define zap_state_wait(x) 								\
+	do {										\
+		int __safety = 100;							\
+		while(__safety-- && zap_test_flag(x, ZAP_CHANNEL_STATE_CHANGE)) {	\
+			zap_sleep(10);							\
+		}									\
+		if(!__safety) {								\
+			zap_log(ZAP_LOG_CRIT, "State change not completed\n");		\
+		}									\
+	} while(0);
+
 static switch_status_t channel_receive_message_b(switch_core_session_t *session, switch_core_session_message_t *msg)
 {
 	switch_channel_t *channel;
@@ -691,6 +702,7 @@ static switch_status_t channel_receive_message_b(switch_core_session_t *session,
 				zap_set_flag_locked(tech_pvt->zchan, ZAP_CHANNEL_PROGRESS);
 			} else {
 				zap_set_state_locked(tech_pvt->zchan, ZAP_CHANNEL_STATE_PROGRESS);
+				zap_state_wait(tech_pvt->zchan);
 			}
 		}
 		break;
@@ -701,6 +713,7 @@ static switch_status_t channel_receive_message_b(switch_core_session_t *session,
 				zap_set_flag_locked(tech_pvt->zchan, ZAP_CHANNEL_MEDIA);
 			} else {
 				zap_set_state_locked(tech_pvt->zchan, ZAP_CHANNEL_STATE_PROGRESS_MEDIA);
+				zap_state_wait(tech_pvt->zchan);
 			}
 		}
 		break;
@@ -710,6 +723,7 @@ static switch_status_t channel_receive_message_b(switch_core_session_t *session,
 				zap_set_flag_locked(tech_pvt->zchan, ZAP_CHANNEL_ANSWERED);
 			} else {
 				zap_set_state_locked(tech_pvt->zchan, ZAP_CHANNEL_STATE_UP);
+				zap_state_wait(tech_pvt->zchan);
 			}
 		}
 		break;
diff --git a/src/include/openzap.h b/src/include/openzap.h
index 4fb5c73..457c97e 100644
--- a/src/include/openzap.h
+++ b/src/include/openzap.h
@@ -134,8 +134,8 @@
 #include "libteletone.h"
 #include "zap_buffer.h"
 #include "zap_threadmutex.h"
-#include "Q931.h"
 #include "Q921.h"
+#include "Q931.h"
 
 #define XX if (0)
 
diff --git a/src/include/zap_isdn.h b/src/include/zap_isdn.h
index 91f35b2..e18a51a 100644
--- a/src/include/zap_isdn.h
+++ b/src/include/zap_isdn.h
@@ -53,10 +53,11 @@ struct zap_isdn_data {
 	struct zap_sigmsg sigmsg;
 	zio_signal_cb_t sig_cb;
 	uint32_t flags;
+	int32_t mode;
+	zap_isdn_opts_t opts;
 	zap_caller_data_t *outbound_crv[32768];
 	zap_channel_t *channels_local_crv[32768];
 	zap_channel_t *channels_remote_crv[32768];
-	zap_isdn_opts_t opts;
 };
 
 
diff --git a/src/include/zap_types.h b/src/include/zap_types.h
index be43d0d..7c092e7 100644
--- a/src/include/zap_types.h
+++ b/src/include/zap_types.h
@@ -117,11 +117,12 @@ typedef enum {
 	ZAP_TRUNK_T1,
 	ZAP_TRUNK_J1,
 	ZAP_TRUNK_BRI,
+	ZAP_TRUNK_BRI_PTMP,
 	ZAP_TRUNK_FXO,
 	ZAP_TRUNK_FXS,
 	ZAP_TRUNK_NONE
 } zap_trunk_type_t;
-#define TRUNK_STRINGS "E1", "T1", "J1", "BRI", "FXO", "FXS", "NONE"
+#define TRUNK_STRINGS "E1", "T1", "J1", "BRI", "BRI_PTMP", "FXO", "FXS", "NONE"
 ZAP_STR2ENUM_P(zap_str2zap_trunk_type, zap_trunk_type2str, zap_trunk_type_t)
 
 typedef enum {
diff --git a/src/isdn/5ESSStateTE.c b/src/isdn/5ESSStateTE.c
index 05a11b0..a9654ce 100644
--- a/src/isdn/5ESSStateTE.c
+++ b/src/isdn/5ESSStateTE.c
@@ -240,7 +240,7 @@ L3INT ATT5ESSProc0x0fTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -283,7 +283,7 @@ L3INT ATT5ESSProc0x07TE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
diff --git a/src/isdn/5ESSmes.c b/src/isdn/5ESSmes.c
index 6a6f468..da47f7a 100644
--- a/src/isdn/5ESSmes.c
+++ b/src/isdn/5ESSmes.c
@@ -158,12 +158,7 @@ L3INT ATT5ESSPmes_Setup(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT I
     L3INT Octet = 0;
 
     /* Q931 Message Header */
-
-    OBuf[Octet++]    = pMes->ProtDisc;          /* Protocol discriminator           */
-    OBuf[Octet++]    = 2;                       /* length is 2 octets               */
-    OBuf[Octet++]    = (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);    /* msb    */
-    OBuf[Octet++]    = (L3UCHAR)(pMes->CRV);    /* lsb                              */
-    OBuf[Octet++]    = pMes->MesType;           /* message header                   */
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
     
     /* Sending Complete                */
     if(Q931IsIEPresent(pMes->SendComplete))
diff --git a/src/isdn/DMSStateTE.c b/src/isdn/DMSStateTE.c
index 5658415..8c435fa 100644
--- a/src/isdn/DMSStateTE.c
+++ b/src/isdn/DMSStateTE.c
@@ -237,7 +237,7 @@ L3INT DMSProc0x0fTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -279,7 +279,7 @@ L3INT DMSProc0x07TE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
diff --git a/src/isdn/DMSmes.c b/src/isdn/DMSmes.c
index 57aeddf..b32664b 100644
--- a/src/isdn/DMSmes.c
+++ b/src/isdn/DMSmes.c
@@ -155,12 +155,7 @@ L3INT DMSPmes_Setup(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISize
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb						*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Sending Complete				*/
 	if(Q931IsIEPresent(pMes->SendComplete))
diff --git a/src/isdn/Q921.c b/src/isdn/Q921.c
index 2dad5cf..a682c23 100644
--- a/src/isdn/Q921.c
+++ b/src/isdn/Q921.c
@@ -37,12 +37,323 @@
   POSSIBILITY OF SUCH DAMAGE.
 
 *****************************************************************************/
-#include "Q921.h"
+
+/****************************************************************************
+ * Changes:
+ *
+ * - June-August 2008: Stefan Knoblich <s.knoblich@axsentis.de>:
+ *     Add PTMP TEI management (NT + TE mode)
+ *     Add timers
+ *     Add retransmit counters
+ *     Add logging
+ *     Various cleanups
+ *     Queues, retransmission of I frames
+ *     PTMP NT mode
+ *
+ *
+ * TODO:
+ *
+ * - Cleanup queueing, test retransmission
+ *
+ * - Q921Start() /-Stop() TEI acquire + release
+ *   (move everything related into these functions)
+ *
+ * - Q.921 '97 Appendix I (and maybe III, IV)
+ *
+ * - More complete Appendix II
+ *
+ * - Test PTP mode
+ *
+ * - PTMP NT mode (in progress)
+ *
+ * - NT mode TEI management: (ab)use T202 for TEI Check Request retransmission
+ *
+ * - General cleanup (move all non-public declarations into private header file)
+ *
+ * - Statistics, per-Frame type debug message filter
+ *
+ ****************************************************************************/
+
 #include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
+#include <stdarg.h>
+
+#include "Q921.h"
+#include "Q921priv.h"
 #include "mfifo.h"
 
-int Q921SendRR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
+
+/******************************************************************************************************
+ * Actual code below this line
+ ******************************************************************************************************/
+
+
+/**
+ * Q921StateNames
+ * \brief	Static array of state name / value mappings
+ */
+static struct Q921StateName {
+	Q921State_t value;
+	const char *name;
+} Q921StateNames[10] = {
+	{ Q921_STATE_STOPPED, "Stopped" },
+	{ Q921_STATE_TEI_UNASSIGNED, "TEI Unassigned" },
+	{ Q921_STATE_TEI_AWAITING, "TEI Awaiting Assignment" },
+	{ Q921_STATE_TEI_ESTABLISH, "TEI Awaiting Establishment" },
+	{ Q921_STATE_TEI_ASSIGNED, "TEI Assigned" },
+	{ Q921_STATE_AWAITING_ESTABLISHMENT, "Awaiting Establishment" },
+	{ Q921_STATE_AWAITING_RELEASE, "Awaiting Release" },
+	{ Q921_STATE_MULTIPLE_FRAME_ESTABLISHED, "Multiple Frame Mode Established" },
+	{ Q921_STATE_TIMER_RECOVERY, "Timer Recovery" },
+	{ 0, 0 }
+};
+
+/**
+ * Q921State2Name
+ * \brief	Convert state value to name
+ * \param[in]	state	the state value
+ * \return	the state name or "Unknown"
+ *
+ * \author	Stefan Knoblich
+ */
+static const char *Q921State2Name(Q921State_t state)
+{
+	struct Q921StateName *p = Q921StateNames;
+
+	while(p->name) {
+		if(p->value == state)
+			return p->name;
+		p++;
+	}
+
+	return "Unknown";
+}
+
+
+/**
+ * Q921SendEnquiry
+ */
+static int Q921SendEnquiry(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	/* send enquiry: begin */
+	if(Q921_CHECK_FLAG(link, Q921_FLAG_RECV_BUSY)) {
+
+		Q921SendRNR(trunk, trunk->sapi, Q921_COMMAND(trunk), tei, 1);
+	}
+	else {
+		Q921SendRR(trunk, trunk->sapi, Q921_COMMAND(trunk), tei, 1);
+	}
+
+	/* clear acknowledge pending */
+	Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+
+	/* "Start" T200 */
+	Q921T200TimerReset(trunk, tei);
+
+	/* send enquiry: end */
+	return 1;
+}
+
+/**
+ * Q921SendEnquiryResponse
+ */
+static int Q921SendEnquiryResponse(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	/* send enquiry: begin */
+	if(Q921_CHECK_FLAG(link, Q921_FLAG_RECV_BUSY)) {
+
+		Q921SendRNR(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, 1);
+	}
+	else {
+		Q921SendRR(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, 1);
+
+		/* clear acknowledge pending */
+		Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+	}
+	/* send enquiry: end */
+	return 1;
+}
+
+/**
+ * Q921ResetExceptionConditions
+ * \brief	Reset Q.921 Exception conditions procedure
+ * \param	trunk	Q.921 data structure
+ * \param	tei	TEI
+ * \todo	Do something
+ */
+static void Q921ResetExceptionConditions(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	/* Clear peer receiver busy */
+	Q921_CLEAR_FLAG(link, Q921_FLAG_PEER_RECV_BUSY);
+
+	/* Clear reject exception */
+	Q921_CLEAR_FLAG(link, Q921_FLAG_REJECT);
+
+	/* Clear own receiver busy */
+	Q921_CLEAR_FLAG(link, Q921_FLAG_RECV_BUSY);
+
+	/* Clear acknowledge pending */
+	Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+
+	return;
+}
+
+/**
+ * Q921EstablishDataLink
+ * \brief	Q.921 Establish data link procedure
+ * \param	trunk	Q.921 data structure
+ * \param	tei	TEI
+ * \return	always 1 (success)
+ */
+static int Q921EstablishDataLink(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	/* reset exception conditions */
+	Q921ResetExceptionConditions(trunk, tei);
+
+	/* RC = 0 */
+	link->N200 = 0;
+
+	/* Send SABME */
+	Q921SendSABME(trunk, trunk->sapi, Q921_COMMAND(trunk), tei, 1);
+
+	/* Restart T200, stop T203 */
+	Q921T200TimerReset(trunk, tei);
+	Q921T203TimerStop(trunk, tei);
+
+	return 1;
+}
+
+/**
+ * Q921NrErrorRecovery
+ * \brief	NR(R) Error recovery procedure
+ * \param	trunk	Q.921 data structure
+ * \param	tei	TEI
+ * \return	always 1 (success)
+ */
+static int Q921NrErrorRecovery(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	/* MDL Error indication (J) */
+
+	/* Establish datalink */
+	Q921EstablishDataLink(trunk, tei);
+
+	/* Clear L3 initiated */
+	Q921_CLEAR_FLAG(link, Q921_FLAG_L3_INITIATED);
+
+	return 1;
+}
+
+
+/**
+ * Q921InvokeRetransmission
+ * \brief	I Frame retransmission procedure
+ * \param	trunk	Q.921 data structure
+ * \param	tei	TEI
+ * \param	nr	N(R) for retransmission
+ * \return	always 1 (success)
+ */
+static int Q921InvokeRetransmission(L2TRUNK trunk, L2UCHAR tei, L2UCHAR nr)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+	L2UCHAR *mes;
+	L2INT qpos, qnum, size = 0;
+
+	qnum = MFIFOGetMesCount(link->IFrameResendQueue);
+	qpos = qnum - 1;
+
+	/*
+	 * slightly different than what is shown in the spec
+	 * (Q.921 '97 Annex B, Figure B.9, page 104)
+	 * 
+	 * what the above mentioned figure probably means is:
+	 * "as long as V(S) != N(R), move the pointer marking
+	 *  the first frame to start resending at to the previous
+	 *  frame"
+	 *
+	 * if we actually implemented it as shown in the figure, we'd be
+	 * resending frames in the wrong order (moving backwards in time)
+	 * meaning we'd have to add an incoming queue to reorder the frames
+	 *
+	 */
+	/*
+	 * TODO: There's a "traditional" off-by-one error hidden in the original
+	 *       mfifo implementation + it's late, i'm tired and being lazy,
+	 *       so i'll probably have added another one :P
+	 *
+	 *       wow, the first while loop sucks and can be removed
+	 */
+	while(link->vs != nr && qpos > 0) {	/* ???? */
+		/* V(S) = V(S) - 1 */
+		Q921_DEC_COUNTER(link->vs);	/* huh? backwards? */
+
+		/* next frame in queue (backtrack along I queue) ??? */
+		qpos--;
+	}
+
+	/*
+	 * being lazy and trying to avoid mod 128 math this way...
+	 */
+	if(link->vs != nr && !qpos) {
+		/* fatal, we don't have enough history to resend all missing frames */
+		/* TODO: how to handle this? */
+	}
+
+	/*
+	 * resend frames in correct order (oldest missing frame first,
+	 * contrary to what the spec figure shows)
+	 */
+	while(qpos < qnum) {
+		/* Grab frame's buffer ptr and size from queue */
+		mes = MFIFOGetMesPtrOffset(link->IFrameResendQueue, &size, qpos);
+		if(mes) {
+			/* requeue frame (TODO: check queue full condition) */
+			MFIFOWriteMes(link->IFrameQueue, mes, size);
+
+			/* set I frame queued */
+		}
+
+		qpos++;
+	}
+
+	return 1;
+}
+
+
+static int Q921AcknowledgePending(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+	case Q921_STATE_TIMER_RECOVERY:
+		if(Q921_CHECK_FLAG(link, Q921_FLAG_ACK_PENDING)) {
+			/* clear acknowledge pending */
+			Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+
+			/* send RR */
+			Q921SendRR(trunk, trunk->sapi, Q921_COMMAND(trunk), tei, 0);
+
+			return 1;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
 
 /*****************************************************************************
 
@@ -52,47 +363,461 @@ int Q921SendRR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
                 function MUST be called before you call any other functions.
 
 *****************************************************************************/
-void Q921_InitTrunk(L2TRUNK trunk,
+int Q921_InitTrunk(L2TRUNK trunk,
 					L2UCHAR sapi,
 					L2UCHAR tei,
 					Q921NetUser_t NetUser,
+					Q921NetType_t NetType,
 					L2INT hsize,
-					Q921TxCB_t cb21,
-					Q921TxCB_t cb23,
+					Q921Tx21CB_t cb21,
+					Q921Tx23CB_t cb23,
 					void *priv21,
 					void *priv23)
 {
-	if (trunk->initialized != INITIALIZED_MAGIC) {
-		MFIFOCreate(trunk->HDLCInQueue, Q921MAXHDLCSPACE, 10);
-		trunk->initialized = INITIALIZED_MAGIC;
-	}
-	trunk->va = 0;
-	trunk->vs = 0;	
-	trunk->vr = 0;
-	trunk->state = 0;
+	int numlinks = 0;
+
 	trunk->sapi = sapi;
 	trunk->tei = tei;
 	trunk->NetUser = NetUser;
+	trunk->NetType = NetType;
 	trunk->Q921Tx21Proc = cb21;
 	trunk->Q921Tx23Proc = cb23;
 	trunk->PrivateData21 = priv21;
 	trunk->PrivateData23 = priv23;
 	trunk->Q921HeaderSpace = hsize;
-	trunk->T200 = 0;
-	trunk->T203 = 0;
-	trunk->T200Timeout = 1000;
-	trunk->T203Timeout = 10000;
+
+	numlinks = Q921_IS_PTMP_NT(trunk) ? Q921_TEI_MAX : 1;
+
+	if (trunk->initialized != INITIALIZED_MAGIC) {
+		MFIFOCreate(trunk->HDLCInQueue, Q921MAXHDLCSPACE, 10);
+
+		/*
+		 * Allocate space for per-link context(s)
+		 */
+		trunk->context = malloc(numlinks * sizeof(struct Q921_Link));
+		if(!trunk->context)
+			return -1;
+
+		trunk->initialized = INITIALIZED_MAGIC;
+	}
+
+	/* timeout default values */
+	trunk->T200Timeout = 1000;	/*   1 second  */
+	trunk->T203Timeout = 10000;	/*  10 seconds */
+	trunk->T202Timeout = 2000;	/*   2 seconds */
+	trunk->T201Timeout = 200000;	/* 200 seconds */
+	trunk->TM01Timeout = 10000;	/*  10 seconds */
+
+	/* octet / retransmit counter default limits */
+	trunk->N200Limit   = 3;		/*   3 retransmits */
+	trunk->N201Limit   = 260;	/* 260 octets      */
+	trunk->N202Limit   = 3;		/*   3 retransmits */
+	trunk->k           = 7;		/*   7 outstanding ACKs */
+
+	/* reset counters, timers, etc. */
+	trunk->T202 = 0;
+	trunk->N202 = 0;
+
+	/* Reset per-link contexts */
+	memset(trunk->context, 0, numlinks * sizeof(struct Q921_Link));
+
+	/* clear tei map */
+	memset(trunk->tei_map, 0, Q921_TEI_MAX + 1);
+
+	if(Q921_IS_PTMP(trunk)) {
+		/*
+		 * We're either the Network side (NT, TEI = 0)
+		 * or user-side equipment (TE) which will get it's TEI via
+		 * dynamic assignment
+		 */
+		trunk->tei = 0;
+	}
+
+	return 0;
 }
 
-int Q921Tx21Proc(L2TRUNK trunk, L2UCHAR *Msg, L2INT size)
+
+/**
+ * Q921Tx21Proc
+ * \brief	Submit frame to layer 1 (for sending)
+ * \param[in]	trunk	Pointer to trunk struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success; <= 0 on error
+ */
+static int Q921Tx21Proc(L2TRUNK trunk, L2UCHAR *Msg, L2INT size)
 {
+	Q921LogMesg(trunk, Q921_LOG_DEBUG, 0, Msg, size, "Sending frame");
+
 	return trunk->Q921Tx21Proc(trunk->PrivateData21, Msg, size);
 }
 
-int Q921Tx23Proc(L2TRUNK trunk, L2UCHAR *Msg, L2INT size)
+
+/**
+ * Q921Tx23Proc
+ * \brief	Submit frame to layer 3
+ * \param[in]	trunk	Pointer to trunk struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success; <= 0 on error
+ */
+static int Q921Tx23Proc(L2TRUNK trunk, Q921DLMsg_t ind, L2UCHAR tei, L2UCHAR *Msg, L2INT size)
+{
+	return trunk->Q921Tx23Proc(trunk->PrivateData23, ind, tei, Msg, size);
+}
+
+
+/**
+ * Q921LogProc
+ * \brief	Used for logging, converts to string and submits to higher level log function via callback
+ * \param[in]	trunk	Pointer to trunk struct
+ * \param[in]	level	Q921 Loglevel
+ * \param[in]	fmt	format of logmessage
+ * \return	>= 0 on success, < 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921Log(L2TRUNK trunk, Q921LogLevel_t level, const char *fmt, ...)
+{
+	char  buf[Q921_LOGBUFSIZE];
+	L2INT len;
+	va_list ap;
+
+	if(!trunk->Q921LogProc)
+		return 0;
+
+	if(trunk->loglevel < level)
+		return 0;
+
+	va_start(ap, fmt);
+
+	len = vsnprintf(buf, sizeof(buf)-1, fmt, ap);
+	if(len <= 0) {
+		/* TODO: error handling */
+		return -1;
+	}
+	if(len >= sizeof(buf))
+		len = sizeof(buf) - 1;
+
+	buf[len] = '\0';
+
+	va_end(ap);
+
+	return trunk->Q921LogProc(trunk->PrivateDataLog, level, buf, len);
+}
+
+
+static int print_hex(char *buf, int bsize, const unsigned char *in, const int len)
 {
-	return trunk->Q921Tx23Proc(trunk->PrivateData23, Msg, size);
+	static const char hex[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' };
+	int offset = 0;
+	int pos    = 0;
+	int nr     = 0;
+
+	buf[pos++] = '[';
+	bsize -= 3;
+
+	while((bsize - pos) > 0 && offset < len) {
+		buf[pos++] = hex[(in[offset] & 0xF0) >> 4];
+		buf[pos++] = hex[(in[offset++]   & 0x0F)];
+
+		if(++nr == 32 && offset < len && (bsize - pos) > 3) {
+			nr = 0;
+			buf[pos++] = ']';
+			buf[pos++] = '\n';
+			buf[pos++] = '[';
+		}
+		else if(offset < len) {
+			buf[pos++] = ' ';
+		}
+	}
+
+	buf[pos++] = ']';
+	buf[pos++] = '\n';
+	buf[pos]   = '\0';
 
+	return pos;
+}
+
+#define APPEND_MSG(buf, off, lef, fmt, ...) 			\
+	len = snprintf(buf + off, lef, fmt, ##__VA_ARGS__);	\
+	if(len > 0) { 						\
+		off += len;					\
+		lef -= len;					\
+	} else {						\
+		goto out;					\
+	}
+
+/**
+ * Q921LogProcMesg
+ * \brief	Used for logging, converts to string and submits to higher level log function via callback
+ * \param[in]	trunk	Pointer to trunk struct
+ * \param[in]	level	Q921 Loglevel
+ * \param[in]	received	direction of the message (received = 1, sending = 0)
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \param[in]	fmt	format of logmessage
+ * \return	>= 0 on success, < 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921LogMesg(L2TRUNK trunk, Q921LogLevel_t level, L2UCHAR received, L2UCHAR *mes, L2INT size, const char *fmt, ...)
+{
+	char  buf[Q921_LOGBUFSIZE];
+	L2INT len, left;
+	va_list ap;
+
+	if(!trunk->Q921LogProc)
+		return 0;
+
+	if(trunk->loglevel < level)
+		return 0;
+
+	if(!mes)
+		return 0;
+
+
+	left = sizeof(buf) - 1;
+
+	va_start(ap, fmt);
+
+	len = vsnprintf(buf, left, fmt, ap);
+	if(len > 0)
+		left -= len;
+	else {
+		/* TODO: error handling */
+		return -1;
+	}
+
+	va_end(ap);
+
+	if(trunk->loglevel == Q921_LOG_DEBUG) {
+		char pbuf[1024];
+		L2INT pleft, poffset;
+		L2UCHAR sapi, tei, cr;
+		L2UCHAR *pmes = mes + trunk->Q921HeaderSpace;
+		struct Q921_Link *link;
+
+		pleft   = sizeof(pbuf);
+		poffset = 0;
+
+		/*
+		 * Decode packet
+		 */
+		sapi = (pmes[0] & 0xfc) >> 2;
+		cr   = (pmes[0] & 0x02) >> 1;
+		tei  = (pmes[1] & 0xfe) >> 1;
+		link  = Q921_LINK_CONTEXT(trunk, tei);
+
+		/* make cr actually useful */
+		cr   = (received) ? Q921_IS_COMMAND(trunk, cr) : Q921_IS_RESPONSE(trunk, cr);
+
+		/* filter */
+		if((pmes[2] & 0x01) == 0x00) {
+ 			;
+		}
+		else if((pmes[2] & 0x03) == 0x01) {
+			; //return 0;
+		}
+		else if((pmes[2] & 0x03) == 0x03) {
+			;
+		}
+
+		APPEND_MSG(pbuf, poffset, pleft, "\n----------------- Q.921 Packet [%s%s] ---------------\n", received ? "Incoming" : "Outgoing",
+						(tei == link->tei || tei == Q921_TEI_BCAST) ? "" : ", Ignored" );
+
+		/* common header */
+		APPEND_MSG(pbuf, poffset, pleft, "    SAPI: %u, TEI: %u, C/R: %s (%d)\n\n", sapi, tei, (cr) ? "Command" : "Response", (mes[0] & 0x02) >> 1 );
+
+		/*
+		 * message specific
+		 */
+		if((pmes[2] & 0x01) == 0x00) {
+			/*
+			 * I frame
+			 */
+			L2UCHAR pf = pmes[3] & 0x01;	/* poll / final flag */
+			L2UCHAR nr = pmes[3] >> 1;	/* receive sequence number */
+			L2UCHAR ns = pmes[2] >> 1;	/* send sequence number */
+
+			APPEND_MSG(pbuf, poffset, pleft, "    Type: I Frame\n          P/F: %d, N(S): %d, N(R): %d  [V(A): %d, V(R): %d, V(S): %d]\n", pf, ns, nr,
+														link->va, link->vr, link->vs);
+
+			/* Dump content of I Frames for foreign TEIs */
+			if(tei != link->tei) {
+				APPEND_MSG(pbuf, poffset, pleft, "    CONTENT:\n");
+
+				len = print_hex(pbuf + poffset, pleft, &pmes[4], size - (trunk->Q921HeaderSpace + 4));
+				poffset += len;
+				pleft   -= len;
+			}
+		}
+		else if((pmes[2] & 0x03) == 0x01) {
+			/*
+			 * S frame
+			 */
+			L2UCHAR sv = (pmes[2] & 0x0c) >> 2;	/* supervisory format id */
+			L2UCHAR pf =  pmes[3] & 0x01;		/* poll / final flag */
+			L2UCHAR nr =  pmes[3] >> 1;		/* receive sequence number */
+			const char *type;
+
+			switch(sv) {
+			case 0x00:	/* RR : Receive Ready */
+				type = "RR (Receive Ready)";
+				break;
+
+			case 0x02:	/* RNR : Receive Not Ready */
+				type = "RNR (Receiver Not Ready)";
+				break;
+
+			case 0x04:	/* REJ : Reject */
+				type = "REJ (Reject)";
+				break;
+
+			default:	/* Invalid / Unknown */
+				type = "Unknown";
+				break;
+			}
+
+			APPEND_MSG(pbuf, poffset, pleft, "    Type: S Frame, SV: %s\n          P/F: %d, N(R): %d  [V(A): %d, V(R): %d, V(S): %d]\n", type, pf, nr,
+														link->va, link->vr, link->vs);
+		}
+		else if((pmes[2] & 0x03) == 0x03) {
+			/*
+			 * U frame
+			 */
+			L2UCHAR m  = (pmes[2] & 0xe0) >> 3 | (pmes[2] & 0x0c) >> 2;	/* modifier function id */
+			L2UCHAR pf = (pmes[2] & 0x10) >> 4;				/* poll / final flag */
+			const char *type;
+
+			switch(m) {
+			case 0x00:
+				type = "UI (Unnumbered Information)";
+				break;
+
+			case 0x03:
+				type = "DM (Disconnected Mode)";
+				break;
+
+			case 0x08:
+				type = "DISC (Disconnect)";
+				break;
+
+			case 0x0c:
+				type = "UA (Unnumbered Acknowledgement)";
+				break;
+
+			case 0x0f:
+				type = "SABME";
+				break;
+
+			case 0x11:
+				type = "FRMR (Frame Reject)";
+				break;
+
+			case 0x17:
+				type = "XID (Exchange Identification)";
+				break;
+
+			default:
+				type = "Unknown";
+			}
+
+
+			APPEND_MSG(pbuf, poffset, pleft, "    Type: U Frame (%s)\n          P/F: %d\n", type, pf);
+
+			if(m == 0x00) {
+				switch(pmes[3]) {
+				case Q921_LAYER_ENT_ID_TEI:
+					type = "TEI Mgmt";
+					break;
+
+				case Q921_LAYER_ENT_ID_Q931:
+					type = "Q.931";
+					break;
+
+				default:
+					type = "Unknown";
+				}
+
+				if(pmes[3] == Q921_LAYER_ENT_ID_TEI) {
+					const char *command = "";
+
+					switch(pmes[6]) {
+					case Q921_TEI_ID_REQUEST:
+						command = "Request";
+						break;
+					case Q921_TEI_ID_VERIFY:
+						command = "Verify";
+						break;
+					case Q921_TEI_ID_CHECKREQ:
+						command = "Check req";
+						break;
+					case Q921_TEI_ID_CHECKRESP:
+						command = "Check resp";
+						break;
+					case Q921_TEI_ID_REMOVE:
+						command = "Remove";
+						break;
+					case Q921_TEI_ID_ASSIGNED:
+						command = "Assign";
+						break;
+					case Q921_TEI_ID_DENIED:
+						command = "Denied";
+						break;
+					}
+					APPEND_MSG(pbuf, poffset, pleft, "    ENT ID: %d (%s), COMMAND: %d (%s), RI: %#x, AI: %d\n",
+							 pmes[3], type, pmes[6], command, (int)((pmes[4] << 8) | pmes[5]), pmes[7] >> 1);
+				}
+				else {
+					APPEND_MSG(pbuf, poffset, pleft, "    ENT ID: %d (%s), MESSAGE CONTENT:\n", pmes[3], type);
+
+					len = print_hex(pbuf + poffset, pleft, &pmes[3], size - (trunk->Q921HeaderSpace + 3));
+					poffset += len;
+					pleft   -= len;
+				}
+			}
+		}	
+		else {
+			/*
+			 * Unknown
+			 */
+			strncat(pbuf + poffset, "  -- unknown frame type --\n", pleft);
+
+			len = (sizeof(pbuf) - poffset) - strlen(pbuf + poffset);
+			if(len > 0) {
+				poffset += len;
+				pleft   -= len;
+			} else
+				goto out;
+		}
+
+		APPEND_MSG(pbuf, poffset, pleft, "\n    Q.921 state: \"%s\" (%d) [flags: %c%c%c%c]\n", Q921State2Name(link->state), link->state,
+											Q921_CHECK_FLAG(link, Q921_FLAG_ACK_PENDING) ? 'A' : '-',
+											Q921_CHECK_FLAG(link, Q921_FLAG_REJECT) ? 'R' : '-',
+											Q921_CHECK_FLAG(link, Q921_FLAG_PEER_RECV_BUSY) ? 'P' : '-',
+											Q921_CHECK_FLAG(link, Q921_FLAG_RECV_BUSY) ? 'B' : '-');
+
+		strncat(pbuf + poffset, "----------------------------------------------\n\n", pleft);
+
+		len = (sizeof(pbuf) - poffset) - strlen(pbuf + poffset);
+		if(len > 0) {
+			poffset += len;
+			pleft   -= len;
+		} else
+			goto out;
+
+
+		/* concat buffers together */
+		len = strlen(pbuf);
+		if(len <= left)
+			strncat(buf, pbuf, left);
+		else
+			strncat(buf, "-- packet truncated --\n", left);
+	}
+
+out:
+	return trunk->Q921LogProc(trunk->PrivateDataLog, level, buf, strlen(buf));
 }
 
 /*****************************************************************************
@@ -105,82 +830,468 @@ int Q921Tx23Proc(L2TRUNK trunk, L2UCHAR *Msg, L2INT size)
   Return Value: none
 
 *****************************************************************************/
-L2ULONG (*Q921GetTimeProc) (void) = NULL; /* callback for func reading time in ms */
-L2ULONG tLast = {0};
+static L2ULONG (*Q921GetTimeProc) (void) = NULL; /* callback for func reading time in ms */
+static L2ULONG tLast = {0};
 
-L2ULONG Q921GetTime(void)
+static L2ULONG Q921GetTime(void)
 {
-    L2ULONG tNow = 0;
-    if(Q921GetTimeProc != NULL)
-    {
-        tNow = Q921GetTimeProc();
-        if(tNow < tLast)    /* wrapped */
-        {
+	L2ULONG tNow = 0;
+
+	if(Q921GetTimeProc)
+	{
+		tNow = Q921GetTimeProc();
+		if(tNow < tLast)	/* wrapped */
+		{
 			/* TODO */
-        }
+		}
 		tLast = tNow;
-    }
-    return tNow;
+	}
+	return tNow;
 }
 
-void Q921T200TimerStart(L2TRUNK trunk)
+/*
+ * T200 handling (per-TEI in PTMP NT mode, tei=0 otherwise)
+ */
+static void Q921T200TimerStart(L2TRUNK trunk, L2UCHAR tei)
 {
-	if (!trunk->T200) {
-		trunk->T200 = Q921GetTime() + trunk->T200Timeout;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	if (!link->T200) {
+		link->T200 = Q921GetTime() + trunk->T200Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "T200 (timeout: %d msecs) started for TEI %d\n", trunk->T200Timeout, tei);
 	}
 }
 
-void Q921T200TimerStop(L2TRUNK trunk)
+static void Q921T200TimerStop(L2TRUNK trunk, L2UCHAR tei)
 {
-	trunk->T200 = 0;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	link->T200 = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T200 stopped for TEI %d\n", tei);
 }
 
-void Q921T200TimerReset(L2TRUNK trunk)
+static void Q921T200TimerReset(L2TRUNK trunk, L2UCHAR tei)
 {
-	Q921T200TimerStop(trunk);
-	Q921T200TimerStart(trunk);
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	link->T200 = Q921GetTime() + trunk->T200Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T200 (timeout: %d msecs) restarted for TEI %d\n", trunk->T200Timeout, tei);
 }
 
-void Q921T203TimerStart(L2TRUNK trunk)
+/*
+ * T203 handling (per-TEI in PTMP NT mode, tei=0 otherwise)
+ */
+static void Q921T203TimerStart(L2TRUNK trunk, L2UCHAR tei)
 {
-	if (!trunk->T203) {
-		trunk->T203 = Q921GetTime() + trunk->T203Timeout;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	if (!link->T203) {
+		link->T203 = Q921GetTime() + trunk->T203Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "T203 (timeout: %d msecs) started for TEI %d\n", trunk->T203Timeout, tei);
 	}
 }
 
-void Q921T203TimerStop(L2TRUNK trunk)
+static void Q921T203TimerStop(L2TRUNK trunk, L2UCHAR tei)
 {
-	trunk->T203 = 0;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	link->T203 = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T203 stopped for TEI %d\n", tei);
 }
 
-void Q921T203TimerReset(L2TRUNK trunk)
+static void Q921T203TimerReset(L2TRUNK trunk, L2UCHAR tei)
 {
-	Q921T203TimerStop(trunk);
-	Q921T203TimerStart(trunk);
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	link->T203 = Q921GetTime() + trunk->T203Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T203 (timeout: %d msecs) restarted for TEI %d\n", trunk->T203Timeout, tei);
 }
 
-void Q921T200TimerExpire(L2TRUNK trunk)
+/*
+ * T202 handling (TEI message timeout, TE mode only)
+ */
+static void Q921T202TimerStart(L2TRUNK trunk)
 {
-	(void)trunk;
+	if (!trunk->T202) {
+		trunk->T202 = Q921GetTime() + trunk->T202Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "T202 (timeout: %d msecs) started\n", trunk->T202Timeout);
+	}
 }
 
-void Q921T203TimerExpire(L2TRUNK trunk)
+static void Q921T202TimerStop(L2TRUNK trunk)
 {
-	Q921T203TimerReset(trunk);
-	Q921SendRR(trunk, trunk->sapi, trunk->NetUser == Q921_TE ? 0 : 1, trunk->tei, 1);
+	trunk->T202 = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T202 stopped\n");
 }
 
-void Q921TimerTick(L2TRUNK trunk)
+static void Q921T202TimerReset(L2TRUNK trunk)
 {
-	L2ULONG tNow = Q921GetTime();
-	if (trunk->T200 && tNow > trunk->T200) {
-		Q921T200TimerExpire(trunk);
+	trunk->T202 = Q921GetTime() + trunk->T202Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T202 (timeout: %d msecs) restarted\n", trunk->T202Timeout);
+}
+
+/*
+ * T201 handling (TEI management (NT side), per-TEI)
+ */
+static void Q921T201TimerStart(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	if (!link->T201) {
+		link->T201 = Q921GetTime() + trunk->T201Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "T201 (timeout: %d msecs) started for TEI %d\n", trunk->T201Timeout, tei);
+	}	
+}
+
+static void Q921T201TimerStop(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	link->T201 = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T201 stopped for TEI %d\n", tei);
+}
+
+#ifdef __UNUSED_FOR_NOW__
+static void Q921T201TimerReset(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	link->T201 = Q921GetTime() + trunk->T201Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T201 (timeout: %d msecs) restarted for TEI %d\n", trunk->T201Timeout, tei);
+}
+#endif
+
+/*
+ * TM01 handling (Datalink inactivity shutdown timer)
+ */
+static void Q921TM01TimerStart(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	if (!link->TM01) {
+		link->TM01 = Q921GetTime() + trunk->TM01Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "TM01 (timeout: %d msecs) started for TEI %d\n", trunk->TM01Timeout, tei);
+	}
+}
+
+#ifdef __UNUSED_FOR_NOW__
+static void Q921TM01TimerStop(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	link->TM01 = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "TM01 stopped for TEI %d\n", tei);
+}
+#endif
+
+static void Q921TM01TimerReset(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	link->TM01 = Q921GetTime() + trunk->TM01Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "TM01 (timeout: %d msecs) restarted for TEI %d\n", trunk->TM01Timeout, tei);
+}
+
+/*
+ * Expiry callbacks
+ */
+static void Q921T200TimerExpire(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link   = Q921_LINK_CONTEXT(trunk, tei);
+	struct Q921_Link *trlink = Q921_TRUNK_CONTEXT(trunk);
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T200 expired for TEI %d (trunk TEI %d)\n", tei, trlink->tei);
+
+	/* Stop timer first */
+	Q921T200TimerStop(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+		if(link->N200 >= trunk->N200Limit) {
+			/* Discard I queue */
+			MFIFOClear(link->IFrameQueue);
+
+			/* MDL-Error indication (G) */
+			Q921Log(trunk, Q921_LOG_ERROR, "Failed to establish Q.921 link in %d retries\n", link->N200);
+
+			/* DL-Release indication */
+			Q921Tx23Proc(trunk, Q921_DL_RELEASE, tei, NULL, 0);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED, tei);
+		} else {
+			/* Increment retry counter */
+			link->N200++;
+
+			/* Send SABME */
+			Q921SendSABME(trunk,
+					trunk->sapi,
+					Q921_COMMAND(trunk),
+					tei,
+					1);
+
+			/* Start T200 */
+			Q921T200TimerStart(trunk, tei);
+		}
+		break;
+
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		link->N200 = 0;
+
+		if(!Q921_CHECK_FLAG(link, Q921_FLAG_PEER_RECV_BUSY)) {
+			/* get last transmitted I frame */
+
+			/* V(S) = V(S) - 1 */
+			Q921_DEC_COUNTER(link->vs);
+
+			/* retransmit I frame */
+
+			/* V(S) = V(S) + 1 (done by Q921SendI() ) */
+			//Q921_INC_COUNTER(link->vs);
+
+			/* clear acknowledge pending */
+			Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+
+			/* Start T200 */
+			Q921T200TimerStart(trunk, tei);
+		} else {
+			/* transmit enquiry */
+			Q921SendEnquiry(trunk, tei);
+		}
+
+		/* increment counter */
+		link->N200++;
+
+		/* change state (no action) */
+		Q921ChangeState(trunk, Q921_STATE_TIMER_RECOVERY, tei);
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		if(link->N200 == trunk->N200Limit) {
+			/* MDL Error indication (I) */
+
+			/* Establish data link */
+			Q921EstablishDataLink(trunk, tei);
+
+			/* Clear L3 initiated */
+			Q921_CLEAR_FLAG(link, Q921_FLAG_L3_INITIATED);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		} else {
+			if(link->vs == link->va) {
+				/* transmit enquiry */
+				Q921SendEnquiry(trunk, tei);
+
+			} else if(!Q921_CHECK_FLAG(link, Q921_FLAG_PEER_RECV_BUSY)) {
+				/* get last transmitted frame */
+
+				/* V(S) = V(S) - 1 */
+				Q921_DEC_COUNTER(link->vs);
+
+				/* retrans frame */
+
+				/* V(S) = V(S) + 1 (done by Q921SendI() ) */
+				//Q921_INC_COUNTER(link->vs);
+
+				/* clear acknowledge pending */
+				Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+
+				/* Start T200 */
+				Q921T200TimerStart(trunk, tei);
+			}
+
+			/* increment counter */
+			link->N200++;
+
+			/* no state change */
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void Q921T203TimerExpire(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link   = Q921_LINK_CONTEXT(trunk, tei);
+	struct Q921_Link *trlink = Q921_TRUNK_CONTEXT(trunk);
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T203 expired for TEI %d (trunk TEI %d)\n", tei, trlink->tei);
+
+	/* Stop Timer first */
+	Q921T203TimerStop(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* Send Enquiry */
+		Q921SendEnquiry(trunk, tei);
+
+		/* RC = 0 */
+		link->N200 = 0;
+
+		/* no state change */
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void Q921T202TimerExpire(L2TRUNK trunk)
+{
+	struct Q921_Link *link = Q921_TRUNK_CONTEXT(trunk);
+
+	Q921T202TimerReset(trunk);
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T202 expired for Q.921 trunk with TEI %d\n", link->tei);
+
+	/* todo: implement resend counter */
+
+	switch(link->state) {
+	case Q921_STATE_TEI_ASSIGNED:	/* Tei identity verify timeout */
+		Q921TeiSendVerifyRequest(trunk);
+		break;
+
+	default:			/* Tei assignment request timeout (TODO: refine) */
+
+		if(trunk->N202 >= trunk->N202Limit) {
+			/* Too many retransmits, reset counter, stop timer and handle case (TODO) */
+			trunk->N202 = 0;
+
+			Q921T202TimerStop(trunk);
+
+			return;
+		}
+		Q921TeiSendAssignRequest(trunk);
+
+		trunk->N202++;
 	}
-	if (trunk->T203 && tNow > trunk->T203) {
-		Q921T203TimerExpire(trunk);		
+}
+
+static void Q921T201TimerExpire(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link   = Q921_LINK_CONTEXT(trunk, tei);
+	struct Q921_Link *trlink = Q921_TRUNK_CONTEXT(trunk);
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T201 expired for TEI %d (trunk TEI: %d)\n", tei, trlink->tei);
+
+	Q921T201TimerStop(trunk, tei);
+
+	/* NOTE: abusing N202 for this */
+	if(link->N202 < trunk->N202Limit) {
+		/* send check request */
+		Q921TeiSendCheckRequest(trunk, tei);
+
+		/* increment counter */
+		link->N202++;
+	} else {
+		/* put context in STOPPED state */
+		Q921ChangeState(trunk, Q921_STATE_STOPPED, tei);
+
+		/* NOTE: should we clear the link too? */
+		memset(link, 0, sizeof(struct Q921_Link));
+
+		/* mark TEI free */
+		trunk->tei_map[tei] = 0;
 	}
 }
 
+#ifdef __UNUSED_FOR_NOW__
+static void Q921TM01TimerExpire(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link   = Q921_LINK_CONTEXT(trunk, tei);
+	struct Q921_Link *trlink = Q921_TRUNK_CONTEXT(trunk);
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "TM01 expired for TEI %d (trunk TEI: %d)\n", tei, trlink->tei);
+
+	/* Restart TM01 */
+	Q921TM01TimerReset(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+	case Q921_STATE_TIMER_RECOVERY:
+/*
+ * NT-only, needs more support from L3
+ */
+#if 0
+		/* No activity, shutdown link */
+		Q921SendDISC(trunk, trunk->sapi, Q921_COMMAND(trunk), tei, 1);
+
+		/* clear I queue */
+		MFIFOClear(link->IFrameQueue);
+
+		/* change state */
+		Q921ChangeState(trunk, Q921_STATE_AWAITING_RELEASE, tei);
+#endif
+		break;
+
+	default:
+		break;
+	}
+}
+#endif
+
+/*
+ * Timer Tick function
+ */
+void Q921TimerTick(L2TRUNK trunk)
+{
+	struct Q921_Link *link;
+	L2ULONG tNow = Q921GetTime();
+	int numlinks = Q921_IS_PTMP_NT(trunk) ? Q921_TEI_MAX : 1;
+	int x;
+
+	for(x = 0; x <= numlinks; x++) {
+		link = Q921_LINK_CONTEXT(trunk, x);
+
+		/* TODO: check if TEI is assigned and skip check if not (speedup!) */
+		if(link->state == Q921_STATE_STOPPED)
+			continue;
+
+		if (link->T200 && tNow > link->T200) {
+			Q921T200TimerExpire(trunk, link->tei);
+		}
+		if (link->T203 && tNow > link->T203) {
+			Q921T203TimerExpire(trunk, link->tei);		
+		}
+
+		if(Q921_IS_PTMP_NT(trunk) && link->tei) {
+			if (link->T201 && tNow > link->T201) {
+				Q921T201TimerExpire(trunk, link->tei);
+			}
+		}
+
+		if(!Q921_IS_PTMP_NT(trunk)) {
+			if (trunk->T202 && tNow > trunk->T202) {
+				Q921T202TimerExpire(trunk);
+			}
+		}
+
+		/* Send enqueued I frame, if available */
+		Q921SendQueuedIFrame(trunk, link->tei);
+
+		/* Send ack if pending */
+		Q921AcknowledgePending(trunk, link->tei);
+	}
+
+}
 
 void Q921SetGetTimeCB(L2ULONG (*callback)(void))
 {
@@ -211,45 +1322,292 @@ int Q921QueueHDLCFrame(L2TRUNK trunk, L2UCHAR *b, L2INT size)
     return MFIFOWriteMes(trunk->HDLCInQueue, b, size);
 }
 
-/*****************************************************************************
+/**
+ * Q921EnqueueI
+ * \brief	Put I frame into transmit queue
+ *
+ */
+static int Q921EnqueueI(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR *mes, L2INT size)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, Tei);
 
-  Function:     Q921SendI
+	/* I frame header */
+	mes[trunk->Q921HeaderSpace+0] = ((Sapi << 2) & 0xfc) | ((cr << 1) & 0x02);
+	mes[trunk->Q921HeaderSpace+1] = (Tei << 1) | 0x01;
+	mes[trunk->Q921HeaderSpace+2] = 0x00;
+	mes[trunk->Q921HeaderSpace+3] = (pf & 0x01);
 
-  Description:  Compose and Send I Frame to layer. Will receive an I frame
-                with space for L2 header and fill out that header before
-                it call Q921Tx21Proc.
+	Q921Log(trunk, Q921_LOG_DEBUG, "Enqueueing I frame for TEI %d [%d]\n", link->tei, Tei);
 
-  Parameters:   trunk       trunk #
-                Sapi        Sapi
-                cr          C/R field.
-                Tei         Tei.
-                pf          P fiels octet 5
-                mes         ptr to I frame message.
-                size        size of message in bytes.
+	/* transmit queue, (TODO: check for full condition!) */
+	MFIFOWriteMes(link->IFrameQueue, mes, size);
 
-  Return Value: 0 if failed, 1 if Send.
+	/* try to send queued frame */
+	Q921SendQueuedIFrame(trunk, link->tei);
 
-*****************************************************************************/
-int Q921SendI(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR *mes, L2INT size)
+	return 1;
+}
+
+/**
+ * Q921SendQueuedIFrame
+ * \brief	Try to transmit queued I frame (if available)
+ */
+static int Q921SendQueuedIFrame(L2TRUNK trunk, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	L2INT size = 0;
+	L2UCHAR *mes;
+
+	if(MFIFOGetMesCount(link->IFrameQueue) == 0) {
+		return 0;
+	}
+
+	/* Link ready? */
+	if(link->state != Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+		return 0;
+	}
+
+	/* peer receiver busy? */
+	if(Q921_CHECK_FLAG(link, Q921_FLAG_PEER_RECV_BUSY)) {
+		return 0;
+	}
+
+	/* V(S) = V(A) + k? */
+	if(link->vs == ((link->va + trunk->k) % 128)) {
+		Q921Log(trunk, Q921_LOG_WARNING, "Maximum number (%d) of outstanding I frames reached for TEI %d\n", trunk->k, tei);
+		return 0;
+	}
+
+	mes = MFIFOGetMesPtr(link->IFrameQueue, &size);
+	if(mes) {
+		/* Fill in + update counter values */
+		mes[trunk->Q921HeaderSpace+2]  = link->vs << 1;
+		mes[trunk->Q921HeaderSpace+3] |= link->vr << 1;
+
+		if(MFIFOGetMesCount(link->IFrameQueue) == 0) {
+			/* clear I frame queued */
+		}
+
+		/* Send I frame */
+		Q921Tx21Proc(trunk, mes, size);
+
+		/* V(S) = V(S) + 1 */
+		Q921_INC_COUNTER(link->vs);
+
+		/* clear acknowledge pending */
+		Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+
+		/* T200 running? */
+		if(!link->T200) {
+			/* Stop T203, Start T200 */
+			Q921T200TimerStart(trunk, tei);
+			Q921T203TimerStop(trunk, tei);
+		}
+
+		/* put frame into resend queue */
+		MFIFOWriteMesOverwrite(link->IFrameResendQueue, mes, size);
+
+		/* dequeue frame */
+		MFIFOKillNext(link->IFrameQueue);
+
+		/* Restart TM01 */
+		if(Q921_IS_NT(trunk)) {
+			Q921TM01TimerReset(trunk, tei);
+		}
+
+		/* no state change */
+		return 1;
+	}
+
+	return 0;
+}
+
+/**
+ * Q921SendS
+ * \brief	Prepare and send S frame
+ */
+static int Q921SendS(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR sv, L2UCHAR *mes, L2INT size)
 {
-    mes[trunk->Q921HeaderSpace+0] = (Sapi&0xfc) | ((cr<<1)&0x02);
-    mes[trunk->Q921HeaderSpace+1] = (Tei<<1) | 0x01;
-    mes[trunk->Q921HeaderSpace+2] = trunk->vs<<1;
-    mes[trunk->Q921HeaderSpace+3] = (trunk->vr<<1) | (pf & 0x01);
-    trunk->vs++;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, Tei);
 
-    return Q921Tx21Proc(trunk, mes, size);
+	if(!Q921_IS_READY(link)) {
+		/* don't even bother trying */
+		Q921Log(trunk, Q921_LOG_DEBUG, "Link not ready, discarding S frame for TEI %d\n", Tei);
+		return 0;
+	}
+
+	/* S frame header */
+	mes[trunk->Q921HeaderSpace+0] = ((Sapi << 2) & 0xfc) | ((cr << 1) & 0x02);
+	mes[trunk->Q921HeaderSpace+1] = (Tei << 1) | 0x01;
+	mes[trunk->Q921HeaderSpace+2] = ((sv << 2) & 0x0c) | 0x01;
+	mes[trunk->Q921HeaderSpace+3] = (link->vr << 1) | (pf & 0x01);
+
+	return Q921Tx21Proc(trunk, mes, size);
 }
 
-int Q921Rx32(L2TRUNK trunk, L2UCHAR * Mes, L2INT Size)
+
+/**
+ * Q921SendU
+ * \brief	Prepare and send U frame
+ */
+static int Q921SendU(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR m, L2UCHAR *mes, L2INT size)
 {
-	return Q921SendI(trunk, 
-					trunk->sapi, 
-					trunk->NetUser == Q921_TE ? 0 : 1,
-					trunk->tei, 
-					0, 
-					Mes, 
-					Size);
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, Tei);
+
+	/* U frame header */
+	mes[trunk->Q921HeaderSpace+0] = ((Sapi << 2) & 0xfc) | ((cr << 1) & 0x02);
+	mes[trunk->Q921HeaderSpace+1] = (Tei << 1) | 0x01;
+	mes[trunk->Q921HeaderSpace+2] = ((m << 3) & 0xe0) | ((pf << 4) & 0x10) | ((m << 2) & 0x0c) | 0x03;
+
+	/* link not ready? enqueue non-TEI-mgmt UI (DL-UNIT DATA) frames */
+	if(m == 0x00 && Sapi != Q921_SAPI_TEI && link->state < Q921_STATE_TEI_ASSIGNED) {
+
+		/* write frame to queue */
+		MFIFOWriteMes(link->UIFrameQueue, mes, size);
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "Link not ready, UI Frame of size %d bytes queued for TEI %d\n", size, Tei);
+		return 1;
+	}
+
+	return Q921Tx21Proc(trunk, mes, size);
+}
+
+/**
+ * TODO: NT mode handling? Need a way to get Link context from Q.931
+ */
+int Q921Rx32(L2TRUNK trunk, Q921DLMsg_t ind, L2UCHAR tei, L2UCHAR * Mes, L2INT Size)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);	/* TODO: need real link tei for NT mode */
+	L2INT res = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "Got frame from Q.931, type: %d, tei: %d, size: %d\n", ind, tei, Size);
+
+	switch(ind) {
+	case Q921_DL_ESTABLISH:
+		/*
+		 * Hmm...
+		 */
+		switch(link->state) {
+		case Q921_STATE_TEI_ASSIGNED:
+			if(!Q921_IS_NT(trunk)) {
+				/* establish data link */
+				Q921EstablishDataLink(trunk, link->tei);
+
+				/* Set layer 3 initiated */
+				Q921_SET_FLAG(link, Q921_FLAG_L3_INITIATED);
+
+				/* change state (no action) */
+				Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, link->tei);
+			}
+			break;
+
+		case Q921_STATE_AWAITING_ESTABLISHMENT:
+			if(!Q921_IS_NT(trunk)) {
+				/* Discard I queue */
+				MFIFOClear(link->IFrameQueue);
+
+				/* Set layer 3 initiated */
+				Q921_SET_FLAG(link, Q921_FLAG_L3_INITIATED);
+			}
+			break;
+
+		case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		case Q921_STATE_TIMER_RECOVERY:
+			if(!Q921_IS_NT(trunk)) {
+				/* Discard I queue */
+				MFIFOClear(link->IFrameQueue);
+
+				/* establish data link */
+				Q921EstablishDataLink(trunk, link->tei);
+
+				/* Set layer 3 initiated */
+				Q921_SET_FLAG(link, Q921_FLAG_L3_INITIATED);
+
+				/* change state (no action) */
+				Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, link->tei);
+			}
+			break;
+
+		default:
+			break;
+		}
+		break;
+
+	case Q921_DL_RELEASE:
+		switch(link->state) {
+		case Q921_STATE_TEI_ASSIGNED:
+			/* send DL-RELEASE confirm */
+			Q921Tx23Proc(trunk, Q921_DL_RELEASE, tei, NULL, 0);
+			break;
+
+		case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		case Q921_STATE_TIMER_RECOVERY:
+			if(!Q921_IS_NT(trunk)) {
+				/* Discard I queue */
+				MFIFOClear(link->IFrameQueue);
+
+				/* RC = 0 */
+				link->N200 = 0;
+
+				/* send DISC command */
+				Q921SendDISC(trunk, trunk->sapi, Q921_COMMAND(trunk), link->tei, 1);
+
+				/* Stop T203, restart T200 */
+				if(link->state == Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+					Q921T203TimerStop(trunk, link->tei);
+				}
+				Q921T200TimerReset(trunk, link->tei);
+
+				/* change state */
+				Q921ChangeState(trunk, Q921_STATE_AWAITING_RELEASE, link->tei);
+			}
+			break;
+
+		default:
+			break;
+		}
+		break;
+
+	case Q921_DL_DATA:	/* DL-DATA request */
+		res = Q921EnqueueI(trunk, 
+				trunk->sapi, 
+				Q921_COMMAND(trunk),
+				link->tei,
+				0, 
+				Mes, 
+				Size);
+
+		if(link->state < Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+			/* Treat as implicit DL-ESTABLISH request */
+
+			/* establish data link */
+			Q921EstablishDataLink(trunk, link->tei);
+
+			/* Set layer 3 initiated */
+			Q921_SET_FLAG(link, Q921_FLAG_L3_INITIATED);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, link->tei);
+		}
+		break;
+
+	case Q921_DL_UNIT_DATA:		/* DL-UNIT DATA request */
+		res = Q921SendUN(trunk,
+				trunk->sapi,
+				Q921_COMMAND(trunk),
+				Q921_TEI_BCAST,
+				0,
+				Mes,
+				Size);
+		/* NOTE: Let the other side initiate link establishment */
+		break;
+
+	default:
+		break;
+	}
+
+	return res;
 }
 /*****************************************************************************
 
@@ -267,16 +1625,11 @@ int Q921Rx32(L2TRUNK trunk, L2UCHAR * Mes, L2INT Size)
 
 *****************************************************************************/
 
-int Q921SendRR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
+static int Q921SendRR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
 {
-    L2UCHAR mes[400];
+	L2UCHAR mes[25];
 
-    mes[trunk->Q921HeaderSpace+0] = (L2UCHAR)((Sapi&0xfc) | ((cr<<1)&0x02));
-    mes[trunk->Q921HeaderSpace+1] = (L2UCHAR)((Tei<<1) | 0x01);
-    mes[trunk->Q921HeaderSpace+2] = (L2UCHAR)0x01;
-    mes[trunk->Q921HeaderSpace+3] = (L2UCHAR)((trunk->vr<<1) | (pf & 0x01));
-
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+4);
+	return Q921SendS(trunk, Sapi, cr, Tei, pf, 0x00, mes, trunk->Q921HeaderSpace+4);
 }
 
 /*****************************************************************************
@@ -294,16 +1647,11 @@ int Q921SendRR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
   Return Value: 0 if failed, 1 if Send.
 
 *****************************************************************************/
-int Q921SendRNR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
+static int Q921SendRNR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
 {
-    L2UCHAR mes[400];
-
-    mes[trunk->Q921HeaderSpace+0] = (L2UCHAR)((Sapi&0xfc) | ((cr<<1)&0x02));
-    mes[trunk->Q921HeaderSpace+1] = (L2UCHAR)((Tei<<1) | 0x01);
-    mes[trunk->Q921HeaderSpace+2] = (L2UCHAR)0x05;
-    mes[trunk->Q921HeaderSpace+3] = (L2UCHAR)((trunk->vr<<1) | (pf & 0x01));
+	L2UCHAR mes[25];
 
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+4);
+	return Q921SendS(trunk, Sapi, cr, Tei, pf, 0x01, mes, trunk->Q921HeaderSpace+4);
 }
 
 /*****************************************************************************
@@ -321,16 +1669,11 @@ int Q921SendRNR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
   Return Value: 0 if failed, 1 if Send.
 
 *****************************************************************************/
-int Q921SendREJ(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
+static int Q921SendREJ(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
 {
-    L2UCHAR mes[400];
-
-    mes[trunk->Q921HeaderSpace+0] = (L2UCHAR)((Sapi&0xfc) | ((cr<<1)&0x02));
-    mes[trunk->Q921HeaderSpace+1] = (L2UCHAR)((Tei<<1) | 0x01);
-    mes[trunk->Q921HeaderSpace+2] = (L2UCHAR)0x09;
-    mes[trunk->Q921HeaderSpace+3] = (L2UCHAR)((trunk->vr<<1) | (pf & 0x01));
+	L2UCHAR mes[25];
 
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+4);
+	return Q921SendS(trunk, Sapi, cr, Tei, pf, 0x03, mes, trunk->Q921HeaderSpace+4);
 }
 
 /*****************************************************************************
@@ -348,31 +1691,150 @@ int Q921SendREJ(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
   Return Value: 0 if failed, 1 if Send.
 
 *****************************************************************************/
-int Q921SendSABME(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
+static int Q921SendSABME(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
 {
-    L2UCHAR mes[400];
+	L2UCHAR mes[25];
 
-    mes[trunk->Q921HeaderSpace+0] = (L2UCHAR)((Sapi&0xfc) | ((cr<<1)&0x02));
-    mes[trunk->Q921HeaderSpace+1] = (L2UCHAR)((Tei<<1) | 0x01);
-    mes[trunk->Q921HeaderSpace+2] = (L2UCHAR)(0x6f | ((pf<<4)&0x10));
-
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+3);
+	return Q921SendU(trunk, Sapi, cr, Tei, pf, 0x0f, mes, trunk->Q921HeaderSpace+3);
 }
 
+
+/**
+ * Q921Start
+ * \brief	Start trunk
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \return	> 0 on success; <= 0 on error
+ */
 int Q921Start(L2TRUNK trunk)
 {
-	return Q921SendSABME(trunk, 
+	int x, numlinks = Q921_IS_PTMP_NT(trunk) ? Q921_TEI_MAX : 1;
+	struct Q921_Link *link = Q921_TRUNK_CONTEXT(trunk);
+
+	if(trunk->initialized != INITIALIZED_MAGIC)
+		return 0;
+
+	memset(trunk->context, 0, numlinks * sizeof(struct Q921_Link));
+
+	/* Common init part */
+	for(x = 0; x <= numlinks; x++) {
+		link = Q921_LINK_CONTEXT(trunk, x);
+
+		link->state = Q921_STATE_TEI_UNASSIGNED;
+		link->tei   = 0;
+
+		/* Initialize per-TEI I + UI queues */
+		MFIFOCreate(link->UIFrameQueue, Q921MAXHDLCSPACE, 10);
+		MFIFOCreate(link->IFrameQueue,  Q921MAXHDLCSPACE, 10);
+		MFIFOCreate(link->IFrameResendQueue, Q921MAXHDLCSPACE, 10);
+	}
+
+	if(Q921_IS_PTMP_TE(trunk)) {
+		link->state = Q921_STATE_TEI_UNASSIGNED;
+		link->tei   = 0;
+	}
+	else if(Q921_IS_PTMP_NT(trunk)) {
+		link = Q921_TRUNK_CONTEXT(trunk);
+
+		link->state = Q921_STATE_TEI_ASSIGNED;
+		link->tei   = trunk->tei;
+
+		/* clear tei map */
+		memset(trunk->tei_map, 0, Q921_TEI_MAX + 1);
+	}
+	else {
+		link->state = Q921_STATE_TEI_ASSIGNED;
+		link->tei   = trunk->tei;
+	}
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "Starting trunk %p (sapi: %d, tei: %d, mode: %s %s)\n",
+				 trunk,
+				 trunk->sapi,
+				 link->tei,
+				 Q921_IS_PTMP(trunk) ? "PTMP" : "PTP",
+				 Q921_IS_TE(trunk) ? "TE" : "NT");
+
+	if(Q921_IS_PTP(trunk)) {
+		Q921Log(trunk, Q921_LOG_DEBUG, "Sending SABME\n");
+
+		return Q921SendSABME(trunk, 
 					trunk->sapi, 
-					trunk->NetUser == Q921_TE ? 0 : 1,
-					trunk->tei, 
+					Q921_COMMAND(trunk),
+					link->tei, 
 					1);
+
+	} else if(Q921_IS_PTMP_NT(trunk)) {
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "Revoking all TEIs\n");
+
+		return Q921TeiSendRemoveRequest(trunk, Q921_TEI_BCAST);	/* Revoke all TEIs in use */
+	} else {
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "Requesting TEI\n");
+
+		return Q921TeiSendAssignRequest(trunk);
+	}
+}
+
+
+/**
+ * Q921Stop
+ * \brief	Stop trunk
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+int Q921Stop(L2TRUNK trunk)
+{
+	struct Q921_Link *link;
+	int x, numlinks;
+
+	if(!trunk)
+		return -1;
+
+	link = Q921_TRUNK_CONTEXT(trunk);
+	numlinks = Q921_IS_PTMP_NT(trunk) ? Q921_TEI_MAX : 1;
+
+	if(Q921_IS_STOPPED(link))
+		return 0;
+
+	/* Release TEI */
+	if(Q921_IS_PTMP_TE(trunk)) {
+		/* send verify request */
+		Q921TeiSendVerifyRequest(trunk);
+
+		/* drop TEI */
+		link->tei  = 0;
+	}
+
+	/* Stop timers, stop link, flush queues */
+	for(x = 0; x <= numlinks; x++) {
+		Q921T200TimerStop(trunk, x);
+		Q921T203TimerStop(trunk, x);
+		Q921T201TimerStop(trunk, x);
+
+		/* Change state (no action) */
+		Q921ChangeState(trunk, Q921_STATE_STOPPED, x);
+
+		/* Flush per-tei I/UI queues */
+		MFIFOClear(link->UIFrameQueue);
+		MFIFOClear(link->IFrameQueue);
+		MFIFOClear(link->IFrameResendQueue);
+	}
+	Q921T202TimerStop(trunk);
+
+	/* Flush HDLC queue */
+	MFIFOClear(trunk->HDLCInQueue);
+
+	return 0;
 }
 
+
 /*****************************************************************************
 
   Function:     Q921SendDM
 
-  Description:  Comose and Send DM (Disconnected Mode)
+  Description:  Compose and Send DM (Disconnected Mode)
 
   Parameters:   trunk       trunk #
                 Sapi        Sapi
@@ -383,15 +1845,11 @@ int Q921Start(L2TRUNK trunk)
   Return Value: 0 if failed, 1 if Send.
 
 *****************************************************************************/
-int Q921SendDM(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
+static int Q921SendDM(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
 {
-    L2UCHAR mes[400];
+	L2UCHAR mes[25];
 
-    mes[trunk->Q921HeaderSpace+0] = (L2UCHAR)((Sapi&0xfc) | ((cr<<1)&0x02));
-    mes[trunk->Q921HeaderSpace+1] = (L2UCHAR)((Tei<<1) | 0x01);
-    mes[trunk->Q921HeaderSpace+2] = (L2UCHAR)(0x0f | ((pf<<4)&0x10));
-
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+3);
+	return Q921SendU(trunk, Sapi, cr, Tei, pf, 0x03, mes, trunk->Q921HeaderSpace+3);
 }
 
 /*****************************************************************************
@@ -409,15 +1867,11 @@ int Q921SendDM(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
   Return Value: 0 if failed, 1 if Send.
 
 *****************************************************************************/
-int Q921SendDISC(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
+static int Q921SendDISC(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
 {
-    L2UCHAR mes[400];
-
-    mes[trunk->Q921HeaderSpace+0] = (L2UCHAR)((Sapi&0xfc) | ((cr<<1)&0x02));
-    mes[trunk->Q921HeaderSpace+1] = (L2UCHAR)((Tei<<1) | 0x01);
-    mes[trunk->Q921HeaderSpace+2] = (L2UCHAR)(0x43 | ((pf<<4)&0x10));
+	L2UCHAR mes[25];
 
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+3);
+	return Q921SendU(trunk, Sapi, cr, Tei, pf, 0x08, mes, trunk->Q921HeaderSpace+3);
 }
 
 /*****************************************************************************
@@ -435,30 +1889,1000 @@ int Q921SendDISC(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
   Return Value: 0 if failed, 1 if Send.
 
 *****************************************************************************/
-int Q921SendUA(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
+static int Q921SendUA(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
 {
-    L2UCHAR mes[400];
+	L2UCHAR mes[25];
 
-    mes[trunk->Q921HeaderSpace+0] = (L2UCHAR)((Sapi&0xfc) | ((cr<<1)&0x02));
-    mes[trunk->Q921HeaderSpace+1] = (L2UCHAR)((Tei<<1) | 0x01);
-    mes[trunk->Q921HeaderSpace+2] = (L2UCHAR)(0x63 | ((pf<<4)&0x10));
+	return Q921SendU(trunk, Sapi, cr, Tei, pf, 0x0c, mes, trunk->Q921HeaderSpace+3);
+}
 
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+3);
+static int Q921SendUN(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf, L2UCHAR *mes, L2INT size)
+{
+	return Q921SendU(trunk, Sapi, cr, Tei, pf, 0x00, mes, size+trunk->Q921HeaderSpace+3);
 }
 
-int Q921ProcSABME(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+
+/**
+ * Q921ProcSABME
+ * \brief	Handle incoming SABME
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success, <= 0 on error
+ */
+static int Q921ProcSABME(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
 {
-	/* TODO:  Do we need these paramaters? */
-	(void)mes;
-	(void)size;
+	L2UCHAR pf = (mes[2] & 0x10) >> 4;				/* poll / final flag */
+	L2UCHAR tei = (mes[1] & 0xfe) >> 1;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_TEI_ASSIGNED:
+		/* send UA */
+		Q921SendUA(trunk,
+				trunk->sapi,
+				Q921_RESPONSE(trunk),	/* or command? */
+				tei, pf);
+
+		/* clear counters */
+		link->vr=0;
+		link->vs=0;
+		link->va=0;
+
+		/* TODO: send DL-Establish indication to Q.931 */
+		Q921Tx23Proc(trunk, Q921_DL_ESTABLISH, tei, NULL, 0);
+
+		/* start T203 */
+		Q921T203TimerStart(trunk, tei);
+
+		/* change state (no action) */
+		Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED, tei);
+		break;
+
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+		/* send UA */
+		Q921SendUA(trunk,
+				trunk->sapi,
+				Q921_RESPONSE(trunk),
+				tei, pf);
+
+		/* no state change */
+		break;
+
+	case Q921_STATE_AWAITING_RELEASE:
+		/* send DM */
+		Q921SendDM(trunk,
+				trunk->sapi,
+				Q921_RESPONSE(trunk),
+				tei, pf);
+
+		/* no state change */
+		break;
+
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+	case Q921_STATE_TIMER_RECOVERY:
+		/* send UA */
+		Q921SendUA(trunk,
+				trunk->sapi,
+				Q921_RESPONSE(trunk),
+				tei, pf);
+
+		/* clear exception conditions */
+		Q921ResetExceptionConditions(trunk, tei);
+
+		/* send MDL-Error indication */
+
+		/* V(S) == V(A) ? */
+		if(link->vs != link->va) {
+			/* clear I queue */
+			MFIFOClear(link->IFrameQueue);
+
+			/* DL-Establish indication */
+			Q921Tx23Proc(trunk, Q921_DL_ESTABLISH, tei, NULL, 0);
+		}
 
-    trunk->vr=0;
-    trunk->vs=0;
-	trunk->va=0;
+		/* clear counters */
+		link->vr=0;
+		link->vs=0;
+		link->va=0;
 
-    return 1;
+		/* Stop T200, start T203 */
+		Q921T200TimerStop(trunk, tei);
+		Q921T203TimerStart(trunk, tei);
+
+		/* state change only if in TIMER_RECOVERY state */
+		if(link->state == Q921_STATE_TIMER_RECOVERY)
+			Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED, tei);
+		break;
+
+	default:
+		break;
+	}
+
+	return 1;
+}
+
+
+/**
+ * Q921ProcDM
+ * \brief	Handle incoming DM
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success, <= 0 on error
+ */
+static int Q921ProcDM(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR pf = (mes[2] & 0x10) >> 4;				/* poll / final flag */
+	L2UCHAR tei = (mes[1] & 0xfe) >> 1;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_TEI_ASSIGNED:
+		if(!pf) {
+			/* to next state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		}
+		break;
+
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+	case Q921_STATE_AWAITING_RELEASE:
+		if(pf) {
+			if(link->state == Q921_STATE_AWAITING_ESTABLISHMENT) {
+				/* Discard I queue */
+				MFIFOClear(link->IFrameQueue);
+			}
+
+			/* Send DL-Release indication to Q.931 */
+			Q921Tx23Proc(trunk, Q921_DL_RELEASE, tei, NULL, 0);
+
+			/* Stop T200 */
+			Q921T200TimerStop(trunk, tei);
+
+			/* Change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED, tei);
+		}
+		break;
+
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		if(pf) {
+			/* MDL-Error indication (B) */
+
+			/* no state change */
+		} else {
+			/* MDL-Error indication (E) */
+
+			/* establish data link */
+			Q921EstablishDataLink(trunk, tei);
+
+			/* clear L3 initiated */
+			Q921_CLEAR_FLAG(link, Q921_FLAG_L3_INITIATED);
+
+			/* change state (no action?) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		if(pf) {
+			/* MDL Error indication (B) */
+		} else {
+			/* MDL Error indication (E) */
+		}
+
+		/* establish data link */
+		Q921EstablishDataLink(trunk, tei);
+
+		/* clear layer 3 initiated */
+		Q921_CLEAR_FLAG(link, Q921_FLAG_L3_INITIATED);
+
+		/* change state */
+		Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		break;
+
+	default:
+		break;
+	}
+
+	return 1;
+}
+
+/**
+ * Q921ProcUA
+ * \brief	Handle incoming UA
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success, <= 0 on error
+ */
+static int Q921ProcUA(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR pf = (mes[2] & 0x10) >> 4;				/* poll / final flag */
+	L2UCHAR tei = (mes[1] & 0xfe) >> 1;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_TEI_ASSIGNED:
+	case Q921_STATE_TIMER_RECOVERY:
+		/* MDL Error indication (C, D) */
+		Q921Log(trunk, Q921_LOG_ERROR, "Received UA frame in invalid state\n");
+		break;
+
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+		if(pf) {
+			/* TODO: other fancy stuff (see docs) */
+			if(Q921_CHECK_FLAG(link, Q921_FLAG_L3_INITIATED)) {	/* layer3 initiated */
+				link->vr = 0;
+
+				/* DL-Establish confirm */
+				Q921Tx23Proc(trunk, Q921_DL_ESTABLISH_CONFIRM, tei, NULL, 0);
+
+			} else if(link->vs != link->va) {
+
+				/* discard I queue */
+				MFIFOClear(link->IFrameQueue);
+
+				/* DL-Establish indication */
+				Q921Tx23Proc(trunk, Q921_DL_ESTABLISH, tei, NULL, 0);
+			}
+
+			/* Stop T200, start T203 */
+			Q921T200TimerStop(trunk, tei);
+			Q921T203TimerStart(trunk, tei);
+
+			link->vs = 0;
+			link->va = 0;
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED, tei);
+		} else {
+			/* MDL Error indication (C, D) */
+			Q921Log(trunk, Q921_LOG_ERROR, "Received UA frame is not a response to a request\n");
+
+			/* no state change */
+		}
+		break;
+
+	case Q921_STATE_AWAITING_RELEASE:
+		if(pf) {
+			/* DL Release confirm */
+			Q921Tx23Proc(trunk, Q921_DL_RELEASE_CONFIRM, tei, NULL, 0);
+
+			/* Stop T200 */
+			Q921T200TimerStop(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED, tei);
+		} else {
+			/* MDL Error indication (D) */
+			Q921Log(trunk, Q921_LOG_ERROR, "Received UA frame is not a response to a request\n");
+
+			/* no state change */
+		}
+		break;
+
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* MDL Error indication (C, D) */
+		Q921Log(trunk, Q921_LOG_ERROR, "Received UA frame in invalid state\n");
+
+		/* no state change */
+		break;
+
+	default:
+		break;
+	}
+
+	return 1;
+}
+
+
+/**
+ * Q921ProcDISC
+ * \brief	Handle incoming DISC
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success, <= 0 on error
+ */
+static int Q921ProcDISC(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR pf  = (mes[2] & 0x10) >> 4;				/* poll / final flag */
+	L2UCHAR tei = (mes[1] & 0xfe) >> 1;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_TEI_ASSIGNED:
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+		/* Send DM */
+		Q921SendDM(trunk,
+				trunk->sapi,
+				Q921_RESPONSE(trunk),
+				tei, pf);
+
+		/* no state change */
+		break;
+
+	case Q921_STATE_AWAITING_RELEASE:
+		Q921SendUA(trunk,
+				trunk->sapi,
+				Q921_RESPONSE(trunk),
+				tei, pf);
+
+		/* no state change */
+		break;
+
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+	case Q921_STATE_TIMER_RECOVERY:
+		/* Discard I queue */
+		MFIFOClear(link->IFrameQueue);
+
+		/* send UA */
+		Q921SendUA(trunk,
+				trunk->sapi,
+				Q921_RESPONSE(trunk),
+				tei, pf);
+		
+		/* DL Release indication */
+		Q921Tx23Proc(trunk, Q921_DL_RELEASE, tei, NULL, 0);
+
+		/* Stop T200 */
+		Q921T200TimerStop(trunk, tei);
+
+		if(link->state == Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+			/* Stop T203 */
+			Q921T203TimerStop(trunk, tei);
+		}
+
+		/* change state (no action) */
+		Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED, tei);
+		break;
+
+	default:
+		Q921Log(trunk, Q921_LOG_ERROR, "Invalid DISC received in state \"%s\" (%d)", Q921State2Name(link->state), link->state);
+		break;
+	}
+
+	return 1;
+}
+
+
+/**
+ * Q921ProcRR
+ * \brief	Handle incoming RR
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success, <= 0 on error
+ */
+static int Q921ProcRR(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR cr = (mes[0] & 0x02) >> 1;
+	L2UCHAR pf =  mes[3] & 0x01;		/* poll / final flag */
+	L2UCHAR nr = (mes[3] >> 1);
+//	L2UCHAR	sapi = (mes[0] & 0xfc) >> 2;
+	L2UCHAR	tei  = (mes[1] & 0xfe) >> 1;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* clear receiver peer busy */
+		Q921_CLEAR_FLAG(link, Q921_FLAG_PEER_RECV_BUSY);
+
+		if (Q921_IS_COMMAND(trunk, cr)) { /* if this is a command */
+			if(pf) {
+				/* Enquiry response */
+				Q921SendEnquiryResponse(trunk, tei);
+			}
+		} else {
+			if(pf) {
+				/* MDL Error indication */
+			}
+		}
+
+		/* */
+		if(link->va <= nr && nr <= link->vs) {
+
+			if(nr == link->vs) {
+				/* V(A) = N(R) */
+				link->va = nr;
+
+				/* Stop T200, restart T203 */
+				Q921T200TimerStop(trunk, tei);
+				Q921T203TimerReset(trunk, tei);
+
+			} else if(nr == link->va) {
+
+				/* do nothing */
+
+			} else {
+				/* V(A) = N(R) */
+				link->va = nr;
+
+				/* Restart T200 */
+				Q921T200TimerReset(trunk, tei);
+			}
+			/* no state change */
+
+		} else {
+			/* N(R) Error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		/* clear receiver peer busy */
+		Q921_CLEAR_FLAG(link, Q921_FLAG_PEER_RECV_BUSY);
+
+		/* command + P? */
+		if(Q921_IS_COMMAND(trunk, cr) && pf) {
+			/* Enquiry response */
+			Q921SendEnquiryResponse(trunk, tei);
+		}
+
+		/* */
+		if(link->va <= nr && nr <= link->vs) {
+			/* V(A) = N(R) */
+			link->va = nr;
+
+			if(!Q921_IS_COMMAND(trunk, cr) && pf) {
+				/* Stop T200, start T203 */
+				Q921T200TimerStop(trunk, tei);
+				Q921T203TimerStart(trunk, tei);
+
+				/* Invoke retransmission */
+				Q921InvokeRetransmission(trunk, tei, nr);
+
+				/* change state (no action) */
+				Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED, tei);
+			}
+			/* no state change otherwise */
+		} else {
+			/* N(R) Error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		}
+		break;
+
+	default:
+		break;
+	}
+	return 1;
+}
+
+
+/**
+ * Q921ProcREJ
+ * \brief	Handle incoming REJ
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success, <= 0 on error
+ */
+static int Q921ProcREJ(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR cr = (mes[0] & 0x02) >> 1;
+	L2UCHAR pf =  mes[3] & 0x01;		/* poll / final flag */
+	L2UCHAR nr = (mes[3] >> 1);
+//	L2UCHAR	sapi = (mes[0] & 0xfc) >> 2;
+	L2UCHAR	tei  = (mes[1] & 0xfe) >> 1;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* clear receiver peer busy */
+		Q921_CLEAR_FLAG(link, Q921_FLAG_PEER_RECV_BUSY);
+
+		/* command? */
+		if(Q921_IS_COMMAND(trunk, cr)) {
+			if(pf) {
+				/* Enquiry response */
+				Q921SendEnquiryResponse(trunk, tei);
+			}
+		} else {
+			if(pf) {
+				/* MDL Error indication (A) */
+			}
+		}
+
+		/* */
+		if(link->va <= nr && nr <= link->vs) {
+
+			/* V(A) = N(R) */
+			link->va = nr;
+
+			/* Stop T200, start T203 */
+			Q921T200TimerStop(trunk, tei);
+			Q921T203TimerStart(trunk, tei);
+
+			/* Invoke retransmission of frame >N(R)  (?) */
+			Q921InvokeRetransmission(trunk, tei, nr);
+
+			/* no state change */
+		} else {
+			/* N(R) Error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		/* clear receiver peer busy */
+		Q921_CLEAR_FLAG(link, Q921_FLAG_PEER_RECV_BUSY);
+
+		/* command + P ? */
+		if(Q921_IS_COMMAND(trunk, cr) && pf) {
+			/* Enquiry response */
+			Q921SendEnquiryResponse(trunk, tei);
+		}
+
+		/* */
+		if(link->va <= nr && nr <= link->vs) {
+
+			/* V(A) = N(R) */
+			link->va = nr;
+
+			if(!Q921_IS_COMMAND(trunk, cr) && pf) {
+				/* Stop T200, start T203 */
+				Q921T200TimerStop(trunk, tei);
+				Q921T203TimerStart(trunk, tei);
+
+				/* Invoke retransmission */
+				Q921InvokeRetransmission(trunk, tei, nr);
+
+				/* change state (no action) */
+				Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED, tei);
+			}
+			/* no state change otherwise */
+		} else {
+			/* N(R) Error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 1;
+}
+
+
+/**
+ * Q921ProcRNR
+ * \brief	Handle incoming RNR
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success, <= 0 on error
+ */
+static int Q921ProcRNR(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR cr = (mes[0] & 0x02) >> 1;
+	L2UCHAR pf =  mes[3] & 0x01;		/* poll / final flag */
+	L2UCHAR nr = (mes[3] >> 1);
+//	L2UCHAR	sapi = (mes[0] & 0xfc) >> 2;
+	L2UCHAR	tei  = (mes[1] & 0xfe) >> 1;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* set peer receiver busy */
+		Q921_SET_FLAG(link, Q921_FLAG_PEER_RECV_BUSY);
+
+		/* command? */
+		if(Q921_IS_COMMAND(trunk, cr)) {
+			if(pf) {
+				/* Enquiry response */
+				Q921SendEnquiryResponse(trunk, tei);
+			}
+		} else {
+			if(pf) {
+				/* MDL Error indication (A) */
+			}
+		}
+
+		/* */
+		if(link->va <= nr && nr <= link->vs) {
+
+			/* V(A) = N(R) */
+			link->va = nr;
+
+			/* Stop T203, restart T200 */
+			Q921T200TimerReset(trunk, tei);
+			Q921T203TimerStop(trunk, tei);
+
+			/* no state change */
+		} else {
+			/* N(R) Error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		/* set peer receiver busy */
+		Q921_SET_FLAG(link, Q921_FLAG_PEER_RECV_BUSY);
+
+		/* command + P? */
+		if(Q921_IS_COMMAND(trunk, cr) && pf) {
+			/* Enquiry response */
+			Q921SendEnquiryResponse(trunk, tei);
+		}
+
+		/* */
+		if(link->va <= nr && nr <= link->vs) {
+
+			/* V(A) = N(R) */
+			link->va = nr;
+
+			if(!Q921_IS_COMMAND(trunk, cr) && pf) {
+				/* Restart T200 */
+				Q921T200TimerReset(trunk, tei);
+
+				/* Invoke retransmission */
+				Q921InvokeRetransmission(trunk, tei, nr);
+
+				/* change state (no action) */
+				Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED, tei);
+			}
+			/* no state change otherwise */
+		} else {
+			/* N(R) Error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 1;
+}
+
+#if 0
+static int Q921SetReceiverBusy(L2TRUNK trunk)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		if(!Q921_CHECK_FLAG(link, Q921_FLAG_RECV_BUSY)) {
+			/* set own receiver busy */
+			Q921_SET_FLAG(link, Q921_FLAG_RECV_BUSY);
+
+			/* send RR response */
+			Q921SendRR(trunk, trunk->sapi, Q921_RESPONSE(trunk), link->tei, 0);
+
+			/* clear ack pending */
+			Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		if(!Q921_CHECK_FLAG(link, Q921_FLAG_RECV_BUSY)) {
+			/* set own receiver busy */
+			Q921_SET_FLAG(link, Q921_FLAG_RECV_BUSY);
+
+			/* send RNR response */
+			Q921SendRNR(trunk, trunk->sapi, Q921_RESPONSE(trunk), link->tei, 0);
+
+			/* clear ack pending */
+			Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
 }
 
+static int Q921ClearReceiverBusy(L2TRUNK trunk)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	switch(link->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+	case Q921_STATE_TIMER_RECOVERY:
+		if(Q921_CHECK_FLAG(link, Q921_FLAG_RECV_BUSY)) {
+			/* clear own receiver busy */
+			Q921_CLEAR_FLAG(link, Q921_FLAG_RECV_BUSY);
+
+			/* send RNR response */
+			Q921SendRNR(trunk, trunk->sapi, Q921_RESPONSE(trunk), link->tei, 0);
+
+			/* clear ack pending */
+			Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+#endif
+
+static int Q921ProcIFrame(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	/* common fields: get sapi, tei and cr */
+//	L2UCHAR sapi = (mes[0] & 0xfc) >> 2;
+//	L2UCHAR cr   = (mes[0] & 0x02) >> 1;
+	L2UCHAR tei  = (mes[1] & 0xfe) >> 1;
+	L2UCHAR pf   =  mes[3] & 0x01;		/* poll / final flag */
+	L2UCHAR nr   =  mes[3] >> 1;		/* receive sequence number */
+	L2UCHAR ns   =  mes[2] >> 1;		/* send sequence number */
+	L2UCHAR discard = 0;
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+	/* Ignore I frames in earlier states */
+	if(link->state < Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+		Q921Log(trunk, Q921_LOG_NOTICE, "I frame in invalid state ignored\n");
+		return 0;
+	}
+
+	/* Receiver busy? */
+	if(Q921_CHECK_FLAG(link, Q921_FLAG_RECV_BUSY)) {
+		/* discard information */
+		discard = 1;
+
+		if(pf) {
+			/* send RNR Response */
+			Q921SendRNR(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, 1);
+
+			/* Clear ack pending */
+			Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+		}
+	}
+	else {
+		if(ns != link->vr) {
+			/* discard information */
+			discard = 1;
+
+			if(Q921_CHECK_FLAG(link, Q921_FLAG_REJECT) && pf) {
+
+				/* Send RR response */
+				Q921SendRR(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, 1);
+
+				/* clear ack pending */
+				Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+			}
+			else if(!Q921_CHECK_FLAG(link, Q921_FLAG_REJECT)){
+
+				/* set reject exception */
+				Q921_SET_FLAG(link, Q921_FLAG_REJECT);
+
+				/* Send REJ response */
+				Q921SendREJ(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, pf);
+
+				/* clear ack pending */
+				Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+			}
+		}
+		else {
+			/* V(R) = V(R) + 1 */
+			Q921_INC_COUNTER(link->vr);
+
+			/* clear reject exception */
+			Q921_CLEAR_FLAG(link, Q921_FLAG_REJECT);
+
+			/* DL-Data indication */
+			Q921Tx23Proc(trunk, Q921_DL_DATA, tei, mes, size);
+
+			if(pf) {
+				/* Send RR response */
+				Q921SendRR(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, 1);
+
+				/* clear ack pending */
+				Q921_CLEAR_FLAG(link, Q921_FLAG_ACK_PENDING);
+			}
+			else if(!Q921_CHECK_FLAG(link, Q921_FLAG_ACK_PENDING)) {
+				/* ack pending */
+
+				/* Send RR response */
+				Q921SendRR(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, 0);
+				
+				/* set ack pending*/
+				Q921_SET_FLAG(link, Q921_FLAG_ACK_PENDING);
+			}
+		}
+	}
+
+
+	switch(link->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		if(link->va <= nr && nr <= link->vs) {
+			if(Q921_CHECK_FLAG(link, Q921_FLAG_PEER_RECV_BUSY)) {
+				link->va = nr;
+			}
+			else if(nr == link->vs) {
+				/* V(A) = N(R) */
+				link->va = nr;
+
+				/* stop t200, restart t203 */
+				Q921T200TimerStop(trunk, tei);
+				Q921T203TimerReset(trunk, tei);
+			}
+			else if(nr != link->va) {
+				/* V(A) = N(R) */
+				link->va = nr;
+
+				/* restart T200 */
+				Q921T200TimerReset(trunk, tei);
+			}
+
+			/* Restart TM01 */
+			if(Q921_IS_NT(trunk)) {
+				Q921TM01TimerReset(trunk, tei);
+			}
+		}
+		else {
+			/* N(R) error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		if(link->va <= nr && nr <= link->vs) {
+			/* V(A) = N(R) */
+			link->va = nr;
+
+			/* Restart TM01 */
+			if(Q921_IS_NT(trunk)) {
+				Q921TM01TimerReset(trunk, tei);
+			}
+		}
+		else {
+			/* N(R) error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT, tei);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+
+static int Q921ProcSFrame(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR sv = (mes[2] & 0x0c) >> 2;	/* supervisory format id */
+	//L2UCHAR pf = mes[3] & 0x01;		/* poll / final flag */
+	//L2UCHAR nr = mes[3] >> 1;		/* receive sequence number */
+	L2INT res = -1;
+
+	switch(sv) {
+	case 0x00:	/* RR : Receive Ready */
+		res = Q921ProcRR(trunk, mes, size);
+		break;
+
+	case 0x02:	/* RNR : Receive Not Ready */
+		res = Q921ProcRNR(trunk, mes, size);
+		break;
+
+	case 0x04:	/* REJ : Reject */
+		res = Q921ProcREJ(trunk, mes, size);
+		break;
+
+	default:	/* Invalid / Unknown */
+		Q921Log(trunk, Q921_LOG_ERROR, "Invalid S frame type %d\n", sv);
+		break;
+	}
+
+	return res;
+}
+
+
+
+static int Q921ProcUFrame(L2TRUNK trunk, L2UCHAR *mes, L2INT size) 
+{
+	L2UCHAR m  = (mes[2] & 0xe0) >> 3 | (mes[2] & 0x0c) >> 2;	/* modifier function id */
+//	L2UCHAR pf = (mes[2] & 0x10) >> 4;				/* poll / final flag */
+	L2INT res = -1;
+
+	switch(m) {
+	case 0x00:	/* UN : Unnumbered Information */
+		if(mes[3] == Q921_LAYER_ENT_ID_TEI)
+		{
+			if(!Q921_IS_PTMP(trunk)) {
+				/* wtf? nice try */
+				return res;
+			}
+
+			switch(mes[6]) {
+			case Q921_TEI_ID_REQUEST:	/* (TE ->) NT */
+				res = Q921TeiProcAssignRequest(trunk, mes, size);
+				break;
+
+			case Q921_TEI_ID_ASSIGNED:	/* (NT ->) TE */
+			case Q921_TEI_ID_DENIED:
+				res = Q921TeiProcAssignResponse(trunk, mes, size);
+				break;
+
+			case Q921_TEI_ID_CHECKREQ:	/* (NT ->) TE */
+				res = Q921TeiProcCheckRequest(trunk, mes, size);
+				break;
+
+			case Q921_TEI_ID_CHECKRESP:	/* (TE ->) NT */
+				res = Q921TeiProcCheckResponse(trunk, mes, size);
+				break;
+
+			case Q921_TEI_ID_REMOVE:	/* (NT ->) TE */
+				res = Q921TeiProcRemoveRequest(trunk, mes, size);
+				break;
+
+			case Q921_TEI_ID_VERIFY:	/* (TE ->) NT */
+				res = Q921TeiProcVerifyRequest(trunk, mes, size);
+				break;
+
+			default:			/* Invalid / Unknown */
+				Q921Log(trunk, Q921_LOG_ERROR, "Invalid UN message from TEI management/endpoint\n");
+				break;
+			}
+		}
+		else if(mes[3] == Q921_LAYER_ENT_ID_Q931) {
+
+			Q921Log(trunk, Q921_LOG_DEBUG, "UI Frame for Layer 3 received\n");
+
+			res = Q921Tx23Proc(trunk, Q921_DL_UNIT_DATA, 0, mes, size);
+		}
+		break;
+
+	case 0x03:	/* DM : Disconnect Mode */
+		res = Q921ProcDM(trunk, mes, size);
+		break;
+
+	case 0x08:	/* DISC : Disconnect */
+		res = Q921ProcDISC(trunk, mes, size);
+		break;
+
+	case 0x0c:	/* UA : Unnumbered Acknowledgement */
+		res = Q921ProcUA(trunk, mes, size);
+		break;
+
+	case 0x0f:	/* SABME  : Set Asynchronous Balanced Mode Extend */
+		res = Q921ProcSABME(trunk, mes, size);
+		break;
+
+	case 0x11:	/* FRMR : Frame Reject */
+	case 0x17:	/* XID : Exchange Identification */
+		res = 0;
+		break;
+
+	default:	/* Unknown / Invalid */
+		Q921Log(trunk, Q921_LOG_ERROR, "Invalid U frame type: %d\n", m);
+		break;
+	}
+
+	return res;
+}
+
+
 /*****************************************************************************
 
   Function:     Q921Rx12
@@ -479,80 +2903,597 @@ int Q921ProcSABME(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
 *****************************************************************************/
 int Q921Rx12(L2TRUNK trunk)
 {
-    L2UCHAR *mes;
-    L2INT rs,size;     /* receive size & Q921 frame size*/
-    L2UCHAR *smes = MFIFOGetMesPtr(trunk->HDLCInQueue, &size);
-	L2UCHAR nr;
-
-    if(smes != NULL)
-    {
-        rs = size - trunk->Q921HeaderSpace;
-        mes = &smes[trunk->Q921HeaderSpace];
-
-		if ((mes[2] & 3) != 3) {
-			/* we have an S or I frame */
-			/* if nr is between va and vs, update our va counter */
-			nr = (mes[3] >> 1);
-			if (nr >= trunk->va && nr <= trunk->vs) {
-				trunk->va = nr;
-			}
-		}
-
-        /* check for I frame */
-        if((mes[2] & 0x01) == 0)
-        {
-
-			/* we increment before we "really" know its good so that if we send in the callback, we use the right nr */
-			trunk->vr++;
-            if(Q921Tx23Proc(trunk, smes, size-2)) /* -2 to clip away CRC */
-            {
-				Q921SendRR(trunk, (mes[0]&0xfc)>>2, (mes[0]>>1)&0x01, mes[1]>>1, mes[3]&0x01);
-			}
-            else
-            {
-                /* todo: whatever*/
-				/* trunk->vr--; */
-				/* for now, lets just respond for easier debugging on errors */
-				Q921SendRR(trunk, (mes[0]&0xfc)>>2, (mes[0]>>1)&0x01, mes[1]>>1, mes[3]&0x01);
-           }
-        }
-
-        /* check for RR */
-        else if(mes[2] ==0x01)
+	L2INT size;     /* receive size & Q921 frame size*/
+	L2UCHAR *smes = MFIFOGetMesPtr(trunk->HDLCInQueue, &size);
+
+	if(smes)
+	{
+		struct Q921_Link *link;
+		L2UCHAR sapi, tei;
+		L2UCHAR *mes;
+		L2INT rs;
+
+		rs  = size - trunk->Q921HeaderSpace;
+		mes = &smes[trunk->Q921HeaderSpace];
+
+		Q921LogMesg(trunk, Q921_LOG_DEBUG, 1, mes, rs, "New packet received (%d bytes)", rs);
+
+		/* common fields: get sapi, tei and cr */
+		sapi = (mes[0] & 0xfc) >> 2;
+		tei  = (mes[1] & 0xfe) >> 1;
+		link  = Q921_LINK_CONTEXT(trunk, tei);
+
+		if(Q921_IS_PTMP_TE(trunk) && (
+			 (link->state >= Q921_STATE_TEI_ASSIGNED && tei != link->tei && tei != Q921_TEI_BCAST) ||			/* Assigned TEI: Only BCAST and directed */
+			 (link->state == Q921_STATE_TEI_UNASSIGNED && tei != Q921_TEI_BCAST)))					/* No assigned TEI: Only BCAST */
 		{
-			if (((mes[0]>>1)&0x01) == (trunk->NetUser == Q921_TE ? 1 : 0)) { /* if this is a command */
-				Q921SendRR(trunk, (mes[0]&0xfc)>>2, (mes[0]>>1)&0x01, mes[1]>>1, mes[2]&0x01);
+			/* Ignore Messages with foreign TEIs */
+			goto out;
+		}
+
+		if((mes[2] & 0x01) == 0x00) {		/* I frame */
+			Q921ProcIFrame(trunk, mes, rs);
+		}
+		else if((mes[2] & 0x03) == 0x01) {	/* S frame */
+			Q921ProcSFrame(trunk, mes, rs);
+		}
+		else if((mes[2] & 0x03) == 0x03) {	/* U frame */
+			Q921ProcUFrame(trunk, mes, rs);
+		}
+		else {
+			Q921Log(trunk, Q921_LOG_ERROR, "Invalid frame type: %d\n", (int)(mes[2] & 0x03));
+			/* TODO: send FRMR or REJ */
+		}
+
+out:
+		MFIFOKillNext(trunk->HDLCInQueue);
+
+		return 1;
+	}
+
+	return 0;
+}
+
+/*
+ * Misc
+ */
+/**
+ * Q921SetLogCB
+ * \brief	Set logging callback
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	func	pointer to logging callback function
+ * \param[in]	priv	pointer to private data
+ *
+ * \author	Stefan Knoblich
+ */
+void Q921SetLogCB(L2TRUNK trunk, Q921LogCB_t func, void *priv)
+{
+	if(!trunk)
+		return;
+
+	trunk->Q921LogProc = func;
+	trunk->PrivateDataLog = priv;
+}
+
+/**
+ * Q921SetLogLevel
+ * \brief	Set loglevel of Q.921 logging functions
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	level	new loglevel
+ *
+ * \author	Stefan Knoblich
+ */
+void Q921SetLogLevel(L2TRUNK trunk, Q921LogLevel_t level)
+{
+	if(!trunk)
+		return;
+
+	trunk->loglevel = level;
+}
+
+
+/**
+ * Q921ChangeState
+ * \brief	Change state, invoke neccessary actions
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	state	state to change to
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921ChangeState(L2TRUNK trunk, Q921State_t state, L2UCHAR tei)
+{
+	struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+	Q921State_t oldstate = link->state;
+	int res = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "Changing state from \"%s\" (%d) to \"%s\" (%d) for TEI %d\n",
+				Q921State2Name(oldstate), oldstate,
+				Q921State2Name(state), state,
+				tei);
+
+	/*
+	 * generic actions (depending on the target state only)
+	 */
+	switch(state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* Start TM01 */
+		if(Q921_IS_NT(trunk)) {
+			Q921TM01TimerStart(trunk, tei);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	/*
+	 * actions that depend on type of the old -> new state transition
+	 */
+	switch(oldstate) {
+	case Q921_STATE_STOPPED:
+
+		switch(state) {
+		case Q921_STATE_TEI_UNASSIGNED:
+			if(Q921_IS_PTMP_TE(trunk)) {
+				res = Q921TeiSendAssignRequest(trunk);
 			}
+			break;
 
-        }
-		
-        /* check for RNR */
-        /* check for REJ */
-        /* check for SABME */
-        else if((mes[2] & 0xef) == 0x6f)
-        {
-            Q921ProcSABME(trunk, mes, rs);
-            Q921SendUA(trunk, (mes[0]&0xfc)>>2, (mes[0]>>1)&0x01,mes[1]>>1, (mes[2]&0x10)>>4);
-        }
-
-        /* check for DM */
-        /* check for UI */
-        /* check for DISC */
-        /* check for UA */
-        /* check for FRMR */
-        /* check for XID */
-
-        else
-        {
-            /* what the ? Issue an error */
-			/* Q921ErrorProc(trunk, Q921_UNKNOWNFRAME, mes, rs); */
-            /* todo: REJ or FRMR */
-        }
-
-        MFIFOKillNext(trunk->HDLCInQueue);
-
-        return 1;
-    }
-    return 0;
+		case Q921_STATE_TEI_ASSIGNED:
+			if(Q921_IS_PTMP_NT(trunk)) {
+				res = Q921TeiSendRemoveRequest(trunk, Q921_TEI_BCAST);
+			}
+			break;
+
+		default:
+			break;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	link->state = state;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "Q921ChangeState() returns %d, new state is \"%s\" (%d) for TEI %d\n", res, Q921State2Name(state), state, tei);
+
+	return res;
+}
+
+/*
+ * TEI Management functions
+ * \note	All TEI-mgmt UN frames are sent with cr = command!
+ */
+static int Q921TeiSend(L2TRUNK trunk, L2UCHAR type, L2USHORT ri, L2UCHAR ai)
+{
+	L2UCHAR mes[10];
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+
+	mes[offset++] = Q921_LAYER_ENT_ID_TEI;	/* layer management entity identifier */
+	mes[offset++] = (ri & 0xff00) >> 8;	/* reference number upper part */
+	mes[offset++] =  ri & 0xff;		/* reference number lower part */
+	mes[offset++] = type;			/* message type: Identity Request */
+	mes[offset++] = ai << 1 | 0x01;		/* action indicator: TEI */
+
+	return Q921SendU(trunk, Q921_SAPI_TEI, Q921_COMMAND(trunk), Q921_TEI_BCAST, 0, 0x00, mes, offset);
+}
+
+
+/**
+ * Q921TeiSendAssignRequest
+ * \brief	Ask for new TEI (TE mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \return	> 0 on success, <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiSendAssignRequest(L2TRUNK trunk)
+{
+	struct Q921_Link *link = Q921_TRUNK_CONTEXT(trunk);
+	L2INT res;
+
+	if (!Q921_IS_PTMP_TE(trunk))	/* only ptmp te mode*/
+		return 0;
+
+	link->ri = (L2USHORT)(random() % 0xffff);
+
+	/* send TEI assign request */
+	res = Q921TeiSend(trunk, Q921_TEI_ID_REQUEST, link->ri, Q921_TEI_BCAST);
+
+	/* start T202 */
+	Q921T202TimerStart(trunk);
+
+	return res;
 }
 
+
+/**
+ * Q921TeiProcessAssignResponse
+ * \brief	Process assign response (TE mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiProcAssignResponse(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	struct Q921_Link *link = Q921_TRUNK_CONTEXT(trunk);
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+	L2USHORT ri = 0;
+
+	if (!Q921_IS_PTMP_TE(trunk))	/* PTMP TE only */
+		return 0;
+
+	ri = (mes[offset + 1] << 8) | mes[offset + 2];
+
+	if(ri != link->ri) {
+		/* hmmm ..., not our response i guess */
+		return 0;
+	}
+
+	switch(mes[offset + 3]) {
+	case Q921_TEI_ID_ASSIGNED:
+		/* Yay, use the new TEI and change state to assigned */
+		link->tei      = mes[offset + 4] >> 1;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "Assigned TEI %d, setting state to TEI_ASSIGNED\n", link->tei);
+
+		Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED, link->tei);
+		break;
+
+	case Q921_TEI_ID_DENIED:
+		/* oops, what to do now? */
+		if ((mes[offset + 4] >> 1) == Q921_TEI_BCAST) {
+			/* No more free TEIs? this is bad */
+
+			//Q921TeiSendVerifyRequest(trunk, Q921_TEI_BCAST); /* TODO: does this work ?? */
+		} else {
+			/* other reason, this is fatal, shutdown link */
+		}
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "TEI assignment has been denied, reason: %s\n",
+			 ((mes[offset +4] >> 1) == Q921_TEI_BCAST) ? "No free TEIs available" : "Unknown");
+
+		Q921ChangeState(trunk, Q921_STATE_TEI_UNASSIGNED, link->tei);
+		break;
+
+	default:
+		return 0;
+	}
+
+	/* stop T202 */
+	Q921T202TimerStop(trunk);
+
+	return 1;
+}
+
+
+/**
+ * Q921TeiSendVerifyRequest
+ * \brief	Verify TEI (TE mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiSendVerifyRequest(L2TRUNK trunk)
+{
+	struct Q921_Link *link = Q921_TRUNK_CONTEXT(trunk);
+	L2INT res;
+
+	if (!Q921_IS_PTMP_TE(trunk))	/* only ptmp te mode*/
+		return 0;
+
+	/* Request running? */
+	if (trunk->T202)
+		return 0;
+
+	/* Send TEI verify request */
+	res = Q921TeiSend(trunk, Q921_TEI_ID_VERIFY, link->ri, link->tei);
+
+	/* start T202 */
+	Q921T202TimerStart(trunk);
+
+	return res;
+}
+
+
+/**
+ * Q921TeiProcCheckRequest
+ * \brief	Process Check Request (TE mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiProcCheckRequest(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	struct Q921_Link *link = Q921_TRUNK_CONTEXT(trunk);
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+	L2UCHAR tei = (mes[offset + 4] >> 1);		/* action indicator => tei */
+	L2INT res = 0;
+
+	if (!Q921_IS_PTMP_TE(trunk))	/* ptmp te mode only */
+		return 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "Received TEI Check request for TEI %d\n", tei);
+
+	if (tei == Q921_TEI_BCAST || tei == link->tei) {
+		/*
+		 * Broadcast TEI check or for our assigned TEI
+		 */
+
+		/* send TEI check reponse */
+		res = Q921TeiSend(trunk, Q921_TEI_ID_CHECKRESP, link->ri, link->tei);
+
+		Q921T202TimerStop(trunk);
+	}
+
+	return res;
+}
+
+
+/**
+ * Q921TeiProcRemoveRequest
+ * \brief	Process remove Request (TE mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiProcRemoveRequest(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	struct Q921_Link *link = Q921_TRUNK_CONTEXT(trunk);
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+	L2UCHAR tei = (mes[offset + 4] >> 1);		/* action indicator => tei */
+	L2INT res = 0;
+
+	if (!Q921_IS_PTMP_TE(trunk))	/* ptmp te mode only */
+		return 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "Received TEI Remove request for TEI %d\n", tei);
+
+	if (tei == Q921_TEI_BCAST || tei == link->tei) {
+		/*
+		 * Broadcast TEI remove or for our assigned TEI
+		 */
+
+		/* reset tei */
+		link->tei  = 0;
+
+		/* change state (no action) */
+		Q921ChangeState(trunk, Q921_STATE_TEI_UNASSIGNED, link->tei);
+
+		/* TODO: hmm, request new one ? */
+		res = Q921TeiSendAssignRequest(trunk);
+	}
+	return res;
+}
+
+
+/**
+ * Q921TeiProcAssignRequest
+ * \brief	Process assign request from peer (NT mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiProcAssignRequest(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+	L2USHORT ri = 0;
+	L2UCHAR tei = 0;
+
+	if (!Q921_IS_PTMP_NT(trunk))	/* PTMP NT only */
+		return 0;
+
+	ri  = (mes[offset + 1] << 8) | mes[offset + 2];
+	tei =  mes[offset + 4] >> 1;
+
+	if(tei == Q921_TEI_BCAST) {
+		int x;
+
+		/* dynamically allocate TEI */
+		for(x = Q921_TEI_DYN_MIN, tei = 0; x <= Q921_TEI_MAX; x++) {
+			if(!trunk->tei_map[x]) {
+				tei = x;
+				break;
+			}
+		}
+	}
+	else if(!(tei > 0 && tei < Q921_TEI_DYN_MIN)) {
+		/* reject TEIs that are not in the static area */
+		Q921TeiSendDenyResponse(trunk, 0, ri);
+
+		return 0;
+	}
+
+	if(!tei) {
+		/* no free TEI found */
+		Q921TeiSendDenyResponse(trunk, Q921_TEI_BCAST, ri);
+	}
+	else {
+		struct Q921_Link *link = Q921_LINK_CONTEXT(trunk, tei);
+
+		/* mark used */
+		trunk->tei_map[tei] = 1;
+
+		/* assign tei */
+		link->tei = tei;
+
+		/* put context in TEI ASSIGNED state */
+		Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED, tei);
+
+		/* send assign response */
+		Q921TeiSendAssignedResponse(trunk, tei, ri);
+
+		/* Start T201 */
+		Q921T201TimerStart(trunk, tei);
+	}
+	return 0;
+}
+
+/**
+ * Q921TeiSendCheckRequest
+ * \brief	Send check request (NT mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	tei	TEI to check
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiSendCheckRequest(L2TRUNK trunk, L2UCHAR tei)
+{
+	L2INT res = 0;
+
+	if (!Q921_IS_PTMP_NT(trunk))	/* PTMP NT only */
+		return 0;
+
+	/* send TEI check request */
+	res = Q921TeiSend(trunk, Q921_TEI_ID_CHECKREQ, 0, tei);
+
+	/* (Re-)Start T201 timer */
+	Q921T201TimerStart(trunk, tei);
+
+	return res;
+}
+
+/**
+ * Q921TeiProcCheckResponse
+ * \brief	Process Check Response (NT mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiProcCheckResponse(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	struct Q921_Link *link;
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+	L2USHORT ri = 0;
+	L2UCHAR tei = 0;
+
+	if (!Q921_IS_PTMP_NT(trunk))	/* PTMP NT mode only */
+		return 0;
+
+	ri  = (mes[offset + 1] << 8) | mes[offset + 2];
+	tei =  mes[offset + 4] >> 1;
+
+	/* restart T201 */
+	Q921T201TimerStop(trunk, tei);
+
+	/* reset counter */
+	link       = Q921_LINK_CONTEXT(trunk, tei);
+	link->N202 = 0;
+
+	if(!(tei > 0 && tei < Q921_TEI_MAX) || !trunk->tei_map[tei]) {
+		/* TODO: Should we send a DISC first? */
+
+		/* TEI not assigned? Invalid TEI? */
+		Q921TeiSendRemoveRequest(trunk, tei);
+
+		/* change state */
+		Q921ChangeState(trunk, Q921_STATE_STOPPED, tei);
+
+		/* clear */
+		memset(link, 0, sizeof(struct Q921_Link));
+	} else {
+		/* Start T201 */
+		Q921T201TimerStart(trunk, tei);
+	}
+
+	return 0;
+}
+
+
+/**
+ * Q921TeiProcVerifyRequest
+ * \brief	Process Verify Request (NT mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	mes	pointer to message buffer
+ * \param[in]	size	size of message
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiProcVerifyRequest(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR resp[25];
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+	L2UCHAR tei = 0;
+
+	if (!Q921_IS_PTMP_NT(trunk))	/* PTMP NT mode only */
+		return 0;
+
+	tei = mes[offset + 4] >> 1;
+
+	/* todo: handle response... verify assigned TEI */
+	resp[offset + 0] = 0;
+
+	return 0;
+}
+
+/**
+ * Q921TeiSendDenyResponse
+ * \brief	Send Deny Response (NT mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiSendDenyResponse(L2TRUNK trunk, L2UCHAR tei, L2USHORT ri)
+{
+	if (!Q921_IS_PTMP_NT(trunk))	/* PTMP NT only */
+		return 0;
+
+	return Q921TeiSend(trunk, Q921_TEI_ID_DENIED, ri, tei);
+}
+
+
+/**
+ * Q921TeiSendAssignedResponse
+ * \brief	Send Assigned Response (NT mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	tei	TEI to assign
+ * \param[in]	ri	RI of request
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiSendAssignedResponse(L2TRUNK trunk, L2UCHAR tei, L2USHORT ri)
+{
+	if (!Q921_IS_PTMP_NT(trunk))	/* PTMP NT only */
+		return 0;
+
+	return Q921TeiSend(trunk, Q921_TEI_ID_ASSIGNED, ri, tei);
+}
+
+/**
+ * Q921TeiSendRemoveRequest
+ * \brief	Send Remove Request (NT mode only)
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	tei	TEI to remove
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921TeiSendRemoveRequest(L2TRUNK trunk, L2UCHAR tei)
+{
+	if (!Q921_IS_PTMP_NT(trunk))	/* PTMP NT only */
+		return 0;
+
+	return Q921TeiSend(trunk, Q921_TEI_ID_REMOVE, 0, tei);
+}
diff --git a/src/isdn/Q931.c b/src/isdn/Q931.c
index 12ebde2..0cede6f 100644
--- a/src/isdn/Q931.c
+++ b/src/isdn/Q931.c
@@ -36,6 +36,7 @@
   POSSIBILITY OF SUCH DAMAGE.
 *****************************************************************************/
 
+#include "Q921.h"
 #include "Q931.h"
 #include "national.h"
 #include "DMS.h"
@@ -67,6 +68,8 @@ q931pmes_func_t *Q931Pmes[Q931MAXDLCT][Q931MAXMES];
 q931uie_func_t *Q931Uie[Q931MAXDLCT][Q931MAXIE];
 q931pie_func_t *Q931Pie[Q931MAXDLCT][Q931MAXIE];
 
+q931timeout_func_t *Q931Timeout[Q931MAXDLCT][Q931MAXTIMER];
+q931timer_t         Q931Timer[Q931MAXDLCT][Q931MAXTIMER];
 
 void  (*Q931CreateDialectCB[Q931MAXDLCT])       (L3UCHAR iDialect)=
 {
@@ -250,6 +253,10 @@ void Q931Initialize()
             Q931Pie[x][y] = Q931PieDummy;
             Q931Uie[x][y] = Q931UieDummy;
         }
+	for(y=0; y < Q931MAXTIMER; y++) {
+		Q931Timeout[x][y] = Q931TimeoutDummy;
+		Q931Timer[x][y]   = 0;
+	}
     }
 
     if(Q931CreateDialectCB[Q931_Dialect_Q931 + Q931_TE] == NULL)
@@ -286,28 +293,38 @@ void Q931Initialize()
     }
 }
 
-/*****************************************************************************
-
-  Function:     Q931TimeTick
-
-  Description:  Called periodically from an external source to allow the 
-                stack to process and maintain it's own timers.
-
-  Parameters:   ms[IN]        Milliseconds since last call.
-
-  Return Value: none
-
-*****************************************************************************/
-void Q931TimeTick(Q931_TrunkInfo_t *pTrunk, L3ULONG ms)
-{
-    (void)pTrunk;
-    ms=ms; /* avoid warning for now. */
-
-    /*  TODO: Loop through all active calls, check timers and call timout procs
-     *  if timers are expired.
-     *  Implement a function array so each dialect can deal with their own
-     *  timeouts.
-    */
+/**
+ * Q931TimerTick
+ * \brief	Periodically called to update and check for expired timers
+ * \param	pTrunk	Q.931 trunk
+ */
+void Q931TimerTick(Q931_TrunkInfo_t *pTrunk)
+{
+	struct Q931_Call *call = NULL;
+	L3ULONG now = 0;
+	L3INT x;
+
+	/*  TODO: Loop through all active calls, check timers and call timout procs
+	 *  if timers are expired.
+	 *  Implement a function array so each dialect can deal with their own
+	 *  timeouts.
+	 */
+	now = Q931GetTime();
+
+	for(x = 0; x < Q931MAXCALLPERTRUNK; x++) {
+		call = &pTrunk->call[x];
+
+		if(!call->InUse || !call->Timer || !call->TimerID)
+			continue;
+
+		if(call->Timer <= now) {
+			/* Stop Timer */
+			Q931StopTimer(pTrunk, x, call->TimerID);
+
+			/* Invoke dialect timeout callback */
+			Q931Timeout[pTrunk->Dialect][call->TimerID](pTrunk, x);
+		}
+	}
 }
 
 /*****************************************************************************
@@ -331,39 +348,65 @@ void Q931TimeTick(Q931_TrunkInfo_t *pTrunk, L3ULONG ms)
                 see q931errors.h for details.
 
 *****************************************************************************/
-L3INT Q931Rx23(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT Size)
-{
-    L3UCHAR *Mes = &buf[Q931L2HeaderSpace];
-    L3INT RetCode = Q931E_NO_ERROR;
-    Q931mes_Generic *m = (Q931mes_Generic *) pTrunk->L3Buf;
-    L3INT ISize;
-
-    L3INT IOff = 0;
-
-	/* Reset our decode buffer */
-	memset(pTrunk->L3Buf, 0, sizeof(pTrunk->L3Buf));
-
-	/* Protocol Discriminator */
-    m->ProtDisc = Mes[IOff++];
-
-    /* CRV */
-    m->CRVFlag = Mes[IOff + 1] & 0x80;
-    m->CRV = Q931Uie_CRV(pTrunk, Mes, m->buf, &IOff, &ISize);
-
-    /* Message Type */
-    m->MesType = Mes[IOff++];
-
-    /* Call table proc to unpack codec message */
-    /*debug */
-    /* printf("\n\nQ931Rx23-  Dialect: %d, MsgType: %d\n",pTrunk->Dialect,m->MesType); */
-    RetCode = Q931Umes[pTrunk->Dialect][m->MesType](pTrunk, Mes, (Q931mes_Generic *)pTrunk->L3Buf, Q931L4HeaderSpace + IOff , Size - Q931L4HeaderSpace - IOff + 1);
-
-    if(RetCode >= Q931E_NO_ERROR)
-    {
-        RetCode = Q931Proc[pTrunk->Dialect][m->MesType](pTrunk, pTrunk->L3Buf, 2);
-    }
+L3INT Q931Rx23(Q931_TrunkInfo_t *pTrunk, L3INT ind, L3UCHAR tei, L3UCHAR * buf, L3INT Size)
+{
+	L3UCHAR *Mes = NULL;
+	L3INT RetCode = Q931E_NO_ERROR;
+	Q931mes_Generic *m = (Q931mes_Generic *) pTrunk->L3Buf;
+	L3INT ISize;
+	L3INT IOff = 0;
+	L3INT L2HSize = Q931L2HeaderSpace;
+
+	switch(ind) {
+	case Q921_DL_UNIT_DATA:		/* DL-UNITDATA indication (UI frame, 3 byte header) */
+		L2HSize = 3;
+
+	case Q921_DL_DATA:		/* DL-DATA indication (I frame, 4 byte header) */
+		/* Reset our decode buffer */
+		memset(pTrunk->L3Buf, 0, sizeof(pTrunk->L3Buf));
+
+		/* L2 Header Offset */
+		Mes = &buf[L2HSize];
+
+		/* Protocol Discriminator */
+		m->ProtDisc = Mes[IOff++];
+
+		/* CRV */
+		m->CRVFlag = (Mes[IOff + 1] >> 7) & 0x01;
+		m->CRV = Q931Uie_CRV(pTrunk, Mes, m->buf, &IOff, &ISize);
+
+		/* Message Type */
+		m->MesType = Mes[IOff++];
+
+		/* d'oh a little ugly but this saves us from:
+		 *	a) doing Q.921 work in the lower levels (extracting the TEI ourselves)
+		 *	b) adding a tei parameter to _all_ Proc functions
+		 */
+		if(tei) {
+			L3INT callIndex = 0;
+
+			/* Find the call using CRV */
+			RetCode = Q931FindCRV(pTrunk, m->CRV, &callIndex);
+			if(RetCode == Q931E_NO_ERROR && !pTrunk->call[callIndex].Tei) {
+				pTrunk->call[callIndex].Tei = tei;
+			}
+		}
+
+		Q931Log(pTrunk, Q931_LOG_DEBUG, "Received message from Q.921 (ind %d, tei %d, size %d)\nMesType: %d, CRVFlag %d (%s), CRV %d (Dialect: %d)\n", ind, tei, Size,
+						 m->MesType, m->CRVFlag, m->CRVFlag ? "Terminator" : "Originator", m->CRV, pTrunk->Dialect);
+
+		RetCode = Q931Umes[pTrunk->Dialect][m->MesType](pTrunk, Mes, (Q931mes_Generic *)pTrunk->L3Buf, IOff, Size - L2HSize);
+		if(RetCode >= Q931E_NO_ERROR)
+		{
+			RetCode = Q931Proc[pTrunk->Dialect][m->MesType](pTrunk, pTrunk->L3Buf, 2);
+		}
+		break;
+
+	default:
+		break;
+	}
 
-    return RetCode;
+	return RetCode;
 }
 
 /*****************************************************************************
@@ -381,6 +424,8 @@ L3INT Q931Rx23(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT Size)
 *****************************************************************************/
 L3INT Q931Tx34(Q931_TrunkInfo_t *pTrunk, L3UCHAR * Mes, L3INT Size)
 {
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Sending message to Layer4 (size: %d)\n", Size);
+
     if (pTrunk->Q931Tx34CBProc) {
         return pTrunk->Q931Tx34CBProc(pTrunk->PrivateData34, Mes, Size);
     }
@@ -406,7 +451,11 @@ L3INT Q931Rx43(Q931_TrunkInfo_t *pTrunk,L3UCHAR * buf, L3INT Size)
     Q931mes_Header *ptr = (Q931mes_Header*)&buf[Q931L4HeaderSpace];
     L3INT RetCode = Q931E_NO_ERROR;
 
-    RetCode=Q931Proc[pTrunk->Dialect][ptr->MesType](pTrunk,buf,4);
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Receiving message from Layer4 (size: %d, type: %d)\n", Size, ptr->MesType);
+
+    RetCode = Q931Proc[pTrunk->Dialect][ptr->MesType](pTrunk, buf, 4);
+
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Q931Rx43 return code: %d\n", RetCode);
 
     return RetCode;
 }
@@ -428,25 +477,40 @@ L3INT Q931Rx43(Q931_TrunkInfo_t *pTrunk,L3UCHAR * buf, L3INT Size)
                 see q931errors.h for details.
 
 *****************************************************************************/
-L3INT Q931Tx32(Q931_TrunkInfo_t *pTrunk, L3UCHAR * Mes, L3INT Size)
+L3INT Q931Tx32Data(Q931_TrunkInfo_t *pTrunk, L3UCHAR bcast, L3UCHAR * Mes, L3INT Size)
 {
-    L3INT     OSize;
-    Q931mes_Generic *ptr = (Q931mes_Generic*)Mes;
-    L3INT RetCode = Q931E_NO_ERROR;
-    L3INT iDialect = pTrunk->Dialect;
+	Q931mes_Generic *ptr = (Q931mes_Generic*)Mes;
+	L3INT RetCode = Q931E_NO_ERROR;
+	L3INT iDialect = pTrunk->Dialect;
+	L3INT Offset = bcast ? (Q931L2HeaderSpace - 1) : Q931L2HeaderSpace;
+	L3INT OSize;
 
-    /* Call pack function through table. */
-    RetCode = Q931Pmes[iDialect][ptr->MesType](pTrunk, (Q931mes_Generic *)Mes, Size, &pTrunk->L2Buf[Q931L2HeaderSpace], &OSize);
-    if(RetCode >= Q931E_NO_ERROR)
-    {
-        if (pTrunk->Q931Tx32CBProc) {
-            RetCode = pTrunk->Q931Tx32CBProc(pTrunk->PrivateData32, pTrunk->L2Buf, OSize + Q931L2HeaderSpace);
-        } else {
-            RetCode = Q931E_MISSING_CB;
-        }
-    }
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Sending message to Q.921 (size: %d)\n", Size);
 
-    return RetCode;
+	memset(pTrunk->L2Buf, 0, sizeof(pTrunk->L2Buf));
+
+	/* Call pack function through table. */
+	RetCode = Q931Pmes[iDialect][ptr->MesType](pTrunk, (Q931mes_Generic *)Mes, Size, &pTrunk->L2Buf[Offset], &OSize);
+	if(RetCode >= Q931E_NO_ERROR)
+	{
+		L3INT callIndex;
+		L3UCHAR tei = 0;
+
+		/* Find the call using CRV */
+		RetCode = Q931FindCRV(pTrunk, ptr->CRV, &callIndex);
+		if(RetCode != Q931E_NO_ERROR)
+			return RetCode;
+
+		tei = pTrunk->call[callIndex].Tei;
+
+		if (pTrunk->Q931Tx32CBProc) {
+			RetCode = pTrunk->Q931Tx32CBProc(pTrunk->PrivateData32, bcast ? Q921_DL_UNIT_DATA : Q921_DL_DATA, tei, pTrunk->L2Buf, OSize + Offset);
+		} else {
+			RetCode = Q931E_MISSING_CB;
+		}
+	}
+
+	return RetCode;
 }
 
 
@@ -575,28 +639,40 @@ L3INT    Q931GetCallState(Q931_TrunkInfo_t *pTrunk, L3INT iCRV)
     return 0; /* assume state zero for non existing CRV's */
 }
 
-/*****************************************************************************
-
-  Function:     Q931StartTimer
-
-  Description:  Start a timer.
-
-  Parameters:   pTrunk      Trunk number
-                callindex   call index.
-                iTimer      timer id
-*****************************************************************************/
+/**
+ * Q931StartTimer
+ * \brief	Start a timer
+ * \param	pTrunk		Q.931 trunk
+ * \param	callindex	Index of the call
+ * \param	iTimerID	ID of timer
+ * \return	always 0
+ */
 L3INT Q931StartTimer(Q931_TrunkInfo_t *pTrunk, L3INT callIndex, L3USHORT iTimerID)
 {
-    pTrunk->call[callIndex].Timer   = Q931GetTime();  
-    pTrunk->call[callIndex].TimerID = iTimerID;
-    return 0;
+	L3ULONG duration = Q931Timer[pTrunk->Dialect][iTimerID];
+
+	if(duration) {
+		pTrunk->call[callIndex].Timer   = Q931GetTime() + duration;
+		pTrunk->call[callIndex].TimerID = iTimerID;
+	}
+
+	return 0;
 }
 
+/**
+ * Q931StopTimer
+ * \brief	Stop a timer
+ * \param	pTrunk		Q.931 trunk
+ * \param	callindex	Index of the call
+ * \param	iTimerID	ID of timer
+ * \return	always 0
+ */
 L3INT Q931StopTimer(Q931_TrunkInfo_t *pTrunk, L3INT callindex, L3USHORT iTimerID)
 {
-    if(pTrunk->call[callindex].TimerID == iTimerID)
-        pTrunk->call[callindex].TimerID = 0;
-    return 0;
+	if(pTrunk->call[callindex].TimerID == iTimerID)
+		pTrunk->call[callindex].TimerID = 0;
+
+	return 0;
 }
 
 L3INT Q931SetState(Q931_TrunkInfo_t *pTrunk, L3INT callIndex, L3INT iState)
@@ -609,7 +685,8 @@ L3INT Q931SetState(Q931_TrunkInfo_t *pTrunk, L3INT callIndex, L3INT iState)
 L3ULONG Q931GetTime()
 {
     L3ULONG tNow = 0;
-    static L3ULONG tLast={0};
+    static L3ULONG tLast = 0;
+
     if(Q931GetTimeProc != NULL)
     {
         tNow = Q931GetTimeProc();
@@ -758,5 +835,68 @@ const char *q931_error_to_name(q931_error_t error)
     }
     return q931_error_names[index];
 }
+/*
+ * Logging
+ */
+#include <stdarg.h>
+
+L3INT Q931Log(Q931_TrunkInfo_t *trunk, Q931LogLevel_t level, const char *fmt, ...)
+{
+	char  buf[Q931_LOGBUFSIZE];
+	L3INT len;
+	va_list ap;
+
+	if(!trunk->Q931LogCBProc)
+		return 0;
+
+	if(trunk->loglevel < level)
+		return 0;
+
+	va_start(ap, fmt);
+
+	len = vsnprintf(buf, sizeof(buf)-1, fmt, ap);
+	if(len <= 0) {
+		/* TODO: error handling */
+		return -1;
+	}
+	if(len >= sizeof(buf))
+		len = sizeof(buf) - 1;
+
+	buf[len] = '\0';
+
+	va_end(ap);
+
+	return trunk->Q931LogCBProc(trunk->PrivateDataLog, level, buf, len);
+}
 
+/**
+ * Q921SetLogCB
+ * \brief	Set Logging callback function and private data
+ */
+void Q931SetLogCB(Q931_TrunkInfo_t *trunk, Q931LogCB_t func, void *priv)
+{
+	trunk->Q931LogCBProc = func;
+	trunk->PrivateDataLog = priv;
+}
+
+/**
+ * Q921SetLogLevel
+ * \brief	Set Loglevel
+ */
+void Q931SetLogLevel(Q931_TrunkInfo_t *trunk, Q931LogLevel_t level)
+{
+	trunk->loglevel = level;
+}
+
+/**
+ * Q931TimeoutDummy
+ * \brief	Dummy handler for timeouts
+ * \param	pTrunk		Q.931 trunk
+ * \param	callIndex	Index of call
+ */
+L3INT Q931TimeoutDummy(Q931_TrunkInfo_t *pTrunk, L3INT callIndex)
+{
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Timer %d of call %d (CRV: %d) timed out\n", pTrunk->call[callIndex].TimerID, callIndex, pTrunk->call[callIndex].CRV);
 
+	return 0;
+}
diff --git a/src/isdn/Q931StateNT.c b/src/isdn/Q931StateNT.c
index 3343170..af74ef0 100644
--- a/src/isdn/Q931StateNT.c
+++ b/src/isdn/Q931StateNT.c
@@ -116,11 +116,34 @@ void Q931CreateNT(L3UCHAR i)
     Q931SetIEProc(Q931ie_HIGH_LAYER_COMPATIBILITY,         i,Q931Pie_HLComp,			Q931Uie_HLComp);
     Q931SetIEProc(Q931ie_USER_USER,                        i,Q931Pie_UserUser,			Q931Uie_UserUser);
 
+	Q931SetIEProc(Q931ie_CONNECTED_NUMBER, i, Q931Pie_Generic, Q931Uie_Generic);
+	Q931SetIEProc(Q931ie_FACILITY,         i, Q931Pie_Generic, Q931Uie_Generic);
+
 	/* The following define a state machine. The point is that the Message	*/
 	/* procs can when search this to find out if the message/state			*/
 	/* combination is legale. If not, the proc for unexpected message apply.*/
 
 	/* TODO define state table here */
+
+
+	/* Timer default values */
+	Q931SetTimerDefault(i, Q931_TIMER_T301, 180000);	/* T301: 180s */
+	Q931SetTimerDefault(i, Q931_TIMER_T302,  15000);	/* T302:  15s */
+	Q931SetTimerDefault(i, Q931_TIMER_T303,   4000);	/* T303:   4s */
+	Q931SetTimerDefault(i, Q931_TIMER_T304,  20000);	/* T304:  20s */
+	Q931SetTimerDefault(i, Q931_TIMER_T305,  30000);	/* T305:  30s */
+	Q931SetTimerDefault(i, Q931_TIMER_T306,  30000);	/* T306:  30s */
+	Q931SetTimerDefault(i, Q931_TIMER_T307, 180000);	/* T307: 180s */
+	Q931SetTimerDefault(i, Q931_TIMER_T308,   4000);	/* T308:   4s */
+	Q931SetTimerDefault(i, Q931_TIMER_T309,  60000);	/* T309:  60s */
+	Q931SetTimerDefault(i, Q931_TIMER_T310,  10000);	/* T310:  10s */
+	Q931SetTimerDefault(i, Q931_TIMER_T312,  12000);	/* T312:  12s */
+	Q931SetTimerDefault(i, Q931_TIMER_T314,   4000);	/* T314:   4s */
+	Q931SetTimerDefault(i, Q931_TIMER_T316, 120000);	/* T316: 120s */
+	Q931SetTimerDefault(i, Q931_TIMER_T317,  90000);	/* T317:  90s */
+	Q931SetTimerDefault(i, Q931_TIMER_T320,  30000);	/* T320:  30s */
+	Q931SetTimerDefault(i, Q931_TIMER_T321,  30000);	/* T321:  30s */
+	Q931SetTimerDefault(i, Q931_TIMER_T322,   4000);	/* T322:   4s */
 }
 
 /*****************************************************************************
@@ -143,7 +166,7 @@ L3INT Q931ProcAlertingNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
     Q931StartTimer(pTrunk, callIndex, 303);
 	if(iFrom == 4)
 	{
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -175,7 +198,7 @@ L3INT Q931ProcCallProceedingNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iF
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -208,7 +231,7 @@ L3INT Q931ProcConnectNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -241,7 +264,7 @@ L3INT Q931ProcConnectAckNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -274,7 +297,7 @@ L3INT Q931ProcProgressNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -313,7 +336,10 @@ L3INT Q931ProcSetupNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
             return ret;
         pMes->CRV = pTrunk->call[callIndex].CRV;
 
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+	/*
+	 * Outgoing SETUP message will be broadcasted in PTMP mode
+	 */
+        ret = Q931Tx32Data(pTrunk, Q931_IS_PTP(pTrunk) ? 0 : 1, buf, pMes->Size);
         if(ret)
             return ret;
 
@@ -389,7 +415,7 @@ L3INT Q931ProcSetupAckNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -422,7 +448,7 @@ L3INT Q931ProcResumeNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -455,7 +481,7 @@ L3INT Q931ProcResumeAckNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -488,7 +514,7 @@ L3INT Q931ProcResumeRejectNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFro
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -521,7 +547,7 @@ L3INT Q931ProcSuspendNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -554,7 +580,7 @@ L3INT Q931ProcSuspendAckNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -587,7 +613,7 @@ L3INT Q931ProcSuspendRejectNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFr
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -620,7 +646,7 @@ L3INT Q931ProcUserInformationNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT i
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -653,7 +679,7 @@ L3INT Q931ProcDisconnectNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -686,7 +712,7 @@ L3INT Q931ProcReleaseNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -719,7 +745,7 @@ L3INT Q931ProcReleaseCompleteNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT i
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -752,7 +778,7 @@ L3INT Q931ProcRestartNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -785,7 +811,7 @@ L3INT Q931ProcRestartAckNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -818,7 +844,7 @@ L3INT Q931ProcCongestionControlNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -851,7 +877,7 @@ L3INT Q931ProcInformationNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -884,7 +910,7 @@ L3INT Q931ProcNotifyNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -917,7 +943,7 @@ L3INT Q931ProcStatusNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -950,7 +976,7 @@ L3INT Q931ProcStatusEnquiryNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFr
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -983,7 +1009,7 @@ L3INT Q931ProcSegmentNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1020,7 +1046,7 @@ L3INT Q932ProcFacilityNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1053,7 +1079,7 @@ L3INT Q932ProcHoldNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1086,7 +1112,7 @@ L3INT Q932ProcHoldAckNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1119,7 +1145,7 @@ L3INT Q932ProcHoldRejectNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1152,7 +1178,7 @@ L3INT Q932ProcRegisterNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1185,7 +1211,7 @@ L3INT Q932ProcRetrieveNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1218,7 +1244,7 @@ L3INT Q932ProcRetrieveAckNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1251,7 +1277,7 @@ L3INT Q932ProcRetrieveRejectNT(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iF
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
diff --git a/src/isdn/Q931StateTE.c b/src/isdn/Q931StateTE.c
index da028b5..552b39f 100644
--- a/src/isdn/Q931StateTE.c
+++ b/src/isdn/Q931StateTE.c
@@ -123,6 +123,9 @@ void Q931CreateTE(L3UCHAR i)
     Q931SetIEProc(Q931ie_HIGH_LAYER_COMPATIBILITY,         i,Q931Pie_HLComp,			Q931Uie_HLComp);
     Q931SetIEProc(Q931ie_USER_USER,                        i,Q931Pie_UserUser,			Q931Uie_UserUser);
 
+	Q931SetIEProc(Q931ie_CONNECTED_NUMBER, i, Q931Pie_Generic, Q931Uie_Generic);
+	Q931SetIEProc(Q931ie_FACILITY,         i, Q931Pie_Generic, Q931Uie_Generic);
+
 	/* The following define a state machine. The point is that the Message	*/
 	/* procs can when search this to find out if the message/state			*/
 	/* combination is legale. If not, the proc for unexpected message apply.*/
@@ -205,6 +208,24 @@ void Q931CreateTE(L3UCHAR i)
 	Q931AddStateEntry(i,Q931_U19,
 	Q931AddStateEntry(i,Q931_U25,
 */
+
+	/* Timer default values */
+	Q931SetTimerDefault(i, Q931_TIMER_T301, 180000);	/* T301: 180s */
+	Q931SetTimerDefault(i, Q931_TIMER_T302,  15000);	/* T302:  15s */
+	Q931SetTimerDefault(i, Q931_TIMER_T303,   4000);	/* T303:   4s */
+	Q931SetTimerDefault(i, Q931_TIMER_T304,  30000);	/* T304:  30s */
+	Q931SetTimerDefault(i, Q931_TIMER_T305,  30000);	/* T305:  30s */
+	Q931SetTimerDefault(i, Q931_TIMER_T308,   4000);	/* T308:   4s */
+	Q931SetTimerDefault(i, Q931_TIMER_T309,  60000);	/* T309:  60s */
+	Q931SetTimerDefault(i, Q931_TIMER_T310,  60000);	/* T310:  60s */
+	Q931SetTimerDefault(i, Q931_TIMER_T313,   4000);	/* T313:   4s */
+	Q931SetTimerDefault(i, Q931_TIMER_T314,   4000);	/* T314:   4s */
+	Q931SetTimerDefault(i, Q931_TIMER_T316, 120000);	/* T316: 120s */
+	Q931SetTimerDefault(i, Q931_TIMER_T317,  90000);	/* T317:  90s */
+	Q931SetTimerDefault(i, Q931_TIMER_T318,   4000);	/* T318:   4s */
+	Q931SetTimerDefault(i, Q931_TIMER_T319,   4000);	/* T319:   4s */
+	Q931SetTimerDefault(i, Q931_TIMER_T321,  30000);	/* T321:  30s */
+	Q931SetTimerDefault(i, Q931_TIMER_T322,   4000);	/* T322:   4s */
 }
 
 /*****************************************************************************
@@ -227,7 +248,7 @@ L3INT Q931ProcAlertingTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
     Q931StartTimer(pTrunk, callIndex, 303);
 	if(iFrom == 4)
 	{
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -259,7 +280,7 @@ L3INT Q931ProcCallProceedingTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iF
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -292,7 +313,7 @@ L3INT Q931ProcConnectTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -329,7 +350,7 @@ L3INT Q931ProcConnectAckTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -362,7 +383,7 @@ L3INT Q931ProcProgressTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -399,7 +420,7 @@ L3INT Q931ProcSetupTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
             return ret;
         pMes->CRV = pTrunk->call[callIndex].CRV;
 
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+	ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
         if(ret)
             return ret;
 
@@ -475,7 +496,7 @@ L3INT Q931ProcSetupAckTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
     Q931StartTimer(pTrunk, callIndex, 303);
 	if(iFrom == 4)
 	{
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -504,7 +525,7 @@ L3INT Q931ProcResumeTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
         pMes->CRV = pTrunk->call[callIndex].CRV;
 
         /* Send RESUME to network */
-        ret=Q931Tx32(pTrunk,buf, pMes->Size);
+        ret=Q931Tx32Data(pTrunk,0,buf, pMes->Size);
         if(ret != Q931E_NO_ERROR)
             return ret;
 
@@ -544,7 +565,7 @@ L3INT Q931ProcResumeAckTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -577,7 +598,7 @@ L3INT Q931ProcResumeRejectTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFro
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -610,7 +631,7 @@ L3INT Q931ProcSuspendTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -643,7 +664,7 @@ L3INT Q931ProcSuspendAckTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -676,7 +697,7 @@ L3INT Q931ProcSuspendRejectTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFr
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -709,7 +730,7 @@ L3INT Q931ProcUserInformationTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT i
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -730,6 +751,9 @@ L3INT Q931ProcDisconnectTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
     L3INT ret=Q931E_NO_ERROR;
 	Q931mes_Header *pMes = (Q931mes_Header *)&buf[Q931L4HeaderSpace];
 
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Processing DISCONNECT message from %s for CRV: %d (%#hx)\n",
+						 iFrom == 4 ? "Local" : "Remote", pMes->CRV, pMes->CRV);
+
 	/* Find the call using CRV */
 	ret = Q931FindCRV(pTrunk, pMes->CRV, &callIndex);
 	if(ret != Q931E_NO_ERROR)
@@ -742,7 +766,7 @@ L3INT Q931ProcDisconnectTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -765,7 +789,7 @@ L3INT Q931ProcReleaseTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
     L3INT ret = Q931E_NO_ERROR;
     if(iFrom == 4) {
 		/* TODO Add proc here*/
-		ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	} else if(state == Q931_U0 && iFrom == 2) {
 		Q931Tx34(pTrunk,buf,pMes->Size);
         ret = Q931ReleaseComplete(pTrunk, buf);
@@ -805,6 +829,9 @@ L3INT Q931ProcReleaseCompleteTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT i
 			if(ret != Q931E_NO_ERROR)
 				return ret;
 			pTrunk->call[callIndex].InUse = 0;
+
+			/* TODO: experimental, send RELEASE_COMPLETE message */
+		        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 		}
 
 	}
@@ -838,7 +865,7 @@ L3INT Q931ProcRestartTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -877,7 +904,7 @@ L3INT Q931ProcRestartAckTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -910,7 +937,7 @@ L3INT Q931ProcCongestionControlTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -943,7 +970,7 @@ L3INT Q931ProcInformationTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -976,7 +1003,7 @@ L3INT Q931ProcNotifyTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1009,7 +1036,7 @@ L3INT Q931ProcStatusTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1042,7 +1069,7 @@ L3INT Q931ProcStatusEnquiryTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFr
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1075,7 +1102,7 @@ L3INT Q931ProcSegmentTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1112,7 +1139,7 @@ L3INT Q932ProcFacilityTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1145,7 +1172,7 @@ L3INT Q932ProcHoldTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1178,7 +1205,7 @@ L3INT Q932ProcHoldAckTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1211,7 +1238,7 @@ L3INT Q932ProcHoldRejectTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1244,7 +1271,7 @@ L3INT Q932ProcRegisterTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1277,7 +1304,7 @@ L3INT Q932ProcRetrieveTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom)
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1310,7 +1337,7 @@ L3INT Q932ProcRetrieveAckTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iFrom
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1343,7 +1370,7 @@ L3INT Q932ProcRetrieveRejectTE(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT iF
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+        ret = Q931Tx32Data(pTrunk,0,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
diff --git a/src/isdn/Q931api.c b/src/isdn/Q931api.c
index b0119ef..c7200d5 100644
--- a/src/isdn/Q931api.c
+++ b/src/isdn/Q931api.c
@@ -74,8 +74,8 @@ L3INT Q931Api_InitTrunk(Q931_TrunkInfo_t *pTrunk,
 						Q931Dialect_t Dialect,
 						Q931NetUser_t NetUser,
 						Q931_TrunkType_t TrunkType,
-						Q931TxCB_t Q931Tx34CBProc,
-						Q931TxCB_t Q931Tx32CBProc,
+						Q931Tx34CB_t Q931Tx34CBProc,
+						Q931Tx32CB_t Q931Tx32CBProc,
 						Q931ErrorCB_t Q931ErrorCBProc,
 						void *PrivateData32,
 						void *PrivateData34)
@@ -97,6 +97,7 @@ L3INT Q931Api_InitTrunk(Q931_TrunkInfo_t *pTrunk,
 		break;
 
 	case Q931_TrType_BRI:
+	case Q931_TrType_BRI_PTMP:
 		dchannel = 3;
 		maxchans = 3;
 		break;
@@ -166,6 +167,16 @@ void Q931SetIEProc(L3UCHAR iec, L3UCHAR dialect, q931pie_func_t *Q931PieProc, q9
         Q931Uie[dialect][iec] = Q931UieProc;
 }
 
+void Q931SetTimeoutProc(L3UCHAR timer, L3UCHAR dialect, q931timeout_func_t *Q931TimeoutProc)
+{
+	if(Q931Timeout != NULL)
+		Q931Timeout[dialect][timer] = Q931TimeoutProc;
+}
+
+void Q931SetTimerDefault(L3UCHAR timer, L3UCHAR dialect, q931timer_t timeout)
+{
+	Q931Timer[dialect][timer] = timeout;
+}
 
 L3INT Q931GetMesSize(Q931mes_Generic *pMes)
 {
@@ -212,8 +223,11 @@ static L3INT crv={1};
 
 L3INT Q931GetUniqueCRV(Q931_TrunkInfo_t *pTrunk)
 {
+	L3INT max = (Q931_IS_BRI(pTrunk)) ? Q931_BRI_MAX_CRV : Q931_PRI_MAX_CRV;
+
 	crv++;
-	if (crv > 32766) crv = 1;
+	crv = (crv <= max) ? crv : 1;
+
 	return crv;
 }
 
@@ -523,7 +537,7 @@ L3INT Q931ReleaseComplete(Q931_TrunkInfo_t *pTrunk, L3UCHAR *buf)
     Q931mes_Header *ptr = (Q931mes_Header*)&buf[Q931L4HeaderSpace];
 	ptr->MesType = Q931mes_RELEASE_COMPLETE;
 	ptr->CRVFlag = !(ptr->CRVFlag);
-	return Q931Tx32(pTrunk,buf,ptr->Size);
+	return Q931Tx32Data(pTrunk,0,buf,ptr->Size);
 }
 
 L3INT Q931AckRestart(Q931_TrunkInfo_t *pTrunk, L3UCHAR *buf)
diff --git a/src/isdn/Q931ie.c b/src/isdn/Q931ie.c
index 6c77d32..983e158 100644
--- a/src/isdn/Q931ie.c
+++ b/src/isdn/Q931ie.c
@@ -318,73 +318,74 @@ L3INT Q931Pie_BearerCap(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf,
     L3INT Beg=*Octet;/* remember current offset */
     L3INT li;
 
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Encoding Bearer Capability IE\n");
+
     OBuf[(*Octet)++] = Q931ie_BEARER_CAPABILITY ;
     li=(*Octet)++;    /* remember length position */
 
     /* Octet 3 - Coding standard / Information transfer capability */
-    OBuf[(*Octet)++] = 0x80 | (pIE->CodStand<<5) | (pIE->ITC & 0x1f);
+	OBuf[(*Octet)++] = 0x80 | ((pIE->CodStand << 5) & 0x60) | (pIE->ITC & 0x1f);
 
     /* Octet 4 - Transfer mode / Information transfer rate */
-	OBuf[(*Octet)++] = 0x80 | (pIE->TransMode<<5) | (pIE->ITR & 0x1f);
+	OBuf[(*Octet)++] = 0x80 | ((pIE->TransMode << 5) & 0x60) | (pIE->ITR & 0x1f);
+
     if(pIE->ITR == 0x18)
     {
         /* Octet 4.1 - Rate Multiplier */
-        OBuf[(*Octet)++] = 0x80 | pIE->RateMul;
+		OBuf[(*Octet)++] = 0x80 | (pIE->RateMul & 0x7f);
     }
 
-    /* Octet 5 - Layer 1 Ident / User information layer 1 protocol*/
+	/* Octet 5 - Layer 1 Ident / User information layer 1 protocol */
     if(pIE->Layer1Ident == 0x01)
     {
-        if(((pIE->ITC == 0x08) && (pIE->UIL1Prot == 0x01 || pIE->UIL1Prot == 0x08))
-        || ((pIE->ITC == 0x10) && (pIE->UIL1Prot == 0x02 || pIE->UIL1Prot == 0x03)))
+		if(((pIE->ITC == 0x08) && (pIE->UIL1Prot == 0x01 || pIE->UIL1Prot == 0x08)) ||
+		   ((pIE->ITC == 0x10) && (pIE->UIL1Prot == 0x02 || pIE->UIL1Prot == 0x03)))
         {
-            OBuf[(*Octet)++] = 0x00 | (pIE->Layer1Ident<<5) | (pIE->UIL1Prot & 0x15);
+			OBuf[(*Octet)++] = 0x00 | ((pIE->Layer1Ident << 5) & 0x60) | (pIE->UIL1Prot & 0x15);
             
             /* Octet 5a - SyncAsync/Negot/UserRate */
-            OBuf[(*Octet)++] = 0x00 | (pIE->SyncAsync<<6) | (pIE->Negot<<5) | (pIE->UserRate&0x1f);
+			OBuf[(*Octet)++] = 0x00 | ((pIE->SyncAsync << 6) & 0x40) | ((pIE->Negot << 5) & 0x20) | (pIE->UserRate & 0x1f);
 
             /* Octet 5b - one of two types */
             if(pIE->UIL1Prot == 0x01) /* ITU V.110, I.460 and X.30           */
             {
                 /* Octet 5b - Intermed rate/ Nic on Tx/Nix on Rx/FlowCtlTx/FlowCtlRx */
                 OBuf[(*Octet)++] = 0x00 
-                                | (pIE->InterRate<<6)
-                                | (pIE->NIConTx << 4)
-                                | (pIE->NIConRx << 3)
-                                | (pIE->FlowCtlTx << 2)
-                                | (pIE->FlowCtlRx << 1);
+						| ((pIE->InterRate << 6) & 0x60)
+						| ((pIE->NIConTx   << 4) & 0x10)
+						| ((pIE->NIConRx   << 3) & 0x08)
+						| ((pIE->FlowCtlTx << 2) & 0x04)
+						| ((pIE->FlowCtlRx << 1) & 0x02);
             }
             else if(pIE->UIL1Prot == 0x08) /* ITU V.120                      */
             {
                 /* Octet 5b - HDR/Multiframe/Mode/LLINegot/Assignor/Inbandneg*/
                 OBuf[(*Octet)++] = 0x00
-                                | (pIE->InterRate << 6)
-                                | (pIE->MultiFrame << 5)
-                                | (pIE->Mode << 4)
-                                | (pIE->LLInegot << 3)
-                                | (pIE->Assignor << 2)
-                                | (pIE->InBandNeg << 1);
+						| ((pIE->InterRate  << 6) & 0x60)
+						| ((pIE->MultiFrame << 5) & 0x20)
+						| ((pIE->Mode       << 4) & 0x10)
+						| ((pIE->LLInegot   << 3) & 0x08)
+						| ((pIE->Assignor   << 2) & 0x04)
+						| ((pIE->InBandNeg  << 1) & 0x02);
             }
 
             /* Octet 5c - NumStopBits/NumStartBits/Parity                    */
             OBuf[(*Octet)++] = 0x00 
-                            | (pIE->NumStopBits << 5 )
-                            | (pIE->NumDataBits << 3 )
-                            | (pIE->Parity);
+					| ((pIE->NumStopBits << 5) & 0x60)
+					| ((pIE->NumDataBits << 3) & 0x18)
+					| (pIE->Parity & 0x07);
 
             /* Octet 5d - Duplex Mode/Modem Type */
-            OBuf[(*Octet)++] = 0x80 | (pIE->DuplexMode<<6) | (pIE->ModemType);
+			OBuf[(*Octet)++] = 0x80 | ((pIE->DuplexMode << 6) & 0x40) | (pIE->ModemType & 0x3f);
         }
-        else
-        {
-            OBuf[(*Octet)++] = 0x80 | (pIE->Layer1Ident<<5) | (pIE->UIL1Prot & 0x1F);
+		else {
+			OBuf[(*Octet)++] = 0x80 | ((pIE->Layer1Ident << 5) & 0x60) | (pIE->UIL1Prot & 0x1f);
         }
     }
 
     /* Octet 6 - Layer2Ident/User information layer 2 prtocol */
-    if(pIE->Layer2Ident == 0x02)
-    {
-        OBuf[(*Octet)++] = 0x80 | (pIE->Layer2Ident<<5) | (pIE->UIL2Prot);
+	if(pIE->Layer2Ident == 0x02) {
+		OBuf[(*Octet)++] = 0x80 | ((pIE->Layer2Ident << 5) & 0x60) | (pIE->UIL2Prot & 0x1f);
     }
 
     /* Octet 7 - Layer 3 Ident/ User information layer 3 protocol */
@@ -392,17 +393,16 @@ L3INT Q931Pie_BearerCap(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf,
     {
         if(pIE->UIL3Prot == 0x0c)
         {
-            OBuf[(*Octet)++] = 0x00 | (pIE->Layer3Ident<<5) | (pIE->UIL3Prot);
+			OBuf[(*Octet)++] = 0x00 | ((pIE->Layer3Ident << 5) & 0x60) | (pIE->UIL3Prot & 0x1f);
 
             /* Octet 7a - Additional information layer 3 msb */
-            OBuf[(*Octet)++] = 0x00 | (pIE->AL3Info1);
+			OBuf[(*Octet)++] = 0x00 | (pIE->AL3Info1 & 0x0f);
 
             /* Octet 7b - Additional information layer 3 lsb */
-            OBuf[(*Octet)++] = 0x80 | (pIE->AL3Info2);
+			OBuf[(*Octet)++] = 0x80 | (pIE->AL3Info2 & 0x0f);
         }
-        else
-        {
-            OBuf[(*Octet)++] = 0x80 | (pIE->Layer3Ident<<5) | (pIE->UIL3Prot);
+		else {
+			OBuf[(*Octet)++] = 0x80 | ((pIE->Layer3Ident << 5) & 0x60) | (pIE->UIL3Prot & 0x1f);
         }
     }
 
@@ -1152,76 +1152,107 @@ L3INT Q931Pie_CongLevel(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf,
 *****************************************************************************/
 L3INT Q931Uie_ChanID(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *pMsg, L3UCHAR * IBuf, L3UCHAR *OBuf, L3INT *IOff, L3INT *OOff)
 {
-    Q931ie_ChanID * pie = (Q931ie_ChanID*)OBuf;
+	Q931ie_ChanID *pie = (Q931ie_ChanID*)OBuf;
 	ie *pIE = &pMsg->ChanID;
-    L3INT Off = 0;
-    L3INT Octet = 0;
-    L3INT IESize;
+	L3INT Off = 0;
+	L3INT Octet = 0;
+	L3INT IESize;
 //18 04 e1 80 83 01
-    *pIE=0;
+	*pIE = 0;
 
-    /* Octet 1 */
-    pie->IEId        = IBuf[Octet];
-    Octet ++;
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Decoding ChanID IE\n");
 
-    /* Octet 2 */
-    IESize = IBuf[Octet ++]; 
+	/* Octet 1 */
+	pie->IEId = IBuf[Octet++];
 
-    /* Octet 3 */
-    pie->IntIDPresent    = (IBuf[Octet] >> 6) & 0x01;
-    pie->IntType        = (IBuf[Octet] >> 5) & 0x01;
-    pie->PrefExcl        = (IBuf[Octet] >> 3) & 0x01;
-    pie->DChanInd        = (IBuf[Octet] >> 2) & 0x01;
-    pie->InfoChanSel    = IBuf[Octet] & 0x03;
-
-    Off = Q931ReadExt(&IBuf[Octet+Off], Off);
-    Octet++;
-
-    /* Octet 3.1 */
-
-    if(pie->IntIDPresent)
-    {
-        pie->InterfaceID    = IBuf[Octet+Off] & 0x7f;
-
-        /* Temp fix. Interface id can be extended using the extension bit */
-        /* this will read the octets, but do nothing with them. this is done */
-        /* because the usage of this field is a little unclear */
-        /* 30.jan.2001/JVB */
-        Off = Q931ReadExt(&IBuf[Octet+Off], Off);
-        Off++;
-    }
-
-    if (Octet + Off -2 != IESize) {
-        /* Octet 3.2 */
-        if(pie->IntType == 1)        /* PRI etc */
-        {
-            pie->CodStand    = (IBuf[Octet + Off] >> 5) & 0x03;
-            pie->NumMap        = (IBuf[Octet + Off] >> 4) & 0x01;
-            pie->ChanMapType= IBuf[Octet + Off] & 0x0f;
+	/* Octet 2 */
+	IESize    = IBuf[Octet++]; 
+
+	/* Octet 3 */
+	pie->IntIDPresent = (IBuf[Octet] >> 6) & 0x01;
+	pie->IntType      = (IBuf[Octet] >> 5) & 0x01;
+	pie->PrefExcl     = (IBuf[Octet] >> 3) & 0x01;
+	pie->DChanInd     = (IBuf[Octet] >> 2) & 0x01;
+	pie->InfoChanSel  =  IBuf[Octet] & 0x03;
+
+	Off = Q931ReadExt(&IBuf[Octet++], Off);
+
+	/* Octet 3.1 */
+	if(pie->IntIDPresent)
+	{
+		pie->InterfaceID = IBuf[Octet+Off] & 0x7f;
+
+		/* Temp fix. Interface id can be extended using the extension bit */
+		/* this will read the octets, but do nothing with them. this is done */
+		/* because the usage of this field is a little unclear */
+		/* 30.jan.2001/JVB */
+		Off = Q931ReadExt(&IBuf[Octet+Off], Off);
+		Off++;
+	}
 
-            Off ++;
+	if ((Octet + Off - 2) != IESize) {
+		/* Octet 3.2 */
+		if(pie->IntType == 1)        /* PRI etc */
+		{
+			pie->CodStand    = (IBuf[Octet + Off] >> 5) & 0x03;
+			pie->NumMap      = (IBuf[Octet + Off] >> 4) & 0x01;
+			pie->ChanMapType =  IBuf[Octet + Off] & 0x0f;
+			Off++;
 
+			/* Octet 3.3 */
+			/* Temp fix. Assume B channel. H channels not supported */
+			pie->ChanSlot = IBuf[Octet+Off] & 0x7f;
 
-            /* Octet 3.3 */
-            /* Temp fix. Assume B channel. H channels not supported */
-            pie->ChanSlot = IBuf[Octet+Off] & 0x7f;
 			/* Some dialects don't follow the extension coding properly for this, but this should be safe for all */
-			if (Octet + Off -1 != IESize) {
+			if((Octet + Off - 1) != IESize) {
 				Off = Q931ReadExt(&IBuf[Octet+Off], Off);
 			}
-            Off++;
-        }
-    }
+			Off++;
+		}
+	}
 
-    Q931IESizeTest(Q931E_CHANID);
+	Q931IESizeTest(Q931E_CHANID);
 
-    Q931SetIE(*pIE, *OOff);
+	Q931SetIE(*pIE, *OOff);
 
-    *IOff = (*IOff) + Octet + Off;
-    *OOff = (*OOff) + sizeof(Q931ie_BearerCap);
-    pie->Size = sizeof(Q931ie_BearerCap);
+	*IOff = (*IOff) + Octet + Off;
+	*OOff = (*OOff) + sizeof(Q931ie_ChanID);
+	pie->Size = sizeof(Q931ie_ChanID);
+
+	if(pTrunk->loglevel == Q931_LOG_DEBUG)
+	{
+		const char *iface;
+		char tmp[100] = "";
+
+		if(!pie->IntType) {
+			switch(pie->InfoChanSel) {
+			case 0x0:
+				iface = "None";
+				break;
+			case 0x1:
+				iface = "B1";
+				break;
+			case 0x2:
+				iface = "B2";
+				break;
+			default:
+				iface = "Any Channel";
+			}
 
-    return Q931E_NO_ERROR;
+			snprintf(tmp, sizeof(tmp)-1, "InfoChanSel: %d (%s)", pie->InfoChanSel, iface);
+		}
+
+		Q931Log(pTrunk, Q931_LOG_DEBUG,
+			"\n-------------------------- Q.931 Channel ID ------------------------\n"
+			"    Pref/Excl: %s, Interface Type: %s\n"
+			"    %s\n"
+			"--------------------------------------------------------------------\n\n",
+			((pie->PrefExcl) ? "Preferred" : "Exclusive"),
+			((pie->IntType) ? "PRI/Other" : "BRI"),
+			tmp);
+	}
+
+	return Q931E_NO_ERROR;
 }
 
 /*****************************************************************************
@@ -1238,24 +1269,24 @@ L3INT Q931Uie_ChanID(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *pMsg, L3UCHAR *
 L3INT Q931Pie_ChanID(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf, L3INT *Octet)
 {
     Q931ie_ChanID * pIE = (Q931ie_ChanID*)IBuf;
-    L3INT rc=Q931E_NO_ERROR;
-    L3INT Beg=*Octet;/* remember current offset */
+	L3INT rc = Q931E_NO_ERROR;
+	L3INT Beg = *Octet;	/* remember current offset */
     L3INT li;
 
     OBuf[(*Octet)++] = Q931ie_CHANNEL_IDENTIFICATION;
-    li=(*Octet)++;    /* remember length position */
+	li = (*Octet)++;	/* remember length position */
 
     /* Octet 3 flags & BRI chan # */
     OBuf[(*Octet)++] = 0x80 
-                    | (pIE->IntIDPresent << 6) 
-                    | (pIE->IntType<<5)
-                    | (pIE->PrefExcl<<3) 
-                    | pIE->InfoChanSel;
+			| ((pIE->IntIDPresent << 6) & 0x40)
+			| ((pIE->IntType << 5) & 0x20)
+			| ((pIE->PrefExcl << 3) & 0x08)
+			|  (pIE->InfoChanSel & 0x03);
 
     /* Octet 3.1 - Interface Identifier */
     if(pIE->IntIDPresent)
     {
-        OBuf[(*Octet)++] = 0x80 | pIE->InterfaceID;
+		OBuf[(*Octet)++] = 0x80 | (pIE->InterfaceID & 0x7f);
     }
     else
     {
@@ -1263,12 +1294,21 @@ L3INT Q931Pie_ChanID(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf, L3I
         if(pIE->IntType == 1)
         {
             OBuf[(*Octet)++]  = 0x80
-                            | (pIE->CodStand << 5)
-                            | (pIE->NumMap << 4)
-                            | pIE->ChanMapType;
-
-            /* Octet 3.2 Channel number/slot map */
-            OBuf[(*Octet)++] = 0x80 | pIE->ChanSlot;
+					| ((pIE->CodStand << 5) & 0x60)
+					| ((pIE->NumMap << 4) & 0x10)
+					|  (pIE->ChanMapType & 0x0f);		/* TODO: support all possible channel map types */
+
+			/* Octet 3.3 Channel number */
+			switch(pIE->ChanMapType) {
+			case 0x6:	/* Slot map: H0 Channel Units */	/* unsupported, Octets 3.3.1 - 3.3.3 */
+				return Q931E_CHANID;
+
+			case 0x8:	/* Slot map: H11 Channel Units */
+			case 0x9:	/* Slot map: H12 Channel Units */
+			default:	/* Channel number */
+				OBuf[(*Octet)++] = 0x80 | (pIE->ChanSlot & 0x7f);
+				break;
+			}
         }
     }
 
@@ -1302,28 +1342,25 @@ L3INT Q931Pie_ChanID(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf, L3I
 *****************************************************************************/
 L3USHORT Q931Uie_CRV(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf, L3INT *IOff, L3INT *OOff)
 {
+    L3USHORT CRV = 0;
     L3INT Octet = *IOff;
-    L3INT l = IBuf[Octet]; 
-    L3USHORT CRV;
-    Octet++; /*Octet 2 is length indicator */
+    L3INT l = IBuf[Octet++]; 
 
-    if(l == 1) /* One octet CRV */
+    if(l == 1)	/* One octet CRV */
     {
-        CRV = IBuf[Octet] & 0x7F;
-        Octet++;
+        CRV = IBuf[Octet++] & 0x7F;
     }
-    else if(l==2) /* two octet CRV */
+    else if(l == 2)	/* two octet CRV */
     {
-        CRV = (IBuf[Octet] & 0x7f) << 8;
-        CRV = CRV + IBuf[Octet+1];
-        Octet += 2;
+        CRV  = (IBuf[Octet++] & 0x7f) << 8;
+        CRV |=  IBuf[Octet++];
     }
     else
     {
-		/* Long CRV is not used, so we skip this */
-		/* TODO: is it right to set to 0 here? */
-		CRV = 0;
-        Octet+=l;
+	/* Long CRV is not used, so we skip this */
+	/* TODO: is it right to set to 0 here? */
+	CRV = 0;
+        Octet += l;
     }
 
     *IOff = Octet;
@@ -1634,7 +1671,7 @@ L3INT Q931Pie_HLComp(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf, L3I
     li=(*Octet)++;
 
     /* Octet 3 */
-    OBuf[(*Octet)++] = 0x80 | (pIE->CodStand << 5) | (pIE->Interpret << 2) | pIE->PresMeth;
+    OBuf[(*Octet)++] = 0x80 | ((pIE->CodStand << 5) & 0x60) | ((pIE->Interpret << 2) & 0x1c) | (pIE->PresMeth & 0x03);
 
     /* Octet 4 */
     OBuf[(*Octet)++] = pIE->HLCharID;
@@ -1642,11 +1679,11 @@ L3INT Q931Pie_HLComp(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf, L3I
     /* Octet 4a */
     if(pIE->HLCharID == 0x5e || pIE->HLCharID == 0x5f)
     {
-        OBuf[(*Octet)++] = 0x80 | pIE->EHLCharID;
+        OBuf[(*Octet)++] = 0x80 | (pIE->EHLCharID & 0x7f);
     }
     else if( pIE->HLCharID >= 0xc3 && pIE->HLCharID <= 0xcf)
     {
-        OBuf[(*Octet)++] = 0x80 | pIE->EVideoTlfCharID;
+        OBuf[(*Octet)++] = 0x80 | (pIE->EVideoTlfCharID & 0x7f);
     }
     else
     {
@@ -3151,3 +3188,29 @@ L3INT Q931Pie_ChangeStatus(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBu
     OBuf[li] = (L3UCHAR)((*Octet)-Beg) - 2;
     return rc;
 }
+
+
+
+L3INT Q931Uie_Generic(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *pMsg, L3UCHAR *IBuf, L3UCHAR *OBuf, L3INT *IOff, L3INT *OOff)
+{
+	L3INT Octet = 0;
+	L3UCHAR id = 0;
+
+	/* id */
+	id = IBuf[Octet++];
+
+	/* Length */
+	Octet += IBuf[Octet];
+	Octet++;
+
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Discarding IE %#hhx with length %d\n", id, Octet - 2);
+
+	*IOff += Octet;
+	return Q931E_NO_ERROR;
+}
+
+L3INT Q931Pie_Generic(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf, L3INT *Octet)
+{
+	/* do nothing */
+	return Q931E_NO_ERROR;
+}
diff --git a/src/isdn/Q931mes.c b/src/isdn/Q931mes.c
index 2131c20..4a53516 100644
--- a/src/isdn/Q931mes.c
+++ b/src/isdn/Q931mes.c
@@ -43,6 +43,34 @@
 
 #include "Q931.h"
 
+/**
+ * Q931MesgHeader
+ * \brief	Create Q.931 Message header
+ */
+L3INT Q931MesgHeader(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *mes, L3UCHAR *OBuf, L3INT Size, L3INT *IOff)
+{
+	L3INT Octet = *IOff;
+
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Creating Q.931 Message Header:\n    ProtDisc %d (%#x), CRV %d (%#x), CRVflag: %d (%#x), MesType: %d (%#x)\n",
+			 mes->ProtDisc, mes->ProtDisc, mes->CRV, mes->CRV, mes->CRVFlag, mes->CRVFlag, mes->MesType, mes->MesType);
+
+	OBuf[Octet++] = mes->ProtDisc;				/* Protocol discriminator		*/
+	if(!Q931_IS_BRI(pTrunk)) {
+		OBuf[Octet++] = 2;									/* length is 2 octets			*/
+		OBuf[Octet++] = (L3UCHAR)((mes->CRV >> 8) & 0x7f) | ((mes->CRVFlag << 7) & 0x80);	/* msb					*/
+		OBuf[Octet++] = (L3UCHAR) (mes->CRV & 0xff);						/* lsb					*/
+	} else {
+		OBuf[Octet++] = 1;									/* length is 1 octet			*/
+		OBuf[Octet++] = (L3UCHAR) (mes->CRV & 0x7f) | ((mes->CRVFlag << 7) & 0x80);		/* CRV & flag				*/
+	}
+	OBuf[Octet++] = mes->MesType;				/* message header			*/
+
+	*IOff = Octet;
+
+	return 0;
+}
+
+
 /*****************************************************************************
 
   Function:     Q931Umes_Alerting
@@ -84,17 +112,12 @@ L3INT Q931Umes_Alerting(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generic
 *****************************************************************************/
 L3INT Q931Pmes_Alerting(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISize, L3UCHAR *OBuf, L3INT *OSize)
 {
-    L3INT rc = Q931E_NO_ERROR;
+	L3INT rc = Q931E_NO_ERROR;
 	Q931mes_Generic *pMes = (Q931mes_Generic *)IBuf;
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Bearer capability */
 	if(Q931IsIEPresent(pMes->BearerCap))
@@ -177,12 +200,7 @@ L3INT Q931Pmes_CallProceeding(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Bearer capability */
 	if(Q931IsIEPresent(pMes->BearerCap))
@@ -271,17 +289,23 @@ L3INT Q931Umes_Connect(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generic
 		case Q931ie_SIGNAL:
 		case Q931ie_LOW_LAYER_COMPATIBILITY:
 		case Q931ie_HIGH_LAYER_COMPATIBILITY:
+		case Q931ie_CONNECTED_NUMBER:		/* not actually used, seen while testing BRI PTMP TE */
 			rc = Q931Uie[pTrunk->Dialect][IBuf[IOff]](pTrunk, mes, &IBuf[IOff], &mes->buf[OOff], &IOff, &OOff);
 			if(rc != Q931E_NO_ERROR) 
 				return rc;
 			break;
+
 		default:
+			Q931Log(pTrunk, Q931_LOG_ERROR, "Illegal IE %#hhx in Connect Message\n", IBuf[IOff]);
+
 			return Q931E_ILLEGAL_IE;
 			break;
 		}
 	}
-    mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
-    return Q931E_NO_ERROR;
+
+	mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
+
+	return Q931E_NO_ERROR;
 }
 
 /*****************************************************************************
@@ -296,12 +320,7 @@ L3INT Q931Pmes_Connect(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT IS
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Bearer capability */
 	if(Q931IsIEPresent(pMes->BearerCap))
@@ -392,12 +411,7 @@ L3INT Q931Pmes_ConnectAck(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Display */
 	if(Q931IsIEPresent(pMes->Display))
@@ -432,6 +446,7 @@ L3INT Q931Umes_Disconnect(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Gener
 		case Q931ie_PROGRESS_INDICATOR:
 		case Q931ie_DISPLAY:
 		case Q931ie_SIGNAL:
+		case Q931ie_FACILITY:
 			rc = Q931Uie[pTrunk->Dialect][IBuf[IOff]](pTrunk, mes, &IBuf[IOff], &mes->buf[OOff], &IOff, &OOff);
 			if(rc != Q931E_NO_ERROR) 
 				return rc;
@@ -457,12 +472,7 @@ L3INT Q931Pmes_Disconnect(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Cause */
 	if(Q931IsIEPresent(pMes->Cause))
@@ -516,8 +526,10 @@ L3INT Q931Umes_Information(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Gene
 			break;
 		}
 	}
-    mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
-    return Q931E_NO_ERROR;
+
+	mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
+
+	return Q931E_NO_ERROR;
 }
 
 /*****************************************************************************
@@ -532,13 +544,8 @@ L3INT Q931Pmes_Information(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3IN
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
-	
 	/* Sending Complete				*/
 	if(Q931IsIEPresent(pMes->SendComplete))
 		OBuf[Octet++]	= (L3UCHAR)(pMes->SendComplete & 0x00ff);
@@ -607,12 +614,7 @@ L3INT Q931Pmes_Notify(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISi
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Bearer capability */
 	if(Q931IsIEPresent(pMes->BearerCap))
@@ -675,12 +677,7 @@ L3INT Q931Pmes_Progress(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT I
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Bearer capability */
 	if(Q931IsIEPresent(pMes->BearerCap))
@@ -753,12 +750,7 @@ L3INT Q931Pmes_Release(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT IS
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Cause */
 	if(Q931IsIEPresent(pMes->Cause))
@@ -820,12 +812,7 @@ L3INT Q931Pmes_ReleaseComplete(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf,
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Cause */
 	if(Q931IsIEPresent(pMes->Cause))
@@ -882,34 +869,29 @@ L3INT Q931Umes_Restart(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generic
 *****************************************************************************/
 L3INT Q931Pmes_Restart(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISize, L3UCHAR *OBuf, L3INT *OSize)
 {
-    L3INT rc = Q931E_NO_ERROR;
-    Q931mes_Generic *pMes = (Q931mes_Generic *)IBuf;
-    L3INT Octet = 0;
-
-    /* Q931 Message Header */
+	L3INT rc = Q931E_NO_ERROR;
+	Q931mes_Generic *pMes = (Q931mes_Generic *)IBuf;
+	L3INT Octet = 0;
 
-    OBuf[Octet++]    = pMes->ProtDisc;        /* Protocol discriminator        */
-    OBuf[Octet++]    = 2;                    /* length is 2 octets            */
-    OBuf[Octet++]    = (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);    /* msb                            */
-    OBuf[Octet++]    = (L3UCHAR)(pMes->CRV);    /* lsb                            */
-    OBuf[Octet++]    = pMes->MesType;        /* message header                */
-    
-    /* ChanID */
-    if(Q931IsIEPresent(pMes->ChanID))
-        if((rc=Q931Pie[pTrunk->Dialect][Q931ie_CHANNEL_IDENTIFICATION](pTrunk, Q931GetIEPtr(pMes->ChanID,pMes->buf), OBuf, &Octet))!=0)
-            return rc;
-    /* Display */
-    if(Q931IsIEPresent(pMes->Display))
-        if((rc=Q931Pie[pTrunk->Dialect][Q931ie_DISPLAY](pTrunk, Q931GetIEPtr(pMes->Display,pMes->buf), OBuf, &Octet))!=0)
-            return rc;
-    /* RestartInd */
-    if(Q931IsIEPresent(pMes->RestartInd))
-        if((rc=Q931Pie[pTrunk->Dialect][Q931ie_RESTART_INDICATOR](pTrunk, Q931GetIEPtr(pMes->RestartInd,pMes->buf), OBuf, &Octet))!=0)
-            return rc;
+	/* Q931 Message Header */
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
+   
+	/* ChanID */
+	if(Q931IsIEPresent(pMes->ChanID))
+		if((rc=Q931Pie[pTrunk->Dialect][Q931ie_CHANNEL_IDENTIFICATION](pTrunk, Q931GetIEPtr(pMes->ChanID,pMes->buf), OBuf, &Octet))!=0)
+			return rc;
+	/* Display */
+	if(Q931IsIEPresent(pMes->Display))
+		if((rc=Q931Pie[pTrunk->Dialect][Q931ie_DISPLAY](pTrunk, Q931GetIEPtr(pMes->Display,pMes->buf), OBuf, &Octet))!=0)
+			return rc;
+	/* RestartInd */
+	if(Q931IsIEPresent(pMes->RestartInd))
+		if((rc=Q931Pie[pTrunk->Dialect][Q931ie_RESTART_INDICATOR](pTrunk, Q931GetIEPtr(pMes->RestartInd,pMes->buf), OBuf, &Octet))!=0)
+			return rc;
 
 	*OSize = Octet;	
 
-    return rc;
+	return rc;
 }
 
 /*****************************************************************************
@@ -953,13 +935,8 @@ L3INT Q931Pmes_RestartAck(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT
     Q931mes_Generic *pMes = (Q931mes_Generic *)IBuf;
     L3INT Octet = 0;
 
-    /* Q931 Message Header */
-
-    OBuf[Octet++]    = pMes->ProtDisc;        /* Protocol discriminator        */
-    OBuf[Octet++]    = 2;                    /* length is 2 octets            */
-    OBuf[Octet++]    = (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);    /* msb                            */
-    OBuf[Octet++]    = (L3UCHAR)(pMes->CRV);    /* lsb                            */
-    OBuf[Octet++]    = pMes->MesType;        /* message header                */
+	/* Q931 Message Header */
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
     
     /* ChanID */
     if(Q931IsIEPresent(pMes->ChanID))
@@ -1019,13 +996,8 @@ L3INT Q931Pmes_Resume(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISi
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
-	
 	/* Call Identity */
 	if(Q931IsIEPresent(pMes->CallID))
 		if((rc=Q931Pie[pTrunk->Dialect][Q931ie_CALL_IDENTITY](pTrunk, Q931GetIEPtr(pMes->CallID,pMes->buf), OBuf, &Octet))!=0)
@@ -1061,8 +1033,10 @@ L3INT Q931Umes_ResumeAck(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generi
 			break;
 		}
 	}
-    mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
-    return Q931E_NO_ERROR;
+
+	mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
+
+	return Q931E_NO_ERROR;
 }
 
 
@@ -1073,17 +1047,12 @@ L3INT Q931Umes_ResumeAck(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generi
 *****************************************************************************/
 L3INT Q931Pmes_ResumeAck(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISize, L3UCHAR *OBuf, L3INT *OSize)
 {
-    L3INT rc = Q931E_NO_ERROR;
+	L3INT rc = Q931E_NO_ERROR;
 	Q931mes_Generic *pMes = (Q931mes_Generic *)IBuf;
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Channel Identification */
 	if(Q931IsIEPresent(pMes->ChanID))
@@ -1096,7 +1065,7 @@ L3INT Q931Pmes_ResumeAck(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT
 
 	*OSize = Octet;	
 
-    return rc;
+	return rc;
 }
 
 /*****************************************************************************
@@ -1124,8 +1093,10 @@ L3INT Q931Umes_ResumeReject(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Gen
 			break;
 		}
 	}
-    mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
-    return Q931E_NO_ERROR;
+
+	mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
+
+	return Q931E_NO_ERROR;
 }
 
 
@@ -1141,11 +1112,7 @@ L3INT Q931Pmes_ResumeReject(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3I
 	L3INT Octet = 0;
 
 	/* Q931 Message Header	*/
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb						*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 
 	/* Cause				*/
 	if(Q931IsIEPresent(pMes->Cause))
@@ -1159,23 +1126,23 @@ L3INT Q931Pmes_ResumeReject(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3I
 
 	*OSize = Octet;	
 
-    return rc;
+	return rc;
 }
 
 L3INT Q931Umes_Segment(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generic *mes, L3INT IOff, L3INT OOff)
 {
-    L3INT i = IOff;
+	L3INT i = IOff;
 
-    return IOff - i;
+	return IOff - i;
 }
 
 L3INT Q931Pmes_Segment(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISize, L3UCHAR *OBuf, L3INT *OSize)
 {
-    L3BOOL RetCode = L3FALSE;
+	L3BOOL RetCode = L3FALSE;
 
 	*OSize = 0;	
 
-    return RetCode;
+	return RetCode;
 }
 
 /*****************************************************************************
@@ -1196,6 +1163,7 @@ L3INT Q931Umes_Setup(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generic *m
 		case Q931ie_SENDING_COMPLETE:
 			IOff++;
 			break;
+
 		case Q931ie_BEARER_CAPABILITY:
 		case Q931ie_CHANNEL_IDENTIFICATION:
 		case Q931ie_PROGRESS_INDICATOR:
@@ -1215,6 +1183,7 @@ L3INT Q931Umes_Setup(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generic *m
 			if(rc != Q931E_NO_ERROR) 
 				return rc;
 			break;
+
 		case Q931ie_REPEAT_INDICATOR:
 			if(ir < 2) {
 				rc = Q931Uie[pTrunk->Dialect][IBuf[IOff]](pTrunk, mes, &IBuf[IOff], &mes->buf[OOff], &IOff, &OOff);
@@ -1223,13 +1192,16 @@ L3INT Q931Umes_Setup(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generic *m
 				return Q931E_ILLEGAL_IE;
 			}
 			break;
+
 		default:
 			return Q931E_ILLEGAL_IE;
 			break;
 		}
 	}
-    mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
-    return Q931E_NO_ERROR;
+
+	mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
+
+	return Q931E_NO_ERROR;
 }
 
 /*****************************************************************************
@@ -1252,25 +1224,20 @@ L3INT Q931Umes_Setup(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generic *m
 *****************************************************************************/
 L3INT Q931Pmes_Setup(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISize, L3UCHAR *OBuf, L3INT *OSize)
 {
-    L3INT rc = Q931E_NO_ERROR;
+	L3INT rc = Q931E_NO_ERROR;
 	Q931mes_Generic *pMes = (Q931mes_Generic *)IBuf;
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb						*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
-	
-	/* Sending Complete				*/
+	/* Sending Complete */
 	if(Q931IsIEPresent(pMes->SendComplete))
-		OBuf[Octet++]	= (L3UCHAR)(pMes->SendComplete & 0x00ff);
+		OBuf[Octet++] = (L3UCHAR)Q931ie_SENDING_COMPLETE & 0xff;
 
 	/* Repeat Indicator */
 	if(Q931IsIEPresent(pMes->RepeatInd))
-		OBuf[Octet++]	= (L3UCHAR)(pMes->RepeatInd & 0x00ff);		
+		OBuf[Octet++] = (L3UCHAR)Q931ie_REPEAT_INDICATOR & 0xff;		
 
 	/* Bearer capability */
 	if(Q931IsIEPresent(pMes->BearerCap))
@@ -1359,7 +1326,7 @@ L3INT Q931Pmes_Setup(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISiz
 
 	*OSize = Octet;	
 
-    return rc;
+	return rc;
 }
 
 /*****************************************************************************
@@ -1389,8 +1356,8 @@ L3INT Q931Umes_SetupAck(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, Q931mes_Generic
 			break;
 		}
 	}
-    mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
-    return Q931E_NO_ERROR;
+	mes->Size = sizeof(Q931mes_Generic) - 1 + OOff;
+	return Q931E_NO_ERROR;
 }
 
 /*****************************************************************************
@@ -1405,12 +1372,7 @@ L3INT Q931Pmes_SetupAck(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT I
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Channel Identification */
 	if(Q931IsIEPresent(pMes->ChanID))
@@ -1480,12 +1442,7 @@ L3INT Q931Pmes_Status(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISi
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Cause */
 	if(Q931IsIEPresent(pMes->Cause))
@@ -1545,12 +1502,7 @@ L3INT Q931Pmes_StatusEnquiry(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Display */
 	if(Q931IsIEPresent(pMes->Display))
@@ -1602,12 +1554,7 @@ L3INT Q931Pmes_Suspend(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT IS
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Call Identity */
 	if(Q931IsIEPresent(pMes->CallID))
@@ -1660,12 +1607,7 @@ L3INT Q931Pmes_SuspendAck(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Display */
 	if(Q931IsIEPresent(pMes->Display))
@@ -1718,12 +1660,7 @@ L3INT Q931Pmes_SuspendReject(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Cause */
 	if(Q931IsIEPresent(pMes->Cause))
@@ -1817,12 +1754,7 @@ L3INT Q931Pmes_Service(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT IS
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Display */
 	if(Q931IsIEPresent(pMes->ChanID))
@@ -1883,12 +1815,7 @@ L3INT Q931Pmes_ServiceAck(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb							*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	if(Q931IsIEPresent(pMes->ChangeStatus))
 		if((rc=Q931Pie[pTrunk->Dialect][Q931ie_CHANGE_STATUS](pTrunk, Q931GetIEPtr(pMes->ChangeStatus,pMes->buf), OBuf, &Octet))!=0)
diff --git a/src/isdn/include/Q921.h b/src/isdn/include/Q921.h
index 01e7a14..f09fa75 100644
--- a/src/isdn/include/Q921.h
+++ b/src/isdn/include/Q921.h
@@ -31,7 +31,7 @@
 				NOTE: The following are not yet implemented
 
                 OnQ921Error         Function called every if an error is 
-                                    deteceted.
+                                    detected.
 
                 OnQ921Log           Function called if logging is active.
 
@@ -76,84 +76,149 @@
 
 *****************************************************************************/
 
+/****************************************************************************
+ * Changes:
+ *
+ * - June,July 2008: Stefan Knoblich <s.knoblich@axsentis.de>:
+ *     Add PTMP TEI management
+ *     Add timers
+ *     Add retransmit counters
+ *     Add logging
+ *     Various cleanups
+ *
+ ****************************************************************************/
+
 #ifndef _Q921
 #define _Q921
 
 #define Q921MAXHDLCSPACE 3000
-#define L2UCHAR		unsigned char		/* Min 8 bit						*/
-#define L2INT       int                 /* Min 16 bit signed                */
-#define L2ULONG		unsigned long		/* Min 32 bit						*/
+#define L2UCHAR		unsigned char		/* Min 8 bit			*/
+#define L2USHORT	unsigned short		/* 16 bit			*/
+#define L2INT		int			/* Min 16 bit signed		*/
+#define L2ULONG		unsigned long		/* Min 32 bit			*/
 #define L2TRUNK		Q921Data_t *
 
-typedef enum					/* Network/User Mode.                   */
+#define Q921_TEI_BCAST		127
+#define Q921_TEI_MAX		Q921_TEI_BCAST
+
+#define Q921_TEI_DYN_MIN	64
+#define Q921_TEI_DYN_MAX	126
+
+
+typedef enum			/* Network/User Mode		*/
 {
-	Q921_TE=0,                  /*  0 : User Mode                       */
-    Q921_NT=1                   /*  1 : Network Mode                    */
+	Q921_TE=0,		/*  0 : User Mode		*/
+	Q921_NT=1		/*  1 : Network Mode		*/
 } Q921NetUser_t;
 
-typedef struct Q921Data Q921Data_t;
-typedef int (*Q921TxCB_t) (void *, L2UCHAR *, L2INT);
+typedef enum			/* Type of connection		*/
+{
+	Q921_PTP=0,		/* 0 : Point-To-Point		*/
+	Q921_PTMP=1		/* 1 : Point-To-Multipoint	*/
+} Q921NetType_t;
 
-#define INITIALIZED_MAGIC 42
-struct Q921Data
+typedef enum
+{
+	Q921_LOG_NONE = 0,
+	Q921_LOG_ERROR,
+	Q921_LOG_WARNING,
+	Q921_LOG_NOTICE,
+	Q921_LOG_INFO,
+	Q921_LOG_DEBUG
+} Q921LogLevel_t;
+
+
+/*
+ * Messages for L2 <-> L3 communication
+ */
+typedef enum {
+	Q921_DL_ESTABLISH = 0,
+	Q921_DL_ESTABLISH_CONFIRM,
+	Q921_DL_RELEASE,
+	Q921_DL_RELEASE_CONFIRM,
+	Q921_DL_DATA,
+	Q921_DL_UNIT_DATA
+} Q921DLMsg_t;
+
+typedef int (*Q921Tx21CB_t) (void *, L2UCHAR *, L2INT);
+typedef int (*Q921Tx23CB_t) (void *, Q921DLMsg_t ind, L2UCHAR tei, L2UCHAR *, L2INT);
+typedef int (*Q921LogCB_t) (void *, Q921LogLevel_t, char *, L2INT);
+
+struct Q921_Link;
+
+typedef struct Q921Data
 {
-    L2UCHAR HDLCInQueue[Q921MAXHDLCSPACE];
 	L2INT initialized;
-	L2UCHAR va;
-    L2UCHAR vs;
-    L2UCHAR vr;
-    L2INT state;
-	L2UCHAR sapi;
-	L2UCHAR tei;
+
+	L2UCHAR sapi;			/*!< User assigned SAPI */
+	L2UCHAR tei;			/*!< User assigned TEI value */
+
+	L2INT Q921HeaderSpace;
 	Q921NetUser_t NetUser;
-	L2ULONG T200;
-	L2ULONG T203;
+	Q921NetType_t NetType;
+
+	struct Q921_Link *context;	/*!< per-TEI / link context space */
+
+	/* timers */
+	L2ULONG T202;			/*!< PTMP TE mode TEI retransmit timer */
 	L2ULONG T200Timeout;
+	L2ULONG T201Timeout;
+	L2ULONG T202Timeout;
 	L2ULONG T203Timeout;
-	Q921TxCB_t Q921Tx21Proc;
-	Q921TxCB_t Q921Tx23Proc;
+
+	L2ULONG TM01Timeout;
+
+	/* counters */
+	L2ULONG N200Limit;		/*!< max retransmit */
+
+	L2ULONG N202;			/*!< PTMP TE mode retransmit counter */
+	L2ULONG N202Limit;		/*!< PTMP TE mode max retransmit */
+
+	L2ULONG N201Limit;		/*!< max number of octets */
+	L2ULONG k;			/*!< max number of unacknowledged I frames */
+
+	/* callbacks and callback data pointers */
+	Q921Tx21CB_t Q921Tx21Proc;
+	Q921Tx23CB_t Q921Tx23Proc;
 	void *PrivateData21;
 	void *PrivateData23;
-	L2INT Q921HeaderSpace;
 
-};
+	/* logging */
+	Q921LogLevel_t	loglevel;	/*!< trunk loglevel */
+	Q921LogCB_t	Q921LogProc;	/*!< log callback procedure */
+	void *PrivateDataLog;		/*!< private data pointer for log proc */
+
+	/* tei mgmt */
+	L2UCHAR tei_map[Q921_TEI_MAX];	/*!< */
+
+	L2UCHAR HDLCInQueue[Q921MAXHDLCSPACE];	/*!< HDLC input queue */
+} Q921Data_t;
 
-void Q921_InitTrunk(L2TRUNK trunk,
+/*
+ * Public functions
+ */
+int Q921_InitTrunk(L2TRUNK trunk,
 					L2UCHAR sapi,
 					L2UCHAR tei,
 					Q921NetUser_t NetUser,
+					Q921NetType_t NetType,
 					L2INT hsize,
-					Q921TxCB_t cb21,
-					Q921TxCB_t cb23,
+					Q921Tx21CB_t cb21,
+					Q921Tx23CB_t cb23,
 					void *priv21,
 					void *priv23);
-int Q921QueueHDLCFrame(L2TRUNK trunk, L2UCHAR *b, L2INT size);
-int Q921Rx12(L2TRUNK trunk);
-int Q921Rx32(L2TRUNK trunk, L2UCHAR * Mes, L2INT Size);
 int Q921Start(L2TRUNK trunk);
+int Q921Stop(L2TRUNK trunk);
+
+void Q921SetLogCB(L2TRUNK trunk, Q921LogCB_t func, void *priv);
+void Q921SetLogLevel(L2TRUNK trunk, Q921LogLevel_t level);
+
+int Q921Rx12(L2TRUNK trunk);
+int Q921Rx32(L2TRUNK trunk, Q921DLMsg_t ind, L2UCHAR tei, L2UCHAR * Mes, L2INT Size);
+
+int Q921QueueHDLCFrame(L2TRUNK trunk, L2UCHAR *b, L2INT size);
+
 void Q921SetGetTimeCB(L2ULONG (*callback)(void));
 void Q921TimerTick(L2TRUNK trunk);
 
-int Q921Tx21Proc(L2TRUNK trunk, L2UCHAR *Msg, L2INT size);
-int Q921Tx23Proc(L2TRUNK trunk, L2UCHAR *Msg, L2INT size);
-extern L2ULONG tLast;
-L2ULONG Q921GetTime(void);
-void Q921T200TimerStart(L2TRUNK trunk);
-void Q921T200TimerStop(L2TRUNK trunk);
-void Q921T200TimerReset(L2TRUNK trunk);
-void Q921T203TimerStart(L2TRUNK trunk);
-void Q921T203TimerStop(L2TRUNK trunk);
-void Q921T203TimerReset(L2TRUNK trunk);
-void Q921T200TimerExpire(L2TRUNK trunk);
-void Q921T203TimerExpire(L2TRUNK trunk);
-int Q921SendI(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR *mes, L2INT size);
-int Q921SendRNR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
-int Q921SendREJ(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
-int Q921SendSABME(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
-int Q921SendDM(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
-int Q921SendDISC(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
-int Q921SendUA(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
-int Q921ProcSABME(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
-
 #endif
-
diff --git a/src/isdn/include/Q921priv.h b/src/isdn/include/Q921priv.h
new file mode 100644
index 0000000..c355e9b
--- /dev/null
+++ b/src/isdn/include/Q921priv.h
@@ -0,0 +1,298 @@
+/*****************************************************************************
+
+  FileName:     Q921priv.h
+
+  Description:  Private declarations
+
+  Created:      08.Aug.2008/STKN
+
+  License/Copyright:
+
+  Copyright (c) 2007, Jan Vidar Berger, Case Labs, Ltd. All rights reserved.
+  email:janvb@caselaboratories.com  
+
+  Copyright (c) 2008, Stefan Knoblich, axsentis GmbH. All rights reserved.
+  email:s.knoblich@axsentis.de  
+
+  Redistribution and use in source and binary forms, with or without 
+  modification, are permitted provided that the following conditions are 
+  met:
+
+    * Redistributions of source code must retain the above copyright notice, 
+	  this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright notice, 
+	  this list of conditions and the following disclaimer in the documentation 
+	  and/or other materials provided with the distribution.
+    * Neither the name of the Case Labs, Ltd nor the names of its contributors 
+	  may be used to endorse or promote products derived from this software 
+	  without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
+  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
+  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
+  POSSIBILITY OF SUCH DAMAGE.
+
+*****************************************************************************/
+#ifndef _Q921_PRIV_H_
+#define _Q921_PRIV_H_
+
+typedef enum			/* Q.921 States */
+{
+	Q921_STATE_STOPPED = 0,			/* Trunk stopped */
+	Q921_STATE_TEI_UNASSIGNED = 1,		/* TEI unassigned */
+	Q921_STATE_TEI_AWAITING,		/* Assign awaiting TEI */
+	Q921_STATE_TEI_ESTABLISH,		/* Establish awaiting TEI */
+	Q921_STATE_TEI_ASSIGNED,		/* TEI assigned */
+	Q921_STATE_AWAITING_ESTABLISHMENT,	/* Awaiting establishment */
+	Q921_STATE_AWAITING_RELEASE,		/* Awaiting release */
+	Q921_STATE_MULTIPLE_FRAME_ESTABLISHED,	/* Multiple frame established */
+	Q921_STATE_TIMER_RECOVERY		/* Timer recovery */
+} Q921State_t;
+
+/*
+ * Flags
+ */
+enum Q921_Flags {
+	Q921_FLAG_L3_INITIATED = (1 << 0),
+
+	Q921_FLAG_UI_FRAME_QUEUED = (1 << 1),
+	Q921_FLAG_I_FRAME_QUEUED  = (1 << 2),
+
+	Q921_FLAG_ACK_PENDING = (1 << 3),
+	Q921_FLAG_REJECT      = (1 << 4),
+
+	Q921_FLAG_RECV_BUSY      = (1 << 5),
+	Q921_FLAG_PEER_RECV_BUSY = (1 << 6)
+};
+
+#define Q921_SET_FLAG(x, f)	((x)->flags |= f)
+#define Q921_CHECK_FLAG(x, f)	((x)->flags & f)
+#define Q921_CLEAR_FLAG(x, f)	((x)->flags &= ~f)
+
+
+/*
+ * dynamic TEI handling
+ */
+#define Q921_SAPI_TEI		63	/* SAPI for all TEI Messages */
+#define Q921_LAYER_ENT_ID_TEI	0x0f	/* UN Layer Management Entity ID for TEI Mgmt */
+#define Q921_LAYER_ENT_ID_Q931	0x08	/* Q.931 Layer Management Entity ID */
+
+
+typedef enum {
+	Q921_TEI_ID_REQUEST = 1,
+	Q921_TEI_ID_ASSIGNED,
+	Q921_TEI_ID_DENIED,
+	Q921_TEI_ID_CHECKREQ,
+	Q921_TEI_ID_CHECKRESP,
+	Q921_TEI_ID_REMOVE,
+	Q921_TEI_ID_VERIFY
+} Q921TeiMessageType_t;
+
+
+/**
+ * Per-Datalink context
+ */
+struct Q921_Link {
+	L2UCHAR tei;		/*!< This endpoint's TEI */
+
+	L2UCHAR va;
+	L2UCHAR vs;
+	L2UCHAR vr;
+
+	L2INT flags;
+	Q921State_t state;
+
+	L2ULONG N202;		/*!< PTMP TE mode retransmit counter */
+	L2ULONG N200;		/*!< retransmit counter (per-TEI in PTMP NT mode) */
+
+	L2ULONG TM01;		/*!< Datalink inactivity disconnect timer */
+
+	L2ULONG T200;
+	L2ULONG T201;		/*!< PTMP NT mode timer */
+	L2ULONG T203;
+
+	L2USHORT ri;		/*!< random id for TEI request mgmt */
+
+	/* I + UI Frame queue */
+	L2UCHAR UIFrameQueue[Q921MAXHDLCSPACE];
+	L2UCHAR  IFrameQueue[Q921MAXHDLCSPACE];
+	L2UCHAR  IFrameResendQueue[Q921MAXHDLCSPACE];
+};
+
+
+#define Q921_LINK_CONTEXT(tr, tei) \
+	(Q921_IS_PTMP_NT(tr) && tei != Q921_TEI_BCAST) ? ((struct Q921_Link *)&(tr)->context[tei]) : (tr)->context
+
+#define Q921_TRUNK_CONTEXT(tr) \
+	(tr)->context
+
+#define Q921_LOGBUFSIZE		2000
+#define INITIALIZED_MAGIC	42
+
+/*
+ * Helper macros
+ */
+#define Q921_INC_COUNTER(x)		(x = (x + 1) % 128)
+#define Q921_DEC_COUNTER(x)		(x = (x) ? (x - 1) : 127)
+
+#define Q921_UFRAME_HEADER_SIZE		3
+#define Q921_UFRAME_DATA_OFFSET(tr)	((tr)->Q921HeaderSpace + Q921_UFRAME_HEADER_SIZE)
+
+#define Q921_SFRAME_HEADER_SIZE		4
+#define Q921_SFRAME_DATA_OFFSET(tr)	((tr)->Q921HeaderSpace + Q921_SFRAME_HEADER_SIZE)
+
+#define Q921_IFRAME_HEADER_SIZE		4
+#define Q921_IFRAME_DATA_OFFSET(tr)	((tr)->Q921HeaderSpace + Q921_IFRAME_HEADER_SIZE)
+
+#define Q921_IS_TE(x)			((x)->NetUser == Q921_TE)
+#define Q921_IS_NT(x)			((x)->NetUser == Q921_NT)
+
+#define Q921_IS_STOPPED(tr)		((tr)->state == Q921_STATE_STOPPED)
+
+/* TODO: rework this one */
+#define Q921_IS_READY(tr)		((tr)->state >= Q921_STATE_TEI_ASSIGNED)
+
+#define Q921_IS_PTMP(x)			((x)->NetType == Q921_PTMP)
+#define Q921_IS_PTMP_TE(x)		((x)->NetType == Q921_PTMP && (x)->NetUser == Q921_TE)
+#define Q921_IS_PTMP_NT(x)		((x)->NetType == Q921_PTMP && (x)->NetUser == Q921_NT)
+
+#define Q921_IS_PTP(x)			((x)->NetType == Q921_PTP)
+#define Q921_IS_PTP_TE(x)		((x)->NetType == Q921_PTP && (x)->NetUser == Q921_TE)
+#define Q921_IS_PTP_NT(x)		((x)->NetType == Q921_PTP && (x)->NetUser == Q921_NT)
+
+/* Make life a little easier */
+#define Q921_COMMAND(x)			((x)->NetUser == Q921_TE ? 0 : 1)
+#define Q921_RESPONSE(x)		((x)->NetUser == Q921_TE ? 1 : 0)
+
+#define Q921_IS_COMMAND(tr, x)		((x) == (Q921_IS_TE(tr) ? 1 : 0))
+#define Q921_IS_RESPONSE(tr, x)		((x) == (Q921_IS_TE(tr) ? 0 : 1))
+
+
+/*******************************************************************************
+ * Private functions
+ *******************************************************************************/
+
+/*
+ * L1 / L2 Interface
+ */
+static int Q921Tx21Proc(L2TRUNK trunk, L2UCHAR *Msg, L2INT size);
+static int Q921Tx23Proc(L2TRUNK trunk, Q921DLMsg_t ind, L2UCHAR tei, L2UCHAR *Msg, L2INT size);
+
+
+/*
+ * Timers
+ */
+static L2ULONG Q921GetTime(void);
+
+static void Q921T200TimerStart(L2TRUNK trunk, L2UCHAR tei);
+static void Q921T200TimerStop(L2TRUNK trunk, L2UCHAR tei);
+static void Q921T200TimerReset(L2TRUNK trunk, L2UCHAR tei);
+static void Q921T200TimerExpire(L2TRUNK trunk, L2UCHAR tei);
+
+static void Q921T201TimerStart(L2TRUNK trunk, L2UCHAR tei);
+static void Q921T201TimerStop(L2TRUNK trunk, L2UCHAR tei);
+/* static void Q921T201TimerReset(L2TRUNK trunk, L2UCHAR tei); - Unused for now */
+static void Q921T201TimerExpire(L2TRUNK trunk, L2UCHAR tei);
+
+static void Q921T202TimerStart(L2TRUNK trunk);
+static void Q921T202TimerStop(L2TRUNK trunk);
+static void Q921T202TimerReset(L2TRUNK trunk);
+static void Q921T202TimerExpire(L2TRUNK trunk);
+
+static void Q921T203TimerStart(L2TRUNK trunk, L2UCHAR tei);
+static void Q921T203TimerStop(L2TRUNK trunk, L2UCHAR tei);
+static void Q921T203TimerReset(L2TRUNK trunk, L2UCHAR tei);
+static void Q921T203TimerExpire(L2TRUNK trunk, L2UCHAR tei);
+
+static void Q921TM01TimerStart(L2TRUNK trunk, L2UCHAR tei);
+/* static void Q921TM01TimerStop(L2TRUNK trunk, L2UCHAR tei); - Unused for now */
+static void Q921TM01TimerReset(L2TRUNK trunk, L2UCHAR tei);
+/* static void Q921TM01TimerExpire(L2TRUNK trunk, L2UCHAR tei); - Unused for now */
+
+/*
+ * Frame encoding
+ */
+static int Q921SendS(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR sv, L2UCHAR *mes, L2INT size);
+static int Q921SendU(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR m, L2UCHAR *mes, L2INT size);
+
+static int Q921SendRNR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
+static int Q921SendREJ(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
+static int Q921SendSABME(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
+static int Q921SendDM(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
+static int Q921SendDISC(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
+static int Q921SendUA(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
+static int Q921SendUN(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf, L2UCHAR *mes, L2INT size);
+static int Q921SendRR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
+
+/*
+ * Frame decoding
+ */
+static int Q921ProcIFrame(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921ProcSFrame(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921ProcUFrame(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+
+static int Q921ProcSABME(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921ProcDM(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921ProcUA(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921ProcDISC(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921ProcRR(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921ProcRNR(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921ProcREJ(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+
+
+/*
+ * (Common) procedures defined in the Q.921 SDL
+ */
+static int Q921SendEnquiry(L2TRUNK trunk, L2UCHAR tei);
+static int Q921SendEnquiryResponse(L2TRUNK trunk, L2UCHAR tei);
+static void Q921ResetExceptionConditions(L2TRUNK trunk, L2UCHAR tei);
+static int Q921EstablishDataLink(L2TRUNK trunk, L2UCHAR tei);
+static int Q921NrErrorRecovery(L2TRUNK trunk, L2UCHAR tei);
+static int Q921InvokeRetransmission(L2TRUNK trunk, L2UCHAR tei, L2UCHAR nr);
+static int Q921AcknowledgePending(L2TRUNK trunk, L2UCHAR tei);
+/*
+static int Q921SetReceiverBusy(L2TRUNK trunk);
+static int Q921ClearReceiverBusy(L2TRUNK trunk);
+*/
+
+/*
+ * Queueing
+ */
+static int Q921SendQueuedIFrame(L2TRUNK trunk, L2UCHAR tei);
+static int Q921EnqueueI(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR *mes, L2INT size);
+
+/*
+ * TEI management
+ */
+static int Q921TeiSendAssignRequest(L2TRUNK trunk);
+static int Q921TeiProcAssignResponse(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921TeiSendVerifyRequest(L2TRUNK trunk);
+static int Q921TeiProcCheckRequest(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921TeiProcRemoveRequest(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921TeiProcAssignRequest(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921TeiProcCheckResponse(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921TeiProcVerifyRequest(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921TeiSendRemoveRequest(L2TRUNK trunk, L2UCHAR tei);
+static int Q921TeiSendDenyResponse(L2TRUNK trunk, L2UCHAR tei, L2USHORT ri);
+static int Q921TeiSendAssignedResponse(L2TRUNK trunk, L2UCHAR tei, L2USHORT ri);
+static int Q921TeiSendCheckRequest(L2TRUNK trunk, L2UCHAR tei);
+
+/*
+ * Logging
+ */
+static int Q921Log(L2TRUNK trunk, Q921LogLevel_t level, const char *fmt, ...);
+static int Q921LogMesg(L2TRUNK trunk, Q921LogLevel_t level, L2UCHAR received, L2UCHAR *mes, L2INT size, const char *fmt, ...);
+
+/*
+ * State handling
+ */
+static int Q921ChangeState(L2TRUNK trunk, Q921State_t state, L2UCHAR tei);
+
+#endif
diff --git a/src/isdn/include/Q931.h b/src/isdn/include/Q931.h
index 70b84af..2687930 100644
--- a/src/isdn/include/Q931.h
+++ b/src/isdn/include/Q931.h
@@ -244,23 +244,35 @@ typedef L3USHORT ie;                /* Special data type to hold a dynamic  */
 
 *****************************************************************************/
 
-#define Q931L4BUF  1000             /* size of message buffer               */
+#define	Q931_LOGBUFSIZE	1024		/* size of logging buffer		*/
 
-#define Q931L2BUF	300				/* size of message buffer				*/
+#define Q931L4BUF	1000		/* size of message buffer		*/
 
-#define Q931MAXTRUNKS	4			/* Total number of trunks that will be	*/
-									/* processed by this instance of the	*/
-									/* stack								*/
+#define Q931L2BUF	300		/* size of message buffer		*/
 
-#define Q931MAXCHPERTRUNK	32		/* Number of channels per trunk. The	*/
-									/* stack uses a static set of 32		*/
-									/* channels regardless if it is E1, T1	*/
-									/* or BRI that actually is used.		*/
+#define Q931MAXTRUNKS	4		/* Total number of trunks that will be	*/
+					/* processed by this instance of the	*/
+					/* stack				*/
 
-#define Q931MAXCALLPERTRUNK (Q931MAXCHPERTRUNK * 2)
-                                    /* Number of max active CRV per trunk.  */
-									/* Q.931 can have more calls than there */
-									/* are channels.						*/
+#define Q931MAXCHPERTRUNK	32	/* Number of channels per trunk. The	*/
+					/* stack uses a static set of 32	*/
+					/* channels regardless if it is E1, T1	*/
+					/* or BRI that actually is used.	*/
+
+#define Q931MAXCALLPERTRUNK	(Q931MAXCHPERTRUNK * 2)
+					/* Number of max active CRV per trunk.  */
+					/* Q.931 can have more calls than there */
+					/* are channels.			*/
+
+
+#define Q931_IS_BRI(x)		((x)->TrunkType == Q931_TrType_BRI || (x)->TrunkType == Q931_TrType_BRI_PTMP)
+#define Q931_IS_PRI(x)		(!Q931_IS_BRI(x))
+
+#define Q931_IS_PTP(x)		((x)->TrunkType != Q931_TrType_BRI_PTMP)
+#define Q931_IS_PTMP(X)		((x)->TrunkType == Q931_TrType_BRI_PTMP)
+
+#define Q931_BRI_MAX_CRV	127
+#define Q931_PRI_MAX_CRV	32767
 
 /*****************************************************************************
 
@@ -280,24 +292,19 @@ typedef L3USHORT ie;                /* Special data type to hold a dynamic  */
 
 *****************************************************************************/
 
-/* WARNING! Initialize Q931CreateDialectCB[] will NULL when increasing the  */
-/* Q931MAXDLCT value to avoid Q931Initialize from crashing if one entry is  */
-/* not used.																*/
-#define Q931MAXDLCT 8           /* Max dialects included in this        */
-                                /* compile. User and Network count as   */
-                                /* one dialect each.                    */
-
-
-#define Q931MAXMES  128         /* Number of messages							*/
-
-#define Q931MAXIE  255          /* Number of IE									*/
-
-#define Q931MAXUSEDIE 50		/* Maximum number of ie types per Dialect		*/
-
-#define Q931MAXCODESETS 7		/* Maximum number of codests (by spec, 0-7)		*/
-
-#define Q931MAXSTATE 100		/* Size of state tables					        */
+/* WARNING! Initialize Q931CreateDialectCB[] will NULL when increasing the */
+/* Q931MAXDLCT value to avoid Q931Initialize from crashing if one entry is */
+/* not used.								   */
+#define Q931MAXDLCT	8	/* Max dialects included in this        */
+				/* compile. User and Network count as   */
+				/* one dialect each.                    */
 
+#define Q931MAXMES	128		/* Number of messages				*/
+#define Q931MAXIE	255		/* Number of IE					*/
+#define Q931MAXUSEDIE	50		/* Maximum number of ie types per Dialect	*/
+#define Q931MAXCODESETS	7		/* Maximum number of codests (by spec, 0-7)	*/
+#define Q931MAXSTATE	100		/* Size of state tables			        */
+#define Q931MAXTIMER	25		/* Maximum number of timers 			*/
 
 /*****************************************************************************
 
@@ -381,6 +388,155 @@ typedef L3USHORT ie;                /* Special data type to hold a dynamic  */
 #define Q931mes_SERVICE_ACKNOWLEDGE  0x07 /* 0000 0111                   */
 
 
+/**
+ * Generic Q.931 Timers
+ */
+enum {
+	Q931_TIMER_T300	= 1,		/* */
+	Q931_TIMER_T301,
+	Q931_TIMER_T302,
+	Q931_TIMER_T303,
+	Q931_TIMER_T304,
+	Q931_TIMER_T305,
+	Q931_TIMER_T306,
+	Q931_TIMER_T307,
+	Q931_TIMER_T308,
+	Q931_TIMER_T309,
+	Q931_TIMER_T310,
+	Q931_TIMER_T311,
+	Q931_TIMER_T312,
+	Q931_TIMER_T313,
+	Q931_TIMER_T314,
+	Q931_TIMER_T315,
+	Q931_TIMER_T316,
+	Q931_TIMER_T317,
+	Q931_TIMER_T318,
+	Q931_TIMER_T319,
+	Q931_TIMER_T320,
+	Q931_TIMER_T321,
+	Q931_TIMER_T322,
+};
+
+/**
+ * Q.931 ToN
+ */
+enum {
+	Q931_TON_UNKNOWN		= 0x00,
+	Q931_TON_INTERNATIONAL		= 0x01,
+	Q931_TON_NATIONAL		= 0x02,
+	Q931_TON_NETWORK_SPECIFIC	= 0x03,
+	Q931_TON_SUBSCRIBER_NUMBER	= 0x04,
+	Q931_TON_ABBREVIATED_NUMBER	= 0x06,
+	Q931_TON_RESERVED		= 0x07
+};
+
+/**
+ * Q.931 Numbering Plan
+ */
+enum {
+	Q931_NUMPLAN_UNKNOWN		= 0x00,
+	Q931_NUMPLAN_E164		= 0x01,
+	Q931_NUMPLAN_X121		= 0x03,
+	Q931_NUMPLAN_F69		= 0x04,
+	Q931_NUMPLAN_NATIONAL		= 0x08,
+	Q931_NUMPLAN_PRIVATE		= 0x09,
+	Q931_NUMPLAN_RESERVED		= 0x0e
+};
+
+/**
+ * Q.931 Presentation Indicator
+ */
+enum {
+	Q931_PRES_ALLOWED		= 0x00,
+	Q931_PRES_RESTRICTED		= 0x01,
+	Q931_PRES_NOT_AVAILABLE		= 0x02,
+	Q931_PRES_RESERVED		= 0x03
+};
+
+/**
+ * Q.931 Screening Indicator
+ */
+enum {
+	Q931_SCREEN_USER_NOT_SCREENED		= 0x00,
+	Q931_SCREEN_USER_VERIFIED_PASSED	= 0x01,
+	Q931_SCREEN_USER_VERIFIED_FAILED	= 0x02,
+	Q931_SCREEN_NETWORK			= 0x03
+};
+
+/**
+ * Q.931 Coding Standard
+ */
+enum {
+	Q931_CODING_ITU		= 0x00,
+	Q931_CODING_ISO		= 0x01,
+	Q931_CODING_NATIONAL	= 0x02,
+	Q931_CODING_NETWORK	= 0x03
+};
+
+/**
+ * Q.931 High layer characteristik id
+ */
+enum {
+	Q931_HLCHAR_TELEPHONY	= 0x01,
+	Q931_HLCHAR_FAX_G23	= 0x04,
+	Q931_HLCHAR_FAX_G4	= 0x21,
+	Q931_HLCHAR_FAX_G4II	= 0x24,
+	Q931_HLCHAR_T102	= 0x32,
+	Q931_HLCHAR_T101	= 0x33,
+	Q931_HLCHAR_F60		= 0x35,
+	Q931_HLCHAR_X400	= 0x38,
+	Q931_HLCHAR_X200	= 0x41
+};
+
+/**
+ * Q.931 User information layer 1 protocol
+ */
+enum {
+	Q931_UIL1P_V110		= 0x01,
+	Q931_UIL1P_I460		= 0x01,
+	Q931_UIL1P_X30		= 0x01,
+
+	Q931_UIL1P_G711U	= 0x02,
+	Q931_UIL1P_G711A	= 0x03,
+	Q931_UIL1P_G721		= 0x04,
+
+	Q931_UIL1P_H221		= 0x05,
+	Q931_UIL1P_H242		= 0x05,
+
+	Q931_UIL1P_H223		= 0x06,
+	Q931_UIL1P_H245		= 0x06,
+
+	Q931_UIL1P_RATE_ADAP	= 0x07,
+
+	Q931_UIL1P_V120		= 0x08,
+	Q931_UIL1P_X31		= 0x09
+};
+
+/**
+ * Q.931 Information Transfer Capability
+ */
+enum {
+	Q931_ITC_SPEECH			= 0x00,
+	Q931_ITC_UNRESTRICTED		= 0x08,
+	Q931_ITC_RESTRICTED		= 0x09,
+	Q931_ITC_3K1_AUDIO		= 0x10,
+	Q931_ITC_UNRESTRICTED_TONES	= 0x11,
+	Q931_ITC_VIDEO			= 0x18
+};
+
+/**
+ * Q.931 Information transfer rate
+ */
+enum {
+	Q931_ITR_PACKET	= 0x00,
+	Q931_ITR_64K	= 0x10,
+	Q931_ITR_128K	= 0x11,
+	Q931_ITR_384K	= 0x13,
+	Q931_ITR_1536K	= 0x15,
+	Q931_ITR_1920K	= 0x17,
+	Q931_ITR_MULTI	= 0x18
+};
+
 /*****************************************************************************
 
   Struct:       Q931mes_Header
@@ -479,33 +635,46 @@ typedef struct
 *****************************************************************************/
 typedef struct Q931_TrunkInfo Q931_TrunkInfo_t;
 
-typedef L3INT (*Q931TxCB_t) (void *,L3UCHAR *, L3INT);
+typedef enum
+{
+	Q931_LOG_NONE = 0,
+	Q931_LOG_ERROR,
+	Q931_LOG_WARNING,
+	Q931_LOG_NOTICE,
+	Q931_LOG_INFO,
+	Q931_LOG_DEBUG
+} Q931LogLevel_t;
+
+typedef L3INT (*Q931Tx34CB_t) (void *,L3UCHAR *, L3INT);
+typedef L3INT (*Q931Tx32CB_t) (void *, L3INT, L3UCHAR, L3UCHAR *, L3INT);
 typedef L3INT (*Q931ErrorCB_t) (void *,L3INT,L3INT,L3INT);
+typedef L3INT (*Q931LogCB_t) (void *, Q931LogLevel_t, char *, L3INT);
 
-typedef enum						/* Network/User Mode.                   */
+typedef enum					/* Network/User Mode.			*/
 {
-	Q931_TE=0,						/*  0 : User Mode                       */
-    Q931_NT=1						/*  1 : Network Mode                    */
+	Q931_TE=0,				/*  0 : User Mode			*/
+	Q931_NT=1				/*  1 : Network Mode			*/
 } Q931NetUser_t;
 
-typedef enum						/* Dialect enum                         */
+typedef enum					/* Dialect enum                         */
 {
 	Q931_Dialect_Q931     = 0,
 	Q931_Dialect_National = 2,
 	Q931_Dialect_DMS      = 4,
-	Q931_Dialect_5ESS     = 6,      /* Coming soon to a PRI stack near you! */
+	Q931_Dialect_5ESS     = 6,		/* Coming soon to a PRI stack near you! */
 
 	Q931_Dialect_Count
 } Q931Dialect_t;
-#define DIALECT_STRINGS "q931", "", "national", "", "dms","","5ess",""
+#define DIALECT_STRINGS "q931", "", "national", "", "dms", "", "5ess", ""
 Q931_STR2ENUM_P(q931_str2Q931Dialect_type, q931_Q931Dialect_type2str, Q931Dialect_t)
 
-typedef enum						/* Trunk Line Type.                     */
+typedef enum					/* Trunk Line Type.			*/
 {
-	Q931_TrType_E1=0,				/*  0 : E1 Trunk                        */
-    Q931_TrType_T1=1,				/*  1 : T1 Trunk                        */
-    Q931_TrType_J1=2,				/*  2 : J1 Trunk                        */
-    Q931_TrType_BRI=3				/*  3 : BRI Trunk                       */
+	Q931_TrType_E1 = 0,			/*  0 : E1 Trunk			*/
+	Q931_TrType_T1 = 1,			/*  1 : T1 Trunk			*/
+	Q931_TrType_J1 = 2,			/*  2 : J1 Trunk			*/
+	Q931_TrType_BRI	= 3,			/*  3 : BRI Trunk			*/
+	Q931_TrType_BRI_PTMP = 4		/*  4 : BRI PTMP Trunk			*/
 } Q931_TrunkType_t;
 
 typedef enum						/* Trunk State							*/
@@ -522,19 +691,51 @@ typedef enum {
 	Q931_ChType_Sync=3				/* Sync Channel							*/
 } Q931_ChanType_t;
 
-struct Q931_TrunkInfo
+struct Q931_Call
 {
-	Q931NetUser_t NetUser;			/* Network/User Mode.                   */
+	L3UCHAR InUse;			/* Indicate if entry is in use.         */
+					/*  0 = Not in Use                      */
+					/*  1 = Active Call.                    */
 
-    Q931Dialect_t Dialect;			/* Q.931 Based dialect index.           */
+	L3UCHAR Tei;			/* Associated TEI 			*/
 
+	L3UCHAR BChan;			/* Associated B Channel.                */
+					/* 0 - 31 valid B chan			*/
+					/* 255 = Not allocated			*/
+
+	L3INT   CRV;			/* Associated CRV.                      */
+
+	L3UINT  State;			/* Call State.                          */
+					/*  0 is Idle, but other values are	*/
+					/*  defined per dialect.		*/
+					/*  Default usage is 1-99 for TE and    */
+					/*  101 - 199 for NT.			*/
+        
+	L3ULONG Timer;			/* Timer in ms. The TimeTick will check		*/
+					/* if this has exceeded the timeout, and	*/
+					/* if so call the timers timeout proc.		*/
+
+	L3USHORT TimerID;		/* Timer Identification/State           */
+					/* actual values defined by dialect	*/
+					/*  0 : No timer running                */
+					/*  ITU-T Q.931:301 - 322 Timer running */
+};
+
+struct Q931_TrunkInfo
+{
+	Q931NetUser_t NetUser;			/* Network/User Mode.                   */
 	Q931_TrunkType_t TrunkType;		/* Trunk Line Type.                     */
+	Q931Dialect_t Dialect;			/* Q.931 Based dialect index.           */
 
-	Q931TxCB_t	Q931Tx34CBProc;
-	Q931TxCB_t	Q931Tx32CBProc;
+	Q931Tx34CB_t	Q931Tx34CBProc;
+	Q931Tx32CB_t	Q931Tx32CBProc;
 	Q931ErrorCB_t Q931ErrorCBProc;
+	Q931LogCB_t	Q931LogCBProc;
 	void *PrivateData32;
 	void *PrivateData34;
+	void *PrivateDataLog;
+
+	Q931LogLevel_t	loglevel;
 
 	L3UCHAR     Enabled;            /* Enabled/Disabled                     */
                                     /*  0 = Disabled                        */
@@ -580,37 +781,9 @@ struct Q931_TrunkInfo
 
     }ch[Q931MAXCHPERTRUNK];
 
-    /* Active Call information indentified by CRV. See Q931AllocateCRV for  */
-	/* initialization of call table.										*/
-    struct _ccarray
-    {
-        L3UCHAR InUse;              /* Indicate if entry is in use.         */
-                                    /*  0 = Not in Use                      */
-                                    /*  1 = Active Call.                    */
-
-        L3UCHAR BChan;              /* Associated B Channel.                */
-									/* 0 - 31 valid B chan					*/
-									/* 255 = Not allocated					*/
-
-        L3INT   CRV;                /* Associated CRV.                      */
-
-        L3UINT  State;              /* Call State.                          */
-									/*  0 is Idle, but other values are		*/
-									/*  defined per dialect.				*/
-									/*  Default usage is 1-99 for TE and    */
-									/*  101 - 199 for NT.					*/
-        
-        L3ULONG Timer;              /* Timer in ms. The TimeTick will check */
-									/* if this has exceeded the timeout, and*/
-									/* if so call the timers timeout proc.	*/
-
-        L3USHORT TimerID;           /* Timer Identification/State           */
-									/* actual values defined by dialect		*/
-                                    /*  0 : No timer running                */
-                                    /*  ITU-T Q.931:301 - 322 Timer running */
-
-    }call[Q931MAXCALLPERTRUNK];
-
+	/* Active Call information indentified by CRV. See Q931AllocateCRV for  */
+	/* initialization of call table.					*/
+	struct Q931_Call	call[Q931MAXCALLPERTRUNK];
 };
 
 /*****************************************************************************
@@ -646,6 +819,9 @@ typedef L3INT (q931pmes_func_t) (Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf
 typedef L3INT (q931uie_func_t) (Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *pMsg, L3UCHAR * IBuf, L3UCHAR * OBuf, L3INT *IOff, L3INT *OOff);
 typedef L3INT (q931pie_func_t) (Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf, L3INT *Octet);
 
+typedef L3INT (q931timeout_func_t) (Q931_TrunkInfo_t *pTrunk, L3INT callIndex);
+typedef L3ULONG q931timer_t;
+
 extern q931proc_func_t *Q931Proc[Q931MAXDLCT][Q931MAXMES];
 
 extern q931umes_func_t *Q931Umes[Q931MAXDLCT][Q931MAXMES];
@@ -654,6 +830,10 @@ extern q931pmes_func_t *Q931Pmes[Q931MAXDLCT][Q931MAXMES];
 extern q931uie_func_t *Q931Uie[Q931MAXDLCT][Q931MAXIE];
 extern q931pie_func_t *Q931Pie[Q931MAXDLCT][Q931MAXIE];
 
+extern q931timeout_func_t *Q931Timeout[Q931MAXDLCT][Q931MAXTIMER];
+extern q931timer_t         Q931Timer[Q931MAXDLCT][Q931MAXTIMER];
+
+
 /*****************************************************************************
     
   Macro:        GetIETotoSize
@@ -893,9 +1073,9 @@ L3INT Q931ProcUnexpectedMessage(Q931_TrunkInfo_t *pTrunk,L3UCHAR * b, L3INT iFro
   Interface Function Prototypes. Implemented in Q931.c
 
 *****************************************************************************/
-void    Q931TimeTick(Q931_TrunkInfo_t *pTrunk, L3ULONG ms);
-L3INT   Q931Rx23(Q931_TrunkInfo_t *pTrunk, L3UCHAR * Mes, L3INT Size);
-L3INT   Q931Tx32(Q931_TrunkInfo_t *pTrunk, L3UCHAR * Mes, L3INT Size);
+void    Q931TimerTick(Q931_TrunkInfo_t *pTrunk);
+L3INT   Q931Rx23(Q931_TrunkInfo_t *pTrunk, L3INT ind, L3UCHAR tei, L3UCHAR * Mes, L3INT Size);
+L3INT   Q931Tx32Data(Q931_TrunkInfo_t *pTrunk, L3UCHAR bcast, L3UCHAR * Mes, L3INT Size);
 L3INT   Q931Rx43(Q931_TrunkInfo_t *pTrunk, L3UCHAR * Mes, L3INT Size);
 L3INT   Q931Tx34(Q931_TrunkInfo_t *pTrunk, L3UCHAR * Mes, L3INT Size);
 void    Q931SetError(Q931_TrunkInfo_t *pTrunk,L3INT ErrID, L3INT ErrPar1, L3INT ErrPar2);
@@ -910,6 +1090,8 @@ void    Q931SetHeaderSpace(L3INT space);
 
 void Q931SetMesProc(L3UCHAR mes, L3UCHAR dialect, q931proc_func_t *Q931ProcFunc, q931umes_func_t *Q931UmesFunc, q931pmes_func_t *Q931PmesFunc);
 void Q931SetIEProc(L3UCHAR iec, L3UCHAR dialect, q931pie_func_t *Q931PieProc, q931uie_func_t *Q931UieProc);
+void Q931SetTimeoutProc(L3UCHAR timer, L3UCHAR dialect, q931timeout_func_t *Q931TimeoutProc);
+void Q931SetTimerDefault(L3UCHAR timer, L3UCHAR dialect, q931timer_t timeout);
 
 void Q931Initialize(void);
 void Q931AddDialect(L3UCHAR iDialect, void (*Q931CreateDialectCB)(L3UCHAR iDialect));
@@ -926,7 +1108,7 @@ L3INT	Q931StartTimer(Q931_TrunkInfo_t *pTrunk, L3INT callIndex, L3USHORT iTimer)
 L3INT	Q931StopTimer(Q931_TrunkInfo_t *pTrunk, L3INT callindex, L3USHORT iTimer);
 L3INT	Q931SetState(Q931_TrunkInfo_t *pTrunk, L3INT callIndex, L3INT iState);
 L3ULONG Q931GetTime(void);
-void Q931SetGetTimeCB(L3ULONG (*callback)(void));
+void    Q931SetGetTimeCB(L3ULONG (*callback)(void));
 void	Q931AddStateEntry(L3UCHAR iD, L3INT iState, L3INT iMes, L3UCHAR cDir);
 L3BOOL	Q931IsEventLegal(L3UCHAR iD, L3INT iState, L3INT iMes, L3UCHAR cDir);
 
@@ -965,8 +1147,8 @@ L3INT Q931Api_InitTrunk(Q931_TrunkInfo_t *pTrunk,
 						Q931Dialect_t Dialect,
 						Q931NetUser_t NetUser,
 						Q931_TrunkType_t TrunkType,
-						Q931TxCB_t Q931Tx34CBProc,
-						Q931TxCB_t Q931Tx32CBProc,
+						Q931Tx34CB_t Q931Tx34CBProc,
+						Q931Tx32CB_t Q931Tx32CBProc,
 						Q931ErrorCB_t Q931ErrorCBProc,
 						void *PrivateData32,
 						void *PrivateData34);
@@ -974,7 +1156,9 @@ L3INT Q931Api_InitTrunk(Q931_TrunkInfo_t *pTrunk,
 L3INT Q931GetMesSize(Q931mes_Generic *pMes);
 L3INT Q931InitMesResume(Q931mes_Generic * pMes);
 
-
+L3INT Q931Log(Q931_TrunkInfo_t *trunk, Q931LogLevel_t level, const char *fmt, ...);
+void Q931SetLogCB(Q931_TrunkInfo_t *trunk, Q931LogCB_t func, void *priv);
+void Q931SetLogLevel(Q931_TrunkInfo_t *trunk, Q931LogLevel_t level);
 
 void Q931SetL4HeaderSpace(L3INT space);
 void Q931SetL2HeaderSpace(L3INT space);
@@ -985,7 +1169,8 @@ L3INT Q931PmesDummy(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT ISize
 L3INT Q931PieDummy(Q931_TrunkInfo_t *pTrunk,L3UCHAR *IBuf, L3UCHAR *OBuf, L3INT *Octet);
 L3INT Q931TxDummy(Q931_TrunkInfo_t *pTrunk, L3UCHAR * b, L3INT n);
 L3INT Q931ErrorDummy(void *priv, L3INT a, L3INT b, L3INT c);
+L3INT Q931TimeoutDummy(Q931_TrunkInfo_t *pTrunk, L3INT callIndex);
 
-
+L3INT Q931MesgHeader(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *mes, L3UCHAR *OBuf, L3INT Size, L3INT *IOff);
 
 #endif /* _Q931_NL */
diff --git a/src/isdn/include/Q931ie.h b/src/isdn/include/Q931ie.h
index e64480d..2acb937 100644
--- a/src/isdn/include/Q931ie.h
+++ b/src/isdn/include/Q931ie.h
@@ -57,7 +57,7 @@ typedef enum {
 /* Single octet information elements                                */
 #define Q931ie_SHIFT                            0x90 /* 1001 ----       */
 #define Q931ie_MORE_DATA                        0xa0 /* 1010 ----       */
-#define Q931ie_SENDING_COMPLETE                 0xa1 /* 1010 0000       */
+#define Q931ie_SENDING_COMPLETE                 0xa1 /* 1010 0001       */
 #define Q931ie_CONGESTION_LEVEL                 0xb0 /* 1011 ----       */
 #define Q931ie_REPEAT_INDICATOR                 0xd0 /* 1101 ----       */
 
@@ -101,6 +101,9 @@ typedef enum {
 
 #define Q931ie_GENERIC_DIGITS					0x37 /* 0011 0111       */
 
+/* Variable Length Information Element to shut up BRI testing */
+#define Q931ie_CONNECTED_NUMBER			0x4c /* 0100 1101 */
+#define Q931ie_FACILITY				0x1c /* 0001 1101 */
 
 
 /*****************************************************************************
@@ -1168,5 +1171,7 @@ L3INT Q931Pie_CongLevel(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf,
 L3INT Q931Uie_RevChargeInd(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *pMsg, L3UCHAR * IBuf, L3UCHAR * OBuf, L3INT *IOff, L3INT *OOff);
 L3INT Q931Pie_RevChargeInd(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf, L3INT *Octet);
 
+L3INT Q931Uie_Generic(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *pMsg, L3UCHAR *IBuf, L3UCHAR *OBuf, L3INT *IOff, L3INT *OOff);
+L3INT Q931Pie_Generic(Q931_TrunkInfo_t *pTrunk, L3UCHAR *IBuf, L3UCHAR *OBuf, L3INT *Octet);
 
 #endif /* _Q931IE_NL */
diff --git a/src/isdn/include/mfifo.h b/src/isdn/include/mfifo.h
index 99faa1d..4773624 100644
--- a/src/isdn/include/mfifo.h
+++ b/src/isdn/include/mfifo.h
@@ -58,15 +58,15 @@ typedef struct _mindex
 				queue.
 
 *****************************************************************************/
-typedef struct _mfifo
+typedef struct
 {
 	int first;                      /* first out                            */
 	int last;                       /* last in + 1                          */
 	int bsize;                      /* buffer size                          */
-	unsigned char *buf;                      /* ptr to start of buffer               */
-    int ixsize;                     /* index size                           */
-    MINDEX ix[1];                   /* message index                        */
-}MFIFO;
+	unsigned char *buf;             /* ptr to start of buffer               */
+	int ixsize;                     /* index size                           */
+	MINDEX ix[1];                   /* message index                        */
+} MFIFO;
 
 /*****************************************************************************
   Function prototypes.
@@ -80,4 +80,8 @@ int MFIFOWriteMes(unsigned char *buf, unsigned char *mes, int size);
 unsigned char * MFIFOGetMesPtr(unsigned char *buf, int *size);
 void MFIFOKillNext(unsigned char *buf);
 
+unsigned char * MFIFOGetMesPtrOffset(unsigned char *buf, int *size, const int pos);
+int MFIFOGetMesCount(unsigned char *buf);
+int MFIFOWriteMesOverwrite(unsigned char *buf, unsigned char *mes, int size);
+
 #endif
diff --git a/src/isdn/mfifo.c b/src/isdn/mfifo.c
index a1e85de..64dbced 100644
--- a/src/isdn/mfifo.c
+++ b/src/isdn/mfifo.c
@@ -84,17 +84,16 @@
 *****************************************************************************/
 int MFIFOCreate(unsigned char *buf, int size, int index)
 {
-    MFIFO * mf;
-    mf = (MFIFO*)buf;
+    MFIFO *mf = (MFIFO *)buf;
     
     mf->first = mf->last = 0;
     mf->ixsize = index;
-    mf->buf = &buf[sizeof(MFIFO) + (sizeof(MINDEX) * (index-1))];
+    mf->buf = &buf[sizeof(MFIFO) + (sizeof(MINDEX) * index)];
 
-    if(mf->buf > & buf[size])
+    if(mf->buf > &buf[size])
         return 0;
 
-    mf->bsize = size - sizeof(MFIFO) - (sizeof(MINDEX) * (index-1));
+    mf->bsize = size - sizeof(MFIFO) - (sizeof(MINDEX) * index);
 
     return 1;
 }
@@ -112,9 +111,9 @@ int MFIFOCreate(unsigned char *buf, int size, int index)
 *****************************************************************************/
 void MFIFOClear(unsigned char * buf)
 {
-    MFIFO * mf;
-    mf = (MFIFO*)buf;
-    mf->first = mf->last = 0;
+	MFIFO *mf = (MFIFO *)buf;
+
+	mf->first = mf->last = 0;
 }
 
 /*****************************************************************************
@@ -130,8 +129,8 @@ void MFIFOClear(unsigned char * buf)
 *****************************************************************************/
 int MFIFOGetLBOffset(unsigned char *buf)
 {
-    MFIFO * mf;
-    mf = (MFIFO*)buf;
+    MFIFO *mf = (MFIFO *)buf;
+
     if(mf->last != mf->first)
         return mf->ix[mf->last].offset;
     
@@ -153,15 +152,17 @@ int MFIFOGetLBOffset(unsigned char *buf)
 *****************************************************************************/
 int MFIFOGetFBOffset(unsigned char *buf)
 {
+    MFIFO *mf = (MFIFO *)buf;
     int x;
-    MFIFO * mf;
-    mf = (MFIFO*)buf;
+
     if(mf->last == mf->first)
         return 0;
-    x=mf->first;
-    x--;
-    if(x<0)
-        x=mf->ixsize;
+
+    x = mf->first - 1;
+
+    if(x < 0)
+        x = mf->ixsize - 1;
+
     return mf->ix[x].offset + mf->ix[x].size;
 }
 
@@ -184,16 +185,19 @@ int MFIFOGetFBOffset(unsigned char *buf)
 *****************************************************************************/
 void MFIFOWriteIX(unsigned char *buf, unsigned char *mes, int size, int ix, int off)
 {
+    MFIFO *mf = (MFIFO *)buf;
     int x;
-    MFIFO * mf;
-    mf = (MFIFO*)buf;
-    memcpy(&mf->buf[off],mes,size);
-    mf->ix[ix].offset=off;
-    mf->ix[ix].size=size;
-    x = mf->first+1;
-    if(x > mf->ixsize)
-        x=0;
-    mf->first=x;
+
+    memcpy(&mf->buf[off], mes, size);
+    mf->ix[ix].offset = off;
+    mf->ix[ix].size = size;
+
+    x = mf->first + 1;
+
+    if(x >= mf->ixsize)
+        x = 0;
+
+    mf->first = x;
 }
 
 /*****************************************************************************
@@ -209,13 +213,14 @@ void MFIFOWriteIX(unsigned char *buf, unsigned char *mes, int size, int ix, int
 *****************************************************************************/
 int MFIFOWriteMes(unsigned char *buf, unsigned char *mes, int size)
 {
-    int of,ol,x;
-    MFIFO * mf;
-    mf = (MFIFO*)buf;
+    MFIFO *mf = (MFIFO *)buf;
+    int of, ol, x;
+
+    x = mf->first + 1;
+
+    if(x >= mf->ixsize)
+        x = 0;
 
-    x = mf->first+1;
-    if(x > mf->ixsize)
-        x=0;
     if(x == mf->last)
         return 0; /* full queue */
 
@@ -224,6 +229,7 @@ int MFIFOWriteMes(unsigned char *buf, unsigned char *mes, int size)
     if(mf->last == mf->first) /* empty queue */
     {
         mf->first = mf->last = 0; /* optimize */
+
         MFIFOWriteIX(buf, mes, size, mf->first, 0);
         return 1;
     }
@@ -231,18 +237,18 @@ int MFIFOWriteMes(unsigned char *buf, unsigned char *mes, int size)
     {
         if(mf->bsize - of >= size)
         {
-            MFIFOWriteIX(buf,mes,size,mf->first,of);
+            MFIFOWriteIX(buf, mes, size, mf->first, of);
             return 1;
         }
         else if(ol > size)
         {
-            MFIFOWriteIX(buf,mes,size,mf->first,ol);
+            MFIFOWriteIX(buf, mes, size, mf->first, ol);
             return 1;
         }
     }
     else if(ol - of > size)
     {
-            MFIFOWriteIX(buf,mes,size,mf->first,of);
+            MFIFOWriteIX(buf, mes, size, mf->first, of);
             return 1;
     }
 
@@ -262,12 +268,14 @@ int MFIFOWriteMes(unsigned char *buf, unsigned char *mes, int size)
 *****************************************************************************/
 unsigned char * MFIFOGetMesPtr(unsigned char *buf, int *size)
 {
-    MFIFO * mf;
-    mf = (MFIFO*)buf;
-    if(mf->first==mf->last)
-        return NULL;
-    *size = mf->ix[mf->last].size;
-    return &mf->buf[mf->ix[mf->last].offset];
+	MFIFO *mf = (MFIFO *)buf;
+
+	if(mf->first == mf->last) {
+		return NULL;
+	}
+
+	*size = mf->ix[mf->last].size;
+	return &mf->buf[mf->ix[mf->last].offset];
 }
 
 /*****************************************************************************
@@ -283,14 +291,120 @@ unsigned char * MFIFOGetMesPtr(unsigned char *buf, int *size)
 *****************************************************************************/
 void MFIFOKillNext(unsigned char *buf)
 {
+    MFIFO *mf = (MFIFO *)buf;
     int x;
-    MFIFO * mf;
-    mf = (MFIFO*)buf;
-    if(mf->first!=mf->last)
+
+    if(mf->first != mf->last)
     {
-        x = mf->last+1;
-        if(x > mf->ixsize)
-            x=0;
-        mf->last=x;
+        x = mf->last + 1;
+        if(x >= mf->ixsize) {
+            x = 0;
+	}
+
+        mf->last = x;
     }
 }
+
+
+/*
+ * Queue-style accessor functions
+ */
+
+/**
+ * MFIFOGetMesPtrOffset
+ * \brief	Get pointer to and size of message at position x
+ */
+unsigned char * MFIFOGetMesPtrOffset(unsigned char *buf, int *size, const int pos)
+{
+	MFIFO *mf = (MFIFO *)buf;
+	int x;
+
+	if(mf->first == mf->last) {
+		return NULL;
+	}
+
+	if(pos < 0 || pos >= mf->ixsize) {
+		return NULL;
+	}
+
+	x = pos - mf->last;
+	if(x < 0) {
+		x += (mf->ixsize - 1);
+	}
+
+	*size = mf->ix[x].size;
+	return &mf->buf[mf->ix[x].offset];
+}
+
+
+/**
+ * MFIFOGetMesCount
+ * \brief	How many messages are currently in the buffer?
+ */
+int MFIFOGetMesCount(unsigned char *buf)
+{
+	MFIFO *mf = (MFIFO *)buf;
+
+	if(mf->first == mf->last) {
+		return 0;
+	}
+	else if(mf->first > mf->last) {
+		return mf->first - mf->last;
+	}
+	else {
+		return (mf->ixsize - mf->last) + mf->first;
+	}
+}
+
+/**
+ * MFIFOWriteMesOverwrite
+ * \brief	Same as MFIFOWriteMes but old frames will be overwritten if the fifo is full
+ */
+int MFIFOWriteMesOverwrite(unsigned char *buf, unsigned char *mes, int size)
+{
+	MFIFO *mf = (MFIFO *)buf;
+	int of, ol, x;
+
+	x = mf->first + 1;
+
+	if(x >= mf->ixsize)
+		x = 0;
+
+	if(x == mf->last) {
+		/* advance last pointer */
+		mf->last++;
+
+		if(mf->last >= mf->ixsize)
+			mf->last = 0;
+	}
+
+	of = MFIFOGetFBOffset(buf);
+	ol = MFIFOGetLBOffset(buf);
+
+	if(mf->last == mf->first)	/* empty queue */
+	{
+		mf->first = mf->last = 0;	/* optimize */
+
+		MFIFOWriteIX(buf, mes, size, mf->first, 0);
+		return 1;
+	}
+	else if(of > ol)
+	{
+		if(mf->bsize - of >= size)
+		{
+			MFIFOWriteIX(buf, mes, size, mf->first, of);
+			return 1;
+		}
+		else if(ol > size)
+		{
+			MFIFOWriteIX(buf, mes, size, mf->first, ol);
+			return 1;
+		}
+	}
+	else if(ol - of > size)
+	{
+		MFIFOWriteIX(buf, mes, size, mf->first, of);
+		return 1;
+	}
+	return 0;
+}
diff --git a/src/isdn/nationalStateTE.c b/src/isdn/nationalStateTE.c
index efd1d81..99dcec3 100644
--- a/src/isdn/nationalStateTE.c
+++ b/src/isdn/nationalStateTE.c
@@ -127,6 +127,9 @@ void nationalCreateTE(L3UCHAR i)
     Q931SetIEProc(Q931ie_USER_USER,                        i,Q931Pie_UserUser,			Q931Uie_UserUser);
     Q931SetIEProc(Q931ie_GENERIC_DIGITS,				   i,Q931Pie_GenericDigits,		Q931Uie_GenericDigits);
 
+	Q931SetIEProc(Q931ie_CONNECTED_NUMBER, i, Q931Pie_Generic, Q931Uie_Generic);
+	Q931SetIEProc(Q931ie_FACILITY,         i, Q931Pie_Generic, Q931Uie_Generic);
+
 	/* The following define a state machine. The point is that the Message	*/
 	/* procs can when search this to find out if the message/state			*/
 	/* combination is legale. If not, the proc for unexpected message apply.*/
diff --git a/src/isdn/nationalmes.c b/src/isdn/nationalmes.c
index 81361bd..90f7a1b 100644
--- a/src/isdn/nationalmes.c
+++ b/src/isdn/nationalmes.c
@@ -155,12 +155,7 @@ L3INT nationalPmes_Setup(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *IBuf, L3INT
 	L3INT Octet = 0;
 
 	/* Q931 Message Header */
-
-	OBuf[Octet++]	= pMes->ProtDisc;		/* Protocol discriminator		*/
-	OBuf[Octet++]	= 2;					/* length is 2 octets			*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV>>8) | (pMes->CRVFlag << 7);	/* msb						*/
-	OBuf[Octet++]	= (L3UCHAR)(pMes->CRV);	/* lsb							*/
-	OBuf[Octet++]	= pMes->MesType;		/* message header				*/
+	Q931MesgHeader(pTrunk, pMes, OBuf, *OSize, &Octet);
 	
 	/* Sending Complete				*/
 	if(Q931IsIEPresent(pMes->SendComplete))
diff --git a/src/zap_isdn.c b/src/zap_isdn.c
index 4cd3cee..ddddf96 100644
--- a/src/zap_isdn.c
+++ b/src/zap_isdn.c
@@ -44,6 +44,10 @@
 #define LINE "--------------------------------------------------------------------------------"
 //#define IODEBUG
 
+/* helper macros */
+#define ZAP_SPAN_IS_BRI(x)	((x)->trunk_type == ZAP_TRUNK_BRI || (x)->trunk_type == ZAP_TRUNK_BRI_PTMP)
+#define ZAP_SPAN_IS_NT(x)	(((zap_isdn_data_t *)(x)->signal_data)->mode == Q921_NT)
+
 static L2ULONG zap_time_now(void)
 {
 	return (L2ULONG)zap_current_time_in_ms();
@@ -67,91 +71,117 @@ static ZIO_CHANNEL_REQUEST_FUNCTION(isdn_channel_request)
 	Q931ie_CalledNum CalledNum;
 	Q931ie_CalledNum *ptrCalledNum;
 	Q931ie_Display Display, *ptrDisplay;
+	Q931ie_HLComp HLComp;			/* High-Layer Compatibility IE */
+	Q931ie_SendComplete SComplete;		/* Sending Complete IE */
+	Q931ie_ProgInd Progress;		/* Progress Indicator IE */
 	zap_status_t status = ZAP_FAIL;
 	zap_isdn_data_t *isdn_data = span->signal_data;
 	int sanity = 60000;
+	int codec  = 0;
 
-	Q931InitIEBearerCap(&BearerCap);
-	Q931InitIEChanID(&ChanID);
-	Q931InitIECallingNum(&CallingNum);
-	Q931InitIECalledNum(&CalledNum);
-	Q931InitIEDisplay(&Display);
+	/*
+	 * get codec type
+	 */
+	zap_channel_command(&span->channels[chan_id], ZAP_COMMAND_GET_NATIVE_CODEC, &codec);
 
+	/*
+	 * Q.931 Setup Message
+	 */
 	Q931InitMesGeneric(gen);
 	gen->MesType = Q931mes_SETUP;
+	gen->CRVFlag = 0;		/* outgoing call */
 
-	BearerCap.CodStand = 0; /* ITU-T = 0, ISO/IEC = 1, National = 2, Network = 3 */
-	BearerCap.ITC = 0; /* Speech */
-	BearerCap.TransMode = 0; /* Circuit = 0, Packet = 1 */
-	BearerCap.ITR = 16; /* 64k */
+	/*
+	 * Bearer Capability IE
+	 */
+	Q931InitIEBearerCap(&BearerCap);
+	BearerCap.CodStand  = Q931_CODING_ITU;		/* ITU-T = 0, ISO/IEC = 1, National = 2, Network = 3 */
+	BearerCap.ITC       = Q931_ITC_SPEECH;		/* Speech */
+	BearerCap.TransMode = 0;			/* Circuit = 0, Packet = 1 */
+	BearerCap.ITR       = Q931_ITR_64K;		/* 64k */
 	BearerCap.Layer1Ident = 1;
-	BearerCap.UIL1Prot = 2; /* U-law (a-law = 3)*/
-
+	BearerCap.UIL1Prot = (codec == ZAP_CODEC_ALAW) ? Q931_UIL1P_G711A : Q931_UIL1P_G711U;	/* U-law = 2, A-law = 3 */
 	gen->BearerCap = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &BearerCap);
 
-	ChanID.IntType = 1; /* PRI = 1, BRI = 0 */
-	ChanID.PrefExcl = (isdn_data->opts & ZAP_ISDN_OPT_SUGGEST_CHANNEL) ? 0 : 1; /* 0 = preferred, 1 exclusive */
-	ChanID.InfoChanSel = 1;
-	ChanID.ChanMapType = 3; /* B-Chan */
-	ChanID.ChanSlot = chan_id;
+	/*
+	 * Channel ID IE
+	 */
+	Q931InitIEChanID(&ChanID);
+	ChanID.IntType = ZAP_SPAN_IS_BRI(span) ? 0 : 1;		/* PRI = 1, BRI = 0 */
+
+	if(!ZAP_SPAN_IS_NT(span)) {
+		ChanID.PrefExcl = (isdn_data->opts & ZAP_ISDN_OPT_SUGGEST_CHANNEL) ? 0 : 1; /* 0 = preferred, 1 exclusive */
+	} else {
+		ChanID.PrefExcl = 1;	/* always exclusive in NT-mode */
+	}
+
+	if(ChanID.IntType) {
+		ChanID.InfoChanSel = 1;				/* None = 0, See Slot = 1, Any = 3 */
+		ChanID.ChanMapType = 3; 			/* B-Chan */
+		ChanID.ChanSlot = (unsigned char)chan_id;
+	} else {
+		ChanID.InfoChanSel = (unsigned char)chan_id & 0x03;	/* None = 0, B1 = 1, B2 = 2, Any = 3 */
+	}
 	gen->ChanID = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &ChanID);
-			
+
+	/*
+	 * Progress IE
+	 */
+	Q931InitIEProgInd(&Progress);
+	Progress.CodStand = Q931_CODING_ITU;	/* 0 = ITU */
+	Progress.Location = 0;  /* 0 = User, 1 = Private Network */
+	Progress.ProgDesc = 3;	/* 1 = Not end-to-end ISDN */
+	gen->ProgInd = Q931AppendIE((L3UCHAR *)gen, (L3UCHAR *)&Progress);
+
+	/*
+	 * Display IE
+	 */
+	Q931InitIEDisplay(&Display);
 	Display.Size = Display.Size + (unsigned char)strlen(caller_data->cid_name);
 	gen->Display = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &Display);			
 	ptrDisplay = Q931GetIEPtr(gen->Display, gen->buf);
 	zap_copy_string((char *)ptrDisplay->Display, caller_data->cid_name, strlen(caller_data->cid_name)+1);
 
-	/* TypNum: Type of number		*/
-	/* Bits 7 6 5					*/
-	/* 000	Unknown					*/
-	/* 001	International number	*/
-	/* 010	National number			*/
-	/* 011	Network Specific number */
-	/* 100	Subscriber mumber		*/
-	/* 110	Abbreviated number		*/
-	/* 111	Reserved for extension	*/
-	/* All other values are reserved */
-	CallingNum.TypNum = 2;
-
-	/* NumPlanID									*/
-	/* Bits 4 3 2 1									*/
-	/* 0000	Unknown									*/
-	/* 0001	ISDN/telephony numbering plan (E.164)	*/
-	/* 0011	Data numbering plan (X.121)				*/
-	/* 0100	Telex numbering plan (F.69)				*/
-	/* 1000	National standard numbering plan		*/
-	/* 1001	Private numbering plan					*/
-	/* 1111	Reserved for extension					*/
-	/* All other valures are reserved				*/
-	CallingNum.NumPlanID = 1;
-
-	/* Presentation indicator		*/
-	/* Bits 7 6						*/
-	/* 00	Presenation Allowed		*/
-	/* 01	Presentation Restricted	*/
-	/* 10	Number not available due to internetworking	*/
-	/* 11	Reserved				*/
-	CallingNum.PresInd = 0;
-
-	/* Screening Indicator						*/
-	/* Bits 2 1									*/
-	/* 00	User-provided, not screened			*/
-	/* 01	User-provided, verified and passed	*/
-	/* 10	User-provided, verified and failed	*/
-	/* 11	Network provided					*/
-	CallingNum.ScreenInd = 0;
+	/*
+	 * Calling Number IE
+	 */
+	Q931InitIECallingNum(&CallingNum);
+	CallingNum.TypNum    = Q931_TON_UNKNOWN;
+	CallingNum.NumPlanID = Q931_NUMPLAN_E164;
+	CallingNum.PresInd   = Q931_PRES_ALLOWED;
+	CallingNum.ScreenInd = Q931_SCREEN_USER_NOT_SCREENED;
 	CallingNum.Size = CallingNum.Size + (unsigned char)strlen(caller_data->cid_num.digits);
 	gen->CallingNum = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &CallingNum);			
 	ptrCallingNum = Q931GetIEPtr(gen->CallingNum, gen->buf);
 	zap_copy_string((char *)ptrCallingNum->Digit, caller_data->cid_num.digits, strlen(caller_data->cid_num.digits)+1);
 
-	CalledNum.TypNum = 2;
-	CalledNum.NumPlanID = 1;
+
+	/*
+	 * Called number IE
+	 */
+	Q931InitIECalledNum(&CalledNum);
+	CalledNum.TypNum    = Q931_TON_UNKNOWN;
+	CalledNum.NumPlanID = Q931_NUMPLAN_E164;
 	CalledNum.Size = CalledNum.Size + (unsigned char)strlen(caller_data->ani.digits);
 	gen->CalledNum = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &CalledNum);
 	ptrCalledNum = Q931GetIEPtr(gen->CalledNum, gen->buf);
 	zap_copy_string((char *)ptrCalledNum->Digit, caller_data->ani.digits, strlen(caller_data->ani.digits)+1);
 
+	/*
+	 * High-Layer Compatibility IE   (Note: Required for AVM FritzBox)
+	 */
+	Q931InitIEHLComp(&HLComp);
+	HLComp.CodStand  = Q931_CODING_ITU;	/* ITU */
+	HLComp.Interpret = 4;	/* only possible value */
+	HLComp.PresMeth  = 1;   /* High-layer protocol profile */
+	HLComp.HLCharID  = 1;	/* Telephony = 1, Fax G2+3 = 4, Fax G4 = 65 (Class I)/ 68 (Class II or III) */
+	gen->HLComp = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &HLComp);
+
+	/*
+	 * Sending complete IE (or some NT stuff waits forever in Q.931 overlap dial state...)
+	 */
+	SComplete.IEId = Q931ie_SENDING_COMPLETE;
+	gen->SendComplete = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &SComplete);
 
 	caller_data->call_state = ZAP_CALLER_STATE_DIALING;
 	Q931Rx43(&isdn_data->q931, (L3UCHAR *) gen, gen->Size);
@@ -211,7 +241,6 @@ static ZIO_CHANNEL_REQUEST_FUNCTION(isdn_channel_request)
 				case Q931mes_ALERTING:
 					new_chan->init_state = ZAP_CHANNEL_STATE_PROGRESS_MEDIA;
 					break;
-					break;
 				case Q931mes_CONNECT:
 					new_chan->init_state = ZAP_CHANNEL_STATE_UP;
 					break;
@@ -275,13 +304,17 @@ static L3INT zap_isdn_931_34(void *pvt, L2UCHAR *msg, L2INT mlen)
 
 	if (Q931IsIEPresent(gen->ChanID)) {
 		Q931ie_ChanID *chanid = Q931GetIEPtr(gen->ChanID, gen->buf);
-		chan_id = chanid->ChanSlot;
+
+		if(chanid->IntType)
+			chan_id = chanid->ChanSlot;
+		else
+			chan_id = chanid->InfoChanSel;
 	}
 
 	assert(span != NULL);
 	assert(isdn_data != NULL);
 	
-	zap_log(ZAP_LOG_DEBUG, "Yay I got an event! Type:[%02x] Size:[%d]\n", gen->MesType, gen->Size);
+	zap_log(ZAP_LOG_DEBUG, "Yay I got an event! Type:[%02x] Size:[%d] CRV: %d (%#hx, CTX: %s)\n", gen->MesType, gen->Size, gen->CRV, gen->CRV, gen->CRVFlag ? "Terminator" : "Originator");
 
 	if (gen->CRVFlag && (caller_data = isdn_data->outbound_crv[gen->CRV])) {
 		if (chan_id) {
@@ -313,7 +346,7 @@ static L3INT zap_isdn_931_34(void *pvt, L2UCHAR *msg, L2INT mlen)
 		zchan = isdn_data->channels_remote_crv[gen->CRV];
 	}
 
-
+	zap_log(ZAP_LOG_DEBUG, "zchan %x source isdn_data->channels_%s_crv[%#hx]\n", zchan, gen->CRVFlag ? "local" : "remote", gen->CRV);
 
 
 	if (gen->ProtDisc == 3) {
@@ -376,8 +409,34 @@ static L3INT zap_isdn_931_34(void *pvt, L2UCHAR *msg, L2INT mlen)
 				const char *what = gen->MesType == Q931mes_RELEASE ? "Release" : "Release Complete";
 				if (zchan) {
 					if (zchan->state == ZAP_CHANNEL_STATE_TERMINATING || zchan->state == ZAP_CHANNEL_STATE_HANGUP) {
-						zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_HANGUP_COMPLETE);
-					} else {
+						if (gen->MesType == Q931mes_RELEASE) {
+							zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_HANGUP_COMPLETE);
+						} else {
+							zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_DOWN);
+						}
+					}
+					else if((gen->MesType == Q931mes_RELEASE && zchan->state <= ZAP_CHANNEL_STATE_UP) ||
+						(gen->MesType == Q931mes_RELEASE_COMPLETE && zchan->state == ZAP_CHANNEL_STATE_DIALING)) {
+
+						/*
+						 * Don't keep inbound channels open if the remote side hangs up before we answered
+						 */
+						Q931ie_Cause *cause = Q931GetIEPtr(gen->Cause, gen->buf);
+						zap_sigmsg_t sig;
+						zap_status_t status;
+
+						memset(&sig, 0, sizeof(sig));
+						sig.chan_id = zchan->chan_id;
+						sig.span_id = zchan->span_id;
+						sig.channel = zchan;
+						sig.channel->caller_data.hangup_cause = (cause) ? cause->Value : ZAP_CAUSE_NORMAL_UNSPECIFIED;
+
+						sig.event_id = ZAP_SIGEVENT_STOP;
+						status = isdn_data->sig_cb(&sig);
+
+						zap_log(ZAP_LOG_DEBUG, "Received %s in state %s, requested hangup for channel %d:%d\n", what, zap_channel_state2str(zchan->state), zchan->span_id, chan_id);
+					}
+					else {
 						zap_log(ZAP_LOG_DEBUG, "Ignoring %s on channel %d\n", what, chan_id);
 					}
 				} else {
@@ -418,6 +477,10 @@ static L3INT zap_isdn_931_34(void *pvt, L2UCHAR *msg, L2INT mlen)
 			{
 				if (zchan) {
 					zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_UP);
+
+					gen->MesType = Q931mes_CONNECT_ACKNOWLEDGE;
+					gen->CRVFlag = 0;	/* outbound */
+					Q931Rx43(&isdn_data->q931, (L3UCHAR *) gen, gen->Size);
 				} else {
 					zap_log(ZAP_LOG_CRIT, "Received Connect with no matching channel %d\n", chan_id);
 				}
@@ -425,23 +488,35 @@ static L3INT zap_isdn_931_34(void *pvt, L2UCHAR *msg, L2INT mlen)
 			break;
 		case Q931mes_SETUP:
 			{
-				
 				Q931ie_CallingNum *callingnum = Q931GetIEPtr(gen->CallingNum, gen->buf);
 				Q931ie_CalledNum *callednum = Q931GetIEPtr(gen->CalledNum, gen->buf);
-				zap_status_t status;
 				int fail = 1;
 				uint32_t cplen = mlen;
+
+				if(zchan && zchan == isdn_data->channels_remote_crv[gen->CRV]) {
+					zap_log(ZAP_LOG_INFO, "Duplicate SETUP message(?) for Channel %d:%d ~ %d:%d in state %s [ignoring]\n",
+									zchan->span_id,
+									zchan->chan_id,
+									zchan->physical_span_id,
+									zchan->physical_chan_id,
+									zap_channel_state2str(zchan->state));
+					break;
+				}
 				
 				zchan = NULL;
 				if (chan_id < ZAP_MAX_CHANNELS_SPAN && chan_id <= span->chan_count) {
 					zchan = &span->channels[chan_id];
 				}
 
-				if (zchan && (status = zap_channel_open_chan(zchan) == ZAP_SUCCESS)) {
+				if (zchan) {
 					if (zap_test_flag(zchan, ZAP_CHANNEL_INUSE) || zchan->state != ZAP_CHANNEL_STATE_DOWN) {
 						if (zchan->state == ZAP_CHANNEL_STATE_DOWN || zchan->state >= ZAP_CHANNEL_STATE_TERMINATING) {
 							int x = 0;
-							zap_log(ZAP_LOG_WARNING, "Channel %d:%d ~ %d:%d is already in use waiting for it to become available.\n");
+							zap_log(ZAP_LOG_WARNING, "Channel %d:%d ~ %d:%d is already in use waiting for it to become available.\n",
+									zchan->span_id,
+									zchan->chan_id,
+									zchan->physical_span_id,
+									zchan->physical_chan_id);
 
 							for (x = 0; x < 200; x++) {
 								if (!zap_test_flag(zchan, ZAP_CHANNEL_INUSE)) {
@@ -485,9 +560,10 @@ static L3INT zap_isdn_931_34(void *pvt, L2UCHAR *msg, L2INT mlen)
 				if (fail) {
 					Q931ie_Cause cause;
 					gen->MesType = Q931mes_DISCONNECT;
+					gen->CRVFlag = 1;	/* inbound call */
 					cause.IEId = Q931ie_CAUSE;
 					cause.Size = sizeof(Q931ie_Cause);
-					cause.CodStand  = 0;
+					cause.CodStand = Q931_CODING_ITU;
 					cause.Location = 1;
 					cause.Recom = 1;
 					//should we be casting here.. or do we need to translate value?
@@ -506,11 +582,35 @@ static L3INT zap_isdn_931_34(void *pvt, L2UCHAR *msg, L2INT mlen)
 						zap_log(ZAP_LOG_CRIT, "Failed to open channel for new setup message\n");
 					}
 					
+				} else {
+					Q931ie_ProgInd progress;
+
+					/*
+					 * Setup Progress indicator
+					 */
+					progress.IEId = Q931ie_PROGRESS_INDICATOR;
+					progress.Size = sizeof(Q931ie_ProgInd);
+					progress.CodStand = Q931_CODING_ITU;	/* ITU */ 
+					progress.Location = 1;	/* private network serving the local user */
+					progress.ProgDesc = 1;	/* call is not end-to-end isdn = 1, in-band information available = 8 */
+					gen->ProgInd = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &progress);
+				}
+			}
+			break;
+
+		case Q931mes_CALL_PROCEEDING:
+			{
+				if (zchan) {
+					zap_log(ZAP_LOG_CRIT, "Received CALL PROCEEDING message for channel %d\n", chan_id);
+					zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_PROGRESS);
+				} else {
+					zap_log(ZAP_LOG_CRIT, "Received CALL PROCEEDING with no matching channel %d\n", chan_id);
 				}
-				
 			}
 			break;
+
 		default:
+			zap_log(ZAP_LOG_CRIT, "Received unhandled message %d (%#x)\n", (int)gen->MesType, (int)gen->MesType);
 			break;
 		}
 	}
@@ -518,16 +618,23 @@ static L3INT zap_isdn_931_34(void *pvt, L2UCHAR *msg, L2INT mlen)
 	return 0;
 }
 
-static int zap_isdn_921_23(void *pvt, L2UCHAR *msg, L2INT mlen)
+static int zap_isdn_921_23(void *pvt, Q921DLMsg_t ind, L2UCHAR tei, L2UCHAR *msg, L2INT mlen)
 {
-	int ret;
+	int ret, offset = (ind == Q921_DL_DATA) ? 4 : 3;
 	char bb[4096] = "";
-	print_hex_bytes(msg+4, mlen-2, bb, sizeof(bb));
-	zap_log(ZAP_LOG_DEBUG, "READ %d\n%s\n%s\n\n", (int)mlen-2, LINE, bb);
 
-	ret = Q931Rx23(pvt, msg, mlen);
-	if (ret != 0)
-		zap_log(ZAP_LOG_DEBUG, "931 parse error [%d] [%s]\n", ret, q931_error_to_name(ret));
+	switch(ind) {
+	case Q921_DL_DATA:
+	case Q921_DL_UNIT_DATA:
+		print_hex_bytes(msg + offset, mlen - offset, bb, sizeof(bb));
+		zap_log(ZAP_LOG_DEBUG, "READ %d\n%s\n%s\n\n", (int)mlen - offset, LINE, bb);
+	default:
+		ret = Q931Rx23(pvt, ind, tei, msg, mlen);
+		if (ret != 0)
+			zap_log(ZAP_LOG_DEBUG, "931 parse error [%d] [%s]\n", ret, q931_error_to_name(ret));
+		break;
+	}
+
 	return ((ret >= 0) ? 1 : 0);
 }
 
@@ -568,6 +675,11 @@ static __inline__ void state_advance(zap_channel_t *zchan)
 	case ZAP_CHANNEL_STATE_DOWN:
 		{
 			if (gen->CRV) {
+				if(gen->CRVFlag) {
+					isdn_data->channels_local_crv[gen->CRV] = NULL;
+				} else {
+					isdn_data->channels_remote_crv[gen->CRV] = NULL;
+				}
 				Q931ReleaseCRV(&isdn_data->q931, gen->CRV);
 			}
 			zap_channel_done(zchan);
@@ -582,6 +694,8 @@ static __inline__ void state_advance(zap_channel_t *zchan)
 				}
 			} else {
 				gen->MesType = Q931mes_CALL_PROCEEDING;
+				gen->CRVFlag = 1;	/* inbound */
+
 				Q931Rx43(&isdn_data->q931, (void *)gen, gen->Size);
 			}
 		}
@@ -594,7 +708,6 @@ static __inline__ void state_advance(zap_channel_t *zchan)
 					zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_HANGUP);
 				}
 			}
-
 		}
 		break;
 	case ZAP_CHANNEL_STATE_RESTART:
@@ -613,7 +726,14 @@ static __inline__ void state_advance(zap_channel_t *zchan)
 					zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_HANGUP);
 				}
 			} else {
+				if (!zap_test_flag(zchan, ZAP_CHANNEL_OPEN)) {
+					if (zap_channel_open_chan(zchan) != ZAP_SUCCESS) {
+						zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_HANGUP);
+						return;
+					}
+				}
 				gen->MesType = Q931mes_ALERTING;
+				gen->CRVFlag = 1;	/* inbound call */
 				Q931Rx43(&isdn_data->q931, (void *)gen, gen->Size);
 			}
 		}
@@ -626,8 +746,15 @@ static __inline__ void state_advance(zap_channel_t *zchan)
 					zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_HANGUP);
 				}
 			} else {
+				if (!zap_test_flag(zchan, ZAP_CHANNEL_OPEN)) {
+					if (zap_channel_open_chan(zchan) != ZAP_SUCCESS) {
+						zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_HANGUP);
+						return;
+					}
+				}
 				gen->MesType = Q931mes_CONNECT;
 				gen->BearerCap = 0;
+				gen->CRVFlag = 1;	/* inbound call */
 				Q931Rx43(&isdn_data->q931, (void *)gen, zchan->caller_data.raw_data_len);
 			}
 		}
@@ -641,117 +768,193 @@ static __inline__ void state_advance(zap_channel_t *zchan)
 			Q931ie_CalledNum CalledNum;
 			Q931ie_CalledNum *ptrCalledNum;
 			Q931ie_Display Display, *ptrDisplay;
-			
-			Q931InitIEBearerCap(&BearerCap);
-			Q931InitIEChanID(&ChanID);
-			Q931InitIECallingNum(&CallingNum);
-			Q931InitIECalledNum(&CalledNum);
-			Q931InitIEDisplay(&Display);
-
+			Q931ie_HLComp HLComp;			/* High-Layer Compatibility IE */
+			Q931ie_SendComplete SComplete;		/* Sending Complete IE */
+			Q931ie_ProgInd Progress;		/* Progress Indicator IE */
+			int codec  = 0;
+
+			/*
+			 * get codec type
+			 */
+			zap_channel_command(&zchan->span->channels[zchan->chan_id], ZAP_COMMAND_GET_NATIVE_CODEC, &codec);
+
+			/*
+			 * Q.931 Setup Message
+			 */ 
 			Q931InitMesGeneric(gen);
 			gen->MesType = Q931mes_SETUP;
+			gen->CRVFlag = 0;		/* outbound(?) */
 
-			BearerCap.CodStand = 0; /* ITU-T = 0, ISO/IEC = 1, National = 2, Network = 3 */
-			BearerCap.ITC = 0; /* Speech */
-			BearerCap.TransMode = 0; /* Circuit = 0, Packet = 1 */
-			BearerCap.ITR = 16; /* 64k */
+			/*
+			 * Bearer Capability IE
+			 */
+			Q931InitIEBearerCap(&BearerCap);
+			BearerCap.CodStand  = Q931_CODING_ITU;	/* ITU-T = 0, ISO/IEC = 1, National = 2, Network = 3 */
+			BearerCap.ITC       = Q931_ITC_SPEECH;	/* Speech */
+			BearerCap.TransMode = 0;		/* Circuit = 0, Packet = 1 */
+			BearerCap.ITR       = Q931_ITR_64K;	/* 64k = 16, Packet mode = 0 */
 			BearerCap.Layer1Ident = 1;
-			BearerCap.UIL1Prot = 2; /* U-law (a-law = 3)*/
+			BearerCap.UIL1Prot = (codec == ZAP_CODEC_ALAW) ? 3 : 2;	/* U-law = 2, A-law = 3 */
 			gen->BearerCap = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &BearerCap);
 
-			//is cast right here?
-			ChanID.IntType = 1; /* PRI = 1, BRI = 0 */
-			ChanID.InfoChanSel = 1;
-			ChanID.ChanMapType = 3; /* B-Chan */
-			ChanID.ChanSlot = (unsigned char)zchan->chan_id;
+			/*
+			 * ChannelID IE
+			 */
+			Q931InitIEChanID(&ChanID);
+			ChanID.IntType = ZAP_SPAN_IS_BRI(zchan->span) ? 0 : 1;	/* PRI = 1, BRI = 0 */
+			ChanID.PrefExcl = ZAP_SPAN_IS_NT(zchan->span) ? 1 : 0;  /* Exclusive in NT-mode = 1, Preferred otherwise = 0 */
+			if(ChanID.IntType) {
+				ChanID.InfoChanSel = 1;		/* None = 0, See Slot = 1, Any = 3 */
+				ChanID.ChanMapType = 3;		/* B-Chan */
+				ChanID.ChanSlot = (unsigned char)zchan->chan_id;
+			} else {
+				ChanID.InfoChanSel = (unsigned char)zchan->chan_id & 0x03;	/* None = 0, B1 = 1, B2 = 2, Any = 3 */
+			}
 			gen->ChanID = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &ChanID);
-			
+
+			/*
+			 * Progress IE
+			 */
+			Q931InitIEProgInd(&Progress);
+			Progress.CodStand = Q931_CODING_ITU;	/* 0 = ITU */
+			Progress.Location = 0;  /* 0 = User, 1 = Private Network */
+			Progress.ProgDesc = 3;	/* 1 = Not end-to-end ISDN */
+			gen->ProgInd = Q931AppendIE((L3UCHAR *)gen, (L3UCHAR *)&Progress);
+
+			/*
+			 * Display IE
+			 */			
+			Q931InitIEDisplay(&Display);
 			Display.Size = Display.Size + (unsigned char)strlen(zchan->caller_data.cid_name);
 			gen->Display = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &Display);
 			ptrDisplay = Q931GetIEPtr(gen->Display, gen->buf);
 			zap_copy_string((char *)ptrDisplay->Display, zchan->caller_data.cid_name, strlen(zchan->caller_data.cid_name)+1);
 
-			/* TypNum: Type of number*/
-			/* Bits 7 6 5*/
-			/* 000Unknown*/
-			/* 001International number*/
-			/* 010National number*/
-			/* 011Network Specific number */
-			/* 100Subscriber mumber*/
-			/* 110Abbreviated number*/
-			/* 111Reserved for extension*/
-			/* All other values are reserved */
-			CallingNum.TypNum = 2;
-
-			/* NumPlanID*/
-			/* Bits 4 3 2 1*/
-			/* 0000Unknown*/
-			/* 0001ISDN/telephony numbering plan (E.164)*/
-			/* 0011Data numbering plan (X.121)*/
-			/* 0100Telex numbering plan (F.69)*/
-			/* 1000National standard numbering plan*/
-			/* 1001Private numbering plan*/
-			/* 1111Reserved for extension*/
-			/* All other valures are reserved*/
-			CallingNum.NumPlanID = 1;
-
-			/* Presentation indicator*/
-			/* Bits 7 6*/
-			/* 00Presenation Allowed*/
-			/* 01Presentation Restricted*/
-			/* 10Number not available due to internetworking*/
-			/* 11Reserved*/
-			CallingNum.PresInd = 0;
-
-			/* Screening Indicator*/
-			/* Bits 2 1*/
-			/* 00User-provided, not screened*/
-			/* 01User-provided, verified and passed*/
-			/* 10User-provided, verified and failed*/
-			/* 11Network provided*/
-			CallingNum.ScreenInd = 0;
+			/*
+			 * CallingNum IE
+			 */ 
+			Q931InitIECallingNum(&CallingNum);
+			CallingNum.TypNum    = Q931_TON_UNKNOWN;
+			CallingNum.NumPlanID = Q931_NUMPLAN_E164;
+			CallingNum.PresInd   = Q931_PRES_ALLOWED;
+			CallingNum.ScreenInd = Q931_SCREEN_USER_NOT_SCREENED;
 			CallingNum.Size = CallingNum.Size + (unsigned char)strlen(zchan->caller_data.cid_num.digits);
 			gen->CallingNum = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &CallingNum);
 			ptrCallingNum = Q931GetIEPtr(gen->CallingNum, gen->buf);
 			zap_copy_string((char *)ptrCallingNum->Digit, zchan->caller_data.cid_num.digits, strlen(zchan->caller_data.cid_num.digits)+1);
 
-			CalledNum.TypNum = 2;
-			CalledNum.NumPlanID = 1;
+			/*
+			 * CalledNum IE
+			 */
+			Q931InitIECalledNum(&CalledNum);
+			CalledNum.TypNum    = Q931_TON_UNKNOWN;
+			CalledNum.NumPlanID = Q931_NUMPLAN_E164;
 			CalledNum.Size = CalledNum.Size + (unsigned char)strlen(zchan->caller_data.ani.digits);
 			gen->CalledNum = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &CalledNum);
 			ptrCalledNum = Q931GetIEPtr(gen->CalledNum, gen->buf);
 			zap_copy_string((char *)ptrCalledNum->Digit, zchan->caller_data.ani.digits, strlen(zchan->caller_data.ani.digits)+1);
 
+			/*
+			 * High-Layer Compatibility IE   (Note: Required for AVM FritzBox)
+			 */
+			Q931InitIEHLComp(&HLComp);
+			HLComp.CodStand  = Q931_CODING_ITU;	/* ITU */
+			HLComp.Interpret = 4;	/* only possible value */
+			HLComp.PresMeth  = 1;   /* High-layer protocol profile */
+			HLComp.HLCharID  = Q931_HLCHAR_TELEPHONY;	/* Telephony = 1, Fax G2+3 = 4, Fax G4 = 65 (Class I)/ 68 (Class II or III) */   /* TODO: make accessible from user layer */
+			gen->HLComp = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &HLComp);
+
+			/*
+			 * Sending complete IE (or some NT stuff waits forever in Q.931 overlap dial state...)
+			 */
+			SComplete.IEId = Q931ie_SENDING_COMPLETE;
+			gen->SendComplete = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &SComplete);
+
 			Q931Rx43(&isdn_data->q931, (L3UCHAR *) gen, gen->Size);
 			isdn_data->channels_local_crv[gen->CRV] = zchan;
 		}
 		break;
 	case ZAP_CHANNEL_STATE_HANGUP_COMPLETE:
 		{
+			/* reply RELEASE with RELEASE_COMPLETE message */
+			if(zchan->last_state == ZAP_CHANNEL_STATE_HANGUP) {
+				gen->MesType = Q931mes_RELEASE_COMPLETE;
+
+				Q931Rx43(&isdn_data->q931, (L3UCHAR *) gen, gen->Size);
+			}
 			zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_DOWN);
 		}
 		break;
 	case ZAP_CHANNEL_STATE_HANGUP:
 		{
 			Q931ie_Cause cause;
-			gen->MesType = Q931mes_DISCONNECT;
+
+			zap_log(ZAP_LOG_DEBUG, "Hangup: Direction %s\n", zap_test_flag(zchan, ZAP_CHANNEL_OUTBOUND) ? "Outbound" : "Inbound");
+
+			gen->CRVFlag = zap_test_flag(zchan, ZAP_CHANNEL_OUTBOUND) ? 0 : 1;
+
 			cause.IEId = Q931ie_CAUSE;
 			cause.Size = sizeof(Q931ie_Cause);
-			cause.CodStand  = 0;
-			cause.Location = 1;
-			cause.Recom = 1;
-			//should we be casting here.. or do we need to translate value?
-			cause.Value = (unsigned char) zchan->caller_data.hangup_cause;
-			*cause.Diag = '\0';
-			gen->Cause = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &cause);
-			Q931Rx43(&isdn_data->q931, (L3UCHAR *) gen, gen->Size);
+			cause.CodStand = Q931_CODING_ITU;	/* ITU */
+			cause.Location = 1;	/* private network */
+			cause.Recom    = 1;	/* */
+
+			/*
+			 * BRI PTMP needs special handling here...
+			 * TODO: cleanup / refine (see above)
+			 */
+			if (zchan->last_state == ZAP_CHANNEL_STATE_RING) {
+				/*
+				 * inbound call [was: number unknown (= not found in routing state)]
+				 * (in Q.931 spec terms: Reject request)
+				 */
+				gen->MesType = Q931mes_RELEASE_COMPLETE;
+
+				//cause.Value = (unsigned char) ZAP_CAUSE_UNALLOCATED;
+				cause.Value = (unsigned char) zchan->caller_data.hangup_cause;
+				*cause.Diag = '\0';
+				gen->Cause = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &cause);
+				Q931Rx43(&isdn_data->q931, (L3UCHAR *) gen, gen->Size);
+
+				/* we're done, release channel */
+				//zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_HANGUP_COMPLETE);
+				zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_DOWN);
+			}
+			else if (zchan->last_state <= ZAP_CHANNEL_STATE_PROGRESS) {
+				/*
+				 * just release all unanswered calls [was: inbound call, remote side hung up before we answered]
+				 */
+				gen->MesType = Q931mes_RELEASE;
+
+				cause.Value = (unsigned char) zchan->caller_data.hangup_cause;
+				*cause.Diag = '\0';
+				gen->Cause = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &cause);
+				Q931Rx43(&isdn_data->q931, (void *)gen, gen->Size);
+
+				/* this will be triggered by the RELEASE_COMPLETE reply */
+				/* zap_set_state_locked(zchan, ZAP_CHANNEL_STATE_HANGUP_COMPLETE); */
+			}
+			else {
+				/*
+				 * call connected, hangup
+				 */
+				gen->MesType = Q931mes_DISCONNECT;
+
+				cause.Value = (unsigned char) zchan->caller_data.hangup_cause;
+				*cause.Diag = '\0';
+				gen->Cause = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &cause);
+				Q931Rx43(&isdn_data->q931, (L3UCHAR *) gen, gen->Size);
+			}
 		}
 		break;
 	case ZAP_CHANNEL_STATE_TERMINATING:
 		{
+			zap_log(ZAP_LOG_DEBUG, "Terminating: Direction %s\n", zap_test_flag(zchan, ZAP_CHANNEL_OUTBOUND) ? "Outbound" : "Inbound");
+
 			sig.event_id = ZAP_SIGEVENT_STOP;
 			status = isdn_data->sig_cb(&sig);
 			gen->MesType = Q931mes_RELEASE;
+			gen->CRVFlag = zap_test_flag(zchan, ZAP_CHANNEL_OUTBOUND) ? 0 : 1;
 			Q931Rx43(&isdn_data->q931, (void *)gen, gen->Size);
 		}
 	default:
@@ -871,6 +1074,7 @@ static void *zap_isdn_run(zap_thread_t *me, void *obj)
 		zap_status_t status = zap_channel_wait(isdn_data->dchan, &flags, 100);
 
 		Q921TimerTick(&isdn_data->q921);
+		Q931TimerTick(&isdn_data->q931);
 		check_state(span);
 		check_events(span);
 
@@ -887,8 +1091,6 @@ static void *zap_isdn_run(zap_thread_t *me, void *obj)
 			break;
 		case ZAP_TIMEOUT:
 			{
-				/*zap_log(ZAP_LOG_DEBUG, "Timeout!\n");*/
-				/*Q931TimeTick(isdn_data->q931, L3ULONG ms);*/
 				errs = 0;
 			}
 			break;
@@ -937,7 +1139,8 @@ zap_status_t zap_isdn_init(void)
 	Q931Initialize();
 
 	Q921SetGetTimeCB(zap_time_now);
-	
+	Q931SetGetTimeCB(zap_time_now);
+
 	return ZAP_SUCCESS;
 }
 
@@ -948,16 +1151,66 @@ zap_status_t zap_isdn_start(zap_span_t *span)
 	return zap_thread_create_detached(zap_isdn_run, span);
 }
 
-static int q931_rx_32(void *pvt,L3UCHAR *msg, L3INT mlen)
+static int q931_rx_32(void *pvt, Q921DLMsg_t ind, L3UCHAR tei, L3UCHAR *msg, L3INT mlen)
 {
-	int ret;
+	int offset = 4;
 	char bb[4096] = "";
-	ret = Q921Rx32(pvt, msg, mlen);
-	print_hex_bytes(msg, mlen, bb, sizeof(bb));
-	zap_log(ZAP_LOG_DEBUG, "WRITE %d\n%s\n%s\n\n", (int)mlen, LINE, bb);
-	return ret;
+
+	switch(ind) {
+	case Q921_DL_UNIT_DATA:
+		offset = 3;
+
+	case Q921_DL_DATA:
+		print_hex_bytes(msg + offset, mlen - offset, bb, sizeof(bb));
+		zap_log(ZAP_LOG_DEBUG, "WRITE %d\n%s\n%s\n\n", (int)mlen - offset, LINE, bb);
+		break;
+
+	default:
+		break;
+	}
+
+	return Q921Rx32(pvt, ind, tei, msg, mlen);
+}
+
+static int zap_isdn_q921_log(void *pvt, Q921LogLevel_t level, char *msg, L2INT size)
+{
+	int loglevel = ZAP_LOG_LEVEL_DEBUG;
+
+	switch(level) {
+	case Q921_LOG_DEBUG:
+		loglevel = ZAP_LOG_LEVEL_DEBUG;
+		break;
+
+	case Q921_LOG_INFO:
+		loglevel = ZAP_LOG_LEVEL_INFO;
+		break;
+
+	case Q921_LOG_NOTICE:
+		loglevel = ZAP_LOG_LEVEL_NOTICE;
+		break;
+
+	case Q921_LOG_WARNING:
+		loglevel = ZAP_LOG_LEVEL_WARNING;
+		break;
+
+	case Q921_LOG_ERROR:
+		loglevel = ZAP_LOG_LEVEL_ERROR;
+		break;
+
+	default:
+		return 0;
+	}
+
+	zap_log(__FILE__, "Q.921", __LINE__, loglevel, "%s", msg);
+
+	return 0;
 }
 
+static L3INT zap_isdn_q931_log(void *pvt, Q931LogLevel_t level, char *msg, L3INT size)
+{
+	zap_log(__FILE__, "Q.931", __LINE__, ZAP_LOG_LEVEL_DEBUG, "%s", msg);
+	return 0;
+}
 
 static zap_state_map_t isdn_state_map = {
 	{
@@ -995,7 +1248,7 @@ static zap_state_map_t isdn_state_map = {
 			ZSD_OUTBOUND,
 			ZSM_UNACCEPTABLE,
 			{ZAP_CHANNEL_STATE_HANGUP, ZAP_CHANNEL_STATE_TERMINATING, ZAP_END},
-			{ZAP_CHANNEL_STATE_HANGUP_COMPLETE, ZAP_END}
+			{ZAP_CHANNEL_STATE_HANGUP_COMPLETE, ZAP_CHANNEL_STATE_DOWN, ZAP_END}
 		},
 		{
 			ZSD_OUTBOUND,
@@ -1039,7 +1292,7 @@ static zap_state_map_t isdn_state_map = {
 			ZSD_INBOUND,
 			ZSM_UNACCEPTABLE,
 			{ZAP_CHANNEL_STATE_HANGUP, ZAP_CHANNEL_STATE_TERMINATING, ZAP_END},
-			{ZAP_CHANNEL_STATE_HANGUP_COMPLETE, ZAP_END},
+			{ZAP_CHANNEL_STATE_HANGUP_COMPLETE, ZAP_CHANNEL_STATE_DOWN, ZAP_END},
 		},
 		{
 			ZSD_INBOUND,
@@ -1108,29 +1361,38 @@ zap_status_t zap_isdn_configure_span(zap_span_t *span, Q921NetUser_t mode, Q931D
 	isdn_data->dchans[0] = dchans[0];
 	isdn_data->dchans[1] = dchans[1];
 	isdn_data->dchan = isdn_data->dchans[0];
+	isdn_data->mode  = mode;
 	
 	Q921_InitTrunk(&isdn_data->q921,
 				   0,
 				   0,
 				   mode,
+				   span->trunk_type == ZAP_TRUNK_BRI_PTMP ? Q921_PTMP : Q921_PTP,
 				   0,
 				   zap_isdn_921_21,
-				   (Q921TxCB_t)zap_isdn_921_23,
+				   (Q921Tx23CB_t)zap_isdn_921_23,
 				   span,
 				   &isdn_data->q931);
+
+	Q921SetLogCB(&isdn_data->q921, &zap_isdn_q921_log, isdn_data);
+	Q921SetLogLevel(&isdn_data->q921, Q921_LOG_DEBUG);
 	
 	Q931Api_InitTrunk(&isdn_data->q931,
 					  dialect,
 					  mode,
 					  span->trunk_type,
 					  zap_isdn_931_34,
-					  (Q931TxCB_t)q931_rx_32,
+					  (Q931Tx32CB_t)q931_rx_32,
 					  zap_isdn_931_err,
 					  &isdn_data->q921,
 					  span);
 
+	Q931SetLogCB(&isdn_data->q931, &zap_isdn_q931_log, isdn_data);
+	Q931SetLogLevel(&isdn_data->q931, Q931_LOG_DEBUG);
+
 	isdn_data->q931.autoRestartAck = 1;
-	isdn_data->q931.autoConnectAck = 1;
+//	isdn_data->q931.autoConnectAck = 1;
+	isdn_data->q931.autoConnectAck = 0;
 	isdn_data->q931.autoServiceAck = 1;
 	span->signal_data = isdn_data;
 	span->signal_type = ZAP_SIGTYPE_ISDN;
