Index: src/zap_isdn.c
===================================================================
--- src/zap_isdn.c	(revision 495)
+++ src/zap_isdn.c	(working copy)
@@ -958,7 +958,40 @@
 	return ret;
 }
 
+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__, __FUNCTION__, __LINE__, loglevel, "%s", msg);
+
+	return 0;
+}
+
 static zap_state_map_t isdn_state_map = {
 	{
 		{
@@ -1113,11 +1146,15 @@
 				   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,
 				   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,
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,260 @@
 
 *****************************************************************************/
 
+/****************************************************************************
+ * Changes:
+ *
+ * - June 2008: Stefan Knoblich <s.knoblich@axsentis.de>:
+ *     Add PTMP TEI management
+ *     Add Timers
+ *     Add Retransmit Counters
+ *     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;
+
+/*
+ * 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_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;
+
+#if 0
+/*
+ * Messages for L2 <-> L3 communication
+ */
+typedef enum {
+	Q921_DL_REQUEST = 0,
+	Q921_DL_RELEASE,
+} Q921DLMsg_t;
+#endif
+
 typedef struct Q921Data Q921Data_t;
 typedef int (*Q921TxCB_t) (void *, 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;
+	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;
+
+	/* 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 */
 	Q921TxCB_t Q921Tx21Proc;
 	Q921TxCB_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];
 };
 
+
+/*
+ * 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,
 					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);
+
 void Q921SetGetTimeCB(L2ULONG (*callback)(void));
 void Q921TimerTick(L2TRUNK trunk);
 
 int Q921Tx21Proc(L2TRUNK trunk, L2UCHAR *Msg, L2INT size);
 int Q921Tx23Proc(L2TRUNK trunk, L2UCHAR *Msg, L2INT size);
+
 extern L2ULONG tLast;
 L2ULONG Q921GetTime(void);
-void Q921T200TimerStart(L2TRUNK trunk);
-void Q921T200TimerStop(L2TRUNK trunk);
-void Q921T200TimerReset(L2TRUNK trunk);
-void Q921T203TimerStart(L2TRUNK trunk);
-void Q921T203TimerStop(L2TRUNK trunk);
-void Q921T203TimerReset(L2TRUNK trunk);
-void Q921T200TimerExpire(L2TRUNK trunk);
-void Q921T203TimerExpire(L2TRUNK trunk);
+
+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 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);
 
+/*
+ * 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)		(Q921_IS_PTMP_TE(tr) ? (tr)->state >= Q921_STATE_TEI_ASSIGNED : 1)
+
+#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)
+
 #endif
-
Index: src/isdn/include/Q931.h
===================================================================
--- src/isdn/include/Q931.h	(revision 495)
+++ src/isdn/include/Q931.h	(working copy)
@@ -505,7 +505,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			/*  3 : BRI Trunk                       */
 } Q931_TrunkType_t;
 
 typedef enum						/* Trunk State							*/
Index: src/isdn/Q931api.c
===================================================================
--- src/isdn/Q931api.c	(revision 495)
+++ src/isdn/Q931api.c	(working copy)
@@ -97,6 +97,7 @@
 		break;
 
 	case Q931_TrType_BRI:
+	case Q931_TrType_BRI_PTMP:
 		dchannel = 3;
 		maxchans = 3;
 		break;
Index: src/isdn/Q921.c
===================================================================
--- src/isdn/Q921.c	(revision 495)
+++ src/isdn/Q921.c	(working copy)
@@ -37,13 +37,99 @@
   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);
 
+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);
+/* unused for now
+ *static int Q921TeiSendCheckRequest(L2TRUNK trunk, L2UCHAR tei);
+ */
+static int Q921TeiProcCheckResponse(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+static int Q921TeiProcVerifyRequest(L2TRUNK trunk, L2UCHAR *mes, L2INT size);
+/* unused for now
+ *static int Q921TeiSendDenyResponse(L2TRUNK trunk, L2UCHAR tei, L2USHORT ri);
+ *static int Q921TeiSendAssignedResponse(L2TRUNK trunk, L2UCHAR tei, L2USHORT ri);
+ */
+static int Q921TeiSendRemoveRequest(L2TRUNK trunk, L2UCHAR tei);
+
+/*
+ * Logging
+ */
+static int Q921LogProc(L2TRUNK trunk, Q921LogLevel_t level, const char *fmt, ...);
+static int Q921LogProcMesg(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";
+}
+
+
+
 /*****************************************************************************
 
   Function:     Q921_InitTrunk
@@ -56,6 +142,7 @@
 					L2UCHAR sapi,
 					L2UCHAR tei,
 					Q921NetUser_t NetUser,
+					Q921NetType_t NetType,
 					L2INT hsize,
 					Q921TxCB_t cb21,
 					Q921TxCB_t cb23,
@@ -66,35 +153,363 @@
 		MFIFOCreate(trunk->HDLCInQueue, Q921MAXHDLCSPACE, 10);
 		trunk->initialized = INITIALIZED_MAGIC;
 	}
-	trunk->va = 0;
-	trunk->vs = 0;	
-	trunk->vr = 0;
-	trunk->state = 0;
+
+	trunk->state = Q921_STATE_STOPPED;
 	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)
 {
+	Q921LogProcMesg(trunk, Q921_LOG_DEBUG, 0, Msg, size, "Sending frame\n");
+
 	return trunk->Q921Tx21Proc(trunk->PrivateData21, Msg, 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, L2UCHAR *Msg, L2INT size)
 {
 	return trunk->Q921Tx23Proc(trunk->PrivateData23, 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 Q921LogProc(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 Q921LogProcMesg(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;
+
+		pleft   = sizeof(pbuf);
+		poffset = 0;
+
+		/*
+		 * Decode packet
+		 */
+		sapi = (mes[0] & 0xfc) >> 2;
+		cr   = (mes[0] & 0x02) >> 1;
+		tei  = (mes[1] & 0xfe) >> 1;
+
+		/* make cr acutally 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((mes[2] & 0x01) == 0x00) {
+			/*
+			 * I frame
+			 */
+			L2UCHAR pf = mes[3] & 0x01;	/* poll / final flag */
+			L2UCHAR nr = mes[3] >> 1;	/* receive sequence number */
+			L2UCHAR ns = mes[2] >> 1;	/* send sequence number */
+
+			len = snprintf(pbuf + poffset, pleft, "    Type: I Frame\n          P/F: %d, N(S): %d, N(S): %d\n", pf, ns, nr);
+			if(len > 0) {
+				poffset += len;
+				pleft   -= len;
+			} else
+				goto out;
+		}
+		else if((mes[2] & 0x03) == 0x01) {
+			/*
+			 * S frame
+			 */
+			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 */
+			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\n", type, pf, nr);
+			if(len > 0) {
+				poffset += len;
+				pleft   -= len;
+			} else
+				goto out;
+		}
+		else if((mes[2] & 0x03) == 0x03) {
+			/*
+			 * U frame
+			 */
+			L2UCHAR m  = (mes[2] & 0xe0) >> 3 | (mes[2] & 0x0c) >> 2;	/* modifier function id */
+			L2UCHAR pf = (mes[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 +525,206 @@
 
 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;
 	}
 }
 
-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;
 }
 
-void Q921T200TimerReset(L2TRUNK trunk)
+void Q921T200TimerReset(L2TRUNK trunk, L2UCHAR tei)
 {
-	Q921T200TimerStop(trunk);
-	Q921T200TimerStart(trunk);
+	Q921T200TimerStop(trunk, tei);
+	Q921T200TimerStart(trunk, tei);
 }
 
-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;
 	}
 }
 
-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;
 }
 
-void Q921T203TimerReset(L2TRUNK trunk)
+void Q921T203TimerReset(L2TRUNK trunk, L2UCHAR tei)
 {
-	Q921T203TimerStop(trunk);
-	Q921T203TimerStart(trunk);
+	Q921T203TimerStop(trunk, tei);
+	Q921T203TimerStart(trunk, tei);
 }
 
-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;
+	}
 }
 
-void Q921T203TimerExpire(L2TRUNK trunk)
+void Q921T202TimerStop(L2TRUNK trunk)
 {
-	Q921T203TimerReset(trunk);
+	trunk->T202 = 0;
+}
+
+void Q921T202TimerReset(L2TRUNK trunk)
+{
+	Q921T202TimerStop(trunk);
+	Q921T202TimerStart(trunk);
+}
+
+/*
+ * T201 handling (TEI management (NT side), per-TEI)
+ */
+void Q921T201TimerStart(L2TRUNK trunk, L2UCHAR tei)
+{
+	if (!trunk->T201[tei]) {
+		trunk->T201[tei] = Q921GetTime() + trunk->T201Timeout;
+	}
+}
+
+void Q921T201TimerStop(L2TRUNK trunk, L2UCHAR tei)
+{
+	trunk->T201[tei] = 0;
+}
+
+void Q921T201TimerReset(L2TRUNK trunk, L2UCHAR tei)
+{
+	Q921T201TimerStop(trunk, tei);
+	Q921T201TimerStart(trunk, tei);
+}
+
+/*
+ * Expiry callbacks
+ */
+void Q921T200TimerExpire(L2TRUNK trunk, L2UCHAR tei)
+{
+	Q921LogProc(trunk, Q921_LOG_DEBUG, "T200 expired for TEI %d (trunk TEI %d)\n", tei, trunk->tei);
+}
+
+void Q921T203TimerExpire(L2TRUNK trunk, L2UCHAR tei)
+{
+	Q921LogProc(trunk, Q921_LOG_DEBUG, "T203 expired for TEI %d (trunk TEI %d)\n", tei, trunk->tei);
+
+	Q921T203TimerReset(trunk, trunk->tei);
 	Q921SendRR(trunk, trunk->sapi, trunk->NetUser == Q921_TE ? 0 : 1, trunk->tei, 1);
 }
 
+void Q921T202TimerExpire(L2TRUNK trunk)
+{
+	Q921T202TimerReset(trunk);
+
+	Q921LogProc(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)
+{
+	Q921LogProc(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, 0);
+		}
+		if (trunk->T203[0] && tNow > trunk->T203[0]) {
+			Q921T203TimerExpire(trunk, 0);		
+		}
 	}
-	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,15 +775,44 @@
 *****************************************************************************/
 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++;
+	if(!Q921_IS_READY(trunk)) {
+		/* don't even bother trying */
+		return 0;
+	}
 
-    return Q921Tx21Proc(trunk, mes, size);
+	mes[trunk->Q921HeaderSpace+0] = (Sapi&0xfc) | ((cr<<1)&0x02);
+	mes[trunk->Q921HeaderSpace+1] = (Tei<<1) | 0x01;
+	mes[trunk->Q921HeaderSpace+2] = trunk->vs[Tei]<<1;
+	mes[trunk->Q921HeaderSpace+3] = (trunk->vr[Tei]<<1) | (pf & 0x01);
+	trunk->vs[Tei]++;
+
+	return Q921Tx21Proc(trunk, mes, size);
 }
 
+int Q921SendS(L2TRUNK trunk, L2UCHAR Sapi, char cr, L2UCHAR Tei, char pf, L2UCHAR sv, L2UCHAR *mes, L2INT size)
+{
+	if(!Q921_IS_READY(trunk)) {
+		/* don't even bother trying */
+		return 0;
+	}
+
+	mes[trunk->Q921HeaderSpace+0] = (Sapi&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[Tei]<<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&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;
+
+	return Q921Tx21Proc(trunk, mes, size);
+}
+
 int Q921Rx32(L2TRUNK trunk, L2UCHAR * Mes, L2INT Size)
 {
 	return Q921SendI(trunk, 
@@ -269,14 +841,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 +863,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 +885,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 +907,83 @@
 *****************************************************************************/
 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;
+
+	Q921LogProc(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)) {
+		Q921LogProc(trunk, Q921_LOG_DEBUG, "Sending SABME\n");
+
+		return Q921SendSABME(trunk, 
 					trunk->sapi, 
 					trunk->NetUser == Q921_TE ? 0 : 1,
 					trunk->tei, 
 					1);
+
+	} else if(Q921_IS_PTMP_NT(trunk)) {
+
+		Q921LogProc(trunk, Q921_LOG_DEBUG, "Revoking all TEIs\n");
+
+		return Q921TeiSendRemoveRequest(trunk, Q921_TEI_BCAST);	/* Revoke all TEIs in use */
+	} else {
+
+		Q921LogProc(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;
+
+	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 +996,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 +1018,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 +1040,375 @@
 *****************************************************************************/
 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 Q921Tx21Proc(trunk, mes, trunk->Q921HeaderSpace+3);
+	return Q921SendU(trunk, Sapi, cr, Tei, pf, 0x0c, mes, 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 */
 
-    trunk->vr=0;
-    trunk->vs=0;
-	trunk->va=0;
+	switch(trunk->state) {
+	case Q921_STATE_TEI_ASSIGNED:
+		/* send UA */
+		Q921SendUA(trunk,
+				trunk->sapi,
+				trunk->NetUser == Q921_TE ? 0 : 1,
+				trunk->tei, pf);
 
-    return 1;
+		/* clear counters */
+		trunk->vr[0]=0;
+		trunk->vs[0]=0;
+		trunk->va[0]=0;
+
+		/* TODO: send DL-Establish indication to Q.931 */
+
+		/* start T203 */
+		Q921T203TimerStart(trunk, 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,
+				trunk->NetUser == Q921_TE ? 0 : 1,
+				trunk->tei, pf);
+		/* no state change */
+		break;
+
+	case Q921_STATE_AWAITING_RELEASE:
+		/* send DM */
+		Q921SendDM(trunk,
+				trunk->sapi,
+				trunk->NetUser == Q921_TE ? 0 : 1,
+				trunk->tei, pf);
+		/* no state change */
+		break;
+
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+	case Q921_STATE_TIMER_RECOVERY:
+		/* send UA */
+		Q921SendUA(trunk,
+				trunk->sapi,
+				trunk->NetUser == Q921_TE ? 0 : 1,
+				trunk->tei, pf);
+
+		/* clear exception conditions */
+
+		/* send MDL-Error indication */
+
+		/* other fancy stuff (see docs) */
+
+		/* clear counters */
+		trunk->vr[0]=0;
+		trunk->vs[0]=0;
+		trunk->va[0]=0;
+
+		/* Stop T200, start T203 */
+		Q921T200TimerStop(trunk, trunk->tei);
+		Q921T203TimerStart(trunk, 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 */
+
+	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:
+		if(pf) {
+			/* Discard I queue */
+
+			/* Send DL-Release indication to Q.931 */
+
+			/* Stop T200 */
+			Q921T200TimerStop(trunk, trunk->tei);
+
+			/* Change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED);
+		}
+		break;
+
+	case Q921_STATE_AWAITING_RELEASE:
+		if(pf) {
+			/* Send DL-Release confirm to Q.931 */
+
+			/* Stop T200 */
+			Q921T200TimerStop(trunk, 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 */
+
+			/* clear L3 initiated */
+
+			/* change state (no action?) */
+			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 */
+
+	switch(trunk->state) {
+	case Q921_STATE_TEI_ASSIGNED:
+		/* MDL Error indication (C, D) */
+		break;
+
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+		if(pf) {
+			/* TODO: other fancy stuff (see docs) */
+			trunk->vr[0] = 0;	/* change this (see part above) */
+
+			/* DL-Establish confirm indication */
+
+			/* Stop T200, start T203 */
+			Q921T200TimerStop(trunk, trunk->tei);
+			Q921T203TimerStart(trunk, trunk->tei);
+
+			trunk->vs[0] = 0;
+			trunk->va[0] = 0;
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_MULTIPLE_FRAME_ESTABLISHED);
+
+		} else {
+			/* MDL Error indication (C, D) */
+
+			/* no state change */
+		}
+		break;
+
+	case Q921_STATE_AWAITING_RELEASE:
+		if(pf) {
+			/* DL Release confirm */
+
+			/* Stop T200 */
+			Q921T200TimerStop(trunk, trunk->tei);
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED);
+
+		} else {
+			/* MDL Error indication (D) */
+			/* no state change */
+		}
+		break;
+
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* MDL Error indication (C, D) */
+		/* 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 */
+
+	switch(trunk->state) {
+	case Q921_STATE_TEI_ASSIGNED:
+	case Q921_STATE_AWAITING_ESTABLISHMENT:
+		/* Send DM */
+		Q921SendDM(trunk,
+				trunk->sapi,
+				trunk->NetUser == Q921_TE ? 0 : 1,
+				trunk->tei, pf);
+
+		/* no state change */
+		break;
+
+	case Q921_STATE_AWAITING_RELEASE:
+		Q921SendUA(trunk,
+				trunk->sapi,
+				trunk->NetUser == Q921_TE ? 0 : 1,
+				trunk->tei, pf);
+
+		/* no state change */
+		break;
+
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		/* Discard I queue */
+
+		/* send UA */
+		Q921SendUA(trunk,
+				trunk->sapi,
+				trunk->NetUser == Q921_TE ? 0 : 1,
+				trunk->tei, pf);
+		
+		/* DL Release indication */
+
+		/* Stop T200, T203 */
+		Q921T200TimerStop(trunk, trunk->tei);
+		Q921T203TimerStop(trunk, trunk->tei);
+
+		/* change state (no action) */
+		Q921ChangeState(trunk, Q921_STATE_TEI_ASSIGNED);
+		break;
+
+	default:
+		Q921LogProc(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 = 0;
+
+	if(Q921_IS_PTMP_NT(trunk))
+		idx = tei;
+
+	switch(trunk->state) {
+	case Q921_STATE_MULTIPLE_FRAME_ESTABLISHED:
+		if (command) { /* if this is a command */
+			if(pf) {
+				/* Enquiry response */
+				Q921SendRR(trunk, sapi, cr, tei, pf);
+			}
+		} else {
+			if(pf) {
+				/* MDL Error indication */
+			}
+		}
+
+		/* */
+		if(trunk->va[idx] <= nr && nr <= trunk->vs[idx]) {
+
+			if(nr == trunk->vs[tei]) {
+				/* 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 */
+
+			/* change state (no action) */
+			Q921ChangeState(trunk, Q921_STATE_AWAITING_ESTABLISHMENT);
+		}
+		break;
+
+	default:
+		break;
+	}
+	return 1;
+}
+
+
 /*****************************************************************************
 
   Function:     Q921Rx12
@@ -479,80 +1429,761 @@
 *****************************************************************************/
 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;
+
+		Q921LogProcMesg(trunk, Q921_LOG_DEBUG, 1, mes, rs, "New packet received");
+
+		/* 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;
+		}
+
+		/* 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;
 			}
 		}
 
-        /* check for I frame */
-        if((mes[2] & 0x01) == 0)
-        {
+		/* check for I frame */
+		if((mes[2] & 0x01) == 0x00)
+		{
+			L2UCHAR pf = mes[3] & 0x01;	/* poll / final flag */
+			//L2UCHAR nr = mes[3] >> 1;	/* receive sequence number */
+			//L2UCHAR ns = mes[2] >> 1;	/* send 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);
+			trunk->vr[idx]++;
+
+			if(Q921Tx23Proc(trunk, smes, size-2)) /* -2 to clip away CRC */
+			{
+				Q921SendRR(trunk, sapi, cr, tei, pf);
 			}
-            else
-            {
-                /* todo: whatever*/
-				/* trunk->vr--; */
+			else
+			{
+	                	/* todo: whatever */
+				/* trunk->vr[idx]--; */
 				/* 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);
-           }
-        }
+				Q921SendRR(trunk, sapi, cr, tei, pf);
+			}
+		}
+	
+		/* 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 */
 
-        /* check for RR */
-        else if(mes[2] ==0x01)
-		{
-			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);
+			switch(sv) {
+			case 0x00:	/* RR : Receive Ready */
+				Q921ProcRR(trunk, mes, rs);
+				break;
+
+			case 0x02:	/* RNR : Receive Not Ready */
+				break;
+
+			case 0x04:	/* REJ : Reject */
+				break;
+
+			default:	/* Invalid / Unknown */
+				break;
 			}
+		}
 
-        }
-		
-        /* check for RNR */
-        /* check for REJ */
-        /* check for SABME */
-        else if((mes[2] & 0xef) == 0x6f)
-        {
-            Q921ProcSABME(trunk, mes, rs);
-            Q921SendUA(trunk, (mes[0]&0xfc)>>2, (mes[0]>>1)&0x01,mes[1]>>1, (mes[2]&0x10)>>4);
-        }
+		/* check for U frame */
+		else if((mes[2] & 0x03) == 0x03)
+		{
+			L2UCHAR m  = (mes[2] & 0xe0) >> 3 | (mes[2] & 0x0c) >> 2;	/* modifier function id */
+//			L2UCHAR pf = (mes[2] & 0x10) >> 4;				/* poll / final flag */
 
-        /* check for DM */
-        /* check for UI */
-        /* check for DISC */
-        /* check for UA */
-        /* check for FRMR */
-        /* check for XID */
+			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;
+					}
 
-        else
-        {
-            /* what the ? Issue an error */
+					switch(mes[6]) {
+					case Q921_TEI_ID_REQUEST:	/* (TE ->) NT */
+						Q921TeiProcAssignRequest(trunk, mes, size);
+						break;
+
+					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 */
+						break;
+					}
+				}
+				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 */
+				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);
+out:
+		MFIFOKillNext(trunk->HDLCInQueue);
 
-        return 1;
-    }
-    return 0;
+		return 1;
+	}
+
+	return 0;
 }
 
+/*
+ * Misc
+ */
+
+
+/**
+ * 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;
+}
+
+/**
+ * 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;
+
+	Q921LogProc(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)) {
+				res = Q921SendSABME(trunk, 
+					trunk->sapi,
+					trunk->NetUser == Q921_TE ? 0 : 1,
+					trunk->tei,
+					1);
+
+				/* go directly into AWAITING ESTABLISHMENT state */
+				state = Q921_STATE_AWAITING_ESTABLISHMENT;
+
+				Q921LogProc(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:
+		break;
+
+	case Q921_STATE_TIMER_RECOVERY:
+		break;
+
+
+	}
+
+	trunk->state = state;
+
+	Q921LogProc(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;
+	}
+
+	/* Request running? */
+	if (trunk->T202) {
+		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, 0, 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;
+
+		Q921LogProc(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 */
+		}
+
+		Q921LogProc(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, 1, 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);
+
+	if (!Q921_IS_PTMP_TE(trunk)) {
+		/* ptmp te mode only */
+		return 0;
+	}
+
+	if ((mes[offset + 4] >> 1) == Q921_TEI_BCAST || (mes[offset + 4] >> 1) == 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, 1, 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);
+
+	if (!Q921_IS_PTMP_TE(trunk)) {
+		/* ptmp te mode only */
+		return 0;
+	}
+
+	if ((mes[offset + 4] >> 1) == Q921_TEI_BCAST || (mes[offset + 4] >> 1) == trunk->tei)
+	{
+		/*
+		 * Broadcast TEI remove or for our assigned TEI
+		 */
+		trunk->tei	= 0;
+		trunk->state	= Q921_STATE_TEI_UNASSIGNED;
+
+		/* 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, 1, 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, 1, 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, 1, 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, 1, Q921_TEI_BCAST, 0, 0x00, mes, Q921_UFRAME_DATA_OFFSET(trunk) + 5);	/* TEI Assignment Deny Response UN Frame */
+}
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 {
