Index: src/isdn/Q921.c
===================================================================
--- src/isdn/Q921.c	(revision 495)
+++ src/isdn/Q921.c	(working copy)
@@ -37,13 +37,245 @@
   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 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 */		
+		trunk->vs[idx] = (trunk->vs[idx] ? trunk->vs[idx] - 1 : 127);
+
+		/* <- I Frame queued up (move I frame to tx queue? */
+
+		/* next frame in queue (backtrack along I queue) ??? */
+	}
+#endif
+	return 1;
+}
+
+
 /*****************************************************************************
 
   Function:     Q921_InitTrunk
@@ -56,45 +288,384 @@
 					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 */
+
+	if(!Q921_IS_PTMP_NT(trunk)) {
+		trunk->T200[0] = 0;
+		trunk->T203[0] = 0;
+		trunk->T202 = 0;
+
+		trunk->N200[0] = 0;
+		trunk->N202 = 0;
+
+		trunk->ri[0] = 0;
+		trunk->va[0] = 0;
+		trunk->vs[0] = 0;	
+		trunk->vr[0] = 0;
+	}
+
+	if(Q921_IS_PTMP_TE(trunk)) {
+		/*
+		 * Request new TEI from Master
+		 */
+		trunk->tei = 0;
+	}
+
+	if(Q921_IS_PTMP_NT(trunk)) {
+		int x;
+
+		/*
+		 * Clear list of assigned TEIs, send TEI reset command
+		 */
+		trunk->tei = 0;
+
+		for(x=0; x<Q921_TEI_MAX; x++) {
+			trunk->T200[x] = 0;
+			trunk->T203[x] = 0;
+			trunk->T201[x] = 0;
+
+			trunk->N200[x] = 0;
+
+			trunk->ri[x] = 0;
+			trunk->va[x] = 0;
+			trunk->vs[x] = 0;	
+			trunk->vr[x] = 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, Msg, 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, ...)
+{
+	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   = (cr == (Q921_IS_TE(trunk) ? received : !received));
+
+		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)\n", Q921State2Name(trunk->state), trunk->state);
+		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));
+}
+
 /*****************************************************************************
 
   Function:     Q921TimeTick
@@ -110,78 +681,367 @@
 
 L2ULONG Q921GetTime(void)
 {
-    L2ULONG tNow = 0;
-    if(Q921GetTimeProc != NULL)
-    {
-        tNow = Q921GetTimeProc();
-        if(tNow < tLast)    /* wrapped */
-        {
+	L2ULONG tNow = 0;
+
+	if(Q921GetTimeProc)
+	{
+		tNow = Q921GetTimeProc();
+		if(tNow < tLast)	/* wrapped */
+		{
 			/* TODO */
-        }
+		}
 		tLast = tNow;
-    }
-    return tNow;
+	}
+	return tNow;
 }
 
-void Q921T200TimerStart(L2TRUNK trunk)
+/*
+ * T200 handling (per-TEI in PTMP NT mode, tei=0 otherwise)
+ */
+void Q921T200TimerStart(L2TRUNK trunk, L2UCHAR tei)
 {
-	if (!trunk->T200) {
-		trunk->T200 = Q921GetTime() + trunk->T200Timeout;
+	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)
+void Q921T200TimerStop(L2TRUNK trunk, L2UCHAR tei)
 {
-	trunk->T200 = 0;
+	if(!Q921_IS_PTMP_NT(trunk))
+		tei = 0;
+
+	trunk->T200[tei] = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T200 stopped\n");
 }
 
-void Q921T200TimerReset(L2TRUNK trunk)
+void Q921T200TimerReset(L2TRUNK trunk, L2UCHAR tei)
 {
-	Q921T200TimerStop(trunk);
-	Q921T200TimerStart(trunk);
+	if(!Q921_IS_PTMP_NT(trunk))
+		tei = 0;
+
+	trunk->T200[tei] = Q921GetTime() + trunk->T200Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T200 restarted\n");
 }
 
-void Q921T203TimerStart(L2TRUNK trunk)
+/*
+ * T203 handling (per-TEI in PTMP NT mode, tei=0 otherwise)
+ */
+void Q921T203TimerStart(L2TRUNK trunk, L2UCHAR tei)
 {
-	if (!trunk->T203) {
-		trunk->T203 = Q921GetTime() + trunk->T203Timeout;
+	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)
+void Q921T203TimerStop(L2TRUNK trunk, L2UCHAR tei)
 {
-	trunk->T203 = 0;
+	if(!Q921_IS_PTMP_NT(trunk))
+		tei = 0;
+
+	trunk->T203[tei] = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T203 stopped\n");
 }
 
-void Q921T203TimerReset(L2TRUNK trunk)
+void Q921T203TimerReset(L2TRUNK trunk, L2UCHAR tei)
 {
-	Q921T203TimerStop(trunk);
-	Q921T203TimerStart(trunk);
+	if(!Q921_IS_PTMP_NT(trunk))
+		tei = 0;
+
+	trunk->T203[tei] = Q921GetTime() + trunk->T203Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T203 restarted\n");
 }
 
-void Q921T200TimerExpire(L2TRUNK trunk)
+/*
+ * T202 handling (TEI message timeout, TE mode only)
+ */
+void Q921T202TimerStart(L2TRUNK trunk)
 {
-	(void)trunk;
+	if (!trunk->T202) {
+		trunk->T202 = Q921GetTime() + trunk->T202Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "T202 started\n");
+	}
 }
 
-void Q921T203TimerExpire(L2TRUNK trunk)
+void Q921T202TimerStop(L2TRUNK trunk)
 {
-	Q921T203TimerReset(trunk);
-	Q921SendRR(trunk, trunk->sapi, trunk->NetUser == Q921_TE ? 0 : 1, trunk->tei, 1);
+	trunk->T202 = 0;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T202 stopped\n");
 }
 
+void Q921T202TimerReset(L2TRUNK trunk)
+{
+	trunk->T202 = Q921GetTime() + trunk->T202Timeout;
+
+	Q921Log(trunk, Q921_LOG_DEBUG, "T202 restarted\n");
+}
+
+/*
+ * T201 handling (TEI management (NT side), per-TEI)
+ */
+void Q921T201TimerStart(L2TRUNK trunk, L2UCHAR tei)
+{
+	if (!trunk->T201[tei]) {
+		trunk->T201[tei] = Q921GetTime() + trunk->T201Timeout;
+
+		Q921Log(trunk, Q921_LOG_DEBUG, "T201 started\n");
+	}
+}
+
+void Q921T201TimerStop(L2TRUNK trunk, L2UCHAR tei)
+{
+	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");
+}
+
+
+/*
+ * Expiry callbacks
+ */
+void Q921T200TimerExpire(L2TRUNK trunk, L2UCHAR tei)
+{
+	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 */
+
+			/* 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 */
+			trunk->vs[idx] = (trunk->vs[idx] ? trunk->vs[idx]-1 : 127);
+
+			/* 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 */
+				trunk->vs[idx] = (trunk->vs[idx] ? trunk->vs[idx]-1 : 127);
+
+				/* retrans 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);
+			}
+
+			/* increment counter */
+			trunk->N200[idx]++;
+
+			/* no state change */
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+void Q921T203TimerExpire(L2TRUNK trunk, L2UCHAR tei)
+{
+	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 Q921T202TimerExpire(L2TRUNK 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);
+
+		trunk->N202++;
+	}
+}
+
+void Q921T201TimerExpire(L2TRUNK trunk, L2UCHAR tei)
+{
+	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->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 (trunk->T203 && tNow > trunk->T203) {
-		Q921T203TimerExpire(trunk);		
+
+	if(Q921_IS_PTMP_TE(trunk)) {
+		if (trunk->T202 && tNow > trunk->T202) {
+			Q921T202TimerExpire(trunk);
+		}
 	}
 }
 
-
 void Q921SetGetTimeCB(L2ULONG (*callback)(void))
 {
     Q921GetTimeProc = callback;
@@ -232,24 +1092,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);
-    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++;
+	L2INT idx = (Q921_IS_PTMP_NT(trunk)) ? Tei : 0;
 
-    return Q921Tx21Proc(trunk, mes, size);
+#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 Q921Rx32(L2TRUNK trunk, L2UCHAR * Mes, L2INT Size)
+int Q921SendS(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR sv, L2UCHAR *mes, L2INT size)
 {
-	return Q921SendI(trunk, 
+	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] = ((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, Q921DLMsg_t ind, L2UCHAR * Mes, L2INT Size)
+{
+	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,
-					trunk->tei, 
+					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 +1234,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 +1256,9 @@
 *****************************************************************************/
 int Q921SendRNR(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)0x05;
-    mes[trunk->Q921HeaderSpace+3] = (L2UCHAR)((trunk->vr<<1) | (pf & 0x01));
-
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+4);
+	return Q921SendS(trunk, Sapi, cr, Tei, pf, 0x01, mes, trunk->Q921HeaderSpace+4);
 }
 
 /*****************************************************************************
@@ -323,14 +1278,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 +1300,95 @@
 *****************************************************************************/
 int Q921SendSABME(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
 {
-    L2UCHAR mes[400];
+	L2UCHAR mes[25];
 
-    mes[trunk->Q921HeaderSpace+0] = (L2UCHAR)((Sapi&0xfc) | ((cr<<1)&0x02));
-    mes[trunk->Q921HeaderSpace+1] = (L2UCHAR)((Tei<<1) | 0x01);
-    mes[trunk->Q921HeaderSpace+2] = (L2UCHAR)(0x6f | ((pf<<4)&0x10));
-
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+3);
+	return Q921SendU(trunk, Sapi, cr, Tei, pf, 0x0f, mes, trunk->Q921HeaderSpace+3);
 }
 
+
+/**
+ * Q921Start
+ * \brief	Start trunk
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \return	> 0 on success; <= 0 on error
+ */
 int Q921Start(L2TRUNK trunk)
 {
-	return Q921SendSABME(trunk, 
+	if(!trunk->initialized == INITIALIZED_MAGIC)
+		return 0;
+
+	if(Q921_IS_PTMP_TE(trunk))
+		trunk->state = Q921_STATE_TEI_UNASSIGNED;
+	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, 
-					trunk->NetUser == Q921_TE ? 0 : 1,
+					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);
+
+	/* 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 +1401,9 @@
 *****************************************************************************/
 int Q921SendDM(L2TRUNK trunk, int Sapi, int cr, int Tei, int pf)
 {
-    L2UCHAR mes[400];
+	L2UCHAR mes[25];
 
-    mes[trunk->Q921HeaderSpace+0] = (L2UCHAR)((Sapi&0xfc) | ((cr<<1)&0x02));
-    mes[trunk->Q921HeaderSpace+1] = (L2UCHAR)((Tei<<1) | 0x01);
-    mes[trunk->Q921HeaderSpace+2] = (L2UCHAR)(0x0f | ((pf<<4)&0x10));
-
-    return Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+3);
+	return Q921SendU(trunk, Sapi, cr, Tei, pf, 0x03, mes, trunk->Q921HeaderSpace+3);
 }
 
 /*****************************************************************************
@@ -411,13 +1423,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 +1445,893 @@
 *****************************************************************************/
 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;
+	L2UCHAR pf = (mes[2] & 0x10) >> 4;				/* poll / final flag */
+	L2UCHAR tei = (mes[1] & 0xfe) >> 1;
+	L2INT   idx = tei;
 
-    trunk->vr=0;
-    trunk->vs=0;
-	trunk->va=0;
+	if(!Q921_IS_PTMP_NT(trunk))
+		idx = 0;
 
-    return 1;
+	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 */
+
+		/* 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 */
+		}
+
+		/* 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 */
+
+			/* 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(1) {	/* layer3 initiated */
+				trunk->vr[idx] = 0;
+
+				/* DL-Establish confirm */
+
+			} else if(trunk->vs[idx] != trunk->va[idx]) {
+
+				/* discard I queue */
+				MFIFOClear(trunk->IFrameQueue);
+
+				/* DL-Establish indication */
+			}
+
+			/* 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 */
+
+			/* 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 */
+
+		/* 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;
+	L2UCHAR command = (cr == (trunk->NetUser == Q921_TE ? 1 : 0)) ? 1 : 0;
+	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 (command) { /* 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(command && pf) {
+			/* Enquiry response */
+			Q921SendEnquiryResponse(trunk, tei);
+		}
+
+		/* */
+		if(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+			/* V(A) = N(R) */
+			trunk->va[idx] = nr;
+
+			if(!command && 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;
+	L2UCHAR command = (cr == (trunk->NetUser == Q921_TE ? 1 : 0)) ? 1 : 0;
+	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(command) {
+			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(command && pf) {
+			/* Enquiry response */
+			Q921SendEnquiryResponse(trunk, tei);
+		}
+
+		/* */
+		if(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+
+			/* V(A) = N(R) */
+			trunk->va[idx] = nr;
+
+			if(!command && 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;
+	L2UCHAR command = (cr == (trunk->NetUser == Q921_TE ? 1 : 0)) ? 1 : 0;
+	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(command) {
+			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(command && pf) {
+			/* Enquiry response */
+			Q921SendEnquiryResponse(trunk, tei);
+		}
+
+		/* */
+		if(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+
+			/* V(A) = N(R) */
+			trunk->va[idx] = nr;
+
+			if(!command && 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 */
+			trunk->vr[idx] = (trunk->vr[idx] + 1) % 128;
+
+			/* 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;
+	}
+
+#if 0
+			/* we increment before we "really" know its good so that if we send in the callback, we use the right nr */
+			trunk->vr[idx] = (trunk->vr[idx] + 1) % 128;
+
+			if(Q921Tx23Proc(trunk, Q921_DL_DATA, smes, rs)) /* -2 to clip away CRC */
+			{
+				Q921SendRR(trunk, sapi, cr, tei, pf);
+			}
+			else
+			{
+	                	/* todo: whatever */
+				trunk->vr[idx] = (trunk->vr[idx] ? trunk->vr[idx] - 1 : 127);
+
+				/* for now, lets just respond for easier debugging on errors */
+				Q921SendRR(trunk, sapi, cr, tei, pf);
+			}
+#endif
+	return 0;
+}
+
+
 /*****************************************************************************
 
   Function:     Q921Rx12
@@ -479,80 +2352,774 @@
 *****************************************************************************/
 int Q921Rx12(L2TRUNK trunk)
 {
-    L2UCHAR *mes;
-    L2INT rs,size;     /* receive size & Q921 frame size*/
-    L2UCHAR *smes = MFIFOGetMesPtr(trunk->HDLCInQueue, &size);
-	L2UCHAR nr;
+	L2INT size;     /* receive size & Q921 frame size*/
+	L2UCHAR *smes = MFIFOGetMesPtr(trunk->HDLCInQueue, &size);
 
-    if(smes != NULL)
-    {
-        rs = size - trunk->Q921HeaderSpace;
-        mes = &smes[trunk->Q921HeaderSpace];
+	if(smes)
+	{
+		L2UCHAR sapi, tei, idx, cr;
+		L2UCHAR *mes;
+		L2INT rs;
 
-		if ((mes[2] & 3) != 3) {
+		rs  = size - trunk->Q921HeaderSpace;
+		mes = &smes[trunk->Q921HeaderSpace];
+		idx = 0;
+
+		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;
+		cr   = (mes[0] & 0x02) >> 1;
+		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 */
+			goto out;
+		}
+
+		/* PTMP NT needs per-TEI access to counters */
+		if(Q921_IS_PTMP_NT(trunk)) {
+			idx = tei;
+		}
+
+#if 0
+		/* counter handling */
+		if ((mes[2] & 0x03) != 0x03) {
+			L2UCHAR nr;
+
+			if(!Q921_IS_READY(trunk)) {
+				/* Don't handle S and I frames if L2 is not ready (PTMP TE mode only) (???) */
+				goto out;
+			}
+
 			/* 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;
+			if (nr >= trunk->va[idx] && nr <= trunk->vs[idx]) {
+				trunk->va[idx] = nr;
 			}
 		}
+#endif
 
-        /* check for I frame */
-        if((mes[2] & 0x01) == 0)
-        {
+		/* check for I frame */
+		if((mes[2] & 0x01) == 0x00)
+		{
+			Q921ProcIFrame(trunk, mes, rs);
+		}
+		/* check for S frame */
+		else if((mes[2] & 0x03) == 0x01)
+		{
+			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 */
 
-			/* we increment before we "really" know its good so that if we send in the callback, we use the right nr */
-			trunk->vr++;
-            if(Q921Tx23Proc(trunk, smes, size-2)) /* -2 to clip away CRC */
-            {
-				Q921SendRR(trunk, (mes[0]&0xfc)>>2, (mes[0]>>1)&0x01, mes[1]>>1, mes[3]&0x01);
+			switch(sv) {
+			case 0x00:	/* RR : Receive Ready */
+				Q921ProcRR(trunk, mes, rs);
+				break;
+
+			case 0x02:	/* RNR : Receive Not Ready */
+				Q921ProcRNR(trunk, mes, rs);
+				break;
+
+			case 0x04:	/* REJ : Reject */
+				Q921ProcREJ(trunk, mes, rs);
+				break;
+
+			default:	/* Invalid / Unknown */
+				break;
 			}
-            else
-            {
-                /* todo: whatever*/
-				/* trunk->vr--; */
-				/* for now, lets just respond for easier debugging on errors */
-				Q921SendRR(trunk, (mes[0]&0xfc)>>2, (mes[0]>>1)&0x01, mes[1]>>1, mes[3]&0x01);
-           }
-        }
+		}
 
-        /* check for RR */
-        else if(mes[2] ==0x01)
+		/* check for U frame */
+		else if((mes[2] & 0x03) == 0x03)
 		{
-			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 m  = (mes[2] & 0xe0) >> 3 | (mes[2] & 0x0c) >> 2;	/* modifier function id */
+//			L2UCHAR pf = (mes[2] & 0x10) >> 4;				/* poll / final flag */
 
-        }
-		
-        /* check for RNR */
-        /* check for REJ */
-        /* check for SABME */
-        else if((mes[2] & 0xef) == 0x6f)
-        {
-            Q921ProcSABME(trunk, mes, rs);
-            Q921SendUA(trunk, (mes[0]&0xfc)>>2, (mes[0]>>1)&0x01,mes[1]>>1, (mes[2]&0x10)>>4);
-        }
+			switch(m) {
+			case 0x00:	/* UN : Unnumbered Information */
+				if(mes[3] == Q921_LAYER_ENT_ID_TEI)
+				{
+					if(!Q921_IS_PTMP(trunk)) {
+						/* wtf? nice try */
+						goto out;
+					}
 
-        /* check for DM */
-        /* check for UI */
-        /* check for DISC */
-        /* check for UA */
-        /* check for FRMR */
-        /* check for XID */
+					switch(mes[6]) {
+					case Q921_TEI_ID_REQUEST:	/* (TE ->) NT */
+						Q921TeiProcAssignRequest(trunk, mes, size);
+						break;
 
-        else
-        {
-            /* what the ? Issue an error */
+					case Q921_TEI_ID_ASSIGNED:	/* (NT ->) TE */
+					case Q921_TEI_ID_DENIED:
+						Q921TeiProcAssignResponse(trunk, mes, size);
+						break;
+
+					case Q921_TEI_ID_CHECKREQ:	/* (NT ->) TE */
+						Q921TeiProcCheckRequest(trunk, mes, size);
+						break;
+
+					case Q921_TEI_ID_CHECKRESP:	/* (TE ->) NT */
+						Q921TeiProcCheckResponse(trunk, mes, size);
+						break;
+
+					case Q921_TEI_ID_REMOVE:	/* (NT ->) TE */
+						Q921TeiProcRemoveRequest(trunk, mes, size);
+						break;
+
+					case Q921_TEI_ID_VERIFY:	/* (TE ->) NT */
+						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");
+
+					Q921Tx23Proc(trunk, Q921_DL_UNIT_DATA, smes, rs);
+				}
+				break;
+
+			case 0x03:	/* DM : Disconnect Mode */
+				Q921ProcDM(trunk, mes, rs);
+				break;
+
+			case 0x08:	/* DISC : Disconnect */
+				Q921ProcDISC(trunk, mes, rs);
+				break;
+
+			case 0x0c:	/* UA : Unnumbered Acknowledgement */
+				Q921ProcUA(trunk, mes, rs);
+				break;
+
+			case 0x0f:	/* SABME  : Set Asynchronous Balanced Mode Extend */
+				Q921ProcSABME(trunk, mes, rs);
+				break;
+
+			case 0x11:	/* FRMR : Frame Reject */
+				break;
+
+			case 0x17:	/* XID : Exchange Identification */
+				break;
+
+			default:	/* Unknown / Invalid */
+				Q921Log(trunk, Q921_LOG_ERROR, "Invalid U frame type: %d\n", m);
+				break;
+			}
+		}
+		/* unknown / invalid */
+		else
+		{
+			/* what the ? Issue an error */
 			/* Q921ErrorProc(trunk, Q921_UNKNOWNFRAME, mes, rs); */
-            /* todo: REJ or FRMR */
-        }
+			/* todo: REJ or FRMR */
 
-        MFIFOKillNext(trunk->HDLCInQueue);
+			Q921Log(trunk, Q921_LOG_ERROR, "Received frame is neither I, S nor U type\n");
+		}
 
-        return 1;
-    }
-    return 0;
+out:
+		MFIFOKillNext(trunk->HDLCInQueue);
+
+		return 1;
+	}
+
+	return 0;
 }
 
+/*
+ * Misc
+ */
+
+#if 0
+/**
+ * Q921SetLayer3NotifyCB
+ * \brief	Set notification callback for Layer 3
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	func	pointer to notification callback function
+ * \param[in]	priv	pointer to private data
+ *
+ * \author	Stefan Knoblich
+ */
+void Q921SetLayer3NotifyCB(L2TRUNK trunk, Q921NotifyCB_t func, void *priv)
+{
+	if(!trunk)
+		return;
+
+	trunk->Q921Notify23Proc = func;
+	trunk->PrivateDataNotify23 = priv;
+}
+#endif
+
+/**
+ * Q921SetLogCB
+ * \brief	Set logging callback
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	func	pointer to logging callback function
+ * \param[in]	priv	pointer to private data
+ *
+ * \author	Stefan Knoblich
+ */
+void Q921SetLogCB(L2TRUNK trunk, Q921LogCB_t func, void *priv)
+{
+	if(!trunk)
+		return;
+
+	trunk->Q921LogProc = func;
+	trunk->PrivateDataLog = priv;
+}
+
+/**
+ * Q921SetLogLevel
+ * \brief	Set loglevel of Q.921 logging functions
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	level	new loglevel
+ *
+ * \author	Stefan Knoblich
+ */
+void Q921SetLogLevel(L2TRUNK trunk, Q921LogLevel_t level)
+{
+	if(!trunk)
+		return;
+
+	trunk->loglevel = level;
+}
+
+
+/**
+ * Q921ChangeState
+ * \brief	Change state, invoke neccessary actions
+ * \param[in]	trunk	pointer to Q921 data struct
+ * \param[in]	state	state to change to
+ * \return	> 0 on success; <= 0 on error
+ *
+ * \author	Stefan Knoblich
+ */
+static int Q921ChangeState(L2TRUNK trunk, Q921State_t state)
+{
+	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;
+
+
+	case Q921_STATE_TEI_ASSIGNED:
+		break;
+
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+		break;
+
+	case Q921_STATE_AWAITING_RELEASE:
+		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
+ */
+
+/**
+ * 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)
+{
+	L2UCHAR mes[25];
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+
+	if (!Q921_IS_PTMP_TE(trunk)) {
+		/* only ptmp te mode*/
+		return 0;
+	}
+
+	trunk->ri[0] = (L2USHORT)(random() % 0xffff);
+
+	mes[offset + 0] = Q921_LAYER_ENT_ID_TEI;	/* layer management entity identifier */
+	mes[offset + 1] = (trunk->ri[0] & 0xff00) >> 8;	/* reference number upper part */
+	mes[offset + 2] =  trunk->ri[0] & 0xff;		/* reference number lower part */
+	mes[offset + 3] = Q921_TEI_ID_REQUEST;		/* message type: Identity Request */
+	mes[offset + 4] = Q921_TEI_BCAST << 1 | 0x01;	/* action indicator: request dynamic TEI */
+
+	/* start T202 */
+	Q921T202TimerStart(trunk);
+
+	return Q921SendU(trunk, Q921_SAPI_TEI, Q921_COMMAND(trunk), Q921_TEI_BCAST, 0, 0x00, mes, Q921_UFRAME_DATA_OFFSET(trunk) + 5);	/* TEI Assignment Request UN Frame */
+}
+
+
+/**
+ * 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)
+{
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+	L2USHORT ri = 0;
+
+	if (!Q921_IS_PTMP_TE(trunk)) {
+		/* does not make sense */
+		return 0;
+	}
+
+	ri = (mes[offset + 1] << 8) | mes[offset + 2];
+
+	if (ri != trunk->ri[0]) {
+		/* hmmm ..., not our response i guess */
+		return 0;
+	}
+
+	switch(mes[offset + 3]) {
+	case Q921_TEI_ID_ASSIGNED:
+		/* Yay, use the new TEI and change state to assigned */
+		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)
+{
+	L2UCHAR mes[25];
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+
+	if (!Q921_IS_PTMP_TE(trunk)) {
+		/* only ptmp te mode*/
+		return 0;
+	}
+
+	/* Request running? */
+	if (trunk->T202) {
+		return 0;
+	}
+
+	mes[offset + 0] = Q921_LAYER_ENT_ID_TEI;	/* layer management entity identifier */
+	mes[offset + 1] = (trunk->ri[0] & 0xff00) >> 8;	/* reference number upper part */
+	mes[offset + 2] =  trunk->ri[0] & 0xff;		/* reference number lower part */
+	mes[offset + 3] = Q921_TEI_ID_VERIFY;		/* message type: Identity Verification Request */
+	mes[offset + 4] = trunk->tei << 1 | 0x01;	/* action indicator: our TEI */
+
+	/* start T202 */
+	Q921T202TimerStart(trunk);
+
+	return Q921SendU(trunk, Q921_SAPI_TEI, Q921_RESPONSE(trunk), Q921_TEI_BCAST, 0, 0x00, mes, Q921_UFRAME_DATA_OFFSET(trunk) + 5);	/* TEI Assignment Request UN Frame */
+}
+
+
+/**
+ * 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 resp[25];
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+	L2UCHAR tei = (mes[offset + 4] >> 1);		/* action indicator => tei */
+
+	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
+		 */
+//		trunk->ri[0] = (L2USHORT)(random() % 0xffff);
+
+		resp[offset + 0] = Q921_LAYER_ENT_ID_TEI;	/* layer management entity identifier */
+		resp[offset + 1] = (trunk->ri[0] & 0xff00) >> 8;/* reference number upper part */
+		resp[offset + 2] =  trunk->ri[0] & 0xff;	/* reference number lower part */
+		resp[offset + 3] = Q921_TEI_ID_CHECKRESP;	/* message type: Check Response */
+		resp[offset + 4] = trunk->tei << 1 | 0x01;	/* action indicator: our TEI */
+
+		Q921T202TimerStop(trunk);
+
+		return Q921SendU(trunk, Q921_SAPI_TEI, Q921_RESPONSE(trunk), Q921_TEI_BCAST, 0, 0x00, resp, Q921_UFRAME_DATA_OFFSET(trunk) + 5);	/* TEI Check Response UN Frame */
+	}
+
+	return 0;
+}
+
+
+/**
+ * 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 */
+
+	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 ? */
+		return Q921TeiSendAssignRequest(trunk);
+	}
+
+	return 0;
+}
+
+
+/**
+ * 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)) {
+		/* does not make sense */
+		return 0;
+	}
+
+	/* todo: process message */
+
+	return 1;
+}
+
+/**
+ * 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)
+{
+	L2UCHAR mes[25];
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+
+	if (!Q921_IS_PTMP_NT(trunk)) {
+		/* does not make sense */
+		return 0;
+	}
+
+	mes[offset + 0] = Q921_LAYER_ENT_ID_TEI;	/* layer management entity identifier */
+	mes[offset + 1] = 0;				/* reference number upper part */
+	mes[offset + 2] = 0;				/* reference number lower part */
+	mes[offset + 3] = Q921_TEI_ID_CHECKREQ;		/* message type: Check Request */
+	mes[offset + 4] = tei << 1 | 0x01;	/* action indicator: the TEI to check */
+
+	/* Start NT timer, todo: this one is probably per TEI too */
+	Q921T201TimerStart(trunk, tei);
+
+	return Q921SendU(trunk, Q921_SAPI_TEI, Q921_COMMAND(trunk), Q921_TEI_BCAST, 0, 0x00, mes, Q921_UFRAME_DATA_OFFSET(trunk) + 5);	/* TEI Check Request UN Frame */
+}
+#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 te 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 te 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)
+{
+	L2UCHAR mes[25];
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+
+	if (!Q921_IS_PTMP_NT(trunk)) {
+		/* does not make sense */
+		return 0;
+	}
+
+	mes[offset + 0] = Q921_LAYER_ENT_ID_TEI;	/* layer management entity identifier */
+	mes[offset + 1] = (ri & 0xff00) >> 8;		/* reference number upper part */
+	mes[offset + 2] =  ri & 0xff;			/* reference number lower part */
+	mes[offset + 3] = Q921_TEI_ID_DENIED;		/* message type: Deny Response */
+	mes[offset + 4] = tei << 1 | 0x01;		/* action indicator: the TEI to deny */
+
+	return Q921SendU(trunk, Q921_SAPI_TEI, Q921_RESPONSE(trunk), Q921_TEI_BCAST, 0, 0x00, mes, Q921_UFRAME_DATA_OFFSET(trunk) + 5);	/* TEI Assignment Deny Response UN Frame */
+}
+
+
+/**
+ * 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)
+{
+	L2UCHAR mes[25];
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+
+	if (!Q921_IS_PTMP_NT(trunk)) {
+		/* does not make sense */
+		return 0;
+	}
+
+	mes[offset + 0] = Q921_LAYER_ENT_ID_TEI;	/* layer management entity identifier */
+	mes[offset + 1] = (ri & 0xff00) >> 8;		/* reference number upper part */
+	mes[offset + 2] =  ri & 0xff;			/* reference number lower part */
+	mes[offset + 3] = Q921_TEI_ID_ASSIGNED;		/* message type: Assigned Response */
+	mes[offset + 4] = tei << 1 | 0x01;		/* action indicator: the TEI to assign */
+
+	return Q921SendU(trunk, Q921_SAPI_TEI, Q921_RESPONSE(trunk), Q921_TEI_BCAST, 0, 0x00, mes, Q921_UFRAME_DATA_OFFSET(trunk) + 5);	/* TEI Assigned Response UN Frame */
+}
+#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)
+{
+	L2UCHAR mes[25];
+	L2UCHAR offset = Q921_UFRAME_DATA_OFFSET(trunk);
+
+	if (!Q921_IS_PTMP_NT(trunk)) {
+		/* does not make sense */
+		return 0;
+	}
+
+	mes[offset + 0] = Q921_LAYER_ENT_ID_TEI;	/* layer management entity identifier */
+	mes[offset + 1] = 0;				/* reference number upper part */
+	mes[offset + 2] = 0;				/* reference number lower part */
+	mes[offset + 3] = Q921_TEI_ID_REMOVE;		/* message type: Deny Response */
+	mes[offset + 4] = tei << 1 | 0x01;		/* action indicator: the TEI to remove */
+
+	return Q921SendU(trunk, Q921_SAPI_TEI, Q921_COMMAND(trunk), Q921_TEI_BCAST, 0, 0x00, mes, Q921_UFRAME_DATA_OFFSET(trunk) + 5);	/* TEI Assignment Deny Response UN Frame */
+}
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,310 @@
 
 *****************************************************************************/
 
+/****************************************************************************
+ * 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 L2INT       int                 /* Min 16 bit signed                */
-#define L2ULONG		unsigned long		/* Min 32 bit						*/
+#define L2UCHAR		unsigned char		/* Min 8 bit			*/
+#define L2USHORT	unsigned short		/* 16 bit			*/
+#define L2INT		int			/* Min 16 bit signed		*/
+#define L2ULONG		unsigned long		/* Min 32 bit			*/
 #define L2TRUNK		Q921Data_t *
 
-typedef enum					/* Network/User Mode.                   */
+#define Q921_LOGBUFSIZE	2000
+
+typedef enum			/* Network/User Mode		*/
 {
-	Q921_TE=0,                  /*  0 : User Mode                       */
-    Q921_NT=1                   /*  1 : Network Mode                    */
+	Q921_TE=0,		/*  0 : User Mode		*/
+	Q921_NT=1		/*  1 : Network Mode		*/
 } Q921NetUser_t;
 
+typedef 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_REQUEST = 0,
+	Q921_DL_RELEASE,
+	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 (*Q921NotifyCB_t) (void *, 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;
 
+	/* Layer 3 notify callback */
+//	Q921NotifyCB_t Q921Notify23Proc;
+//	void *PrivateDataNotify23;
+
+	/* 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 Q921SetLayer3NotifyCB(L2TRUNK trunk, Q921NotifyCB_t func, void *priv); */
+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_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
-
