/*
 * libpri: An implementation of Primary Rate ISDN
 *
 * Written by Mark Spencer <markster@linux-support.net>
 *
 * Copyright (C) 2001, Linux Support Services, Inc.
 * All Rights Reserved.
 * Copyright (C) 2003-2006 Junghanns.NET GmbH
 * Klaus-Peter Junghanns <kpj@junghanns.net>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 
 *
 */
 
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include "compat.h"
#include "libpri.h"
#include "pri_internal.h"
#include "pri_q921.h" 
#include "pri_q931.h"

/*
 * Define RANDOM_DROPS To randomly drop packets in order to simulate loss for testing
 * retransmission functionality
 */

/*
#define RANDOM_DROPS
*/

#define Q921_INIT(pri, hf) do { \
	memset(&(hf),0,sizeof(hf)); \
	(hf).h.sapi = (pri)->sapi; \
	(hf).h.ea1 = 0; \
	(hf).h.ea2 = 1; \
	(hf).h.tei = (pri)->tei; \
} while(0)

static void reschedule_t203(struct pri *pri, int tei);
static void q921_flush_txqueue(struct pri *pri, int tei, int devnull);
static void q921_send_teiverify(struct pri *pri,int tei);

static void q921_discard_retransmissions(struct pri *pri, int tei)
{
	struct q921_frame *f, *p;
	int teio = tei - Q921_TEI_BASE;
	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
	
	f = pri->txqueue[teio];
	while(f) {
	    p = f;
	    f = f->next;
	    /* Free frame */
	    free(p);
	}
        pri->txqueue[teio] = NULL;
}

static int q921_transmit(struct pri *pri, q921_h *h, int len) 
{
	int res;
	if (pri->master)
		return q921_transmit(pri->master, h, len);
#ifdef RANDOM_DROPS
   if (!(random() % 3)) {
         pri_message(pri, " === Dropping Packet ===\n");
         return 0;
   }
#endif   
#ifdef LIBPRI_COUNTERS
	pri->q921_txcount++;      
#endif
	/* Just send it raw */
	if (pri->debug & PRI_DEBUG_Q921_DUMP)
		q921_dump(pri, h, len, pri->debug & PRI_DEBUG_Q921_RAW, 1);
	/* Write an extra two bytes for the FCS */
	res = pri->write_func ? pri->write_func(pri, h, len + 2) : 0;
	if (res != (len + 2)) {
		pri_error(pri, "Short write: %d/%d (%s)\n", res, len + 2, strerror(errno));
		return -1;
	}
 	if (pri->localtype == BRI_CPE_PTMP) {
 	    reschedule_t203(pri, pri->tei);
 	} else if (h->h.tei != Q921_TEI_GROUP) {
	    reschedule_t203(pri, h->h.tei);
 	}
	return 0;
}

static void q921_send_ua(struct pri *pri, int pfbit, int tei)
{
	q921_h h;
	Q921_INIT(pri, h);
	h.u.m3 = 3;		/* M3 = 3 */
	h.u.m2 = 0;		/* M2 = 0 */
	h.u.p_f = pfbit;	/* Final bit on */
	h.u.ft = Q921_FRAMETYPE_U;
	h.h.tei = tei;
	switch(pri->localtype) {
	case PRI_NETWORK:
		h.h.c_r = 0;
		break;
	case PRI_CPE:
		h.h.c_r = 1;
		break;
	case BRI_NETWORK_PTMP:
		h.h.c_r = 0;
		break;
	case BRI_CPE_PTMP:
		h.h.tei = pri->tei;
		h.h.c_r = 1;
		break;
	case BRI_NETWORK:
		h.h.c_r = 0;
		break;
	case BRI_CPE:
		h.h.c_r = 1;
		break;
	default:
		pri_error(pri, "Don't know how to U/A on a type %d node\n", pri->localtype);
		return;
	}
	if (pri->debug & PRI_DEBUG_Q921_STATE)
		pri_message(pri, "Sending Unnumbered Acknowledgement\n");
	q921_transmit(pri, &h, 3);
}

/*
static void q921_send_disconnect(struct pri *pri, int pfbit, int tei) {
 	q921_h h;
	Q921_INIT(pri, h);
 	h.u.m3 = 2;
 	h.u.m2 = 0;
 	h.u.p_f = pfbit;
	h.u.ft = Q921_FRAMETYPE_U;
 	h.h.tei = tei;
 	switch(pri->localtype) {
 	case PRI_NETWORK:
 		h.h.c_r = 0;
 		break;
 	case PRI_CPE:
 		h.h.c_r = 1;
 		break;
 	case BRI_NETWORK_PTMP:
 		h.h.c_r = 0;
		break;
  	case BRI_CPE_PTMP:
 		h.h.tei = pri->tei;
 		h.h.c_r = 1;
 		break;
 	case BRI_NETWORK:
 		h.h.c_r = 0;
 		break;
 	case BRI_CPE:
 		h.h.c_r = 1;
 		break;
 	default:
 		pri_error(pri, "Don't know how to disconnect on a type %d node\n", pri->localtype);
 		return;
 	}
 	if (pri->debug & PRI_DEBUG_Q921_STATE)
 		pri_message(pri, "Sending Disconnect\n");
 	q921_transmit(pri, &h, 3);
 }
*/

static void q921_send_dm(struct pri *pri, int pfbit, int tei)
{
 	q921_h h;
 	Q921_INIT(pri, h);
 	h.u.m3 = 0;		/* M3 = 0 */
 	h.u.m2 = 3;		/* M2 = 3 */
 	h.u.p_f = pfbit;	/* Final bit on */
 	h.u.ft = Q921_FRAMETYPE_U;
  	h.h.tei = tei;
 	switch(pri->localtype) {
  	case PRI_NETWORK:
  		h.h.c_r = 0;
  		break;
  	case PRI_CPE:
  		h.h.c_r = 1;
  		break;
  	case BRI_NETWORK_PTMP:
 		h.h.c_r = 0;
  		break;
  	case BRI_CPE_PTMP:
  		h.h.tei = pri->tei;
  		h.h.c_r = 1;
  		break;
  	case BRI_NETWORK:
  		h.h.c_r = 0;
  		break;
  	case BRI_CPE:
  		h.h.c_r = 1;
  		break;
  	default:
  		pri_error(pri, "Don't know how to DM on a type %d node\n", pri->localtype);
  		return;
  	}
  	if (pri->debug & PRI_DEBUG_Q921_STATE)
  		pri_message(pri, "Sending DM\n");
  	q921_transmit(pri, &h, 3);
}
   
static void q921_send_teireq(void *vpri) {
    struct pri *pri = vpri;
    unsigned short ri=0x6464;
    q921_u *f;
    ri = rand();

    if (pri->localtype != BRI_CPE_PTMP) {
	pri_error(pri, "TEI req for non-ptmp???\n"); 
  	return;
    }
    if (pri->t202_timer[0]) { 
	pri_schedule_del(pri, pri->t202_timer[0]);
 	pri->t202_timer[0] = 0;
    }
    if (pri->sabme_retrans[0]++ > (pri->timers[PRI_TIMER_N202])) {
	/* delete txqueue */
	q921_flush_txqueue(pri, 0, 1);
	/* N202 retransmissions, activation of layer 2 failed, tell upper layer, start again */
	pri->schedev = 1;
	pri->ev.gen.e = PRI_EVENT_DCHAN_DOWN;
	pri->ev.gen.tei = 0;
	pri->sabme_retrans[0] = 0;
	q921_send_teiverify(pri, 127);
#ifdef RELAX_TRB
	pri->t202_timer[0] = pri_schedule_event(pri, pri->timers[PRI_TIMER_T202] + 3000, q921_send_teireq, pri);
#endif
	return;
    }
 
    pri->t202_timer[0] = pri_schedule_event(pri, pri->timers[PRI_TIMER_T202], q921_send_teireq, pri);
  
    pri->ri = ri;
    f = malloc(sizeof(q921_u) + 5 + 2);
    memset(f,0x0,sizeof(q921_u) + 5 + 2);
    if (f) {
	f->h.sapi = Q921_SAPI_LAYER2_MANAGEMENT;
        f->h.tei = Q921_TEI_GROUP;
	f->h.c_r = 0;	
        f->h.ea1 = 0;	
        f->h.ea2 = 1;	
        f->m2 = 0;
        f->m3 = 0;
        f->ft = Q921_FRAMETYPE_U;
        f->data[0] = 0xf;
        f->data[1] = ri >> 8;
        f->data[2] = ri & 0xff;
        f->data[3] = Q921_TEI_ID_REQUEST;
        f->data[4] = 0xff;
	if (pri->debug & PRI_DEBUG_Q921_STATE)
	    pri_message(pri, "Sending TEI Request ri=%d\n",ri);
	q921_transmit(pri,(q921_h *)&(f->h),8);
    }
}
  
static void q921_send_teiassign(struct pri *pri,int ri,int tei) {
    q921_u *f;
  
    if (pri->localtype != BRI_NETWORK_PTMP) {
	pri_error(pri, "TEI assign for non-ptmp???\n"); 
  	return;
    }
  
    f = malloc(sizeof(q921_u) + 5 + 2);
    memset(f,0x0,sizeof(q921_u) + 5 + 2);
    if (f) {
	f->h.sapi = Q921_SAPI_LAYER2_MANAGEMENT;
  	f->h.tei = Q921_TEI_GROUP;
  	f->h.c_r = 1;	
  	f->h.ea1 = 0;	
  	f->h.ea2 = 1;	
  	f->m2 = 0;
  	f->m3 = 0;
  	f->ft = Q921_FRAMETYPE_U;
  	f->data[0] = 0xf;
  	f->data[1] = ri >> 8;
  	f->data[2] = ri & 0xff;
  	f->data[3] = Q921_TEI_ID_ASSIGNED;
  	f->data[4] = (tei << 1) | 0x1;
  	if (pri->debug & PRI_DEBUG_Q921_STATE)
      	    pri_message(pri, "Sending TEI assign ri=%d tei=%d\n",ri,tei);
  	q921_transmit(pri,(q921_h *)&(f->h),8);
  	free(f);
    } else {
	pri_error(pri, "q921_send_teiassign: failed to malloc f!\n");
    }
}
  
static void q921_send_teichkresp(struct pri *pri,int tei) {
    q921_u *f;
    unsigned short ri=0x6464;
    ri = rand();
  
    if (pri->localtype != BRI_CPE_PTMP) {
	pri_error(pri, "TEI check response for non-ptmp???\n"); 
  	return;
    }
  
    f = malloc(sizeof(q921_u) + 5 + 2);
    memset(f,0x0,sizeof(q921_u) + 5 + 2);
    if (f) {
	f->h.sapi = Q921_SAPI_LAYER2_MANAGEMENT;
  	f->h.tei = Q921_TEI_GROUP;
  	f->h.c_r = 0; // command u->n	
  	f->h.ea1 = 0;	
  	f->h.ea2 = 1;	
  	f->m2 = 0;
  	f->m3 = 0;
  	f->ft = Q921_FRAMETYPE_U;
  	f->data[0] = 0xf;
  	f->data[1] = ri >> 8;
  	f->data[2] = ri & 0xff;
  	f->data[3] = Q921_TEI_ID_CHK_RES;
  	f->data[4] = (tei << 1) | 0x1;
  	if (pri->debug & PRI_DEBUG_Q921_STATE)
      	    pri_message(pri, "Sending TEI check resp ri=%d tei=%d\n",ri,tei);
  	q921_transmit(pri,(q921_h *)&(f->h),8);
  	free(f);
    }
}

static void q921_send_teichkreq(struct pri *pri,int tei) {
    q921_u *f;
    unsigned short ri=0x6464;
    ri = rand();
  
    if (pri->localtype != BRI_NETWORK_PTMP) {
	pri_error(pri, "TEI check response for non-ptmp???\n"); 
  	return;
    }
  
    f = malloc(sizeof(q921_u) + 5 + 2);
    memset(f,0x0,sizeof(q921_u) + 5 + 2);
    if (f) {
	f->h.sapi = Q921_SAPI_LAYER2_MANAGEMENT;
  	f->h.tei = Q921_TEI_GROUP;
  	f->h.c_r = 1; // command u->n	
  	f->h.ea1 = 0;	
  	f->h.ea2 = 1;	
  	f->m2 = 0;
  	f->m3 = 0;
  	f->ft = Q921_FRAMETYPE_U;
  	f->data[0] = 0xf;
  	f->data[1] = 0;
  	f->data[2] = 0;
  	f->data[3] = Q921_TEI_ID_CHK_REQ;
  	f->data[4] = (tei << 1) | 0x1;
  	if (pri->debug & PRI_DEBUG_Q921_STATE)
      	    pri_message(pri, "Sending TEI check request ri=%d tei=%d\n",ri,tei);
  	q921_transmit(pri,(q921_h *)&(f->h),8);
  	free(f);
    }
}
 
static void q921_send_teiverify(struct pri *pri,int tei) {
    q921_u *f;
  
    if ((pri->localtype != BRI_CPE) && (pri->localtype != BRI_CPE_PTMP)) {
  	pri_error(pri, "TEI verify for non-ptmp???\n"); 
  	return;
    }
  
    f = malloc(sizeof(q921_u) + 5 + 2);
    memset(f,0x0,sizeof(q921_u) + 5 + 2);
    if (f) {
  	f->h.sapi = Q921_SAPI_LAYER2_MANAGEMENT;
  	f->h.tei = Q921_TEI_GROUP;
  	f->h.c_r = 0; // command u->n	
  	f->h.ea1 = 0;	
  	f->h.ea2 = 1;	
  	f->m2 = 0;
  	f->m3 = 0;
  	f->ft = Q921_FRAMETYPE_U;
  	f->data[0] = 0xf;
 	f->data[1] = 0;
  	f->data[2] = 0;
  	f->data[3] = Q921_TEI_ID_VERIFY;
  	f->data[4] = (tei << 1) | 0x1;
  	if (pri->debug & PRI_DEBUG_Q921_STATE)
      	    pri_message(pri, "Sending TEI verify tei=%d\n", tei);
  	q921_transmit(pri,(q921_h *)&(f->h),8);
  	free(f);
    }
}

  static void q921_send_teiremove(struct pri *pri, int tei) {
      q921_u *f;
      unsigned short ri=0x6464;
      ri = rand();
  
      if (pri->localtype != BRI_NETWORK_PTMP) {
  	pri_error(pri, "TEI remove for non-ptmp???\n"); 
  	return;
      }
  
      f = malloc(sizeof(q921_u) + 5 + 2);
      memset(f,0x0,sizeof(q921_u) + 5 + 2);
      if (f) {
  	f->h.sapi = Q921_SAPI_LAYER2_MANAGEMENT;
  	f->h.tei = Q921_TEI_GROUP;
  	f->h.c_r = 1;	
  	f->h.ea1 = 0;	
  	f->h.ea2 = 1;	
  	f->m2 = 0;
  	f->m3 = 0;
  	f->ft = Q921_FRAMETYPE_U;
  	f->data[0] = 0xf;
  	f->data[1] = ri >> 8;
  	f->data[2] = ri & 0xff;
  	f->data[3] = Q921_TEI_ID_REMOVE;
  	f->data[4] = (tei << 1) | 0x1;
  	if (pri->debug & PRI_DEBUG_Q921_STATE)
      	    pri_message(pri, "Sending TEI remove tei=%d\n",tei);
  	q921_transmit(pri,(q921_h *)&(f->h),8);
  	free(f);
      }
  }
  
  static void q921_send_teidenied(struct pri *pri, int ri, int tei) {
      q921_u *f;
  
      if (pri->localtype != BRI_NETWORK_PTMP) {
  	pri_error(pri, "TEI ID denied for non-ptmp???\n"); 
  	return;
      }
  
      f = malloc(sizeof(q921_u) + 5 + 2);
      memset(f,0x0,sizeof(q921_u) + 5 + 2);
      if (f) {
  	f->h.sapi = Q921_SAPI_LAYER2_MANAGEMENT;
  	f->h.tei = Q921_TEI_GROUP;
  	f->h.c_r = 1;	
  	f->h.ea1 = 0;	
  	f->h.ea2 = 1;	
  	f->m2 = 0;
  	f->m3 = 0;
  	f->ft = Q921_FRAMETYPE_U;
  	f->data[0] = 0xf;
  	f->data[1] = ri >> 8;
  	f->data[2] = ri & 0xff;
  	f->data[3] = Q921_TEI_ID_DENIED;
  	f->data[4] = (tei << 1) | 0x1;
  	if (pri->debug & PRI_DEBUG_Q921_STATE)
      	    pri_message(pri, "Sending TEI ID denied tei=%d\n",tei);
  	q921_transmit(pri,(q921_h *)&(f->h),8);
  	free(f);
      }
  }
  
 
static void q921_send_sabme_now(void *vpri, int tei);
  
static void q921_send_sabme(void *vpri, int now, int tei)
{
	struct pri *pri = vpri;
	q921_h h;
  	int teio=tei - Q921_TEI_BASE;
  	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
  
  	if (pri->sabme_timer[teio]) {
  	    pri_schedule_del(pri, pri->sabme_timer[teio]);
  	    pri->sabme_timer[teio] = 0;
  	}
  	pri->sabme_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T200], q921_send_sabme_now, pri, tei);
	if (!now)
		return;
	if (pri->sabme_retrans[teio]++ > (pri->timers[PRI_TIMER_N202])) {
	    /* delete txqueue */
	    q921_flush_txqueue(pri, tei, 1);
	    /* N202 retransmissions, activation of layer 2 failed, tell upper layer, start again */
	    pri->schedev = 1;
	    pri->ev.gen.e = PRI_EVENT_DCHAN_DOWN;
	    pri->ev.gen.tei = tei;
	    pri->sabme_retrans[teio] = 0;
#ifdef RELAX_TRB
  	    pri->sabme_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T200] + 3000, q921_send_sabme_now, pri, tei);
#endif
	    return;
	}
	Q921_INIT(pri, h);
 	// XXX
 	h.h.tei = tei;
	h.u.m3 = 3;	/* M3 = 3 */
	h.u.m2 = 3;	/* M2 = 3 */
	h.u.p_f = 1;	/* Poll bit set */
	h.u.ft = Q921_FRAMETYPE_U;
	switch(pri->localtype) {
	case PRI_NETWORK:
		h.h.c_r = 1;
		break;
	case PRI_CPE:
		h.h.c_r = 0;
		break;
	case BRI_NETWORK_PTMP:
		h.h.c_r = 1;
		break;
	case BRI_CPE_PTMP:
		h.h.c_r = 0;
		h.h.tei = pri->tei;
		break;
	case BRI_NETWORK:
		h.h.c_r = 1;
		break;
	case BRI_CPE:
		h.h.c_r = 0;
		break;
	default:
		pri_error(pri, "Don't know how to send SABME on a type %d node\n", pri->localtype);
		return;
	}
	if (pri->debug & PRI_DEBUG_Q921_STATE)
		pri_message(pri, "Sending Set Asynchronous Balanced Mode Extended\n");
	q921_transmit(pri, &h, 3);
 	pri->q921_state[teio] = Q921_AWAITING_ESTABLISH;
}

static void q921_send_sabme_now(void *vpri, int tei)
{
	q921_send_sabme(vpri, 1, tei);
}

  

static int q921_ack_packet(struct pri *pri, int num, int tei)
{
	struct q921_frame *f, *prev = NULL;
	int teio=tei - Q921_TEI_BASE;
	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
	f = pri->txqueue[teio];
	while(f) {
		if (f->h.n_s == num) {
			/* Cancel each packet as necessary */
			/* That's our packet */
			if (prev)
				prev->next = f->next;
			else
				pri->txqueue[teio] = f->next;
			if (pri->debug & PRI_DEBUG_Q921_STATE)
				pri_message(pri, "-- ACKing packet %d, new txqueue is %d (-1 means empty)\n", f->h.n_s, pri->txqueue[teio] ? pri->txqueue[teio]->h.n_s : -1);
			/* Update v_a */
			pri->v_a[teio] = num;
			free(f);
			/* Reset retransmission counter if we actually acked something */
			pri->retrans[teio] = 0;
			/* Decrement window size */
			pri->windowlen[teio]--;
			/* Search for something to send */
			f = pri->txqueue[teio];
			while(f) {
				if (!f->transmitted) {
					/* Send it now... */
					if (pri->debug & PRI_DEBUG_Q921_STATE)
						pri_message(pri, "-- Finally transmitting %d, since window opened up\n", f->h.n_s);
					f->transmitted++;
					pri->windowlen[teio]++;
					f->h.n_r = pri->v_r[teio];
					q921_transmit(pri, (q921_h *)(&f->h), f->len);
					break;
				}
				f = f->next;
			}
			return 1;
		}
		prev = f;
		f = f->next;
	}
	return 0;
}

static void t203_expire(void *, int tei);
static void t200_expire(void *, int tei);
static pri_event *q921_dchannel_down(struct pri *pri, int tei);

static void reschedule_t203(struct pri *pri, int tei)
{
 	int teio=tei - Q921_TEI_BASE;
 	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
 	if (pri->t203_timer[teio]) {
 		pri_schedule_del(pri, pri->t203_timer[teio]);
		pri->t203_timer[teio] = 0;
		if (pri->q921_state[teio] != Q921_LINK_CONNECTION_RELEASED) {
  		    if (pri->debug &  PRI_DEBUG_Q921_STATE)
  			pri_message(pri, "-- Restarting T203 counter\n");
		    /* Nothing to transmit, start the T203 counter instead */
 		    pri->t203_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T203], t203_expire, pri, tei);
		}
  	}
}

static void q921_flush_txqueue(struct pri *pri, int tei, int devnull)
{
	struct q921_frame *f, *tmp = NULL;	
	int teio=tei - Q921_TEI_BASE;
	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
	f = pri->txqueue[teio];

	/* nothing to send */
	if (!f) return;

	/* transmit all i-frames that were queued up while we were waiting for layer 2 to rise */
	while(f) {
	    if (devnull) {
		tmp = f;
		f = f->next;
		free(tmp);
		tmp = NULL;
	    } else {
		if (pri->localtype == BRI_CPE_PTMP) {
		    /* update TEI, it might have changed */
		    f->h.h.tei = pri->tei;
		}
		q921_transmit(pri, (q921_h *)&f->h, f->len);
		f->transmitted++;
		f = f->next;
	    }
	}
	
	if (devnull) {
	    pri->txqueue[teio] = NULL;
	    pri->v_s[teio] = 0;
	} else {
	    if (pri->t200_timer[teio]) {
	        pri_schedule_del(pri, pri->t200_timer[teio]);
	        pri->t200_timer[teio] = 0;
	    }
	    if (pri->t203_timer[teio]) {
	        pri_schedule_del(pri, pri->t203_timer[teio]);
	        pri->t203_timer[teio] = 0;
	    }
	    pri->t200_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T200], t200_expire, pri, tei);
	}
}

static pri_event *q921_ack_rx(struct pri *pri, int ack, int tei)
{
	int x;
	int cnt=0;
	pri_event *ev;
 	int teio=tei - Q921_TEI_BASE;
 	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
	/* Make sure the ACK was within our window */
	for (x=pri->v_a[teio]; (x != pri->v_s[teio]) && (x != ack); Q921_INC(x));
	if (x != ack) {
		/* ACK was outside of our window --- ignore */
		pri_error(pri, "ACK received for '%d' outside of window of '%d' to '%d', restarting\n", ack, pri->v_a[teio], pri->v_s[teio]);
		ev = q921_dchannel_down(pri, tei);
		q921_start(pri, 1, tei);
		pri->schedev = 1;
		return ev;
	}
	/* Cancel each packet as necessary */
	if (pri->debug & PRI_DEBUG_Q921_STATE)
		pri_message(pri, "-- ACKing all packets from %d to (but not including) %d\n", pri->v_a[teio], ack);
	for (x=pri->v_a[teio]; x != ack; Q921_INC(x)) 
		cnt += q921_ack_packet(pri, x, tei);	
	if (!pri->txqueue[teio]) {
		/* Something was ACK'd.  Stop T200 counter */
		if (pri->t200_timer[teio]) {
		    pri_schedule_del(pri, pri->t200_timer[teio]);
		    pri->t200_timer[teio] = 0;
		}
		if (pri->debug &  PRI_DEBUG_Q921_STATE)
			pri_message(pri, "-- Since there was nothing left, stopping T200 counter\n");
	}
	if (pri->t203_timer[teio]) {
		pri_schedule_del(pri, pri->t203_timer[teio]);
		pri->t203_timer[teio] = 0;
		if (pri->debug &  PRI_DEBUG_Q921_STATE)
			pri_message(pri, "-- Stopping T203 counter since we got an ACK\n");
	}
	if (pri->txqueue[teio]) {
		/* Something left to transmit, Start the T200 counter again if we stopped it */
		if (pri->t200_timer[teio]) {
		    pri_schedule_del(pri, pri->t200_timer[teio]);
		    pri->t200_timer[teio] = 0;
		}
		if (pri->debug &  PRI_DEBUG_Q921_STATE)
			pri_message(pri, "-- Something left to transmit (%d), restarting T200 counter\n", pri->txqueue[teio]->h.n_s);
		pri->t200_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T200], t200_expire, pri, tei);

	} else {
		if (pri->debug &  PRI_DEBUG_Q921_STATE)
			pri_message(pri, "-- Nothing left, starting T203 counter\n");
		/* Nothing to transmit, start the T203 counter instead */
		pri->t203_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T203], t203_expire, pri, tei);
	}
	return NULL;
}

static void q921_reject(struct pri *pri, int pf, int tei)
{
	q921_h h;
 	int teio=tei - Q921_TEI_BASE;
 	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
	Q921_INIT(pri, h);
	h.s.x0 = 0;	/* Always 0 */
	h.s.ss = 2;	/* Reject */
	h.s.ft = 1;	/* Frametype (01) */
	h.s.n_r = pri->v_r[teio];	/* Where to start retransmission */
	h.s.p_f = pf;	
	switch(pri->localtype) {
	case PRI_NETWORK:
		h.h.c_r = 0;
		break;
	case PRI_CPE:
		h.h.c_r = 1;
		break;
	case BRI_NETWORK_PTMP:
		h.h.c_r = 0;
		break;
	case BRI_CPE_PTMP:
		h.h.c_r = 1;
		h.h.tei = tei;
		break;
	case BRI_NETWORK:
		h.h.c_r = 0;
		break;
	case BRI_CPE:
		h.h.c_r = 1;
		break;
	default:
		pri_error(pri, "Don't know how to REJECT on a type %d node\n", pri->localtype);
		return;
	}
	if (pri->debug & PRI_DEBUG_Q921_STATE)
		pri_message(pri, "Sending Reject (%d)\n", pri->v_r[teio]);
	pri->sentrej[teio] = 1;
	q921_transmit(pri, &h, 4);
}

static void q921_rr(struct pri *pri, int pbit, int cmd, int tei) {
	q921_h h;
	int teio=tei - Q921_TEI_BASE;
	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
	Q921_INIT(pri, h);
	h.s.x0 = 0;	/* Always 0 */
	h.s.ss = 0; /* Receive Ready */
	h.s.ft = 1;	/* Frametype (01) */
	h.s.n_r = pri->v_r[teio];	/* N/R */
	h.s.p_f = pbit;		/* Poll/Final set appropriately */
	switch(pri->localtype) {
	case PRI_NETWORK:
		if (cmd)
			h.h.c_r = 1;
		else
			h.h.c_r = 0;
		break;
	case PRI_CPE:
		if (cmd)
			h.h.c_r = 0;
		else
			h.h.c_r = 1;
		break;
 	case BRI_NETWORK_PTMP:
 		h.h.tei = tei;
 		if (cmd)
 			h.h.c_r = 1;
 		else
 			h.h.c_r = 0;
 		break;
 	case BRI_CPE_PTMP:
 		h.h.tei = tei;
 		if (cmd)
 			h.h.c_r = 0;
 		else
 			h.h.c_r = 1;
 		break;
 	case BRI_NETWORK:
 		if (cmd)
 			h.h.c_r = 1;
 		else
 			h.h.c_r = 0;
 		break;
 	case BRI_CPE:
 		if (cmd)
 			h.h.c_r = 0;
 		else
			h.h.c_r = 1;
 		break;
	default:
		pri_error(pri, "Don't know how to RR on a type %d node\n", pri->localtype);
		return;
	}
	pri->v_na[teio] = pri->v_r[teio];	/* Make a note that we've already acked this */
	if (pri->debug & PRI_DEBUG_Q921_STATE)
		pri_message(pri, "Sending Receiver Ready (%d)\n", pri->v_r[teio]);
	q921_transmit(pri, &h, 4);
}

static void t200_expire(void *vpri, int tei)
{
	struct pri *pri = vpri;
 	int teio=tei - Q921_TEI_BASE;
 	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
 	if (pri->txqueue[teio]) {
		/* Retransmit first packet in the queue, setting the poll bit */
		if (pri->debug & PRI_DEBUG_Q921_STATE)
			pri_message(pri, "-- T200 counter expired, What to do...\n");
		if (pri->txqueue[teio]->transmitted) {
		    /* Force Poll bit, if this is a retransmission */
		    pri->txqueue[teio]->h.p_f = 1;	
		    pri->solicitfbit[teio] = 1;
		    /* Update nr */
		    pri->txqueue[teio]->h.n_r = pri->v_r[teio];
		    pri->v_na[teio] = pri->v_r[teio];
		    pri->retrans[teio]++;
		}
      /* Up to three retransmissions */
      if ((pri->retrans[teio] + 1)  <= pri->timers[PRI_TIMER_N200]) {
         /* Reschedule t200_timer */
         if (pri->debug & PRI_DEBUG_Q921_STATE)
        	pri_message(pri, "-- Retransmitting %d bytes\n", pri->txqueue[teio]->len);
		if (pri->busy[teio]) {
        	// pri_message(pri, "-- q921_rr(pri, 1, 1) \n", tei);
			q921_rr(pri, 1, 1, tei);
		} else {
			if (!pri->txqueue[teio]->transmitted)
				pri_error(pri, "!! Not good - head of queue has not been transmitted yet\n");
			q921_transmit(pri, (q921_h *)&pri->txqueue[teio]->h, pri->txqueue[teio]->len);
	    //layer 3
			pri->txqueue[teio]->transmitted++;
		}
         if (pri->debug & PRI_DEBUG_Q921_STATE) 
               pri_message(pri, "-- Rescheduling retransmission (%d)\n", pri->retrans[teio]);
         pri->t200_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T200], t200_expire, pri, tei);
      } else {
         if (pri->debug & PRI_DEBUG_Q921_STATE) 
               pri_message(pri, "-- Timeout occured, restarting PRI\n");
         pri->q921_state[teio] = Q921_LINK_CONNECTION_RELEASED;
      	 pri->t200_timer[teio] = 0;
         q921_dchannel_down(pri, tei);
         q921_start(pri, 1, tei);
         pri->schedev = 1;
      }
	} else if (pri->solicitfbit[teio]) {
         if (pri->debug & PRI_DEBUG_Q921_STATE)
            pri_message(pri, "-- Retrying poll with f-bit\n");
		pri->retrans[teio]++;
		if (pri->retrans[teio] < pri->timers[PRI_TIMER_N200]) {
			pri->solicitfbit[teio] = 1;
			q921_rr(pri, 1, 1, tei);
			pri->t200_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T200], t200_expire, pri, tei);
		} else {
			if (pri->debug & PRI_DEBUG_Q921_STATE) 
				pri_message(pri, "-- Timeout occured, restarting PRI\n");
			pri->q921_state[teio] = Q921_LINK_CONNECTION_RELEASED;
			pri->t200_timer[teio] = 0;
			q921_dchannel_down(pri, tei);
			q921_start(pri, 1, tei);
			pri->schedev = 1;
		}
	} else {
		pri_error(pri, "T200 counter expired, nothing to send...\n");
	   	pri->t200_timer[teio] = 0;
		if (pri->busy[teio]) {
		    if ((pri->retrans[teio] + 1) <= pri->timers[PRI_TIMER_N200]) {
			/* poll RR */
    			pri->t200_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T200], t200_expire, pri, tei);
			pri->retrans[teio]++;
			q921_rr(pri, 1, 1, tei);
		    } else {
			q921_reset(pri, tei, 1);
     			q921_start(pri, 1, tei);
		    }
		}
	}
}

int q921_transmit_uframe(struct pri *pri, void *buf, int len, int cr, int tei)
{
 	q921_u *uf;
	uf = malloc(sizeof(q921_u) + len + 2);
	memset(uf,0,sizeof(q921_u) + len + 2);

	uf->h.sapi = 0;
	uf->h.ea1 = 0;
 	uf->h.ea2 = 1;
	uf->h.tei = tei;
	uf->m3 = 0;
	uf->m2 = 0;
	uf->ft = Q921_FRAMETYPE_U;
	switch(pri->localtype) {
	case PRI_NETWORK:
		uf->h.c_r = 1;
		break;
	case PRI_CPE:
		uf->h.c_r = 0;
		break;
	case BRI_NETWORK_PTMP:
		uf->h.c_r = 1;
 		break;
 	case BRI_CPE_PTMP:
  		uf->h.c_r = 0;
  		break;
  	case BRI_NETWORK:
  		uf->h.c_r = 1;
  		break;
  	case BRI_CPE:
  		uf->h.c_r = 0;
  		break;
  	default:
  		pri_error(pri, "Don't know how to send U frames on a type %d node\n", pri->localtype);
  		return -1;
   	}
  	memcpy(uf->data,buf,len);
  	q921_transmit(pri, (q921_h*)&(uf->h), 3+len);
  	free(uf);
  	return 0;
}


int q921_transmit_iframe(struct pri *pri, void *buf, int len, int cr, int tei)
{
	q921_frame *f, *prev=NULL;
 	int teio=tei - Q921_TEI_BASE;
 	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
	if ((pri->q921_state[teio] == Q921_LINK_CONNECTION_RELEASED) && (!pri->sabme_timer[teio])) {
	    if (pri->localtype == BRI_CPE_PTMP) {
		if (pri->tei > 0) {
		    /* p2p datalink is down */
		    pri->sabme_retrans[teio] = 0;
		    q921_send_sabme_now(pri, pri->tei);
		    if (pri->debug & PRI_DEBUG_Q921_STATE)
		        pri_message(pri, "Reactivating layer 2\n");
		} else {
		    /* no tei */
		    if (pri->debug & PRI_DEBUG_Q921_STATE)
			pri_message(pri, "reactivating layer 2, sending tei req\n");
		    q921_send_teireq(pri);
		}
	    } else if ((pri->localtype == BRI_CPE) || (pri->localtype == BRI_NETWORK)) {
		/* p2p datalink is down */
		pri->sabme_retrans[teio] = 0;
		q921_send_sabme_now(pri, pri->tei);
		if (pri->debug & PRI_DEBUG_Q921_STATE)
		    pri_message(pri, "Reactivating layer 2\n");
	    }
	}
 	for (f=pri->txqueue[teio]; f; f = f->next) prev = f;
	f = malloc(sizeof(q921_frame) + len + 2);
	if (f) {
		memset(f,0,sizeof(q921_frame) + len + 2);
		Q921_INIT(pri, f->h);
		switch(pri->localtype) {
		case PRI_NETWORK:
			if (cr)
				f->h.h.c_r = 1;
			else
				f->h.h.c_r = 0;
		break;
		case PRI_CPE:
			if (cr)
				f->h.h.c_r = 0;
			else
				f->h.h.c_r = 1;
		break;
  		case BRI_NETWORK_PTMP:
  			f->h.h.tei = tei;
  			if (cr)
  				f->h.h.c_r = 1;
  			else
  				f->h.h.c_r = 0;
  		break;
  		case BRI_CPE_PTMP:
  			f->h.h.tei = pri->tei;
  			if (cr)
  				f->h.h.c_r = 0;
  			else
  				f->h.h.c_r = 1;
  		break;
  		case BRI_NETWORK:
  			if (cr)
  				f->h.h.c_r = 1;
  			else
  				f->h.h.c_r = 0;
  		break;
  		case BRI_CPE:
  			if (cr)
  				f->h.h.c_r = 0;
  			else
  				f->h.h.c_r = 1;
  		break;
		}
		f->next = NULL;
		f->transmitted = 0;
		f->len = len + 4;
		memcpy(f->h.data, buf, len);
		f->h.n_s = pri->v_s[teio];
		f->h.n_r = pri->v_r[teio];
		pri->v_s[teio]++;
		pri->v_na[teio] = pri->v_r[teio];
		f->h.p_f = 0;
		f->h.ft = 0;
		if (prev)
			prev->next = f;
		else
			pri->txqueue[teio] = f;

		if  ((pri->q921_state[teio] != Q921_LINK_CONNECTION_ESTABLISHED) && ((pri->localtype == BRI_CPE_PTMP) || (pri->localtype == BRI_CPE) || (pri->localtype == BRI_NETWORK))){
		    /* no p2p datalink, yet. queue up the iframes... */
		    if (pri->debug & PRI_DEBUG_Q921_STATE)
			pri_message(pri, "Layer 3 transmit waiting for layer 2\n");		
		} else {
		    /* Immediately transmit unless we're in a recovery state, or the window
			size is too big */
		    if (!pri->retrans[teio] && !pri->busy[teio]) {
			if (pri->windowlen[teio] < pri->window[teio]) {
				pri->windowlen[teio]++;
				q921_transmit(pri, (q921_h *)(&f->h), f->len);
				f->transmitted++;
			} else {
				if (pri->debug & PRI_DEBUG_Q921_STATE)
					pri_message(pri, "Delaying transmission of %d, window is %d/%d long\n", 
						f->h.n_s, pri->windowlen[teio], pri->window[teio]);
			}
		    }
		    if (pri->t203_timer[teio]) {
			pri_schedule_del(pri, pri->t203_timer[teio]);
			pri->t203_timer[teio] = 0;
			if (pri->debug & PRI_DEBUG_Q921_STATE)
				pri_message(pri, "Stopping T_203 timer\n");
		    }
		    if (!pri->t200_timer[teio]) {
			if (pri->debug & PRI_DEBUG_Q921_STATE)
				pri_message(pri, "Starting T_200 timer\n");
			pri->t200_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T200], t200_expire, pri, tei);
		    } else
			if (pri->debug & PRI_DEBUG_Q921_STATE)
				pri_message(pri, "T_200 timer already going (%d)\n", pri->t200_timer[teio]);
		}
		
	} else {
		pri_error(pri, "!! Out of memory for Q.921 transmit\n");
		return -1;
	}
	return 0;
}

static void t203_expire(void *vpri, int tei)
{
  	struct pri *pri = vpri;
 	int teio=tei - Q921_TEI_BASE;
 	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
 
 	if (pri->q921_state[teio] == Q921_LINK_CONNECTION_ESTABLISHED) {
  		if (pri->debug &  PRI_DEBUG_Q921_STATE)
			pri_message(pri, "T203 counter expired, sending RR and scheduling T203 again\n");
  		/* Solicit an F-bit in the other's RR */
		pri->solicitfbit[teio] = 1;
		pri->retrans[teio] = 0;
		q921_rr(pri, 1, 1, tei);
  		/* Start timer T200 to resend our RR if we don't get it */
		pri->t203_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T200], t200_expire, pri, tei);
  	} else {
  		if (pri->debug &  PRI_DEBUG_Q921_STATE)
			pri_message(pri, "T203 counter expired in weird state %d\n", pri->q921_state[teio]);
		pri->t203_timer[teio] = 0;
  	}
}

static void q921_start_tei(struct pri *pri, int tei);


static void t201_expire(void *vpri, int tei)
{
  	struct pri *pri = vpri;
 	int teio=tei - Q921_TEI_BASE;
	int i = 0;
 	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
 	if (tei == Q921_TEI_GROUP) {
	    for (i=0;i<Q921_MAX_TEIS;i++) {
		if (pri->q921_tei_check[i] == 1) {
		    pri->q921_tei_check[i] = 0;
		    q921_start_tei(pri, Q921_TEI_BASE + i);
		}
	    }
	} else {
	    if (pri->q921_tei_check[teio] == 1) {
		pri->q921_tei_check[teio] = 0;
		q921_start_tei(pri, tei);
	    }
	}
	pri->t201_timer[teio] = 0;
}

static pri_event *q921_handle_iframe(struct pri *pri, q921_i *i, int len)
{
	int res;
	pri_event *ev;
	int teio= i->h.tei - Q921_TEI_BASE;
	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
//	pri_error(pri, "q921_handle_iframe: i->n_s %d pri->v_r[%d] %d \n", i->n_s, teio, pri->v_r[teio]);
	/* Make sure this is a valid packet */
	if (i->n_s == pri->v_r[teio]) {
		/* Increment next expected I-frame */
		Q921_INC(pri->v_r[teio]);
		/* Handle their ACK */
		pri->sentrej[teio] = 0;
		ev = q921_ack_rx(pri, i->n_r, i->h.tei);
		if (ev) {
		    pri_error(pri, "q921_handle_iframe: ev = %d \n", ev->e);
		}
		if (ev)
			return ev;
		if (i->p_f) {
			/* If the Poll/Final bit is set, immediate send the RR */
			q921_rr(pri, 1, 0, i->h.tei);
		} else if (pri->busy[teio]) {
			q921_rr(pri, 0, 0, i->h.tei);
		} else if (pri->t200_timer[teio] && pri->retrans[teio]) {
		pri_error(pri, "q921_handle_iframe: sending RR \n");
			q921_rr(pri, 0, 0, i->h.tei);
		}
		/* Receive Q.931 data */
		res = q931_receive(pri, (q931_h *)i->data, len - 4, i->h.tei);
		/* Send an RR if one wasn't sent already */
		if (pri->v_na[teio] != pri->v_r[teio]) 
			q921_rr(pri, 0, 0,  i->h.tei);
		if (res == -1) {
			return NULL;
		}
		if (res & Q931_RES_HAVEEVENT)
			return &pri->ev;
	} else {
		/* If we haven't already sent a reject, send it now, otherwise
		   we are obliged to RR */
		if (!pri->sentrej[teio])
			q921_reject(pri, i->p_f, i->h.tei);
		else if (i->p_f)
			q921_rr(pri, 1, 0, i->h.tei);
	}
	return NULL;
}

void q921_dump(struct pri *pri, q921_h *h, int len, int showraw, int txrx)
{
	int x;
        char *type;
	char direction_tag;
	
	direction_tag = txrx ? '>' : '<';
	if (showraw) {
		char *buf = malloc(len * 3 + 1);
		int buflen = 0;
		if (buf) {
			for (x=0;x<len;x++) 
				buflen += sprintf(buf + buflen, "%02x ", h->raw[x]);
			pri_message(pri, "\n%c [ %s]\n", direction_tag, buf);
			free(buf);
		}
	}

	switch (h->h.data[0] & Q921_FRAMETYPE_MASK) {
	case 0:
	case 2:
		pri_message(pri, "\n%c Informational frame:\n", direction_tag);
		break;
	case 1:
		pri_message(pri, "\n%c Supervisory frame:\n", direction_tag);
		break;
	case 3:
		pri_message(pri, "\n%c Unnumbered frame:\n", direction_tag);
		break;
	}
	
	pri_message(pri, 
"%c SAPI: %02d  C/R: %d EA: %d\n"
"%c  TEI: %03d        EA: %d\n", 
    	direction_tag,
	h->h.sapi, 
	h->h.c_r,
	h->h.ea1,
    	direction_tag,
	h->h.tei,
	h->h.ea2);
	switch (h->h.data[0] & Q921_FRAMETYPE_MASK) {
	case 0:
	case 2:
		/* Informational frame */
		pri_message(pri, 
"%c N(S): %03d   0: %d\n"
"%c N(R): %03d   P: %d\n"
"%c %d bytes of data\n",
    	direction_tag,
	h->i.n_s,
	h->i.ft,
    	direction_tag,
	h->i.n_r,
	h->i.p_f, 
    	direction_tag,
	len - 4);
		break;
	case 1:
		/* Supervisory frame */
		type = "???";
		switch (h->s.ss) {
		case 0:
			type = "RR (receive ready)";
			break;
		case 1:
			type = "RNR (receive not ready)";
			break;
		case 2:
			type = "REJ (reject)";
			break;
		}
		pri_message(pri, 
"%c Zero: %d     S: %d 01: %d  [ %s ]\n"
"%c N(R): %03d P/F: %d\n"
"%c %d bytes of data\n",
    	direction_tag,
	h->s.x0,
	h->s.ss,
	h->s.ft,
	type,
	direction_tag,
	h->s.n_r,
	h->s.p_f, 
	direction_tag,
	len - 4);
		break;
	case 3:		
		/* Unnumbered frame */
		type = "???";
		if (h->u.ft == 3) {
			switch (h->u.m3) {
			case 0:
				if (h->u.m2 == 3)
					type = "DM (disconnect mode)";
				else if (h->u.m2 == 0)
					type = "UI (unnumbered information)";
				break;
			case 2:
				if (h->u.m2 == 0)
					type = "DISC (disconnect)";
				break;
			case 3:
			       	if (h->u.m2 == 3)
					type = "SABME (set asynchronous balanced mode extended)";
				else if (h->u.m2 == 0)
					type = "UA (unnumbered acknowledgement)";
				break;
			case 4:
				if (h->u.m2 == 1)
					type = "FRMR (frame reject)";
				break;
			case 5:
				if (h->u.m2 == 3)
					type = "XID (exchange identification note)";
				break;
			}
		}
		pri_message(pri, 
"%c   M3: %d   P/F: %d M2: %d 11: %d  [ %s ]\n"
"%c %d bytes of data\n",
	direction_tag,
	h->u.m3,
	h->u.p_f,
	h->u.m2,
	h->u.ft,
	type,
	direction_tag,
	len - 3);
		break;
	};
}

static void q921_tei_recovery(struct pri *pri, int tei) {
    int i = 0;
    int teio=tei - Q921_TEI_BASE;
    if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
    for (i=0;i<Q921_MAX_TEIS;i++) {
	if ((i + Q921_TEI_BASE == tei) || (tei == Q921_TEI_GROUP)) {
	    pri->q921_tei_check[i] = 1;
	}
    }
    q921_send_teichkreq(pri, tei);
    if (!pri->t201_timer[teio]) {
	pri->t201_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T201], t201_expire, pri, tei);
    }
}
/*
static void q921_invoke_tei_recovery(void *vpri, int tei) {
  	struct pri *pri = vpri;
	q921_tei_recovery(pri, tei);
}
*/

static pri_event *q921_dchannel_up(struct pri *pri, int tei)
{
 	// we treat this as MFE
 	int teio=tei - Q921_TEI_BASE;
 	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }

	if  (((pri->localtype == BRI_CPE_PTMP) || (pri->localtype == BRI_CPE) || (pri->localtype == BRI_NETWORK)) && (pri->txqueue[teio])) {
	    /* Reset counters, etc */
	    q921_reset(pri, tei, 0);
	} else {
	    /* Reset counters, discard frames, etc */
	    q921_reset(pri, tei, 1);
	}
	
	/* Stop any SABME retransmissions */
 	if (pri->sabme_timer[teio]) {
 	    pri_schedule_del(pri, pri->sabme_timer[teio]);
 	    pri->sabme_timer[teio] = 0;
 	}
 	
 	if (pri->t202_timer[teio]) {
 	    pri_schedule_del(pri, pri->t202_timer[teio]);
 	    pri->t202_timer[teio] = 0;
 	}
 	
/* neonova test */
 	if (pri->t203_timer[teio]) {
 	    pri_schedule_del(pri, pri->t203_timer[teio]);
 	    pri->t203_timer[teio] = 0;
 	}
	
	/* Reset any rejects */
	pri->sentrej[teio] = 0;
	
	/* Go into connection established state */
	pri->q921_state[teio] = Q921_LINK_CONNECTION_ESTABLISHED;
	
	if  (((pri->localtype == BRI_CPE_PTMP) || (pri->localtype == BRI_CPE) || (pri->localtype == BRI_NETWORK)) && (pri->txqueue[teio])) {
	    /* transmit queued iframes ans start T200 (handled by flush_txqueue) */
	    q921_flush_txqueue(pri, tei, 0);
	    /* dont upset upper layers if we reactivate layer 2 */
	    return NULL;
	} else {
	    /* Start the T203 timer */
	    pri->t203_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T203], t203_expire, pri, tei);
	    
	    /* Report event that D-Channel is now up */
	    pri->ev.gen.e = PRI_EVENT_DCHAN_UP;
	    pri->ev.gen.tei = tei;
	    return &pri->ev;
	}
}

static pri_event *q921_dchannel_down(struct pri *pri, int tei)
{
 	int teio=tei - Q921_TEI_BASE;
 	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }

	pri->q921_state[teio] = Q921_LINK_CONNECTION_RELEASED;

	/* Reset counters, reset sabme timer etc */
 	if (pri->localtype == BRI_NETWORK_PTMP) {
 	    if (pri->t203_timer[teio]) {
 		pri_schedule_del(pri, pri->t203_timer[teio]);
 		pri->t203_timer[teio] = 0;
 		if (pri->debug & PRI_DEBUG_Q921_STATE) {
 			pri_message(pri, "Stopping T_203 timer for TEI %d\n", tei);
 		}
 	    }
 	}

	q921_reset(pri, tei, 1);
	
 	/* Report event that D-Channel is now down */
	pri->ev.gen.e = PRI_EVENT_DCHAN_DOWN;
 	pri->ev.gen.tei = tei;
	return &pri->ev;
}

void q921_reset(struct pri *pri, int tei, int discard)
{
 	int teio=tei - Q921_TEI_BASE;
 	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }
	/* Having gotten a SABME we MUST reset our entire state */
	if (discard) {
	    pri->v_s[teio] = 0;
	}
	pri->v_a[teio] = 0;
	pri->v_r[teio] = 0;
	pri->v_na[teio] = 0;
	pri->window[teio] = pri->timers[PRI_TIMER_K];
	pri->windowlen[teio] = 0;
	pri_schedule_del(pri, pri->sabme_timer[teio]);
	pri_schedule_del(pri, pri->t203_timer[teio]);
	pri_schedule_del(pri, pri->t200_timer[teio]);
	pri->sabme_timer[teio] = 0;
	pri->t203_timer[teio] = 0;
	pri->t200_timer[teio] = 0;
 	pri_schedule_del(pri, pri->t202_timer[teio]);
 	pri->t202_timer[teio] = 0;
 	pri_schedule_del(pri, pri->t201_timer[teio]);
 	pri->t201_timer[teio] = 0;
	pri->busy[teio] = 0;
	pri->solicitfbit[teio] = 0;
	pri->q921_state[teio] = Q921_LINK_CONNECTION_RELEASED;
	pri->retrans[teio] = 0;
	pri->sabme_retrans[teio] = 0;
	pri->sentrej[teio] = 0;
	
	if (discard) {
	    /* Discard anything waiting to go out */
	    q921_discard_retransmissions(pri, tei);
	}
}


static pri_event *__q921_receive_qualified(struct pri *pri, q921_h *h, int len)
{
	q921_frame *f;
	pri_event *ev;
	int sendnow;
	int tei;
	int res=-1;
	int teio=h->h.tei - Q921_TEI_BASE;
	if (((teio < 0) || (teio > Q921_MAX_TEIS)) || (pri->localtype != BRI_NETWORK_PTMP)) { teio=0; }

	switch(h->h.data[0] & Q921_FRAMETYPE_MASK) {
	case 0:
	case 2:
		if (pri->q921_state[teio] != Q921_LINK_CONNECTION_ESTABLISHED) {
			pri_error(pri, "!! Got I-frame while link state %d\n", pri->q921_state[teio]);
			return NULL;
		}
		/* Informational frame */
		if (len < 4) {
			pri_error(pri, "!! Received short I-frame (expected 4, got %d)\n", len);
			break;
		}
		return q921_handle_iframe(pri, &h->i, len);	
		break;
	case 1:
		if (pri->q921_state[teio] != Q921_LINK_CONNECTION_ESTABLISHED) {
			pri_error(pri, "!! Got S-frame while link down\n");
			q921_send_dm(pri, 1,  h->h.tei);
			q921_reset(pri, h->h.tei, 1);
			return NULL;
		}
		if (len < 4) {
			pri_error(pri, "!! Received short S-frame (expected 4, got %d)\n", len);
			break;
		}
		switch(h->s.ss) {
		case 0:
			/* Receiver Ready */
			pri->busy[teio] = 0;
			/* Acknowledge frames as necessary */
			ev = q921_ack_rx(pri, h->s.n_r, h->h.tei);
			if (ev)
				return ev;
			if (h->s.p_f) {
			/* If it's a p/f one then send back a RR in return with the p/f bit set */
//			pri_error(pri, "-- h->s.pf %d pri->solicitfbit[teio] %d\n", h->s.p_f, pri->solicitfbit[teio]);
				if (pri->solicitfbit[teio]) {
					if (pri->debug & PRI_DEBUG_Q921_STATE) 
						pri_message(pri, "-- Got RR response to our frame\n");
				} else {
					if (pri->txqueue[teio]) {
					    q921_flush_txqueue(pri, pri->tei, 0);
					} else {
					    if (pri->debug & PRI_DEBUG_Q921_STATE) 
						pri_message(pri, "-- Unsolicited RR with P/F bit, responding\n");
						q921_rr(pri, 1, 0, h->h.tei);
					}
				}
				pri->solicitfbit[teio] = 0;
			}
			break;
      case 1:
         /* Receiver not ready */
         if (pri->debug & PRI_DEBUG_Q921_STATE)
            pri_message(pri, "-- Got receiver not ready\n");
	 if ((pri->localtype != PRI_NETWORK) && (pri->localtype != BRI_NETWORK) && (pri->localtype != BRI_NETWORK_PTMP)) {
	     if (h->s.p_f && h->s.h.c_r) {
//	    	if (!pri->t200_timer[teio]) {
		    /* Send RR if poll bit set */
		    q921_rr(pri, h->s.p_f, 0, h->h.tei);
//	    	}
	    }
	 } else {
	     if (h->s.p_f && (!h->s.h.c_r)) {
//	    	if (!pri->t200_timer[teio]) {
		    /* Send RR if poll bit set */
		    q921_rr(pri, h->s.p_f, 0, h->h.tei);
//	    	}
	    }
	 }
         pri->busy[teio] = 1;
	 if (pri->t200_timer[teio]) {
	    pri_schedule_del(pri, pri->t200_timer[teio]);
	    pri->t200_timer[teio] = 0;
	 }
	 pri->t200_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T200], t200_expire, pri, h->h.tei);
         break;
      case 2:
         /* Just retransmit */
         if (pri->debug & PRI_DEBUG_Q921_STATE)
            pri_message(pri, "-- Got reject requesting packet %d...  Retransmitting.\n", h->s.n_r);
		 sendnow = 0;
// XXX		 
	q921_ack_rx(pri, h->s.n_r, h->h.tei);
         /* Reset t200 timer if it was somehow going */
         if (pri->t200_timer[teio]) {
               pri_schedule_del(pri, pri->t200_timer[teio]);
               pri->t200_timer[teio] = 0;
         }
         /* Resend the proper I-frame */
         for(f=pri->txqueue[teio];f;f=f->next) {
//	    pri_error(pri, "!! frame n_s %d transmitted %d (searching for %d)!\n", f->h.n_s, f->transmitted, h->s.n_r);
               if ((sendnow || (f->h.n_s == h->s.n_r)) && f->transmitted) {
                     /* Matches the request, or follows in our window, and has
					    already been transmitted. */

		     /* multiframe established */
		     f->transmitted = 0;
		     f->h.n_r = pri->v_r[teio];
		    
		    if (pri->busy[teio]) {
		      pri->busy[teio] = 0;
		      sendnow = 0;
		     } else {
		        f->h.p_f = 0;
		        sendnow = 1;
			pri_error(pri, "!! Got reject for frame %d, retransmitting frame %d now, updating n_r!\n", h->s.n_r, f->h.n_s);
                        q921_transmit(pri, (q921_h *)(&f->h), f->len);
			f->transmitted++;
		     }
               }
         }
         if (!sendnow) {
               if (pri->txqueue[teio]) {
		     if (h->s.p_f) {
        		q921_rr(pri, 1, 0, h->h.tei);
		     } else if (!h->s.p_f || h->s.n_r) {
			pri_error(pri, "!! Got reject for frame %d, but we only have others!\n", h->s.n_r);
		     }
               } else {
                     /* Hrm, we have nothing to send, but have been REJ'd.  Reset v_a, v_s, etc */
/*				pri_error(pri, "!! Got reject for frame %d, but we have nothing -- resetting!\n", h->s.n_r);
                     pri->v_a[teio] = h->s.n_r;
                     pri->v_s[teio] = h->s.n_r; */
                     /* Reset and restart t203 timer */
                     if (pri->t203_timer[teio])
                           pri_schedule_del(pri, pri->t203_timer[teio]);
                     pri->t203_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T203], t203_expire, pri, tei);
		     pri->solicitfbit[teio] = 0;
               }
         } else {
    	    if (h->s.p_f) {
        	/* If it has the poll bit set (and an iframe was retransmitted), send an appropriate supervisory response */
//        	q921_rr(pri, 1, 0, h->h.tei);
                 /* Reset t200 timer if it was somehow going */
                 if (pri->t200_timer[teio]) {
                       pri_schedule_del(pri, pri->t200_timer[teio]);
                       pri->t200_timer[teio] = 0;
                 }
                 /* Reset and restart t203 timer */
		  pri->solicitfbit[teio] = 0;
                 if (pri->t203_timer[teio])
                       pri_schedule_del(pri, pri->t203_timer[teio]);
                 pri->t203_timer[teio] = pri_schedule_event2(pri, pri->timers[PRI_TIMER_T203], t203_expire, pri, h->h.tei);
            }
	 }
         break;
		default:
			pri_error(pri, "!! XXX Unknown Supervisory frame ss=0x%02x,pf=%02xnr=%02x vs=%02x, va=%02x XXX\n", h->s.ss, h->s.p_f, h->s.n_r,
					pri->v_s[teio], pri->v_a[teio]);
		}
		break;
	case 3:
		if (len < 3) {
			pri_error(pri, "!! Received short unnumbered frame\n");
			break;
		}
		switch(h->u.m3) {
		case 0:
			if (h->u.m2 == 3) {
				if (h->u.p_f) {
					/* Section 5.7.1 says we should restart on receiving a DM response with the f-bit set to
					   one, but we wait T200 first */
					if (pri->debug & PRI_DEBUG_Q921_STATE)
						pri_message(pri, "-- Got DM Mode from peer.\n");
					/* Disconnected mode, try again after T200 */
					ev = q921_dchannel_down(pri, h->h.tei);
					if ((pri->localtype == BRI_CPE_PTMP) || (pri->localtype == BRI_CPE) || (pri->localtype == BRI_NETWORK)) {
    					    /* release layer 2 */
					//    pri_error(pri, "got DM, restarting layer 2.\n");
					//    return NULL;
     					    q921_start(pri, 0, h->h.tei);
 					}
 					if ((pri->localtype == PRI_NETWORK) || (pri->localtype == PRI_CPE)){
     					    q921_start(pri, 0, h->h.tei);
 					}
					return ev;
						
				} else {
					if (pri->debug & PRI_DEBUG_Q921_STATE)
						pri_message(pri, "-- Ignoring unsolicited DM with p/f set to 0\n");
#if 0
					/* Requesting that we start */
					q921_start(pri, 0, h->h.tei);
#endif					
				}
				break;
			} else if (h->u.m2 == 0) {
				if (h->u.ft == 3) {
 				    switch (h->u.data[0]) { /* Management Entity Identifier  */
 					case 0x0f:
 					/* TEI Procedure */
 					switch (h->u.data[3]) {
 					    case Q921_TEI_ID_VERIFY:
 						if (pri->localtype != BRI_NETWORK_PTMP)
 						    break;
 						    if (pri->debug & PRI_DEBUG_Q921_STATE)
 							pri_message(pri, "got TEI verify for TEI = %d\n",h->u.data[4] >> 1);
 					    break;
 					    case Q921_TEI_ID_ASSIGNED:
 						if (pri->localtype != BRI_CPE_PTMP)
 						    break;
 						if (pri->tei == (h->u.data[4] >> 1)) {
 						    // TEI already assgined, CHECK_TEI or REMOVE_TEI
 						    pri_error(pri, "Double assgined TEI!\n");
 						}
 						if (pri->ri == ((unsigned short) (h->u.data[1] << 8) + h->u.data[2])) {
 						    if (pri->t202_timer[0]) {
 							pri_schedule_del(pri, pri->t202_timer[0]);
 							pri->t202_timer[0] = 0;
 						    }
 						    pri->tei = h->u.data[4] >> 1; 
 						    pri->ri = 0;
 						    if (pri->debug & PRI_DEBUG_Q921_STATE)
 							pri_message(pri, "received TEI assign TEI = %d ri=%d\n",pri->tei,(unsigned short) (h->u.data[1] << 8) + h->u.data[2]);
						    pri->sabme_retrans[teio] = 0;
     						    q921_send_sabme_now(pri, pri->tei);
 						}
 					    break;
 					    case Q921_TEI_ID_REMOVE:
 					    	if (pri->localtype != BRI_CPE_PTMP)
 						    break;
 						if (((h->u.data[4] >> 1) == Q921_TEI_GROUP) || (pri->tei == (h->u.data[4] >> 1))){ 
 						    if (pri->debug & PRI_DEBUG_Q921_STATE)
 							pri_message(pri, "received TEI remove TEI = %d\n",pri->tei);
 						    pri->tei = 0;
 						    pri->ri = 0;
 						    // get a new TEI
 						    q921_reset(pri, 0, 1);
 						    q921_send_teireq(pri);
 						}
 					    break;
 					    case Q921_TEI_ID_REQUEST:
 						if (pri->localtype != BRI_NETWORK_PTMP)
 						    break;
 						
 						tei = h->u.data[4] >> 1;
 						if (tei != Q921_TEI_GROUP) {
 						    pri_message(pri, "got TEI request for unavailable TEI..\n");
 						    q921_send_teidenied(pri,((unsigned int)(h->u.data[1] << 8) + h->u.data[2]),h->u.data[4] >> 1);
 						    break;
 						}
 						
 						for (tei=0;tei<Q921_MAX_TEIS;tei++) {
 						    if (pri->q921_teis[tei] == 0) {
 						        pri->q921_teis[tei] = 1;
 						        break;
 						    }
 						}
 						if (tei < Q921_MAX_TEIS) {
 						    q921_send_teiassign(pri,((unsigned int)(h->u.data[1] << 8) + h->u.data[2]),tei + Q921_TEI_BASE);
 						} else {
 						    pri_error(pri, "no TEI available. starting TEI recovery procedure. dont worry!\n");
						    q921_tei_recovery(pri, 127);
						    
 						}
 					    break;
 					    case Q921_TEI_ID_CHK_REQ:
 						if (pri->localtype != BRI_CPE_PTMP)
 						    break;
 						    if ((((h->u.data[4] >> 1) == Q921_TEI_GROUP) || ((h->u.data[4] >> 1) == 0) || ((h->u.data[4] >> 1) == pri->tei)) && (pri->tei > 0)) {
 							pri_message(pri, "received TEI check request for TEI = %d\n",h->u.data[4] >> 1);
 							q921_send_teichkresp(pri, pri->tei);
 						    }
 					    break;
 					    case Q921_TEI_ID_CHK_RES:
 						if (pri->localtype != BRI_NETWORK_PTMP)
 						    break;
						    teio = (h->u.data[4] >> 1) - Q921_TEI_BASE;
						    if ((teio < 0) || (teio >= Q921_MAX_TEIS)) break;
 						    if (pri->q921_tei_check[teio] == 1) {
 						        pri->q921_tei_check_ri[teio] = (h->u.data[1] << 8) + h->u.data[2];
 						        pri->q921_tei_check[teio] = 0;
 						    } else {
							// d a t
							pri_error(pri, "double assgined tei for tei %d teio %d\n", h->u.data[4] >> 1, teio);
 						        pri->q921_tei_check[teio] = 0;
     							q921_start_tei(pri, h->u.data[4] >> 1);
						    }
 					    break;
 					    default:
 						pri_message(pri, "Ri = %d TEI msg = %x TEI = %x\n", (h->u.data[1] << 8) + h->u.data[2], h->u.data[3], h->u.data[4] >> 1);
 					} 
 					break;
 					case Q931_PROTOCOL_DISCRIMINATOR:
 					    if ((pri->localtype == BRI_CPE_PTMP) || (pri->localtype == BRI_CPE)){ 
 						res = q931_receive(pri, (q931_h *)h->u.data, len-3, h->h.tei);
 						/* Send an RR if one wasn't sent already */
 						if (pri->v_na[teio] != pri->v_r[teio]) 
 						    q921_rr(pri, 0, 0, pri->tei);
 						if (res == -1) {
 						    return NULL;
 						}
 						if (res & Q931_RES_HAVEEVENT)
 						    return &pri->ev;
					    }
 					break;
 				    }
 				} else {
 				    pri_message(pri, "XXX Unnumbered Information not implemented XXX\n");
				}
			}
			break;
		case 2:
			if (pri->debug &  PRI_DEBUG_Q921_STATE)
				pri_message(pri, "-- Got Disconnect from peer.\n");
#ifndef RELAX_TRB
			if (pri->q921_state[teio] != Q921_LINK_CONNECTION_ESTABLISHED) {
 			    q921_send_dm(pri, 1, h->h.tei);
			    return NULL;
			}
#endif
			/* Acknowledge */
			q921_send_ua(pri, h->u.p_f, h->h.tei);
			ev = q921_dchannel_down(pri, h->h.tei);
			if ((pri->localtype == BRI_CPE_PTMP) || (pri->localtype == BRI_CPE) || (pri->localtype == BRI_NETWORK)) {
#ifndef LAYER2ALWAYSUP
    			    /* release layer 2 */
			    return NULL;
#else
			    /* keep layer 2 up */
			    if (pri->t203_timer[teio])
				pri_schedule_del(pri, pri->t203_timer[teio]);
			    pri->t203_timer[teio] = 0;
			    q921_send_sabme(pri, 1, pri->tei);
#endif
 			}
 			if ((pri->localtype == PRI_NETWORK) || (pri->localtype == PRI_CPE)){
     			    q921_start(pri, 0, 0);
 			}
			return ev;
		case 3:
			if (h->u.m2 == 3) {
				/* SABME */
				if (pri->debug & PRI_DEBUG_Q921_STATE) {
					pri_message(pri, "-- Got SABME from %s peer.\n", h->h.c_r ? "network" : "cpe");
				}
				if (h->h.c_r) {
					pri->remotetype = PRI_NETWORK;
					if (pri->localtype == PRI_NETWORK) {
						/* We can't both be networks */
						return pri_mkerror(pri, "We think we're the network, but they think they're the network, too.");
					}
				} else {
					pri->remotetype = PRI_CPE;
					if (pri->localtype == PRI_CPE) {
						/* We can't both be CPE */
						return pri_mkerror(pri, "We think we're the CPE, but they think they're the CPE too.\n");
					}
				}
				/* Send Unnumbered Acknowledgement */
				q921_send_ua(pri, h->u.p_f, h->h.tei);
				return q921_dchannel_up(pri, h->h.tei);
			} else if (h->u.m2 == 0) {
					/* It's a UA */
				if (pri->q921_state[teio] == Q921_AWAITING_ESTABLISH) {
					if (pri->debug & PRI_DEBUG_Q921_STATE) {
						pri_message(pri, "-- Got UA from %s peer  Link up.\n", h->h.c_r ? "cpe" : "network");
					}
					return q921_dchannel_up(pri, h->h.tei);
				} else {
					pri_error(pri, "!! Got a UA, but i'm in state %d\n", pri->q921_state[teio]);
					if ((pri->localtype == BRI_CPE_PTMP) || (pri->localtype == BRI_CPE)) {
					    q921_reset(pri, h->h.tei, 1);
					    q921_send_teiverify(pri, h->h.tei);
					} else {
					    /* send DM */
 					//    q921_send_dm(pri, 1, h->h.tei);
#ifndef RELAX_TRB
					    q921_reset(pri, h->h.tei, 1);
#endif
					}
				}
			} else 
				pri_error(pri, "!! Weird frame received (m3=3, m2 = %d)\n", h->u.m2);
			break;
		case 4:
			pri_error(pri, "!! Frame got rejected!\n");
			break;
		case 5:
			pri_error(pri, "!! XID frames not supported\n");
			break;
		default:
			pri_error(pri, "!! Don't know what to do with M3=%d u-frames\n", h->u.m3);
		}
		break;
				
	}
	return NULL;
}

static pri_event *__q921_receive(struct pri *pri, q921_h *h, int len)
{
	pri_event *ev;
	/* Discard FCS */
	len -= 2;
	
	/* Check some reject conditions -- Start by rejecting improper ea's */
	if (h->h.ea1 || !(h->h.ea2))
		return NULL;
 	if ((pri->localtype == PRI_CPE) || (pri->localtype == PRI_NETWORK)) {
 	    /* Check for broadcasts - not yet handled (for PRI) */
 	    if (h->h.tei == Q921_TEI_GROUP) {
  		return NULL;
 	    }
 	} else if ((pri->localtype == BRI_CPE) || (pri->localtype == BRI_CPE_PTMP)) {
     	    if ((h->h.tei != pri->tei) && (h->h.tei != Q921_TEI_GROUP)) {
 	    	return NULL;
 	    }
 	} else if (pri->localtype == BRI_NETWORK_PTMP) {
 	    /* discard anything from a strange TEI (strange == not assigned by us or the broadcast tei) */
     	    if (((h->h.tei < Q921_TEI_BASE) || (h->h.tei > Q921_TEI_BASE + Q921_MAX_TEIS)) && (h->h.tei != Q921_TEI_GROUP)) {
 		if (pri->debug & PRI_DEBUG_Q921_DUMP)
 		    pri_message(pri, "Received a Q.921 message from strange/unassigned TEI %d.\n");
 	    	return NULL;
 	    } else {
 		if ((pri->q921_teis[h->h.tei - Q921_TEI_BASE] != 1) && (h->h.tei != Q921_TEI_GROUP)) {
 		    if (pri->debug & PRI_DEBUG_Q921_DUMP)
 			pri_message(pri, "Received a Q.921 message from unassigned TEI %d.. Sending DM and assigning.\n", h->h.tei);
 		    // send DM 
 		    q921_send_dm(pri, 1, h->h.tei);
 		    pri->q921_teis[h->h.tei - Q921_TEI_BASE] = 1; 
 		}
 	    }
 	}    
  
 	if (!pri->master && pri->debug & PRI_DEBUG_Q921_DUMP)
 		q921_dump(pri, h, len, pri->debug & PRI_DEBUG_Q921_RAW, 0);


 	if (pri->localtype != BRI_NETWORK_PTMP) {
	/* Check for SAPIs we don't yet handle */
	    if (((h->h.sapi != pri->sapi) && (h->h.sapi != Q921_SAPI_LAYER2_MANAGEMENT)) || ((h->h.tei != pri->tei) && (h->h.tei != Q921_TEI_GROUP) )) {
#ifdef PROCESS_SUBCHANNELS
		/* If it's not us, try any subchannels we have */
		if (pri->subchannel)
			return q921_receive(pri->subchannel, h, len + 2);
		else 
#endif
			return NULL;
    	    }
	}
	ev = __q921_receive_qualified(pri, h, len);

// Q921_GROUP_TEI
	if (pri->localtype == BRI_CPE_PTMP) {
	    reschedule_t203(pri, pri->tei);
	} else {
	    reschedule_t203(pri, h->h.tei);
	}
	return ev;
}

pri_event *q921_receive(struct pri *pri, q921_h *h, int len)
{
	pri_event *e;
	e = __q921_receive(pri, h, len);
#ifdef LIBPRI_COUNTERS
	pri->q921_rxcount++;
#endif
	return e;
}

static void q921_start_tei(struct pri *pri, int tei)
{
 int teio=tei - Q921_TEI_BASE;
 if (pri->localtype != BRI_NETWORK_PTMP) { return; }
 if (((teio < 0) || (teio > Q921_MAX_TEIS))) { teio=0; }
 pri->q921_teis[teio] = 0;
 q921_send_teiremove(pri, tei);
 q921_reset(pri,tei,1);
}

void q921_start(struct pri *pri, int now, int tei)
{
	int i=0;
/*	if (pri->q921_state != Q921_LINK_CONNECTION_RELEASED) {
 		pri_error(pri, "!! q921_start: Not in 'Link Connection Released' state\n");
 		return;
	} */
 	/* Reset our interface */
	if (pri->localtype != BRI_NETWORK_PTMP) {
	    q921_reset(pri,0,1);
	}
 	/* Do the SABME XXX Maybe we should implement T_WAIT? XXX */
	if ((pri->localtype == PRI_NETWORK) || (pri->localtype == PRI_CPE) || (pri->localtype == BRI_NETWORK) || (pri->localtype == BRI_CPE)) { 
	    pri->sabme_retrans[0] = 0;
	    q921_send_sabme(pri, now, 0);
	}
	
	if (pri->localtype == BRI_NETWORK_PTMP) { 
	    if (tei == 0) {
		// initial start or complete restart
		q921_send_teiremove(pri, 127);
		pri->dchanup = 0;
		for (i=0;i<Q921_MAX_TEIS;i++) {
		    q921_start_tei(pri,Q921_TEI_BASE+i);
		}
		/* clean up those TEIs */
//		pri_schedule_event2(pri, 10000, q921_invoke_tei_recovery, pri, 127 );
	    } else if ((tei >= Q921_TEI_BASE) && (tei < Q921_TEI_BASE + Q921_MAX_TEIS)){
		// restart of a single p2p datalink
		q921_start_tei(pri,tei);
	    }
	} 
	if (pri->localtype == BRI_CPE_PTMP){ 
	    if (tei == 0) {
#ifdef RELAX_TRB
		/* let's get a TEI */
		q921_send_teireq(pri);
#endif
	    } else {
		/* save the planet by recycling */
		pri->sabme_retrans[0] = 0;
		q921_send_sabme(pri, now, tei);		
	    }
	}
}