ldl.c

7号信令功能代码,为开源代码

 *  (But if bufcall fails mp is freed and a bogus DONE is returned
 *   to avoid hanging.)
 */
STATIC int reply_error_ack(struct dl *dl, msgb_t *mp, dl_ulong primitive,
			   dl_ulong err, dl_ulong uerr)
{
	if (!reuse_msg(mp, DL_ERROR_ACK_SIZE)) {
		mblk_t *bp;

		if ((bp = allocb(DL_ERROR_ACK_SIZE, BPRI_HI)) == NULL)
			return dl_bufcall(dl, mp, DL_ERROR_ACK_SIZE);
		freemsg(mp);
		mp = bp;
	}
	make_dl_error_ack(mp, primitive, err, uerr);
	putnext(dl->rq, mp);
	ginc(error_ack_cnt) ;
	return DONE;
}


/****************************************************************************/
/*                                                                          */
/*  Frame type support routines.                                            */
/*                                                                          */
/*  For every different kind of data link framing, we set up three          */
/*  functions to handle it:                                                 */
/*                                                                          */
/*  int xxx_want(struct dl *dl, unsigned char *fr, int len);                */
/*    Given a raw frame, return 0 or 1 to indicate if the given stream      */
/*    wants to receive this frame. In case of a memory allocation failure,  */
/*    the frame may have been truncated, but this function is still called  */
/*    in order to get correct frame drop statistics.                        */
/*                                                                          */
/*  mblk_t *xxx_rcvind(struct dl *dl, mblk_t *dp);                          */
/*    On entry, dp contains a raw data link frame that the stream has       */
/*    indicated it wants to receive (xxx_want function above)               */
/*    This function should remove the data link header from dp and create   */
/*    an DL_UNITDATA_IND message.                                           */
/*    If succesfull, a pointer to the resulting DL_UNITDATA_IND message     */
/*    is returned.                                                          */
/*    In case of failure, dp should be freed, and NULL should be returned.  */
/*                                                                          */
/*  int xxx_mkhdr(struct dl *dl, unsigned char *src, unsigned char *dst,    */
/*                int datalen, struct sk_buff *skb);                        */
/*    On entry, src and dst are pointers to the source and destination      */
/*    DLSAPs, and skb is a pointer to a sk_buff that has just been          */
/*    allocated and has had dl->machdr_reserve bytes reserved at the        */
/*    beginning.                                                            */
/*    The length of src and dst has been validated, but the function must   */
/*    validate their contents and fill in the header.                       */
/*    If succesfull, the header has been filled in and 1 is returned.       */
/*    In case of failure, 0 is returned.                                    */
/*                                                                          */
/****************************************************************************/

/*
 *  Ethernet II
 */

STATIC int eth_ii_want(struct dl *dl, unsigned char *fr, int len)
{
	ASSERT(dl->sap_len == 2);
	if (len < 14)
		return 0;
	if (*(short *)(fr + 12) != *(short *)dl->sap->sap.sap) {
		struct sap *sap = dl->subs;

		while (sap != NULL) {
			if (*(short *)(fr + 12) == *(short *)sap->sap.sap)
				break;
			sap = sap->next_sap;
		}
		if (sap == NULL)
			return 0;
	}
	if (memcmp(fr, dl->ndev->dev->dev_addr, 6) &&
	    memcmp(fr, dl->ndev->dev->broadcast, 6))
		return 0;
	return 1;
}

STATIC mblk_t *eth_ii_rcvind(struct dl *dl, mblk_t *dp)
{
	mblk_t *bp;
	dl_unitdata_ind_t *ud;
	unsigned short dsap = ntohs(*(unsigned short *)(dp->b_rptr + 12));

	ASSERT(dl->sap_len == 2);
	ASSERT(dl->addr_len == 6);

	if ((bp = allocb(DL_UNITDATA_IND_SIZE + 16, BPRI_LO)) == NULL) {
		freeb(dp);
		return NULL;
	}

	bp->b_datap->db_type = M_PROTO;
	ud = (dl_unitdata_ind_t *)bp->b_rptr;
	ud->dl_primitive = DL_UNITDATA_IND;
	ud->dl_dest_addr_length = 8;
	ud->dl_dest_addr_offset = DL_UNITDATA_IND_SIZE + 8;
	ud->dl_src_addr_length = 8;
	ud->dl_src_addr_offset = DL_UNITDATA_IND_SIZE;
	ud->dl_group_address = *dp->b_rptr & 1;
	bp->b_wptr += DL_UNITDATA_IND_SIZE;

	memcpy(bp->b_wptr, dp->b_rptr + 6, 6);
	bp->b_wptr += 6;
	*(unsigned short *)(bp->b_wptr) = dsap;
	bp->b_wptr += 2;

	memcpy(bp->b_wptr, dp->b_rptr, 6);
	bp->b_wptr += 6;
	*(unsigned short *)(bp->b_wptr) = dsap;
	bp->b_wptr += 2;

	dp->b_rptr += 14;

	linkb(bp, dp);
	return bp;
}

STATIC int eth_ii_mkhdr(struct dl *dl, unsigned char *dst,
		        int datalen, struct sk_buff *skb)
{
	unsigned char *hdr;
	unsigned short dsap;

	dsap = htons(*(unsigned short *)(dst + 6));
	hdr = skb_push(skb, 14);
	memcpy(hdr, dst, 6);
	memcpy(hdr + 6, dl->ndev->dev->dev_addr, 6);
	*(unsigned short *)(hdr + 12) = dsap;

	if (dsap != *(unsigned short *)dl->sap->sap.sap) {
		struct sap *sap;

		for (sap = dl->subs; sap; sap = sap->next_sap)
			if (dsap == *(unsigned short *)sap->sap.sap)
				return 1;
		return 0;
	}
	return 1;
}

/*
 *  Ethernet 802.2
 */
STATIC int eth_8022_want(struct dl *dl, unsigned char *fr, int len)
{
	ASSERT(dl->sap_len == 1);

	/*
	 * The LDLFLAG_RAW check will disappear when we
	 * get the RAW LLC frametype to work.
	 * It means that when running in RAW mode, any value of the
	 * control field will be accepted.
	 */
	if (len < 17 || (!(dl->flags & LDLFLAG_RAW) && *(fr + 16) != 0x03))
		return 0;
	if (ntohs(*(unsigned short *)(fr + 12)) < 3)
		return 0;
	if (dl->flags & LDLFLAG_PROMISC_SAP)
		return 1;
	if (*(fr + 14) != dl->sap->sap.sap[0]) {
		struct sap *sap = dl->subs;
		while (sap != NULL) {
			if (*(fr + 14) == sap->sap.sap[0])
				break;
			sap = sap->next_sap;
		}
		if (sap == NULL)
			return 0;
	}
#if 0
	{       
		int i ; 
		printk("eth_8022_want: SAP checked, MAC@ not yet ...") ;
		for (i = 0; i < 16; i++) printk("%02x ", fr[i]) ;
		printk("\n") ;
	}       
#endif
	/*
	 * Start filtering on MAC addresses ...
	 */
	if (!memcmp(fr, dl->ndev->dev->dev_addr, 6))
		return 1;
	if (!memcmp(fr, dl->ndev->dev->broadcast, 6))
		return 1;
	/*
	 * Check the registered multicast address list ...
	 */
	{
		struct dev_mc_list *dmi = dl->ndev->dev->mc_list;
		while (dmi != NULL)
			if (!memcmp(fr, dmi->dmi_addr, 6))
			       return 1;
			else dmi = dmi->next;	
	}       
	return 0;
}

STATIC mblk_t *eth_8022_rcvind(struct dl *dl, mblk_t *dp)
{
	mblk_t *bp;
	dl_unitdata_ind_t *ud;
	unsigned short len;

	ASSERT(dl->sap_len == 1);
	ASSERT(dl->addr_len == 6);

	if ((bp = allocb(DL_UNITDATA_IND_SIZE + 14, BPRI_LO)) == NULL) {
		freeb(dp);
		return NULL;
	}

	bp->b_datap->db_type = M_PROTO;
	ud = (dl_unitdata_ind_t *)bp->b_rptr;
	ud->dl_primitive = DL_UNITDATA_IND;
	ud->dl_dest_addr_length = 7;
	ud->dl_dest_addr_offset = DL_UNITDATA_IND_SIZE + 7;
	ud->dl_src_addr_length = 7;
	ud->dl_src_addr_offset = DL_UNITDATA_IND_SIZE;
	ud->dl_group_address = *dp->b_rptr & 1;
	bp->b_wptr += DL_UNITDATA_IND_SIZE;

	memcpy(bp->b_wptr, dp->b_rptr + 6, 6);
	bp->b_wptr += 6;
	*bp->b_wptr++ = *(dp->b_rptr + 15);

	memcpy(bp->b_wptr, dp->b_rptr, 6);
	bp->b_wptr += 6;
	*bp->b_wptr++ = *(dp->b_rptr + 14);

	len = ntohs(*(unsigned short *)(dp->b_rptr + 12)) - 3;
	dp->b_rptr += 17;
	if (dp->b_wptr - dp->b_rptr > len)
		dp->b_wptr = dp->b_rptr + len;

	linkb(bp, dp);
	return bp;
}

STATIC int eth_8022_mkhdr(struct dl *dl, unsigned char *dst,
			  int datalen, struct sk_buff *skb)
{
	unsigned char *hdr;
	unsigned char dsap;

	dsap = *(dst + 6);
	if (dsap != dl->sap->sap.sap[0]) {
		struct sap *sap;

		for (sap = dl->subs; sap; sap = sap->next_sap)
			if (dsap == sap->sap.sap[0])
				break;
		if (sap == NULL)
			return 0;
	}

	hdr = skb_push(skb, 17);
	memcpy(hdr, dst, 6);
	memcpy(hdr + 6, dl->ndev->dev->dev_addr, 6);
	*(unsigned short *)(hdr + 12) = htons(datalen + 3);
	*(hdr + 14) = *(hdr + 15) = dsap;
	*(hdr + 16) = 3;
	return 1;
}

/*
 *  Ethernet RAW 802.2
 */
STATIC int eth_raw8022_want(struct dl *dl, unsigned char *fr, int len)
{
	ASSERT(dl->sap_len == 1);

	if (len < 17 || ntohs(*(unsigned short *)(fr + 12)) < 3)
		return 0;
	if (*(unsigned short *)(fr + 14) == 0xffff)
		return 0; /* "raw" 802.3 IPX */
	if (dl->flags & LDLFLAG_PROMISC_SAP)
		return 1;
	if (*(fr + 14) != dl->sap->sap.sap[0]) {
		struct sap *sap = dl->subs;
		while (sap != NULL) {
			if (*(fr + 14) == sap->sap.sap[0])
				break;
			sap = sap->next_sap;
		}
		if (sap == NULL)
			return 0;
	}
	if (memcmp(fr, dl->ndev->dev->dev_addr, 6) &&
	    memcmp(fr, dl->ndev->dev->broadcast, 6))
		return 0;
	return 1;
}

STATIC mblk_t *eth_raw8022_rcvind(struct dl *dl, mblk_t *dp)
{
	mblk_t *bp;
	dl_unitdata_ind_t *ud;
	unsigned short len;

	ASSERT(dl->sap_len == 1);
	ASSERT(dl->addr_len == 6);

	if ((bp = allocb(DL_UNITDATA_IND_SIZE + 14, BPRI_LO)) == NULL) {
		freeb(dp);
		return NULL;
	}

	bp->b_datap->db_type = M_PROTO;
	ud = (dl_unitdata_ind_t *)bp->b_rptr;
	ud->dl_primitive = DL_UNITDATA_IND;
	ud->dl_dest_addr_length = 7;
	ud->dl_dest_addr_offset = DL_UNITDATA_IND_SIZE + 7;
	ud->dl_src_addr_length = 7;
	ud->dl_src_addr_offset = DL_UNITDATA_IND_SIZE;
	ud->dl_group_address = *dp->b_rptr & 1;
	bp->b_wptr += DL_UNITDATA_IND_SIZE;

	memcpy(bp->b_wptr, dp->b_rptr + 6, 6);
	bp->b_wptr += 6;
	*bp->b_wptr++ = *(dp->b_rptr + 15);

	memcpy(bp->b_wptr, dp->b_rptr, 6);
	bp->b_wptr += 6;
	*bp->b_wptr++ = *(dp->b_rptr + 14);

	len = ntohs(*(unsigned short *)(dp->b_rptr + 12));
	dp->b_rptr += 14;
	if (dp->b_wptr - dp->b_rptr > len)
		dp->b_wptr = dp->b_rptr + len;

	linkb(bp, dp);
	return bp;
}

STATIC int eth_raw8022_mkhdr(struct dl *dl, unsigned char *dst,
			  int datalen, struct sk_buff *skb)
{
	unsigned char *hdr;

	hdr = skb_push(skb, 14);
	memcpy(hdr, dst, 6);
	memcpy(hdr + 6, dl->ndev->dev->dev_addr, 6);
	*(unsigned short *)(hdr + 12) = htons(datalen);
	*(hdr + 14) = *(dst + 6);
	*(hdr + 15) = dl->sap->sap.sap[0];
	return 1;
}

/*
 *  Ethernet SNAP
 */

STATIC int eth_snap_want(struct dl *dl, unsigned char *fr, int len)
{
	ASSERT(dl->sap_len == 2);
	if (len < 22 ||
	    ntohs(*(unsigned short *)(fr + 12)) < 8 ||
	    *(unsigned short *)(fr + 14) != 0xAAAA ||
	    *(fr + 16) != 0x03 ||
	    memcmp(dl->oui, fr + 17, 3))
		return 0;

	if (memcmp(fr + 20, dl->sap->sap.sap, 2)) {
		struct sap *sap = dl->subs;

		while (sap != NULL) {
			if (!memcmp(fr + 20, sap->sap.sap, 2))
				break;
			sap = sap->next_sap;
		}
		if (sap == NULL)
			return 0;
	}

	if (memcmp(fr, dl->ndev->dev->dev_addr, 6) &&
	    memcmp(fr, dl->ndev->dev->broadcast, 6))
		return 0;

	return 1;
}

STATIC mblk_t *eth_snap_rcvind(struct dl *dl, mblk_t *dp)
{
	mblk_t *bp;
	dl_unitdata_ind_t *ud;
	unsigned short len;
	unsigned short dsap = ntohs(*(unsigned short *)(dp->b_rptr + 17));

	ASSERT(dl->sap_len == 2);
	ASSERT(dl->addr_len == 6);

	if ((bp = allocb(DL_UNITDATA_IND_SIZE + 22, BPRI_LO)) == NULL) {
		freeb(dp);
		return NULL;
	}

	bp->b_datap->db_type = M_PROTO;
	ud = (dl_unitdata_ind_t *)bp->b_rptr;
	ud->dl_primitive = DL_UNITDATA_IND;
	ud->dl_dest_addr_length = 8;
	ud->dl_dest_addr_offset = DL_UNITDATA_IND_SIZE + 8;
	ud->dl_src_addr_length = 8;
	ud->dl_src_addr_offset = DL_UNITDATA_IND_SIZE;
	ud->dl_group_address = *dp->b_rptr & 1;
	bp->b_wptr += DL_UNITDATA_IND_SIZE;

	memcpy(bp->b_wptr, dp->b_rptr + 6, 6);
	bp->b_wptr += 6;
	*(unsigned short *)(bp->b_wptr) = dsap;
	bp->b_wptr += 2;

	memcpy(bp->b_wptr, dp->b_rptr, 6);
	bp->b_wptr += 6;
	*(unsigned short *)(bp->b_wptr) = dsap;
	bp->b_wptr += 2;

	len = ntohs(*(unsigned short *)(dp->b_rptr + 12)) - 8;
	dp->b_rptr += 22;
	if (dp->b_wptr - dp->b_rptr > len)
		dp->b_wptr = dp->b_rptr + len;

	linkb(bp, dp);
	return bp;
}

STATIC int eth_snap_mkhdr(struct dl *dl, unsigned char *dst,
			  int datalen, struct sk_buff *skb)
{
	unsigned char *hdr;
	unsigned short dsap;

	dsap = *(unsigned short *)(dst + 6);
	if (dsap != *(unsigned short *)dl->sap->sap.sap) {
		struct sap *sap = dl->subs;

		while (sap != NULL) {
			if (dsap == *(unsigned short *)sap->sap.sap)
				break;
			sap = sap->next_sap;
		}
		if (sap == NULL)
			return 0;
	}

	hdr = skb_push(skb, 22);
	memcpy(hdr, dst, 6);
	memcpy(hdr + 6, dl->ndev->dev->dev_addr, 6);
	*(unsigned short *)(hdr + 12) = htons(datalen + 8);
	*(unsigned short *)(hdr + 14) = 0xaaaa;
	*(hdr + 16) = 3;
	memcpy(hdr + 17, dl->oui, 3);
	*(unsigned short *)(hdr + 20) = dsap;
	return 1;
}

/*
 *  Ethernet 802.3 IPX
 */

STATIC int ipx_8023_want(struct dl *dl, unsigned char *fr, int len)
{
	ASSERT(dl->sap_len == 0);
	/* Must have a complete IPX header */
	if (len < 44 || ntohs(*(unsigned short *)(fr + 12)) < 30)
		return 0;
	/* Must have no IPX checksum */
	if (ntohs(*(unsigned short *)(fr + 14)) != 0xffff)
		return 0;
	if (memcmp(fr, dl->ndev->dev->dev_addr, 6) &&
	    memcmp(fr, dl->ndev->dev->broadcast, 6))
		return 0;
	return 1;
}

STATIC mblk_t *ipx_8023_rcvind(struct dl *dl, mblk_t *dp)
{
	mblk_t *bp;
	dl_unitdata_ind_t *ud;
	unsigned short len;

	ASSERT(dl->sap_len == 0);
	ASSERT(dl->addr_len == 6);

	if ((bp = allocb(DL_UNITDATA_IND_SIZE + 12, BPRI_LO)) == NULL) {
		freeb(dp);
		return NULL;
	}