m2pa_sdl.c

No7信令,我需要交换类似的代码, 请店长审核,谢谢了,急着交换,谢谢

		freeb(mp);
		return(err);
	}
	rare();
	return(-ENOBUFS);
}
/*
 *  M2PA DELIV MSU
 *  - - - - - - - - - - - - - - - - - - - - - - -
 *  This function delivers an MSU to the SDL User.
 *  The forward sequence number is incremented
 *  upon delivery of each MSU.
 */
static int
m2pa_deliver_msu(m2pa_t *m2, mblk_t *dp)
{
	int err;
	mblk_t *mp, *db;
	uint li0 = *dp->b_rptr++ & 0xa0;
	uint li  = dp->b_wptr - dp->b_rptr;
	uint bsnr = m2->bsnr;
	uint fsnr = m2->fsnr;
//	ptrace(("%x, delivering MSU length = %u\n",(uint)m2, li));
//	if ( 3 > li || li > m2->config.m+1 )
//		return(-EMSGSIZE);
	if ( !(db = dupmsg(dp)) )
		return(-ENOBUFS);
	if ( m2->options.popt & SS7_POPT_XSN )
	{
		if ( (mp = allocb(6, BPRI_MED)) )
		{
			bsnr &= 0xfff;
			fsnr &= 0xfff;
			if ( li > 0x1ff ) li = 0x1ff;
			*mp->b_wptr++ = (bsnr>> 0);
			*mp->b_wptr++ = (bsnr>> 8) | 0x80;
			*mp->b_wptr++ = (fsnr>> 0);
			*mp->b_wptr++ = (fsnr>> 8) | 0x80;
			*mp->b_wptr++ = (li>>0);
			*mp->b_wptr++ = (li>>8)|li0;
		}
	} else {
		if ( (mp = allocb(2, BPRI_MED)) )
		{
			bsnr &= 0x7f;
			fsnr &= 0x7f;
			if ( li > 0x3f ) li = 0x3f;
			*mp->b_wptr++ = bsnr | 0x80;
			*mp->b_wptr++ = fsnr | 0x80;
			*mp->b_wptr++ = li | li0;
		}
	}
	if ( mp )
	{
		mp->b_datap->db_type = M_DATA;
		mp->b_cont = db;
		pullupmsg(mp, -1);

		if ( !(err = sdl_read(m2, mp)) )
			return(0);
		dp->b_rptr--;
		assert(mp->b_datap->db_ref);
		freemsg(mp);
		return(err);
	}
	rare();
	assert(db->b_datap->db_ref);
	freemsg(db);
	return(-ENOBUFS);
}
/*
 *  M2PA DELIV
 *  ---------------------------------------------
 */
static int
m2pa_deliver_now(m2pa_t *m2, uint8_t status, ulong count, mblk_t *mp)
{
	int err;
	switch ( status )
	{
		case M2PA_FISU:
			if ( (err = m2pa_deliver_fisu(m2, count)) )
				return(err);
			break;
		case M2PA_MSU:
			if ( (err = m2pa_deliver_msu(m2, mp)) )
				return(err);
			break;
		default: never();
		case M2PA_LSSU_SIO:
		case M2PA_LSSU_SIN:
		case M2PA_LSSU_SIE:
		case M2PA_LSSU_SIOS:
		case M2PA_LSSU_SIPO:
		case M2PA_LSSU_SIB:
			if ( (err = m2pa_deliver_lssu(m2, count, status)) )
				return(err);
			break;
	}
	return(0);
}
static int
m2pa_deliver(m2pa_t *m2, uint8_t status, mblk_t *mp)
{
	int err;
#ifdef M2PA_RX_COMPRESSION
	if ( m2->rx_status != M2PA_MSU )
	{
		if ( status == m2->rx_status )
		{
			m2->count++;
			return(0);
		}
		if ( m2->count )
		{
			if ( (err = m2pa_deliver_now(m2, m2->rx_status, m2->count, NULL)) )
				return(err);
			m2->count = 0;
		}
	}
#endif
	if ( (err = m2pa_deliver_now(m2, status, 1, mp)) )
		return(err);
	m2->rx_status = status;
	return(0);
}

static void
m2pa_rx_sib_timeout(caddr_t data)
{
	m2pa_t *m2 = (m2pa_t *)data;
	if ( !m2->timer_rx_sib ) return; else m2->timer_rx_sib = 0;
	if ( !(m2->flags & M2PA_FLAG_RX_BUSY) ) return;
	m2->timer_rx_sib = timeout(&m2pa_rx_sib_timeout, (caddr_t)m2, M2PA_RX_SIB_TIME);
	putctl1(m2->rq, M_CTL, M2PA_EVENT_RX_SIB_TIMEOUT);
	return;
}

/*
 *  M2PA RECV DATACK
 *  ---------------------------------------------
 */
static int
m2pa_recv_datack(m2pa_t *m2, mblk_t *mp, size_t mlen)
{
	int err;
	uint bsnr = m2->bsnr;
	(void)mp;

	if ( m2->version >= M2PA_VERSION_DRAFT4 ) {
	if ( mlen >= sizeof(uint32_t) ) {
		m2->bsnr += ntohl(*((uint32_t *)mp->b_rptr)++);
		if ( (err = m2pa_deliver(m2, M2PA_FISU, NULL)) ) {
			m2->bsnr = bsnr;
			return(err);
		}
		return(0);
	} rare();   return(-EMSGSIZE);
	} seldom(); return(-EPROTO);
}
/*
 *  M2PA RECV DATA
 *  ---------------------------------------------
 */
static int
m2pa_recv_data(m2pa_t *m2, mblk_t *mp, size_t mlen)
{
	int err;
	uint fsnr = m2->fsnr++;

	if ( (err = m2pa_deliver(m2, M2PA_MSU, mp)) )
	{
		m2->fsnr = fsnr;
		return(err);
	}
	return(0);
}

/*
 *  M2PA RECV STATUS
 *  ---------------------------------------------
 */
static int
m2pa_recv_status(m2pa_t *m2, mblk_t *mp, size_t mlen)
{
	int err;
	uint status;
	if ( mlen == sizeof(uint32_t) ) {

		switch ( *((uint32_t *)mp->b_rptr)++ )
		{
			case M2PA_STATUS_BUSY_ENDED:
				if ( m2->flags & M2PA_FLAG_RX_BUSY )
				{
					m2->flags &= ~M2PA_FLAG_RX_BUSY;
					untimeout(xchg(&m2->timer_rx_sib,0));
				}
				return(0);
			case M2PA_STATUS_IN_SERVICE:	    status = M2PA_FISU;		break;
			case M2PA_STATUS_PROCESSOR_ENDED:   status = M2PA_FISU;		break;
			case M2PA_STATUS_OUT_OF_SERVICE:    status = M2PA_LSSU_SIOS;	break;
			case M2PA_STATUS_PROCESSOR_OUTAGE:  status = M2PA_LSSU_SIPO;	break;
			case M2PA_STATUS_BUSY:		    status = M2PA_LSSU_SIB;
				if ( !(m2->flags & M2PA_FLAG_RX_BUSY) )
				{
					m2->flags |= M2PA_FLAG_RX_BUSY;
					m2->timer_rx_sib =
						timeout(&m2pa_rx_sib_timeout, (caddr_t)m2, M2PA_RX_SIB_TIME);
				}
				break;
			case M2PA_STATUS_ALIGNMENT:	    status = M2PA_LSSU_SIO;	break;
			case M2PA_STATUS_PROVING_NORMAL:    status = M2PA_LSSU_SIN;	break;
			case M2PA_STATUS_PROVING_EMERGENCY: status = M2PA_LSSU_SIE;	break;
			default: rare(); return(-EPROTO);
		}
		if ( (err = m2pa_deliver(m2, status, NULL)) )
			return(err);
		return(0);

	} rare(); return (-EMSGSIZE);
}
/*
 *  M2PA RECV PROVING
 *  ---------------------------------------------
 */
static int
m2pa_recv_proving(m2pa_t *m2, mblk_t *mp, size_t mlen)
{
	int err;
	if ( mlen > 0 )
	{
#ifdef M2PA_RX_COMPRESSION
		switch ( m2->rx_status )
		{
			case M2PA_LSSU_SIE: m2->count++; return(0);
			case M2PA_LSSU_SIN: m2->count++; return(0);
		}
		if ( (err = m2pa_deliver(m2, M2PA_LSSU_SIN, NULL)) )
			return(err);
#else
		uint status;
		switch ( m2->rx_status )
		{
			default:
			case M2PA_LSSU_SIN: status = M2PA_LSSU_SIN; break;
			case M2PA_LSSU_SIE: status = M2PA_LSSU_SIE; break;
		}
		if ( (err = m2pa_deliver(m2, status, NULL)) )
			return(err);
#endif
	}
	return(0);
}
/*
 *  M2PA RECV
 *  ---------------------------------------------
 */
static int
m2pa_recv_msg(m2pa_t *m2, mblk_t *mp)
{
	size_t mlen;
	uint32_t type;

	if ( mp->b_wptr >= mp->b_rptr + 2*sizeof(uint32_t) ) {
	type = ((uint32_t *)mp->b_rptr)[0];
	mlen = ntohl(((uint32_t *)mp->b_rptr)[1]);
	if ( mp->b_wptr >= mp->b_rptr + mlen ) {
	mp->b_rptr += 2*sizeof(uint32_t);
	mlen -= 2*sizeof(uint32_t);
	switch ( type ) {

		case M2PA_DATA_MESSAGE:	    return m2pa_recv_data   (m2, mp, mlen);
		case M2PA_STATUS_MESSAGE:   return m2pa_recv_status (m2, mp, mlen);
		case M2PA_PROVING_MESSAGE:  return m2pa_recv_proving(m2, mp, mlen);
		case M2PA_DATACK_MESSAGE:   return m2pa_recv_datack (m2, mp, mlen);

	} rare(); return(-EPROTO);
	} rare(); return(-EMSGSIZE);
	} rare(); return(-EPROTO);
}

/*
 *  =========================================================================
 *
 *  SDL Provider (M2PA) <- SDL User Primitives
 *
 *  =========================================================================
 */
/*
 *  LMI_INFO_REQ
 *  ---------------------------------------------
 */
static int
lmi_info_req(m2pa_t *m2, mblk_t *mp)
{
	(void)mp;
	return lmi_info_ack(m2);
}
/*
 *  LMI_ATTACH_REQ
 *  ---------------------------------------------
 */
static int
lmi_attach_req(m2pa_t *m2, mblk_t *mp)
{
	int err;
	size_t mlen = mp->b_wptr - mp->b_rptr;
	lmi_attach_req_t *p = (lmi_attach_req_t *)mp->b_rptr;
	do {
	if ( mlen >= sizeof(*p) ) {
	if ( m2->state == LMI_UNATTACHED ) { m2->state = LMI_ATTACH_PENDING;

		lmi_error_ack(m2, LMI_ATTACH_REQ, LMI_NOTSUPP);

	} seldom(); err = LMI_OUTSTATE;	    break;  /* would place interface out of state */
	} seldom(); err = LMI_PROTOSHORT;   break;  /* M_PROTO block too short	    */
	} while(0); seldom(); return lmi_error_ack(m2, LMI_ATTACH_REQ, err);
}
/*
 *  LMI_DETACH_REQ
 *  ---------------------------------------------
 */
static int
lmi_detach_req(m2pa_t *m2, mblk_t *mp)
{
	int err;
	size_t mlen = mp->b_wptr - mp->b_rptr;
	lmi_detach_req_t *p = (lmi_detach_req_t *)mp->b_rptr;
	do {
	if ( mlen >= sizeof(*p) ) {
	if ( m2->state == LMI_DISABLED ) { m2->state = LMI_DETACH_PENDING;

		lmi_error_ack(m2, LMI_DETACH_REQ, LMI_NOTSUPP);

	} seldom(); err = LMI_OUTSTATE;	    break;  /* would place interface out of state */
	} seldom(); err = LMI_PROTOSHORT;   break;  /* M_PROTO block too short	    */
	} while(0); seldom(); return lmi_error_ack(m2, LMI_DETACH_REQ, err);
}
/*
 *  LMI_ENABLE_REQ
 *  ---------------------------------------------
 */
static int
lmi_enable_req(m2pa_t *m2, mblk_t *mp)
{
	int err;
	size_t mlen = mp->b_wptr - mp->b_rptr;
	lmi_enable_req_t *p = (lmi_enable_req_t *)mp->b_rptr;
	do {
	if ( mlen >= sizeof(*p) ) {
	if ( m2->state == LMI_DISABLED ) { m2->state = LMI_ENABLE_PENDING;

		return lmi_enable_con(m2);

	} seldom(); err = LMI_OUTSTATE;	    break;  /* would place interface out of state */
	} seldom(); err = LMI_PROTOSHORT;   break;  /* M_PROTO block too short	    */
	} while(0); seldom(); return lmi_error_ack(m2, LMI_ENABLE_REQ, err);
}
/*
 *  LMI_DISABLE_REQ
 *  ---------------------------------------------
 */
static int
lmi_disable_req(m2pa_t *m2, mblk_t *mp)
{
	int err;
	size_t mlen = mp->b_wptr - mp->b_rptr;
	lmi_disable_req_t *p = (lmi_disable_req_t *)mp->b_rptr;
	do {
	if ( mlen >= sizeof(*p) ) {
	if ( m2->state == LMI_ENABLED || m2->state == LMI_ENABLE_PENDING ) {

		if ( m2->state == LMI_ENABLED ) {
			m2->flags &= ~(M2PA_FLAG_RX_BUSY|M2PA_FLAG_RX_BUSY);
			untimeout(xchg(&m2->timer_rx_sib,0));
			untimeout(xchg(&m2->timer_tx_sib,0));
			m2pa_send_status(m2, M2PA_STATUS_OUT_OF_SERVICE);
		}
		m2->state = LMI_DISABLE_PENDING;

		return lmi_disable_con(m2);

	} seldom(); err = LMI_OUTSTATE;	    break;  /* would place interface out of state */
	} seldom(); err = LMI_PROTOSHORT;   break;  /* M_PROTO block too short	    */
	} while(0); seldom(); return lmi_error_ack(m2, LMI_DISABLE_REQ, err);
}
/*
 *  LMI_OPTMGMT_REQ
 *  ---------------------------------------------
 */
static int
lmi_optmgmt_req(m2pa_t *m2, mblk_t *mp)
{
	int err;
	size_t mlen = mp->b_wptr - mp->b_rptr;
	lmi_optmgmt_req_t *p = (lmi_optmgmt_req_t *)mp->b_rptr;
	do {
	if ( mlen >= sizeof(*p) ) {

		lmi_error_ack(m2, LMI_OPTMGMT_REQ, LMI_NOTSUPP);

	} seldom(); err = LMI_PROTOSHORT;   break;  /* M_PROTO block too short	    */
	} while(0); seldom(); return lmi_error_ack(m2, LMI_OPTMGMT_REQ, err);
}
/*
 *  SDL WRITE
 *  ---------------------------------------------
 *  Just the M_DATA portion of SDL_DAEDT_TRANSMISSION_REQ
 */
#ifndef abs
#define abs(x) ((x)<0 ? -(x):(x))
#endif
static int
m_error_reply(m2pa_t *m2, int err)
{
	mblk_t *mp;
	switch ( err )
	{
		case -EBUSY:
		case -EAGAIN:
		case -ENOMEM:
		case -ENOBUFS:
			seldom(); return(err);
		case 0:
		case 1:
		case 2:
			never(); return(err);
	}
	if ( (mp = allocb(2, BPRI_HI)) )
	{
		mp->b_datap->db_type = M_ERROR;
		*(mp->b_wptr)++ = abs(err);
		*(mp->b_wptr)++ = abs(err);
		putnext(m2->rq, mp);
		return(0);
	}
	rare(); return(-ENOBUFS);
}
static int
sdl_write(m2pa_t *m2, mblk_t *mp)
{
	int err;
	do {
	if ( m2->state != LMI_DISABLED ) {
	if ( m2->state == LMI_ENABLED ) {

		if ( m2->flags & M2PA_FLAG_TX_ENABLED )
		{
			if ( (err = m2pa_send(m2, mp)) )
				break;
			return(0);
		}
		rare();
		return(0);

	} seldom(); err = LMI_OUTSTATE;	    break;  /* would place interface out of state   */
	} seldom(); return(0);			    /* ingore data in DISABLED state	    */
	} while(0); rare(); return m_error_reply(m2, err); /* XXX */
}
/*
 *  SDL_DAEDT_TRAMSMISSION_REQ
 *  ---------------------------------------------
 */
static int
sdl_daedt_xmit_req(m2pa_t *m2, mblk_t *mp)
{
	int err;
	mblk_t *dp = mp->b_cont;
	size_t mlen = mp->b_wptr - mp->b_rptr;
	sdl_daedt_transmission_req_t *p = (sdl_daedt_transmission_req_t *)mp->b_rptr;
	do {
	if ( dp ) {
	if ( mlen >= sizeof(*p) ) {
	if ( m2->state != LMI_DISABLED ) {
	if ( m2->state == LMI_ENABLED ) {

		if ( m2->flags & M2PA_FLAG_TX_ENABLED )
		{
			if ( (err = m2pa_send(m2, dp)) )
				break;
			return(0);
		}
		rare();
		return(0);

	} seldom(); err = LMI_OUTSTATE;	    break;  /* would place interface out of state   */
	} seldom(); return(0);		    break;  /* ingore data in DISABLED state	    */
	} seldom(); err = LMI_PROTOSHORT;   break;  /* M_PROTO block too short		    */
	} rare();   err = -EFAULT;	    break;  /* no M_DATA blocks			    */
	} while(0); rare(); return m_error_reply(m2, err); /* XXX */
}
/*
 *  SDL_DAEDT_START_REQ
 *  ---------------------------------------------
 */
static int
sdl_daedt_start_req(m2pa_t *m2, mblk_t *mp)
{
	int err;
	size_t mlen = mp->b_wptr - mp->b_rptr;
	sdl_daedt_start_req_t *p = (sdl_daedt_start_req_t *)mp->b_rptr;
	do {
	if ( mlen >= sizeof(*p) ) {
	if ( m2->state == LMI_ENABLED ) {

		/* enable the transmitter section */
		m2->flags |= M2PA_FLAG_TX_ENABLED;
		m2->tx_status = -1;
		return(0);

	} seldom(); err = LMI_OUTSTATE;	    break;  /* would place interface out of state */
	} seldom(); err = LMI_PROTOSHORT;   break;  /* M_PROTO block too short	    */