Index: src/zap_isdn.c
===================================================================
--- src/zap_isdn.c	(revision 495)
+++ src/zap_isdn.c	(working copy)
@@ -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_ALAW(x)	((x)->trunk_type == ZAP_TRUNK_E1 || (x)->trunk_type == ZAP_TRUNK_BRI || (x)->trunk_type == ZAP_TRUNK_BRI_PTMP)	/* TODO: baaaad hack */
+
 static L2ULONG zap_time_now(void)
 {
 	return (L2ULONG)zap_current_time_in_ms();
@@ -67,40 +71,64 @@
 	Q931ie_CalledNum CalledNum;
 	Q931ie_CalledNum *ptrCalledNum;
 	Q931ie_Display Display, *ptrDisplay;
+	Q931ie_HLComp HLComp;			/* High-Layer Compatibility IE */
 	zap_status_t status = ZAP_FAIL;
 	zap_isdn_data_t *isdn_data = span->signal_data;
 	int sanity = 60000;
 
-	Q931InitIEBearerCap(&BearerCap);
-	Q931InitIEChanID(&ChanID);
-	Q931InitIECallingNum(&CallingNum);
-	Q931InitIECalledNum(&CalledNum);
-	Q931InitIEDisplay(&Display);
-
 	Q931InitMesGeneric(gen);
 	gen->MesType = Q931mes_SETUP;
 
+	/*
+	 * Bearer Capability IE
+	 */
+	Q931InitIEBearerCap(&BearerCap);
 	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 */
 	BearerCap.Layer1Ident = 1;
-	BearerCap.UIL1Prot = 2; /* U-law (a-law = 3)*/
-
+	BearerCap.UIL1Prot = ZAP_SPAN_IS_ALAW(span) ? 3 : 2;	/* U-law = 2, A-law = 3 */
 	gen->BearerCap = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &BearerCap);
 
-	ChanID.IntType = 1; /* PRI = 1, BRI = 0 */
+	/*
+	 * Channel ID IE
+	 */
+	Q931InitIEChanID(&ChanID);
+	ChanID.IntType = ZAP_SPAN_IS_BRI(span) ? 1 : 0;		/* PRI = 1, BRI = 0 */
 	ChanID.PrefExcl = (isdn_data->opts & ZAP_ISDN_OPT_SUGGEST_CHANNEL) ? 0 : 1; /* 0 = preferred, 1 exclusive */
-	ChanID.InfoChanSel = 1;
+	if(ChanID.IntType) {
+		ChanID.InfoChanSel = 1;				/* None = 0, See Slot = 1, Any = 3 */
 	ChanID.ChanMapType = 3; /* B-Chan */
-	ChanID.ChanSlot = chan_id;
+		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);
 			
+	/*
+	 * 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);
 
+	/*
+	 * High-Layer Compatibility IE   (Note: Required for AVM FritzBox)
+	 */
+	Q931InitIEHLComp(&HLComp);
+	HLComp.CodStand  = 0;	/* ITU */
+	HLComp.Interpret = 4;	/* only possible value */
+	HLComp.PresMeth  = 1;   /* High-layer protocol profile */
+	HLComp.HLCharID  = 0;	/* Telephony = 0, Fax G2+3 = 4, Fax G4 = 65 (Class I)/ 68 (Class II or III) */
+	gen->HLComp = Q931AppendIE((L3UCHAR *) gen, (L3UCHAR *) &HLComp);
+
+	/*
+	 * Calling Number IE
+	 */
+	Q931InitIECallingNum(&CallingNum);
 	/* TypNum: Type of number		*/
 	/* Bits 7 6 5					*/
 	/* 000	Unknown					*/
@@ -145,6 +173,11 @@
 	ptrCallingNum = Q931GetIEPtr(gen->CallingNum, gen->buf);
 	zap_copy_string((char *)ptrCallingNum->Digit, caller_data->cid_num.digits, strlen(caller_data->cid_num.digits)+1);
 
+
+	/*
+	 * Called number IE
+	 */
+	Q931InitIECalledNum(&CalledNum);
 	CalledNum.TypNum = 2;
 	CalledNum.NumPlanID = 1;
 	CalledNum.Size = CalledNum.Size + (unsigned char)strlen(caller_data->ani.digits);
@@ -211,7 +245,6 @@
 				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;
@@ -258,6 +294,12 @@
 
 }
 
+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 L3INT zap_isdn_931_err(void *pvt, L3INT id, L3INT p1, L3INT p2)
 {
 	zap_log(ZAP_LOG_ERROR, "ERROR: [%s] [%d] [%d]\n", q931_error_to_name(id), p1, p2);
@@ -275,7 +317,11 @@
 
 	if (Q931IsIEPresent(gen->ChanID)) {
 		Q931ie_ChanID *chanid = Q931GetIEPtr(gen->ChanID, gen->buf);
+
+		if(chanid->IntType)
 		chan_id = chanid->ChanSlot;
+		else
+			chan_id = chanid->InfoChanSel;
 	}
 
 	assert(span != NULL);
@@ -295,9 +341,7 @@
 		case Q931mes_ALERTING:
 		case Q931mes_PROGRESS:
 		case Q931mes_CONNECT:
-			{
 				caller_data->call_state = ZAP_CALLER_STATE_SUCCESS;
-			}
 			break;
 		default:
 			caller_data->call_state = ZAP_CALLER_STATE_FAIL;
@@ -432,12 +473,19 @@
 				int fail = 1;
 				uint32_t cplen = mlen;
 				
+				zap_log(ZAP_LOG_DEBUG, "Q.931 SETUP Message recived, requested Channel ID: %d (available on span: %d)\n", chan_id, span->chan_count);
+
 				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)) {
+					zap_log(ZAP_LOG_DEBUG, "Successfully opened requested channel %d [inuse: %s, state: %d]\n",
+										chan_id,
+										zap_test_flag(zchan, ZAP_CHANNEL_INUSE) ? "yes" : "no",
+										zchan->state);
+
 					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;
@@ -462,6 +510,8 @@
 					}
 
 					if (zchan && zchan->state == ZAP_CHANNEL_STATE_DOWN) {
+						zap_log(ZAP_LOG_DEBUG, "Request channel is down, preparing for activation\n");
+
 						isdn_data->channels_remote_crv[gen->CRV] = zchan;
 						memset(&zchan->caller_data, 0, sizeof(zchan->caller_data));
 
@@ -518,16 +570,23 @@
 	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 *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);
+	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, 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);
 }
 
@@ -641,36 +699,51 @@
 			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 */
 
 			Q931InitMesGeneric(gen);
 			gen->MesType = Q931mes_SETUP;
 
+			/*
+			 * Bearer Capability IE
+			 */
+			Q931InitIEBearerCap(&BearerCap);
 			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 */
+			BearerCap.ITR = 16;			/* 64k = 16, Packet mode = 0 */
 			BearerCap.Layer1Ident = 1;
-			BearerCap.UIL1Prot = 2; /* U-law (a-law = 3)*/
+			BearerCap.UIL1Prot = ZAP_SPAN_IS_ALAW(zchan->span) ? 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;
+			/*
+			 * ChannelID IE
+			 */
+			Q931InitIEChanID(&ChanID);
+			ChanID.IntType = ZAP_SPAN_IS_BRI(zchan->span) ? 0 : 1;	/* PRI = 1, BRI = 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);
 			
+			/*
+			 * 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);
 
+			/*
+			 * CallingNum IE
+			 */ 
+			Q931InitIECallingNum(&CallingNum);
+
 			/* TypNum: Type of number*/
 			/* Bits 7 6 5*/
 			/* 000Unknown*/
@@ -715,6 +788,10 @@
 			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 IE
+			 */
+			Q931InitIECalledNum(&CalledNum);
 			CalledNum.TypNum = 2;
 			CalledNum.NumPlanID = 1;
 			CalledNum.Size = CalledNum.Size + (unsigned char)strlen(zchan->caller_data.ani.digits);
@@ -722,6 +799,16 @@
 			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  = 0;	/* ITU */
+			HLComp.Interpret = 4;	/* only possible value */
+			HLComp.PresMeth  = 1;   /* High-layer protocol profile */
+			HLComp.HLCharID  = 0;	/* Telephony = 0, 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);
+
 			Q931Rx43(&isdn_data->q931, (L3UCHAR *) gen, gen->Size);
 			isdn_data->channels_local_crv[gen->CRV] = zchan;
 		}
@@ -948,16 +1038,60 @@
 	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 *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, 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 zap_state_map_t isdn_state_map = {
 	{
@@ -1113,22 +1247,29 @@
 				   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.autoServiceAck = 1;
Index: src/isdn/Q931StateNT.c
===================================================================
--- src/isdn/Q931StateNT.c	(revision 495)
+++ src/isdn/Q931StateNT.c	(working copy)
@@ -143,7 +143,7 @@
     Q931StartTimer(pTrunk, callIndex, 303);
 	if(iFrom == 4)
 	{
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -175,7 +175,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -208,7 +208,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -241,7 +241,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -274,7 +274,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -313,7 +313,7 @@
             return ret;
         pMes->CRV = pTrunk->call[callIndex].CRV;
 
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
         if(ret)
             return ret;
 
@@ -389,7 +386,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -422,7 +419,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -455,7 +452,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -488,7 +485,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -521,7 +518,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -554,7 +551,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -587,7 +584,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -620,7 +617,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -653,7 +650,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -686,7 +683,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -719,7 +716,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -752,7 +749,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -785,7 +782,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -818,7 +815,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -851,7 +848,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -884,7 +881,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -917,7 +914,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -950,7 +947,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -983,7 +980,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1020,7 +1017,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1053,7 +1050,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1086,7 +1083,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1119,7 +1116,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1152,7 +1149,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1185,7 +1182,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1218,7 +1215,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1251,7 +1248,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
Index: src/isdn/include/Q921.h
===================================================================
--- src/isdn/include/Q921.h	(revision 495)
+++ src/isdn/include/Q921.h	(working copy)
@@ -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,312 @@
 
 *****************************************************************************/
 
+/****************************************************************************
+ * Changes:
+ *
+ * - June 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 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_LOGBUFSIZE	2000
+
+typedef enum			/* Network/User Mode		*/
 {
 	Q921_TE=0,                  /*  0 : User Mode                       */
     Q921_NT=1                   /*  1 : Network Mode                    */
 } Q921NetUser_t;
 
+typedef enum			/* Type of connection		*/
+{
+	Q921_PTP=0,		/* 0 : Point-To-Point		*/
+	Q921_PTMP=1		/* 1 : Point-To-Multipoint	*/
+} Q921NetType_t;
+
+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;
+
+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;
+
+/*
+ * 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 */
+
+#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 {
+	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;
+
+
 typedef struct Q921Data Q921Data_t;
-typedef int (*Q921TxCB_t) (void *, L2UCHAR *, L2INT);
+typedef int (*Q921Tx21CB_t) (void *, L2UCHAR *, L2INT);
+typedef int (*Q921Tx23CB_t) (void *, Q921DLMsg_t ind, L2UCHAR *, L2INT);
+typedef int (*Q921LogCB_t) (void *, Q921LogLevel_t, char *, L2INT);
 
 #define INITIALIZED_MAGIC 42
 struct Q921Data
 {
-    L2UCHAR HDLCInQueue[Q921MAXHDLCSPACE];
 	L2INT initialized;
-	L2UCHAR va;
-    L2UCHAR vs;
-    L2UCHAR vr;
-    L2INT state;
+	L2INT flags;
+	Q921State_t state;
+
+	/* packet & ack counters */
+	L2UCHAR va[Q921_TEI_MAX];
+	L2UCHAR vs[Q921_TEI_MAX];
+	L2UCHAR vr[Q921_TEI_MAX];
+
 	L2UCHAR sapi;
-	L2UCHAR tei;
+	L2UCHAR tei;			/* This endpoint's TEI */
+
+	/* */
+	L2USHORT ri[Q921_TEI_MAX];
+
+	L2INT Q921HeaderSpace;
 	Q921NetUser_t NetUser;
-	L2ULONG T200;
-	L2ULONG T203;
+	Q921NetType_t NetType;
+
+	/* timers */
+	L2ULONG T200[Q921_TEI_MAX];
+	L2ULONG T201[Q921_TEI_MAX];	/* PTMP NT mode timer */
+	L2ULONG T203[Q921_TEI_MAX];
+	L2ULONG T202;			/* PTMP TE mode TEI retransmit timer */
 	L2ULONG T200Timeout;
+	L2ULONG T201Timeout;
+	L2ULONG T202Timeout;
 	L2ULONG T203Timeout;
-	Q921TxCB_t Q921Tx21Proc;
-	Q921TxCB_t Q921Tx23Proc;
+
+	/* counters */
+	L2ULONG N200[Q921_TEI_MAX];	/* retransmit counter (per-TEI in PTMP NT mode) */
+	L2ULONG N200Limit;		/* max retransmit */
+
+	L2ULONG N202;			/* PTMP TE mode retransmit counter */
+	L2ULONG N202Limit;		/* PTMP TE mode max retransmit */
+
+	/* limits */
+	L2ULONG N201Limit;		/* max number of octets */
+
+	/* callbacks and callback data pointers */
+	Q921Tx21CB_t Q921Tx21Proc;
+	Q921Tx23CB_t Q921Tx23Proc;
 	void *PrivateData21;
 	void *PrivateData23;
-	L2INT Q921HeaderSpace;
 
+	/* Log callback */
+	Q921LogLevel_t	loglevel;
+	Q921LogCB_t	Q921LogProc;
+	void *PrivateDataLog;
+
+	/* HDLC queue */
+	L2UCHAR HDLCInQueue[Q921MAXHDLCSPACE];
+
+	/* I + UI Frame queue */
+	L2UCHAR UIFrameQueue[Q921MAXHDLCSPACE];
+	L2UCHAR  IFrameQueue[Q921MAXHDLCSPACE];
 };
 
+
+/*
+ * Functions
+ * TODO: lots of stuff that should not be here
+ */
 void 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 Q921Start(L2TRUNK trunk);
+int Q921Stop(L2TRUNK trunk);
+
+void Q921SetLogCB(L2TRUNK trunk, Q921LogCB_t func, void *priv);
+void Q921SetLogLevel(L2TRUNK trunk, Q921LogLevel_t level);
+
 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 Q921Rx32(L2TRUNK trunk, Q921DLMsg_t ind, L2UCHAR * Mes, 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);
+int Q921Tx23Proc(L2TRUNK trunk, Q921DLMsg_t ind, 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);
+
+void Q921T200TimerStart(L2TRUNK trunk, L2UCHAR tei);
+void Q921T200TimerStop(L2TRUNK trunk, L2UCHAR tei);
+void Q921T200TimerReset(L2TRUNK trunk, L2UCHAR tei);
+void Q921T200TimerExpire(L2TRUNK trunk, L2UCHAR tei);
+
+void Q921T201TimerStart(L2TRUNK trunk, L2UCHAR tei);
+void Q921T201TimerStop(L2TRUNK trunk, L2UCHAR tei);
+void Q921T201TimerReset(L2TRUNK trunk, L2UCHAR tei);
+void Q921T201TimerExpire(L2TRUNK trunk, L2UCHAR tei);
+
+void Q921T202TimerStart(L2TRUNK trunk);
+void Q921T202TimerStop(L2TRUNK trunk);
+void Q921T202TimerReset(L2TRUNK trunk);
+void Q921T202TimerExpire(L2TRUNK trunk);
+
+void Q921T203TimerStart(L2TRUNK trunk, L2UCHAR tei);
+void Q921T203TimerStop(L2TRUNK trunk, L2UCHAR tei);
+void Q921T203TimerReset(L2TRUNK trunk, L2UCHAR tei);
+void Q921T203TimerExpire(L2TRUNK trunk, L2UCHAR tei);
+
 int Q921SendI(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR *mes, L2INT size);
+int Q921SendS(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR sv, L2UCHAR *mes, L2INT size);
+int Q921SendU(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR m, 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 Q921SendUN(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf, L2UCHAR *mes, L2INT size);
+
 int Q921ProcSABME(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+int Q921ProcDM(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+int Q921ProcUA(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+int Q921ProcDISC(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+int Q921ProcRR(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+int Q921ProcRNR(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+int Q921ProcREJ(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+
+
+/*
+ * (Common) procedures defined in the Q.921 SDL
+ */
+int Q921SendEnquiry(L2TRUNK trunk, L2UCHAR tei);
+int Q921SendEnquiryResponse(L2TRUNK trunk, L2UCHAR tei);
+void Q921ResetExceptionConditions(L2TRUNK trunk, L2UCHAR tei);
+int Q921EstablishDataLink(L2TRUNK trunk, L2UCHAR tei);
+int Q921NrErrorRecovery(L2TRUNK trunk, L2UCHAR tei);
+int Q921InvokeRetransmission(L2TRUNK trunk, L2UCHAR tei, L2UCHAR nr);
+
+
+/*
+ * 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_TN)
+
+#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)
 
-#endif
+#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))
 
+#endif
Index: src/isdn/include/Q931.h
===================================================================
--- src/isdn/include/Q931.h	(revision 495)
+++ src/isdn/include/Q931.h	(working copy)
@@ -244,6 +244,8 @@
 
 *****************************************************************************/
 
+#define	Q931_LOGBUFSIZE		1024	/* size of logging buffer */
+
 #define Q931L4BUF  1000             /* size of message buffer               */
 
 #define Q931L2BUF	300				/* size of message buffer				*/
@@ -257,8 +259,7 @@
 									/* channels regardless if it is E1, T1	*/
 									/* or BRI that actually is used.		*/
 
-#define Q931MAXCALLPERTRUNK (Q931MAXCHPERTRUNK * 2)
-                                    /* Number of max active CRV per trunk.  */
+#define Q931MAXCALLPERTRUNK (Q931MAXCHPERTRUNK * 2)	/* Number of max active CRV per trunk.  */
 									/* Q.931 can have more calls than there */
 									/* are channels.						*/
 
@@ -371,7 +373,7 @@
 #define Q931mes_RESTART              0x46 /* 0100 0110                   */
 #define Q931mes_RESTART_ACKNOWLEDGE  0x4e /* 0100 1110                   */
 #define Q931mes_CONGESTION_CONTROL   0x79 /* 0111 1001                   */
-#define Q931mes_INFORMATION          0x7a /* 0111 1011                   */
+#define Q931mes_INFORMATION          0x7b /* 0111 1011                   */
 #define Q931mes_NOTIFY               0x6e /* 0110 1110                   */
 #define Q931mes_STATUS               0x7d /* 0111 1101                   */
 #define Q931mes_STATUS_ENQUIRY       0x75 /* 0111 0101                   */
@@ -479,8 +481,23 @@
 *****************************************************************************/
 typedef struct Q931_TrunkInfo Q931_TrunkInfo_t;
 
-typedef L3INT (*Q931TxCB_t) (void *,L3UCHAR *, L3INT);
+/*
+ * Logging
+ */
+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 *, L3INT);
 typedef L3INT (*Q931ErrorCB_t) (void *,L3INT,L3INT,L3INT);
+typedef L3INT (*Q931LogCB_t) (void *, Q931LogLevel_t, char *, L3INT);
 
 typedef enum						/* Network/User Mode.                   */
 {
@@ -505,7 +522,8 @@
 	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=3,				/*  3 : BRI Trunk                       */
+	Q931_TrType_BRI_PTMP=4				/*  4 : BRI Trunk                       */
 } Q931_TrunkType_t;
 
 typedef enum						/* Trunk State							*/
@@ -530,11 +549,15 @@
 
 	Q931_TrunkType_t TrunkType;		/* Trunk Line Type.                     */
 
-	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                        */
@@ -894,8 +916,9 @@
 
 *****************************************************************************/
 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);
+L3INT   Q931Rx23(Q931_TrunkInfo_t *pTrunk, L3INT, L3UCHAR * Mes, L3INT Size);
+L3INT   Q931Tx32Data(Q931_TrunkInfo_t *pTrunk, L3UCHAR * Mes, L3INT Size);
+L3INT   Q931Tx32UnitData(Q931_TrunkInfo_t *pTrunk, 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);
@@ -965,8 +988,8 @@
 						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);
@@ -986,6 +1009,11 @@
 L3INT Q931TxDummy(Q931_TrunkInfo_t *pTrunk, L3UCHAR * b, L3INT n);
 L3INT Q931ErrorDummy(void *priv, L3INT a, L3INT b, L3INT c);
 
+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);
 
+L3INT Q931MesgHeader(Q931_TrunkInfo_t *pTrunk, Q931mes_Generic *mes, L3UCHAR *OBuf, L3INT Size, L3INT *IOff);
 
 #endif /* _Q931_NL */
Index: src/isdn/Q931mes.c
===================================================================
--- src/isdn/Q931mes.c	(revision 495)
+++ src/isdn/Q931mes.c	(working copy)
@@ -43,6 +43,33 @@
 
 #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->CRV, mes->CRVFlag, mes->MesType);
+
+	OBuf[Octet++] = mes->ProtDisc;				/* Protocol discriminator		*/
+	if(mes->CRV > 127) {
+		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
@@ -89,12 +116,7 @@
 	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 +199,7 @@
 	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))
@@ -296,12 +315,7 @@
 	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 +406,7 @@
 	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))
@@ -457,12 +466,7 @@
 	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))
@@ -532,12 +538,7 @@
 	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))
@@ -607,12 +608,7 @@
 	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 +671,7 @@
 	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 +744,7 @@
 	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 +806,7 @@
 	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))
@@ -887,12 +868,7 @@
     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);
     
     /* ChanID */
     if(Q931IsIEPresent(pMes->ChanID))
@@ -954,12 +930,7 @@
     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);
     
     /* ChanID */
     if(Q931IsIEPresent(pMes->ChanID))
@@ -1019,12 +990,7 @@
 	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))
@@ -1078,12 +1046,7 @@
 	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))
@@ -1141,11 +1106,7 @@
 	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))
@@ -1257,12 +1223,7 @@
 	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))
@@ -1405,12 +1366,7 @@
 	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 +1436,7 @@
 	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 +1496,7 @@
 	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 +1548,7 @@
 	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 +1601,7 @@
 	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 +1654,7 @@
 	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 +1748,7 @@
 	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 +1809,7 @@
 	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))
Index: src/isdn/5ESSStateTE.c
===================================================================
--- src/isdn/5ESSStateTE.c	(revision 495)
+++ src/isdn/5ESSStateTE.c	(working copy)
@@ -240,7 +240,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -283,7 +283,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
Index: src/isdn/nationalmes.c
===================================================================
--- src/isdn/nationalmes.c	(revision 495)
+++ src/isdn/nationalmes.c	(working copy)
@@ -155,12 +155,7 @@
 	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))
Index: src/isdn/Q931ie.c
===================================================================
--- src/isdn/Q931ie.c	(revision 495)
+++ src/isdn/Q931ie.c	(working copy)
@@ -71,8 +71,7 @@
 #define Q931MoreIE() (Octet+Off - 2 < IESize)
 
 #define Q931IESizeTest(x)   {\
-                            if(Octet+Off-2 != IESize)\
-                            {\
+		if(Octet+Off-2 != IESize) {\
                                 Q931SetError(pTrunk,x, Octet, Off);\
                                 return x;\
                             }\
@@ -135,9 +134,10 @@
     L3INT IESize;
     *pIE=0;
 
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Decoding Bearer Capability IE\n");
+
     /* Octet 1 */
-    pie->IEId        = IBuf[Octet];
-    Octet ++;
+	pie->IEId = IBuf[Octet++];
 
     /* Octet 2 */
     IESize = IBuf[Octet ++]; 
@@ -176,16 +176,15 @@
 
         if(IsQ931Ext(IBuf[Octet+Off-1]))
         {
-            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)))
             {
                 pie->SyncAsync   = ieGetOctet((IBuf[Octet+Off] & 0x40) >> 6);
                 pie->Negot       = ieGetOctet((IBuf[Octet+Off] & 0x20) >> 5);
                 pie->UserRate    = ieGetOctet(IBuf[Octet+Off] & 0x1f);
                 Off ++;
             }
-            else
-            {
+			else {
                 /* We have detected bit 8 = 0, but no setting that require the  */
                 /* additional octets ???                                        */
                 Q931SetError(pTrunk, Q931E_BEARERCAP, 5,Off);
@@ -214,11 +213,9 @@
                     pie->InBandNeg   = ieGetOctet((IBuf[Octet+Off] & 0x02) >> 1);
                     Off++;
                 }
-                else
-                {
+				else {
                     Q931SetError(pTrunk,Q931E_BEARERCAP, 5,Off);
                     return Q931E_BEARERCAP;
-
                 }
 
                 /* Octet 5c */
@@ -275,11 +270,9 @@
                     Off++;
                 }
             }
-            else
-            {
+			else {
                 Q931SetError(pTrunk,Q931E_BEARERCAP, 7, Off);
                 return Q931E_BEARERCAP;
-
             }
         }
     }
@@ -318,73 +311,74 @@
     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*/
     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 +386,16 @@
     {
         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);
         }
     }
 
@@ -1160,9 +1154,10 @@
 //18 04 e1 80 83 01
     *pIE=0;
 
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Decoding Channel ID IE\n");
+
     /* Octet 1 */
-    pie->IEId        = IBuf[Octet];
-    Octet ++;
+	pie->IEId = IBuf[Octet++];
 
     /* Octet 2 */
     IESize = IBuf[Octet ++]; 
@@ -1174,11 +1169,9 @@
     pie->DChanInd        = (IBuf[Octet] >> 2) & 0x01;
     pie->InfoChanSel    = IBuf[Octet] & 0x03;
 
-    Off = Q931ReadExt(&IBuf[Octet+Off], Off);
-    Octet++;
+	Off = Q931ReadExt(&IBuf[Octet++], Off);
 
     /* Octet 3.1 */
-
     if(pie->IntIDPresent)
     {
         pie->InterfaceID    = IBuf[Octet+Off] & 0x7f;
@@ -1191,7 +1184,7 @@
         Off++;
     }
 
-    if (Octet + Off -2 != IESize) {
+	if ((Octet + Off - 2) != IESize) {
         /* Octet 3.2 */
         if(pie->IntType == 1)        /* PRI etc */
         {
@@ -1198,13 +1191,12 @@
             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 */
+			/* TODO: 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) {
 				Off = Q931ReadExt(&IBuf[Octet+Off], Off);
@@ -1221,6 +1213,39 @@
     *OOff = (*OOff) + sizeof(Q931ie_BearerCap);
     pie->Size = sizeof(Q931ie_BearerCap);
 
+	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";
+			}
+
+			snprintf(tmp, sizeof(tmp)-1, "InfoChanSel: %d (%s)", pie->InfoChanSel, iface);
+		}
+
+		Q931Log(pTrunk, Q931_LOG_DEBUG,
+			"\n\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;
 }
 
@@ -1247,15 +1272,15 @@
 
     /* 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 +1288,21 @@
         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,21 +1336,18 @@
 *****************************************************************************/
 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 */
     {
-        CRV = IBuf[Octet] & 0x7F;
-        Octet++;
+        CRV = IBuf[Octet++] & 0x7F;
     }
     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
     {
@@ -1326,6 +1357,8 @@
         Octet+=l;
     }
 
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Call reference Value: %d (%#x)\n", CRV);
+
     *IOff = Octet;
     return CRV;
 }
@@ -1354,50 +1387,49 @@
     L3INT Off = 0;
     L3INT Octet = 0;
     L3INT IESize=0;
-
     *pIE=0;
 
-    pie->IEId        = IBuf[Octet];
-    IESize = IBuf[Octet ++];
+	/* Octet 1: ID */
+	pie->IEId  = IBuf[Octet++];
     
-    /* Octet 2 */
+	/* Octet 2: Size */
     IESize = IBuf[Octet ++]; 
 
-    /* Octet 3 - Year */
+	/* Octet 3: Year */
     pie->Year = IBuf[Octet++];
 
-    /* Octet 4 - Month */
+	/* Octet 4: Month */
     pie->Month = IBuf[Octet++];
 
-    /* Octet 5 - Day */
+	/* Octet 5: Day */
     pie->Day = IBuf[Octet++];
 
     /*******************************************************************
-        The remaining part of the IE are optioinal, but only the length 
+		The remaining part of the IE are optional, but only the length 
         can now tell us wherever these fields are present or not
     ********************************************************************/
     pie->Format=0;
 
     /* Octet 6 - Hour (optional)*/
-    if(IESize >= 6)
+	if(IESize >= 4)
     {
         pie->Format = 1;
         pie->Hour = IBuf[Octet++];
+	}
 
         /* Octet 7 - Minute (optional)*/
-        if(IESize >= 7)
+	if(IESize >= 5)
         {
             pie->Format = 2;
             pie->Minute = IBuf[Octet++];
+	}
 
             /* Octet 8 - Second (optional)*/
-            if(IESize >= 8)
+	if(IESize >= 6)
             {
                 pie->Format = 3;
                 pie->Second = IBuf[Octet++];
             }
-        }
-    }
 
     Q931IESizeTest(Q931E_DATETIME);
 
@@ -1434,6 +1466,8 @@
     OBuf[(*Octet)++] = pIE->Year;
     OBuf[(*Octet)++] = pIE->Month;
     OBuf[(*Octet)++] = pIE->Day;
+
+	/* optional fields */
     if(pIE->Format >= 1)
     {
         OBuf[(*Octet)++] = pIE->Hour;
@@ -1475,12 +1509,9 @@
     L3INT Octet = 0;
     L3INT IESize;
     L3INT x;
-
     *pIE=0;
 
-    pie->IEId        = IBuf[Octet];
-    Octet ++;
-
+	pie->IEId = IBuf[Octet++];
     IESize = IBuf[Octet ++]; 
 
     for(x=0; x<IESize; x++)
@@ -1634,7 +1665,7 @@
     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 +1673,11 @@
     /* 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
     {
Index: src/isdn/DMSStateTE.c
===================================================================
--- src/isdn/DMSStateTE.c	(revision 495)
+++ src/isdn/DMSStateTE.c	(working copy)
@@ -237,7 +237,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -279,7 +278,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
Index: src/isdn/5ESSmes.c
===================================================================
--- src/isdn/5ESSmes.c	(revision 495)
+++ src/isdn/5ESSmes.c	(working copy)
@@ -158,12 +158,7 @@
     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))
Index: src/isdn/Q931api.c
===================================================================
--- src/isdn/Q931api.c	(revision 495)
+++ src/isdn/Q931api.c	(working copy)
@@ -74,8 +74,8 @@
 						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 @@
 		break;
 
 	case Q931_TrType_BRI:
+	case Q931_TrType_BRI_PTMP:
 		dchannel = 3;
 		maxchans = 3;
 		break;
@@ -521,9 +522,11 @@
 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,buf,ptr->Size);
 }
 
 L3INT Q931AckRestart(Q931_TrunkInfo_t *pTrunk, L3UCHAR *buf)
Index: src/isdn/Q931StateTE.c
===================================================================
--- src/isdn/Q931StateTE.c	(revision 495)
+++ src/isdn/Q931StateTE.c	(working copy)
@@ -227,7 +227,7 @@
     Q931StartTimer(pTrunk, callIndex, 303);
 	if(iFrom == 4)
 	{
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -259,7 +259,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -292,7 +292,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -329,7 +329,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -362,7 +362,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -399,7 +399,14 @@
             return ret;
         pMes->CRV = pTrunk->call[callIndex].CRV;
 
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		/*
+		 * PTMP NT should use DL-UNIT DATA broadcast (?)
+		 */ 
+		if(pTrunk->TrunkType == Q931_TrType_BRI_PTMP && pTrunk->NetUser == Q931_NT)
+			ret = Q931Tx32UnitData(pTrunk,buf,pMes->Size);
+		else
+			ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
+
         if(ret)
             return ret;
 
@@ -475,7 +480,7 @@
     Q931StartTimer(pTrunk, callIndex, 303);
 	if(iFrom == 4)
 	{
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -504,7 +509,7 @@
         pMes->CRV = pTrunk->call[callIndex].CRV;
 
         /* Send RESUME to network */
-        ret=Q931Tx32(pTrunk,buf, pMes->Size);
+		ret=Q931Tx32Data(pTrunk,buf, pMes->Size);
         if(ret != Q931E_NO_ERROR)
             return ret;
 
@@ -544,7 +549,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -577,7 +582,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -610,7 +615,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -643,7 +648,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -676,7 +681,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -709,7 +714,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -742,7 +747,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -765,7 +770,7 @@
     L3INT ret = Q931E_NO_ERROR;
     if(iFrom == 4) {
 		/* TODO Add proc here*/
-		ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	} else if(state == Q931_U0 && iFrom == 2) {
 		Q931Tx34(pTrunk,buf,pMes->Size);
         ret = Q931ReleaseComplete(pTrunk, buf);
@@ -838,7 +842,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -877,7 +881,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -910,7 +914,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -943,7 +947,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -976,7 +980,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1009,7 +1013,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1042,7 +1046,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1075,7 +1079,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1112,7 +1116,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1145,7 +1149,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1178,7 +1182,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1211,7 +1215,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1244,7 +1248,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1277,7 +1281,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1310,7 +1314,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
@@ -1343,7 +1347,7 @@
 	if(iFrom == 4)
 	{
 		/* TODO Add proc here*/
-        ret = Q931Tx32(pTrunk,buf,pMes->Size);
+		ret = Q931Tx32Data(pTrunk,buf,pMes->Size);
 	}
 	else if (iFrom ==2)
 	{
Index: src/isdn/DMSmes.c
===================================================================
--- src/isdn/DMSmes.c	(revision 495)
+++ src/isdn/DMSmes.c	(working copy)
@@ -155,12 +155,7 @@
 	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))
Index: src/isdn/Q921.c
===================================================================
--- src/isdn/Q921.c	(revision 495)
+++ src/isdn/Q921.c	(working copy)
@@ -37,12 +37,247 @@
   POSSIBILITY OF SUCH DAMAGE.
 
 *****************************************************************************/
-#include "Q921.h"
 #include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
+#include <stdarg.h>
+
+#include "Q921.h"
 #include "mfifo.h"
 
+
+/*
+ * Declarations
+ */
 int Q921SendRR(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf);
+int Q921ProcIFrame(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+int Q921ProcSFrame(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+int Q921ProcUFrame(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+
+int Q921SetReceiverBusy(L2TRUNK trunk);
+int Q921ClearReceiverBusy(L2TRUNK trunk);
+
+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);
+/* unused for now
+ *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);
+
+
+/******************************************************************************************************
+ * 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
+ */
+int Q921SendEnquiry(L2TRUNK trunk, L2UCHAR tei)
+{
+	/* send enquiry: begin */
+	if(Q921_CHECK_FLAG(trunk, 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(trunk, Q921_FLAG_ACK_PENDING);
+
+	/* "Start" T200 */
+	Q921T200TimerReset(trunk, tei);
+
+	/* send enquiry: end */
+	return 1;
+}
+
+/**
+ * Q921SendEnquiryResponse
+ */
+int Q921SendEnquiryResponse(L2TRUNK trunk, L2UCHAR tei)
+{
+	/* send enquiry: begin */
+	if(Q921_CHECK_FLAG(trunk, 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(trunk, 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
+ */
+void Q921ResetExceptionConditions(L2TRUNK trunk, L2UCHAR tei)
+{
+	/* Clear peer receiver busy */
+	Q921_CLEAR_FLAG(trunk, Q921_FLAG_PEER_RECV_BUSY);
+
+	/* Clear reject exception */
+	Q921_CLEAR_FLAG(trunk, Q921_FLAG_REJECT);
+
+	/* Clear own receiver busy */
+	Q921_CLEAR_FLAG(trunk, Q921_FLAG_RECV_BUSY);
+
+	/* Clear acknowledge pending */
+	Q921_CLEAR_FLAG(trunk, 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)
+ */
+int Q921EstablishDataLink(L2TRUNK trunk, L2UCHAR tei)
+{
+	L2UCHAR idx = tei;
+
+	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
+
+	/* reset exception conditions */
+	Q921ResetExceptionConditions(trunk, tei);
+
+	/* RC = 0 */
+	trunk->N200[idx] = 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)
+ */
+int Q921NrErrorRecovery(L2TRUNK trunk, L2UCHAR tei)
+{
+	/* MDL Error indication (J) */
+
+	/* Establish datalink */
+	Q921EstablishDataLink(trunk, tei);
+
+	/* Clear L3 initiated */
+	Q921_CLEAR_FLAG(trunk, 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)
+ */
+int Q921InvokeRetransmission(L2TRUNK trunk, L2UCHAR tei, L2UCHAR nr)
+{
+	L2UCHAR idx = tei;
+
+	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
+#if __TODO__
+	while(trunk->vs[idx] != nr) {	/* ???? */
+		/* V(S) = V(S) - 1 */
+		Q921_DEC_COUNTER(trunk->vs[idx]);
+
+		/* <- I Frame queued up (move I frame to tx queue? */
+
+		/* next frame in queue (backtrack along I queue) ??? */
+	}
+#endif
+	return 1;
+}
+
 
 /*****************************************************************************
 
@@ -56,43 +291,364 @@
 					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);
+
+		/* Initialize I + UI queue */
+		MFIFOCreate(trunk->UIFrameQueue, Q921MAXHDLCSPACE, 10);
+		MFIFOCreate(trunk->IFrameQueue, Q921MAXHDLCSPACE, 10);
+
 		trunk->initialized = INITIALIZED_MAGIC;
 	}
-	trunk->va = 0;
-	trunk->vs = 0;	
-	trunk->vr = 0;
-	trunk->state = 0;
+
+	trunk->state = Q921_STATE_STOPPED;
+	trunk->flags = 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;
+
+	/* timeout default values */
+	trunk->T200Timeout = 1000;	/*   1 second  */
+	trunk->T203Timeout = 10000;	/*  10 seconds */
+	trunk->T202Timeout = 2000;	/*   2 seconds */
+	trunk->T201Timeout = 200000;	/* 200 seconds */
+
+	/* octet / retransmit counter default limits */
+	trunk->N200Limit   = 3;		/*   3 retransmits */
+	trunk->N201Limit   = 260;	/* 260 octets      */
+	trunk->N202Limit   = 3;		/*   3 retransmits */
+
+	/* reset counters, timers, etc. */
+	trunk->T202 = 0;
+	trunk->N202 = 0;
+
+	memset(trunk->T200, 0, sizeof(*trunk->T200)*Q921_TEI_MAX);
+	memset(trunk->T201, 0, sizeof(*trunk->T201)*Q921_TEI_MAX);
+	memset(trunk->T203, 0, sizeof(*trunk->T203)*Q921_TEI_MAX);
+	memset(trunk->N200, 0, sizeof(*trunk->N200)*Q921_TEI_MAX);
+
+	memset(trunk->ri, 0, sizeof(*trunk->ri)*Q921_TEI_MAX);
+	memset(trunk->va, 0, sizeof(*trunk->va)*Q921_TEI_MAX);
+	memset(trunk->vs, 0, sizeof(*trunk->vs)*Q921_TEI_MAX);
+	memset(trunk->vr, 0, sizeof(*trunk->vr)*Q921_TEI_MAX);
+
+	if(Q921_IS_PTMP(trunk)) {
+		/*
+		 * Clear list of assigned TEIs, send TEI reset command
+		 */
+		trunk->tei = 0;
 }
+}
+
 
+/**
+ * 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
+ */
 int Q921Tx21Proc(L2TRUNK trunk, L2UCHAR *Msg, L2INT size)
 {
+	Q921LogMesg(trunk, Q921_LOG_DEBUG, 0, Msg, size, "Sending frame\n");
+
 	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
+ */
+int Q921Tx23Proc(L2TRUNK trunk, Q921DLMsg_t ind, L2UCHAR *Msg, L2INT size)
+{
+	return trunk->Q921Tx23Proc(trunk->PrivateData23, ind, 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;
+	}
+
+	va_end(ap);
+
+	return trunk->Q921LogProc(trunk->PrivateDataLog, level, buf, strnlen(buf, sizeof(buf)-1));
+}
+
+
+/**
+ * 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, ...)
 {
-	return trunk->Q921Tx23Proc(trunk->PrivateData23, Msg, size);
+	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;
+		L2INT idx;
+
+		pleft   = sizeof(pbuf);
+		poffset = 0;
+
+		/*
+		 * Decode packet
+		 */
+		sapi = (pmes[0] & 0xfc) >> 2;
+		cr   = (pmes[0] & 0x02) >> 1;
+		tei  = (pmes[1] & 0xfe) >> 1;
+		idx  = Q921_IS_PTMP_NT(trunk) ? tei : 0;
+
+		/* make cr actually useful */
+		cr   = (received) ? Q921_IS_COMMAND(trunk, cr) : Q921_IS_RESPONSE(trunk, cr);
+
+		len = snprintf(pbuf + poffset, pleft, "\n----------------- Q.921 Packet ---------------\n");
+		if(len > 0) {
+			poffset += len;
+			pleft   -= len;
+		} else {
+			goto out;			
+		}
+
+		/* common header */
+		len = snprintf(pbuf + poffset, pleft, "    SAPI: %u, TEI: %u, C/R: %s (%d)\n\n", sapi, tei, (cr) ? "Command" : "Response", (mes[0] & 0x02) >> 1 );
+		if(len > 0) {
+			poffset += len;
+			pleft   -= len;
+		} else
+			goto out;
+
+		/*
+		 * 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 */
+
+			len = snprintf(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,
+														trunk->va[idx], trunk->vr[idx], trunk->vs[idx]);
+			if(len > 0) {
+				poffset += len;
+				pleft   -= len;
+			} else
+				goto out;
+		}
+		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;
+			}
+
+			len = snprintf(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,
+														trunk->va[idx], trunk->vr[idx], trunk->vs[idx]);
+			if(len > 0) {
+				poffset += len;
+				pleft   -= len;
+			} else
+				goto out;
+		}
+		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";
+			}
+
+
+			len = snprintf(pbuf + poffset, pleft, "    Type: U Frame (%s)\n          P/F: %d\n", type, pf);
+			if(len > 0) {
+				poffset += len;
+				pleft   -= len;
+			} else
+				goto out;
+		}	
+		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;
+		}
+
+		len = snprintf(pbuf + poffset, pleft, "\n    Q.921 state: \"%s\" (%d) [flags: %c%c%c%c]\n", Q921State2Name(trunk->state), trunk->state,
+											Q921_CHECK_FLAG(trunk, Q921_FLAG_ACK_PENDING) ? 'A' : '-',
+											Q921_CHECK_FLAG(trunk, Q921_FLAG_REJECT) ? 'R' : '-',
+											Q921_CHECK_FLAG(trunk, Q921_FLAG_PEER_RECV_BUSY) ? 'P' : '-',
+											Q921_CHECK_FLAG(trunk, Q921_FLAG_RECV_BUSY) ? 'B' : '-');
+		if(len > 0) {
+			poffset += len;
+			pleft   -= len;
+		} else
+			goto out;
+
+
+		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));
 }
 
 /*****************************************************************************
@@ -111,7 +667,8 @@
 L2ULONG Q921GetTime(void)
 {
     L2ULONG tNow = 0;
-    if(Q921GetTimeProc != NULL)
+
+	if(Q921GetTimeProc)
     {
         tNow = Q921GetTimeProc();
         if(tNow < tLast)    /* wrapped */
@@ -123,64 +680,353 @@
     return tNow;
 }
 
-void Q921T200TimerStart(L2TRUNK trunk)
+/*
+ * T200 handling (per-TEI in PTMP NT mode, tei=0 otherwise)
+ */
+void Q921T200TimerStart(L2TRUNK trunk, L2UCHAR tei)
+{
+	if(!Q921_IS_PTMP_NT(trunk))
+		tei = 0;
+
+	if (!trunk->T200[tei]) {
+		trunk->T200[tei] = Q921GetTime() + trunk->T200Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "T200 started\n");
+	}
+}
+
+void Q921T200TimerStop(L2TRUNK trunk, L2UCHAR tei)
+{
+	if(!Q921_IS_PTMP_NT(trunk))
+		tei = 0;
+
+	trunk->T200[tei] = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T200 stopped\n");
+}
+
+void Q921T200TimerReset(L2TRUNK trunk, L2UCHAR tei)
+{
+	if(!Q921_IS_PTMP_NT(trunk))
+		tei = 0;
+
+	trunk->T200[tei] = Q921GetTime() + trunk->T200Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T200 restarted\n");
+}
+
+/*
+ * T203 handling (per-TEI in PTMP NT mode, tei=0 otherwise)
+ */
+void Q921T203TimerStart(L2TRUNK trunk, L2UCHAR tei)
+{
+	if(!Q921_IS_PTMP_NT(trunk))
+		tei = 0;
+
+	if (!trunk->T203[tei]) {
+		trunk->T203[tei] = Q921GetTime() + trunk->T203Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "T203 started\n");
+	}
+}
+
+void Q921T203TimerStop(L2TRUNK trunk, L2UCHAR tei)
+{
+	if(!Q921_IS_PTMP_NT(trunk))
+		tei = 0;
+
+	trunk->T203[tei] = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T203 stopped\n");
+}
+
+void Q921T203TimerReset(L2TRUNK trunk, L2UCHAR tei)
+{
+	if(!Q921_IS_PTMP_NT(trunk))
+		tei = 0;
+
+	trunk->T203[tei] = Q921GetTime() + trunk->T203Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T203 restarted\n");
+}
+
+/*
+ * T202 handling (TEI message timeout, TE mode only)
+ */
+void Q921T202TimerStart(L2TRUNK trunk)
 {
-	if (!trunk->T200) {
-		trunk->T200 = Q921GetTime() + trunk->T200Timeout;
+	if (!trunk->T202) {
+		trunk->T202 = Q921GetTime() + trunk->T202Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "T202 started\n");
+	}
 	}
+
+void Q921T202TimerStop(L2TRUNK trunk)
+{
+	trunk->T202 = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T202 stopped\n");
 }
 
-void Q921T200TimerStop(L2TRUNK trunk)
+void Q921T202TimerReset(L2TRUNK trunk)
 {
-	trunk->T200 = 0;
+	trunk->T202 = Q921GetTime() + trunk->T202Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T202 restarted\n");
 }
 
-void Q921T200TimerReset(L2TRUNK trunk)
+/*
+ * T201 handling (TEI management (NT side), per-TEI)
+ */
+void Q921T201TimerStart(L2TRUNK trunk, L2UCHAR tei)
 {
-	Q921T200TimerStop(trunk);
-	Q921T200TimerStart(trunk);
+	if (!trunk->T201[tei]) {
+		trunk->T201[tei] = Q921GetTime() + trunk->T201Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "T201 started\n");
+	}
 }
 
-void Q921T203TimerStart(L2TRUNK trunk)
+void Q921T201TimerStop(L2TRUNK trunk, L2UCHAR tei)
 {
-	if (!trunk->T203) {
-		trunk->T203 = Q921GetTime() + trunk->T203Timeout;
+	trunk->T201[tei] = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T201 stopped\n");
 	}
+
+void Q921T201TimerReset(L2TRUNK trunk, L2UCHAR tei)
+{
+	trunk->T201[tei] = Q921GetTime() + trunk->T201Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T201 restarted\n");
 }
 
-void Q921T203TimerStop(L2TRUNK trunk)
+
+/*
+ * Expiry callbacks
+ */
+void Q921T200TimerExpire(L2TRUNK trunk, L2UCHAR tei)
 {
-	trunk->T203 = 0;
+	L2INT idx = tei;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T200 expired for TEI %d (trunk TEI %d)\n", tei, trunk->tei);
+
+	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
+
+	/* Stop timer first */
+	Q921T200TimerStop(trunk, tei);
+
+	switch(trunk->state) {
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+		if(trunk->N200[idx] >= trunk->N200Limit) {
+			/* Discard I queue */
+			MFIFOClear(trunk->IFrameQueue);
+
+			/* MDL-Error indication (G) */
+			Q921Log(trunk, Q921_LOG_ERROR, "Failed to establish Q.921 link in %d retries\n", trunk->N200[idx]);
+
+			/* DL-Release indication */
+			Q921Tx23Proc(trunk, Q921_DL_RELEASE, NULL, 0);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED);
+		} else {
+			/* Increment retry counter */
+			trunk->N200[idx]++;
+
+			/* Send SABME */
+			Q921SendSABME(trunk,
+					trunk->sapi,
+					Q921_COMMAND(trunk),
+					tei,
+					1);
+
+			/* Start T200 */
+			Q921T200TimerStart(trunk, tei);
+		}
+		break;
+
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		trunk->N200[idx] = 0;
+
+		if(!Q921_CHECK_FLAG(trunk, Q921_FLAG_PEER_RECV_BUSY)) {
+			/* get last transmitted I frame */
+
+			/* V(S) = V(S) - 1 */
+			Q921_DEC_COUNTER(trunk->vs[idx]);
+
+			/* retransmit I frame */
+
+			/* V(S) = V(S) + 1 (done by Q921SendI() ) */
+			//trunk->vs[idx]++;
+
+			/* clear acknowledge pending */
+			Q921_CLEAR_FLAG(trunk, Q921_FLAG_ACK_PENDING);
+
+			/* Start T200 */
+			Q921T200TimerStart(trunk, tei);
+		} else {
+			/* transmit enquiry */
+			Q921SendEnquiry(trunk, tei);
+		}
+
+		/* increment counter */
+		trunk->N200[idx]++;
+
+		/* change state (no action) */
+		Q921ChangeState(trunk, Q921_STATE_TIMER_RECOVERY);
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		if(trunk->N200[idx] == trunk->N200Limit) {
+			/* MDL Error indication (I) */
+
+			/* Establish data link */
+			Q921EstablishDataLink(trunk, tei);
+
+			/* Clear L3 initiated */
+			Q921_CLEAR_FLAG(trunk, Q921_FLAG_L3_INITIATED);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		} else {
+			if(trunk->vs[idx] == trunk->va[idx]) {
+				/* transmit enquiry */
+				Q921SendEnquiry(trunk, tei);
+
+			} else if(!Q921_CHECK_FLAG(trunk, Q921_FLAG_PEER_RECV_BUSY)) {
+				/* get last transmitted frame */
+
+				/* V(S) = V(S) - 1 */
+				Q921_DEC_COUNTER(trunk->vs[idx]);
+
+				/* retrans frame */
+
+				/* V(S) = V(S) + 1 (done by Q921SendI() ) */
+				//Q921_INC_COUNTER(trunk->vs[idx]);
+
+				/* clear acknowledge pending */
+				Q921_CLEAR_FLAG(trunk, Q921_FLAG_ACK_PENDING);
+
+				/* Start T200 */
+				Q921T200TimerStart(trunk, tei);
+			}
+
+			/* increment counter */
+			trunk->N200[idx]++;
+
+			/* no state change */
+		}
+		break;
+
+	default:
+		break;
+	}
 }
 
-void Q921T203TimerReset(L2TRUNK trunk)
+void Q921T203TimerExpire(L2TRUNK trunk, L2UCHAR tei)
 {
-	Q921T203TimerStop(trunk);
-	Q921T203TimerStart(trunk);
+	L2INT idx = tei;
+
+	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T203 expired for TEI %d (trunk TEI %d)\n", tei, trunk->tei);
+
+	/* Stop Timer first */
+	Q921T203TimerStop(trunk, tei);
+
+	switch(trunk->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* Send Enquiry */
+		Q921SendEnquiry(trunk, tei);
+
+		/* RC = 0 */
+		trunk->N200[idx] = 0;
+
+		/* no state change */
+		break;
+
+	default:
+		break;
+	}
 }
 
-void Q921T200TimerExpire(L2TRUNK trunk)
+void Q921T202TimerExpire(L2TRUNK trunk)
 {
-	(void)trunk;
+	Q921T202TimerReset(trunk);
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T202 expired for Q.921 trunk with TEI %d\n", trunk->tei);
+
+	/* todo: implement resend counter */
+
+	switch(trunk->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);
 
-void Q921T203TimerExpire(L2TRUNK trunk)
+		trunk->N202++;
+	}
+}
+
+void Q921T201TimerExpire(L2TRUNK trunk, L2UCHAR tei)
 {
-	Q921T203TimerReset(trunk);
-	Q921SendRR(trunk, trunk->sapi, trunk->NetUser == Q921_TE ? 0 : 1, trunk->tei, 1);
+	Q921Log(trunk, Q921_LOG_DEBUG, "T201 expired for TEI %d (trunk TEI: %d)\n", tei, trunk->tei);
 }
 
+/*
+ * Timer Tick function
+ */
 void Q921TimerTick(L2TRUNK trunk)
 {
 	L2ULONG tNow = Q921GetTime();
-	if (trunk->T200 && tNow > trunk->T200) {
-		Q921T200TimerExpire(trunk);
+
+	if(Q921_IS_PTMP_NT(trunk))
+	{
+		int x;
+
+		for(x=0; x<Q921_TEI_MAX; x++) {
+			/* TODO: check if TEI is assigned and skip check if not (speedup!) */
+
+			if (trunk->T200[x] && tNow > trunk->T200[x]) {
+				Q921T200TimerExpire(trunk, x);
+			}
+			if (trunk->T203[x] && tNow > trunk->T203[x]) {
+				Q921T203TimerExpire(trunk, x);		
 	}
-	if (trunk->T203 && tNow > trunk->T203) {
-		Q921T203TimerExpire(trunk);		
+			if (trunk->T201[x] && tNow > trunk->T201[x]) {
+				Q921T201TimerExpire(trunk, x);
+			}
+		}
+	} else {
+		if (trunk->T200[0] && tNow > trunk->T200[0]) {
+			Q921T200TimerExpire(trunk, trunk->tei);
+		}
+		if (trunk->T203[0] && tNow > trunk->T203[0]) {
+			Q921T203TimerExpire(trunk, trunk->tei);		
 	}
 }
 
+	if(Q921_IS_PTMP_TE(trunk)) {
+		if (trunk->T202 && tNow > trunk->T202) {
+			Q921T202TimerExpire(trunk);
+		}
+	}
+}
 
 void Q921SetGetTimeCB(L2ULONG (*callback)(void))
 {
@@ -232,24 +1078,129 @@
 *****************************************************************************/
 int Q921SendI(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR *mes, L2INT size)
 {
-    mes[trunk->Q921HeaderSpace+0] = (Sapi&0xfc) | ((cr<<1)&0x02);
+	L2INT idx = (Q921_IS_PTMP_NT(trunk)) ? Tei : 0;
+
+#if 0
+	if(!Q921_IS_READY(trunk)) {
+		/* don't even bother trying */
+		return 0;
+	}
+#endif
+
+#if 0
+	if(trunk->state < Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+		/* TODO: enqueue frame and establish data-link, for now: drop it */
+
+		Q921Log(trunk, Q921_LOG_ERROR, "Dropped I frame because data-link not ready\n");
+		return 0;
+	}
+#endif
+
+	mes[trunk->Q921HeaderSpace+0] = ((Sapi<<2)&0xfc) | ((cr<<1)&0x02);
+	mes[trunk->Q921HeaderSpace+1] = (Tei<<1) | 0x01;
+	mes[trunk->Q921HeaderSpace+2] = trunk->vs[idx]<<1;
+	mes[trunk->Q921HeaderSpace+3] = (trunk->vr[idx]<<1) | (pf & 0x01);
+	trunk->vs[idx]++;
+
+	/* TODO: queueing etc. */
+	/* link not ready? enqueue I frames */
+	if(trunk->state < Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+
+		MFIFOWriteMes(trunk->IFrameQueue, mes, size);
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "I Frame of size %d bytes queued\n", size);
+		return 1;
+	}
+
+	/* Start T200, Stop T203 */
+	Q921T200TimerStart(trunk, Tei);
+	Q921T203TimerStop(trunk, Tei);
+
+	return Q921Tx21Proc(trunk, mes, size);
+}
+
+int Q921SendS(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR sv, L2UCHAR *mes, L2INT size)
+{
+	L2INT idx = (Q921_IS_PTMP_NT(trunk)) ? Tei : 0;
+
+	if(!Q921_IS_READY(trunk)) {
+		/* don't even bother trying */
+		return 0;
+	}
+
+	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] = (trunk->vr[idx]<<1) | (pf & 0x01);
+
+	return Q921Tx21Proc(trunk, mes, size);
+}
+
+int Q921SendU(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR m, L2UCHAR *mes, L2INT size)
+{
+	mes[trunk->Q921HeaderSpace+0] = ((Sapi<<2)&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++;
+	mes[trunk->Q921HeaderSpace+2] = ((m<<3)&0xe0) | ((pf<<4)&0x10) | ((m<<2)&0x0c) | 0x03;
+
+	/* link not ready? enqueue non-TEI-mgmt UI frames */
+	if(m == 0x00 && mes[trunk->Q921HeaderSpace+3] != Q921_LAYER_ENT_ID_TEI && trunk->state < Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+
+		MFIFOWriteMes(trunk->UIFrameQueue, mes, size);
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "UI Frame of size %d bytes queued\n", size);
+		return 1;
+	}
 
     return Q921Tx21Proc(trunk, mes, size);
 }
 
-int Q921Rx32(L2TRUNK trunk, L2UCHAR * Mes, L2INT Size)
+int Q921Rx32(L2TRUNK trunk, Q921DLMsg_t ind, L2UCHAR * Mes, L2INT Size)
 {
-	return Q921SendI(trunk, 
+	L2INT res = 0;
+
+	/* TODO: proper queueing etc. */
+
+	switch(ind) {
+	case Q921_DL_DATA:	/* DL-DATA request */
+		if(trunk->state >= Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+			res = Q921SendI(trunk, 
 					trunk->sapi, 
-					trunk->NetUser == Q921_TE ? 0 : 1,
+					Q921_COMMAND(trunk),
 					trunk->tei, 
 					0, 
 					Mes, 
 					Size);
+		} else {
+			/* Treat as DL-ESTABLISH request */
+
+			/* queue i frame */
+			MFIFOWriteMes(trunk->IFrameQueue, Mes, Size);
+
+			/* establish data link */
+			Q921EstablishDataLink(trunk, trunk->tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		}
+		break;
+
+	case Q921_DL_UNIT_DATA:		/* DL-UNIT DATA request */
+		if(trunk->state >= Q921_STATE_TEI_ASSIGNED) {
+			res = Q921SendUN(trunk,
+					trunk->sapi,
+					Q921_COMMAND(trunk),
+					Q921_TEI_BCAST,
+					0,
+					Mes,
+					Size);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return res;
 }
 /*****************************************************************************
 
@@ -269,14 +1220,9 @@
 
 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);
 }
 
 /*****************************************************************************
@@ -296,14 +1242,9 @@
 *****************************************************************************/
 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);
 }
 
 /*****************************************************************************
@@ -323,14 +1264,9 @@
 *****************************************************************************/
 int Q921SendREJ(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)0x09;
-    mes[trunk->Q921HeaderSpace+3] = (L2UCHAR)((trunk->vr<<1) | (pf & 0x01));
-
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+4);
+	return Q921SendS(trunk, Sapi, cr, Tei, pf, 0x03, mes, trunk->Q921HeaderSpace+4);
 }
 
 /*****************************************************************************
@@ -350,29 +1286,105 @@
 *****************************************************************************/
 int Q921SendSABME(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)(0x6f | ((pf<<4)&0x10));
+	L2UCHAR mes[25];
 
-    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, 
-					trunk->sapi, 
-					trunk->NetUser == Q921_TE ? 0 : 1,
-					trunk->tei, 
+	if(trunk->initialized != INITIALIZED_MAGIC)
+		return 0;
+
+	if(Q921_IS_PTMP_TE(trunk)) {
+		trunk->state = Q921_STATE_TEI_UNASSIGNED;
+		trunk->tei   = 0;
+	} else
+		trunk->state = Q921_STATE_TEI_ASSIGNED;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "Starting trunk %p (sapi: %d, tei: %d, mode: %s %s)\n",
+				 trunk,
+				 trunk->sapi,
+				 trunk->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, 
+					Q921_COMMAND(trunk),
+					trunk->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)
+{
+	if(!trunk)
+		return -1;
+
+	if(Q921_IS_STOPPED(trunk))
+		return 0;
+
+	/* Flush queues, TODO: per-tei I/UI queues?  */
+	MFIFOClear(trunk->UIFrameQueue);
+	MFIFOClear(trunk->IFrameQueue);
+	MFIFOClear(trunk->HDLCInQueue);
+
+	/* Release TEI */
+	if(Q921_IS_PTMP_TE(trunk)) {
+		/* send verify request */
+		Q921TeiSendVerifyRequest(trunk);
+
+		/* drop TEI => not answering NT's check request anymore */
+		trunk->tei = 0;
+	}
+
+	/* Stop timers */
+	Q921T200TimerStop(trunk, trunk->tei);
+	Q921T203TimerStop(trunk, trunk->tei);
+	Q921T201TimerStop(trunk, trunk->tei);
+	Q921T202TimerStop(trunk);
+
+	/* Change state (no action) */
+	return Q921ChangeState(trunk, Q921_STATE_STOPPED);
+}
+
+
 /*****************************************************************************
 
   Function:     Q921SendDM
 
-  Description:  Comose and Send DM (Disconnected Mode)
+  Description:  Compose and Send DM (Disconnected Mode)
 
   Parameters:   trunk       trunk #
                 Sapi        Sapi
@@ -385,13 +1397,9 @@
 *****************************************************************************/
 int Q921SendDM(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)(0x0f | ((pf<<4)&0x10));
+	L2UCHAR mes[25];
 
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+3);
+	return Q921SendU(trunk, Sapi, cr, Tei, pf, 0x03, mes, trunk->Q921HeaderSpace+3);
 }
 
 /*****************************************************************************
@@ -411,13 +1419,9 @@
 *****************************************************************************/
 int Q921SendDISC(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)(0x43 | ((pf<<4)&0x10));
-
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+3);
+	return Q921SendU(trunk, Sapi, cr, Tei, pf, 0x08, mes, trunk->Q921HeaderSpace+3);
 }
 
 /*****************************************************************************
@@ -437,28 +1441,994 @@
 *****************************************************************************/
 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);
+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);
 }
 
+
+/**
+ * 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
+ */
 int Q921ProcSABME(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
 {
-	/* TODO:  Do we need these paramaters? */
-	(void)mes;
-	(void)size;
-
-    trunk->vr=0;
-    trunk->vs=0;
-	trunk->va=0;
+	L2UCHAR pf = (mes[2] & 0x10) >> 4;				/* poll / final flag */
+	L2UCHAR tei = (mes[1] & 0xfe) >> 1;
+	L2INT   idx = tei;
+
+	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
+
+	switch(trunk->state) {
+	case Q921_STATE_TEI_ASSIGNED:
+		/* send UA */
+		Q921SendUA(trunk,
+				trunk->sapi,
+				Q921_RESPONSE(trunk),	/* or command? */
+				tei, pf);
+
+		/* clear counters */
+		trunk->vr[idx]=0;
+		trunk->vs[idx]=0;
+		trunk->va[idx]=0;
+
+		/* TODO: send DL-Establish indication to Q.931 */
+		Q921Tx23Proc(trunk, Q921_DL_ESTABLISH, NULL, 0);
+
+		/* start T203 */
+		Q921T203TimerStart(trunk, tei);
+
+		/* change state (no action) */
+		Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED);
+		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(trunk->vs[idx] != trunk->va[idx]) {
+			/* clear I queue */
+			MFIFOClear(trunk->IFrameQueue);
+
+			/* DL-Establish indication */
+			Q921Tx23Proc(trunk, Q921_DL_ESTABLISH, NULL, 0);
+		}
+
+		/* clear counters */
+		trunk->vr[idx]=0;
+		trunk->vs[idx]=0;
+		trunk->va[idx]=0;
+
+		/* Stop T200, start T203 */
+		Q921T200TimerStop(trunk, tei);
+		Q921T203TimerStart(trunk, tei);
+
+		/* state change only if in TIMER_RECOVERY state */
+		if(trunk->state == Q921_STATE_TIMER_RECOVERY)
+			Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED);
+		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
+ */
+int Q921ProcDM(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR pf = (mes[2] & 0x10) >> 4;				/* poll / final flag */
+	L2UCHAR tei = (mes[1] & 0xfe) >> 1;
+
+	switch(trunk->state) {
+	case Q921_STATE_TEI_ASSIGNED:
+		if(!pf) {
+			/* to next state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		}
+		break;
+
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+	case Q921_STATE_AWAITING_RELEASE:
+		if(pf) {
+			if(trunk->state == Q921_STATE_AWAITING_ESTABLISHMENT) {
+				/* Discard I queue */
+				MFIFOClear(trunk->IFrameQueue);
+			}
+
+			/* Send DL-Release indication to Q.931 */
+			Q921Tx23Proc(trunk, Q921_DL_RELEASE, NULL, 0);
+
+			/* Stop T200 */
+			Q921T200TimerStop(trunk, tei);
+
+			/* Change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED);
+		}
+		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(trunk, Q921_FLAG_L3_INITIATED);
+
+			/* change state (no action?) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		}
+		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(trunk, Q921_FLAG_L3_INITIATED);
+
+		/* change state */
+		Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		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
+ */
+int Q921ProcUA(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR pf = (mes[2] & 0x10) >> 4;				/* poll / final flag */
+	L2UCHAR tei = (mes[1] & 0xfe) >> 1;
+	L2INT   idx = tei;
+
+	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
+
+	switch(trunk->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(trunk, Q921_FLAG_L3_INITIATED)) {	/* layer3 initiated */
+				trunk->vr[idx] = 0;
+
+				/* DL-Establish confirm */
+				Q921Tx23Proc(trunk, Q921_DL_ESTABLISH_CONFIRM, NULL, 0);
+
+			} else if(trunk->vs[idx] != trunk->va[idx]) {
+
+				/* discard I queue */
+				MFIFOClear(trunk->IFrameQueue);
+
+				/* DL-Establish indication */
+				Q921Tx23Proc(trunk, Q921_DL_ESTABLISH, NULL, 0);
+			}
+
+			/* Stop T200, start T203 */
+			Q921T200TimerStop(trunk, tei);
+			Q921T203TimerStart(trunk, tei);
+
+			trunk->vs[idx] = 0;
+			trunk->va[idx] = 0;
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED);
+		} 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, NULL, 0);
+
+			/* Stop T200 */
+			Q921T200TimerStop(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED);
+		} 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
+ */
+int Q921ProcDISC(L2TRUNK trunk, L2UCHAR *mes, L2INT size)
+{
+	L2UCHAR pf  = (mes[2] & 0x10) >> 4;				/* poll / final flag */
+	L2UCHAR tei = (mes[1] & 0xfe) >> 1;
+
+	switch(trunk->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(trunk->IFrameQueue);
+
+		/* send UA */
+		Q921SendUA(trunk,
+				trunk->sapi,
+				Q921_RESPONSE(trunk),
+				tei, pf);
+		
+		/* DL Release indication */
+		Q921Tx23Proc(trunk, Q921_DL_RELEASE, NULL, 0);
+
+		/* Stop T200 */
+		Q921T200TimerStop(trunk, tei);
+
+		if(trunk->state == Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+			/* Stop T203 */
+			Q921T203TimerStop(trunk, tei);
+		}
+
+		/* change state (no action) */
+		Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED);
+		break;
+
+	default:
+		Q921Log(trunk, Q921_LOG_ERROR, "Invalid DISC received in state \"%s\" (%d)", Q921State2Name(trunk->state), trunk->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
+ */
+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;
+	L2INT idx = tei;
+
+	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
+
+	switch(trunk->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* clear receiver peer busy */
+		Q921_CLEAR_FLAG(trunk, 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(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+
+			if(nr == trunk->vs[idx]) {
+				/* V(A) = N(R) */
+				trunk->va[idx] = nr;
+
+				/* Stop T200, restart T203 */
+				Q921T200TimerStop(trunk, tei);
+				Q921T203TimerReset(trunk, tei);
+
+			} else if(nr == trunk->va[idx]) {
+
+				/* do nothing */
+
+			} else {
+				/* V(A) = N(R) */
+				trunk->va[idx] = 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);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		/* clear receiver peer busy */
+		Q921_CLEAR_FLAG(trunk, Q921_FLAG_PEER_RECV_BUSY);
+
+		/* command + P? */
+		if(Q921_IS_COMMAND(trunk, cr) && pf) {
+			/* Enquiry response */
+			Q921SendEnquiryResponse(trunk, tei);
+		}
+
+		/* */
+		if(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+			/* V(A) = N(R) */
+			trunk->va[idx] = nr;
+
+			if(!Q921_IS_COMMAND(trunk, cr) && pf) {
+				/* Stop T200, start T203 */
+				Q921T200TimerStop(trunk, tei);
+				Q921T203TimerStart(trunk, tei);
+
+				/* Invoke retransmission */
+
+				/* change state (no action) */
+				Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED);
+			}
+			/* no state change otherwise */
+		} else {
+			/* N(R) Error recovery */
+			Q921NrErrorRecovery(trunk, tei);
 
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		}
+		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
+ */
+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;
+	L2INT idx = tei;
+
+	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
+
+	switch(trunk->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* clear receiver peer busy */
+		Q921_CLEAR_FLAG(trunk, 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(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+
+			/* V(A) = N(R) */
+			trunk->va[idx] = nr;
+
+			/* Stop T200, start T203 */
+			Q921T200TimerStop(trunk, tei);
+			Q921T203TimerStart(trunk, tei);
+
+			/* Invoke retransmission of frame >N(R)  (?) */
+
+			/* no state change */
+		} else {
+			/* N(R) Error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		/* clear receiver peer busy */
+		Q921_CLEAR_FLAG(trunk, Q921_FLAG_PEER_RECV_BUSY);
+
+		/* command + P ? */
+		if(Q921_IS_COMMAND(trunk, cr) && pf) {
+			/* Enquiry response */
+			Q921SendEnquiryResponse(trunk, tei);
+		}
+
+		/* */
+		if(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+
+			/* V(A) = N(R) */
+			trunk->va[idx] = nr;
+
+			if(!Q921_IS_COMMAND(trunk, cr) && pf) {
+				/* Stop T200, start T203 */
+				Q921T200TimerStop(trunk, tei);
+				Q921T203TimerStart(trunk, tei);
+
+				/* Invoke retransmission */
+
+				/* change state (no action) */
+				Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED);
+			}
+			/* no state change otherwise */
+		} else {
+			/* N(R) Error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		}
+		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
+ */
+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;
+	L2INT idx = tei;
+
+ 	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
+
+	switch(trunk->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* set peer receiver busy */
+		Q921_SET_FLAG(trunk, 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(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+
+			/* V(A) = N(R) */
+			trunk->va[idx] = 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);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		/* set peer receiver busy */
+		Q921_SET_FLAG(trunk, Q921_FLAG_PEER_RECV_BUSY);
+
+		/* command + P? */
+		if(Q921_IS_COMMAND(trunk, cr) && pf) {
+			/* Enquiry response */
+			Q921SendEnquiryResponse(trunk, tei);
+		}
+
+		/* */
+		if(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+
+			/* V(A) = N(R) */
+			trunk->va[idx] = nr;
+
+			if(!Q921_IS_COMMAND(trunk, cr) && pf) {
+				/* Restart T200 */
+				Q921T200TimerReset(trunk, tei);
+
+				/* Invoke retransmission */
+
+				/* change state (no action) */
+				Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED);
+			}
+			/* no state change otherwise */
+		} else {
+			/* N(R) Error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 1;
+}
+
+int Q921SetReceiverBusy(L2TRUNK trunk) {
+
+	switch(trunk->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		if(!Q921_CHECK_FLAG(trunk, Q921_FLAG_RECV_BUSY)) {
+			/* set own receiver busy */
+			Q921_SET_FLAG(trunk, Q921_FLAG_RECV_BUSY);
+
+			/* send RR response */
+			Q921SendRR(trunk, trunk->sapi, Q921_RESPONSE(trunk), trunk->tei, 0);
+
+			/* clear ack pending */
+			Q921_CLEAR_FLAG(trunk, Q921_FLAG_ACK_PENDING);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		if(!Q921_CHECK_FLAG(trunk, Q921_FLAG_RECV_BUSY)) {
+			/* set own receiver busy */
+			Q921_SET_FLAG(trunk, Q921_FLAG_RECV_BUSY);
+
+			/* send RNR response */
+			Q921SendRNR(trunk, trunk->sapi, Q921_RESPONSE(trunk), trunk->tei, 0);
+
+			/* clear ack pending */
+			Q921_CLEAR_FLAG(trunk, Q921_FLAG_ACK_PENDING);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+int Q921ClearReceiverBusy(L2TRUNK trunk) {
+
+	switch(trunk->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+	case Q921_STATE_TIMER_RECOVERY:
+		if(Q921_CHECK_FLAG(trunk, Q921_FLAG_RECV_BUSY)) {
+			/* clear own receiver busy */
+			Q921_CLEAR_FLAG(trunk, Q921_FLAG_RECV_BUSY);
+
+			/* send RNR response */
+			Q921SendRNR(trunk, trunk->sapi, Q921_RESPONSE(trunk), trunk->tei, 0);
+
+			/* clear ack pending */
+			Q921_CLEAR_FLAG(trunk, Q921_FLAG_ACK_PENDING);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+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;
+	L2INT idx = tei;
+
+	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
+
+	/* Ignore I frames in earlier states */
+	if(trunk->state < Q921_STATE_MULTIPLE_FRAME_ESTABLISHED) {
+		Q921Log(trunk, Q921_LOG_NOTICE, "I fFrame in invalid state ignored\n");
+		return 0;
+	}
+
+	/* Receiver busy? */
+	if(Q921_CHECK_FLAG(trunk, 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(trunk, Q921_FLAG_ACK_PENDING);
+		}
+	}
+	else {
+		if(ns != trunk->vr[idx]) {
+			/* discard information */
+			discard = 1;
+
+			if(Q921_CHECK_FLAG(trunk, Q921_FLAG_REJECT) && pf) {
+
+				/* Send RR response */
+				Q921SendRR(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, 1);
+
+				/* clear ack pending */
+				Q921_CLEAR_FLAG(trunk, Q921_FLAG_ACK_PENDING);
+			}
+			else if(!Q921_CHECK_FLAG(trunk, Q921_FLAG_REJECT)){
+
+				/* set reject exception */
+				Q921_SET_FLAG(trunk, Q921_FLAG_REJECT);
+
+				/* Send REJ response */
+				Q921SendREJ(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, pf);
+
+				/* clear ack pending */
+				Q921_CLEAR_FLAG(trunk, Q921_FLAG_ACK_PENDING);
+			}
+		}
+		else {
+			/* V(R) = V(R) + 1 */
+			Q921_INC_COUNTER(trunk->vr[idx]);
+
+			/* clear reject exception */
+			Q921_CLEAR_FLAG(trunk, Q921_FLAG_REJECT);
+
+			/* DL-Data indication */
+			Q921Tx23Proc(trunk, Q921_DL_DATA, mes, size);
+
+			if(pf) {
+				/* Send RR response */
+				Q921SendRR(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, 1);
+
+				/* clear ack pending */
+				Q921_CLEAR_FLAG(trunk, Q921_FLAG_ACK_PENDING);
+			}
+			else if(!Q921_CHECK_FLAG(trunk, Q921_FLAG_ACK_PENDING)) {
+				/* ack pending */
+
+				/* Send RR response */
+				Q921SendRR(trunk, trunk->sapi, Q921_RESPONSE(trunk), tei, 0);
+				
+				/* set ack pending*/
+				Q921_SET_FLAG(trunk, Q921_FLAG_ACK_PENDING);
+			}
+		}
+	}
+
+
+	switch(trunk->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		if(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+			if(Q921_CHECK_FLAG(trunk, Q921_FLAG_PEER_RECV_BUSY)) {
+				trunk->va[idx] = nr;
+			}
+			else if(nr == trunk->vs[idx]) {
+				/* V(A) = N(R) */
+				trunk->va[idx] = nr;
+
+				/* stop t200, restart t203 */
+				Q921T200TimerStop(trunk, tei);
+				Q921T203TimerReset(trunk, tei);
+			}
+			else if(nr != trunk->va[idx]) {
+				/* V(A) = N(R) */
+				trunk->va[idx] = nr;
+
+				/* restart T200 */
+				Q921T200TimerReset(trunk, tei);
+			}
+		}
+		else {
+			/* N(R) error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		}
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		if(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+			/* V(A) = N(R) */
+			trunk->va[idx] = nr;
+		}
+		else {
+			/* N(R) error recovery */
+			Q921NrErrorRecovery(trunk, tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+
+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;
+}
+
+
+
+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, 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 +2449,577 @@
 *****************************************************************************/
 int Q921Rx12(L2TRUNK trunk)
 {
-    L2UCHAR *mes;
-    L2INT rs,size;     /* receive size & Q921 frame size*/
+	L2INT size;     /* receive size & Q921 frame size*/
     L2UCHAR *smes = MFIFOGetMesPtr(trunk->HDLCInQueue, &size);
-	L2UCHAR nr;
 
-    if(smes != NULL)
+	if(smes)
     {
+		L2UCHAR sapi, tei;
+		L2UCHAR *mes;
+		L2INT rs;
+
         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;
+		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;
+
+		if(Q921_IS_PTMP_TE(trunk) && (
+			 (trunk->state >= Q921_STATE_TEI_ASSIGNED && tei != trunk->tei && tei != Q921_TEI_BCAST) ||		/* Assigned TEI: Only BCAST and directed */
+			 (trunk->state == Q921_STATE_TEI_UNASSIGNED && tei != Q921_TEI_BCAST)))					/* No assigned TEI: Only BCAST */
+		{
+			/* Ignore Messages with foreign TEIs */
+			Q921Log(trunk, Q921_LOG_DEBUG, "Ignoring incoming frame for non-local/-broadcast TEI\n");
+			goto out;
 			}
+
+		switch(mes[2] & 0x03) {
+		case 0x00:	/* I frame */
+			Q921ProcIFrame(trunk, mes, rs);
+			break;
+
+		case 0x01:	/* S frame */
+			Q921ProcSFrame(trunk, mes, rs);
+			break;
+
+		case 0x03:	/* U frame */
+			Q921ProcUFrame(trunk, mes, rs);
+			break;
+
+		default:
+			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;
 		}
 
-        /* check for I frame */
-        if((mes[2] & 0x01) == 0)
+	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;
+}
 
-			/* 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 */
+/**
+ * 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)
             {
-				Q921SendRR(trunk, (mes[0]&0xfc)>>2, (mes[0]>>1)&0x01, mes[1]>>1, mes[3]&0x01);
+	if(!trunk)
+		return;
+
+	trunk->loglevel = level;
 			}
-            else
+
+
+/**
+ * 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)
             {
-                /* 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);
+	Q921State_t oldstate = trunk->state;
+	int res = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "Changing state from \"%s\" (%d) to \"%s\" (%d)\n",
+				Q921State2Name(oldstate), oldstate,
+				Q921State2Name(state), state);
+
+	switch(oldstate) {
+	case Q921_STATE_STOPPED:
+
+		switch(state) {
+		case Q921_STATE_TEI_UNASSIGNED:
+			if(Q921_IS_PTMP_TE(trunk)) {
+				res = Q921TeiSendAssignRequest(trunk);
+			}
+			break;
+
+		case Q921_STATE_TEI_ASSIGNED:
+			if(Q921_IS_PTMP_NT(trunk)) {
+				res = Q921TeiSendRemoveRequest(trunk, Q921_TEI_BCAST);
+			}
+			break;
+
+		default:
+			break;
            }
+		break;
+
+
+	case Q921_STATE_TEI_AWAITING:
+	case Q921_STATE_TEI_ESTABLISH:
+	case Q921_STATE_TEI_UNASSIGNED:
+
+		switch(state) {
+		case Q921_STATE_TEI_ASSIGNED:
+			/* establish data link */
+			if(Q921_IS_PTMP_TE(trunk)) {		/* TODO: bad hack, Layer3 is responsible for requesting DL establishment */
+				res = Q921EstablishDataLink(trunk, trunk->tei);
+
+				/* go directly into AWAITING ESTABLISHMENT state */
+				state = Q921_STATE_AWAITING_ESTABLISHMENT;
+
+				Q921Log(trunk, Q921_LOG_DEBUG, "Forcing \"%s\" (%d) state\n", Q921State2Name(state), state);
+			}
+			break;
+
+		default:
+			break;
         }
+		break;
 
-        /* check for RR */
-        else if(mes[2] ==0x01)
+
+	case Q921_STATE_TEI_ASSIGNED:
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+	case Q921_STATE_AWAITING_RELEASE:
+		/* do nothing */
+		break;
+
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+	case Q921_STATE_TIMER_RECOVERY:
+
+		/*  auto-reconnect data-layer link */
+		switch(state) {
+		case Q921_STATE_TEI_ASSIGNED:
+			/* establish data link */
+			if(Q921_IS_PTMP_TE(trunk)) {		/* TODO: bad hack, Layer3 is responsible for requesting DL establishment */
+				res = Q921EstablishDataLink(trunk, trunk->tei);
+
+				/* go directly into AWAITING ESTABLISHMENT state */
+				state = Q921_STATE_AWAITING_ESTABLISHMENT;
+
+				Q921Log(trunk, Q921_LOG_DEBUG, "Forcing \"%s\" (%d) state\n", Q921State2Name(state), state);
+			}
+			break;
+
+		default:
+			break;
+		}
+		break;
+	}
+
+	trunk->state = state;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "Q921ChangeState() returns %d, new state is \"%s\" (%d)\n", res, Q921State2Name(state), state);
+
+	return res;
+}
+
+/*
+ * TEI Management functions
+ */
+static int Q921TeiSend(L2TRUNK trunk, L2UCHAR type, L2USHORT ri, L2UCHAR ai)
 		{
-			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);
+	L2UCHAR mes[10];
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+	L2UCHAR cr;
+
+	switch(type) {
+	case Q921_TEI_ID_CHECKRESP:
+	case Q921_TEI_ID_DENIED:
+	case Q921_TEI_ID_ASSIGNED:
+		cr = Q921_RESPONSE(trunk);
+		break;
+
+	default:
+		cr = Q921_COMMAND(trunk);
+		break;
 			}
 
+	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, cr, Q921_TEI_BCAST, 0, 0x00, mes, offset);
         }
 		
-        /* check for RNR */
-        /* check for REJ */
-        /* check for SABME */
-        else if((mes[2] & 0xef) == 0x6f)
+
+/**
+ * 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)
         {
-            Q921ProcSABME(trunk, mes, rs);
-            Q921SendUA(trunk, (mes[0]&0xfc)>>2, (mes[0]>>1)&0x01,mes[1]>>1, (mes[2]&0x10)>>4);
+	L2INT res;
+
+	if (!Q921_IS_PTMP_TE(trunk))	/* only ptmp te mode*/
+		return 0;
+
+	trunk->ri[0] = (L2USHORT)(random() % 0xffff);
+
+	/* send TEI assign request */
+	res = Q921TeiSend(trunk, Q921_TEI_ID_REQUEST, trunk->ri[0], Q921_TEI_BCAST);
+
+	/* start T202 */
+	Q921T202TimerStart(trunk);
+
+	return res;
         }
 
-        /* check for DM */
-        /* check for UI */
-        /* check for DISC */
-        /* check for UA */
-        /* check for FRMR */
-        /* check for XID */
 
-        else
+/**
+ * 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)
         {
-            /* what the ? Issue an error */
-			/* Q921ErrorProc(trunk, Q921_UNKNOWNFRAME, mes, rs); */
-            /* todo: REJ or FRMR */
+	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 != trunk->ri[0]) {
+		/* hmmm ..., not our response i guess */
+		return 0;
         }
 
-        MFIFOKillNext(trunk->HDLCInQueue);
+	switch(mes[offset + 3]) {
+	case Q921_TEI_ID_ASSIGNED:
+		/* Yay, use the new TEI and change state to assigned */
+		trunk->tei      = mes[offset + 4] >> 1;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "Assigned TEI %d, setting state to TEI_ASSIGNED\n", trunk->tei);
+
+		Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED);
+		break;
+
+	case Q921_TEI_ID_DENIED:
+		/* oops, what to do now? */
+		if ((mes[offset + 4] >> 1) == 127) {
+			/* 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);
+		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)
+{
+	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, trunk->ri[0], trunk->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)
+{
+	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 == trunk->tei) {
+		/*
+		 * Broadcast TEI check or for our assigned TEI
+		 */
+
+		/* send TEI check reponse */
+		res = Q921TeiSend(trunk, Q921_TEI_ID_CHECKRESP, trunk->ri[0], trunk->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)
+{
+	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 == trunk->tei) {
+		/*
+		 * Broadcast TEI remove or for our assigned TEI
+		 */
+
+		/* reset tei */
+		trunk->tei = 0;
+
+		/* change state (no action) */
+		Q921ChangeState(trunk, Q921_STATE_TEI_UNASSIGNED);
+
+		/* 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)
+{
+	if (!Q921_IS_PTMP_NT(trunk))	/* PTMP NT only */
+		return 0;
+
+	/* todo: process message */
+	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
+ */
+#ifdef __UNUSED__
+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);
+
+	/* Start NT timer, todo: this one is probably per TEI too */
+	Q921T201TimerStart(trunk, tei);
+
+	return res;
+}
+#endif
+
+/**
+ * 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)
+{
+	L2UCHAR resp[25];
+	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;
+
+	/* todo: handle response... check assigned TEI(s) */
+	resp[offset + 0] = 0;
+
+	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
+ */
+#ifdef __UNUSED__
+static int Q921TeiSendDenyResponse(L2TRUNK trunk, L2UCHAR tei, L2USHORT ri)
+{
+	L2INT res = 0;
+
+	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);
+}
+#endif
+
+/**
+ * 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);
+}
Index: src/isdn/Q931.c
===================================================================
--- src/isdn/Q931.c	(revision 495)
+++ src/isdn/Q931.c	(working copy)
@@ -36,6 +36,7 @@
   POSSIBILITY OF SUCH DAMAGE.
 *****************************************************************************/
 
+#include "Q921.h"
 #include "Q931.h"
 #include "national.h"
 #include "DMS.h"
@@ -93,8 +94,7 @@
 
 *****************************************************************************/
 
-Q931ErrorCB_t Q931ErrorProc;
-                                    /* callback for error messages.         */
+Q931ErrorCB_t Q931ErrorProc;			/* callback for error messages.         */
 
 L3ULONG (*Q931GetTimeProc) (void) = NULL; /* callback for func reading time in ms */
 
@@ -331,15 +331,21 @@
                 see q931errors.h for details.
 
 *****************************************************************************/
-L3INT Q931Rx23(Q931_TrunkInfo_t *pTrunk, L3UCHAR * buf, L3INT Size)
+L3INT Q931Rx23(Q931_TrunkInfo_t *pTrunk, L3INT ind, L3UCHAR * buf, L3INT Size)
 {
-    L3UCHAR *Mes = &buf[Q931L2HeaderSpace];
+	L3UCHAR *Mes = buf;
     L3INT RetCode = Q931E_NO_ERROR;
     Q931mes_Generic *m = (Q931mes_Generic *) pTrunk->L3Buf;
     L3INT ISize;
+	L3INT IOff = Q931L2HeaderSpace;
 
-    L3INT IOff = 0;
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Received message from Q.921 (%d) size: %d\n", ind, Size);
 
+	switch(ind) {
+	case Q921_DL_UNIT_DATA:		/* DL-UNITDATA indication (UI frame, 3 byte header) */
+		IOff = 3;
+
+	case Q921_DL_DATA:		/* DL-DATA indication (I frame, 4 byte header) */
     /* Protocol Discriminator */
     m->ProtDisc = Mes[IOff++];
 
@@ -350,15 +356,17 @@
     /* 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);
+		RetCode = Q931Umes[pTrunk->Dialect][m->MesType](pTrunk, Mes, (Q931mes_Generic *)pTrunk->L3Buf, IOff, Size - IOff + 1);
 
     if(RetCode >= Q931E_NO_ERROR)
     {
         RetCode = Q931Proc[pTrunk->Dialect][m->MesType](pTrunk, pTrunk->L3Buf, 2);
     }
+		break;
+
+	default:
+		break;
+	}
 
     return RetCode;
 }
@@ -378,6 +386,8 @@
 *****************************************************************************/
 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);
     }
@@ -401,18 +411,17 @@
 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)\n", Size);
 
-    return RetCode;
+	return Q931Proc[pTrunk->Dialect][ptr->MesType](pTrunk,buf,4);;
 }
 
 /*****************************************************************************
 
-  Function:     Q931Tx32
+  Function:     Q931Tx32Data
 
-  Description:  Called from the stack to send a message to L2. The input is 
+  Description:  Called from the stack to send a DL-DATA message to L2. The input is 
                 always a non-packed message so it will first make a proper
                 call to create a packed message before it transmits that
                 message to layer 2.
@@ -425,19 +434,21 @@
                 see q931errors.h for details.
 
 *****************************************************************************/
-L3INT Q931Tx32(Q931_TrunkInfo_t *pTrunk, L3UCHAR * Mes, L3INT Size)
+L3INT Q931Tx32Data(Q931_TrunkInfo_t *pTrunk, L3UCHAR * Mes, L3INT Size)
 {
-    L3INT     OSize;
     Q931mes_Generic *ptr = (Q931mes_Generic*)Mes;
     L3INT RetCode = Q931E_NO_ERROR;
     L3INT iDialect = pTrunk->Dialect;
+	L3INT OSize;
+
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Sending DL-DATA request message to Q.921 (size: %d)\n", Size);
 
     /* 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);
+			RetCode = pTrunk->Q931Tx32CBProc(pTrunk->PrivateData32, Q921_DL_DATA, pTrunk->L2Buf, OSize + Q931L2HeaderSpace);
         } else {
             RetCode = Q931E_MISSING_CB;
         }
@@ -446,6 +457,32 @@
     return RetCode;
 }
 
+/**
+ * Q931Tx32UnitData
+ * \brief	Used to send a DL-UNIT DATA Request to Layer2
+ */
+L3INT Q931Tx32UnitData(Q931_TrunkInfo_t *pTrunk, L3UCHAR * Mes, L3INT Size)
+{
+	Q931mes_Generic *ptr = (Q931mes_Generic*)Mes;
+	L3INT RetCode = Q931E_NO_ERROR;
+	L3INT iDialect = pTrunk->Dialect;
+	L3INT OSize;
+
+	Q931Log(pTrunk, Q931_LOG_DEBUG, "Sending DL-UNIT DATA request message to Q.921 (size: %d)\n", Size);
+
+	/* Call pack function through table. */
+	RetCode = Q931Pmes[iDialect][ptr->MesType](pTrunk, (Q931mes_Generic *)Mes, Size, &pTrunk->L2Buf[Q931L2HeaderSpace-1], &OSize);
+	if(RetCode >= Q931E_NO_ERROR)
+	{
+		if (pTrunk->Q931Tx32CBProc) {
+			RetCode = pTrunk->Q931Tx32CBProc(pTrunk->PrivateData32, Q921_DL_UNIT_DATA, pTrunk->L2Buf, OSize + Q931L2HeaderSpace - 1);
+		} else {
+			RetCode = Q931E_MISSING_CB;
+		}
+	}
+
+	return RetCode;
+}
 
 /*****************************************************************************
 
@@ -757,3 +794,51 @@
 }
 
 
+/*
+ * 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;
+	}
+
+	va_end(ap);
+
+	return trunk->Q931LogCBProc(trunk->PrivateDataLog, level, buf, strnlen(buf, sizeof(buf)-1));
+}
+
+/**
+ * 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;
+}
Index: src/include/openzap.h
===================================================================
--- src/include/openzap.h	(revision 495)
+++ src/include/openzap.h	(working copy)
@@ -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)
 
Index: src/include/zap_types.h
===================================================================
--- src/include/zap_types.h	(revision 495)
+++ src/include/zap_types.h	(working copy)
@@ -117,11 +117,12 @@
 	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 {
