ppp_ahdlc.c

unix and linux net driver

	break;

    default:
	putnext(q, mp);
    }
    return 0;
}

/* Extract bit c from map m, to determine if c needs to be escaped. */
#define ESCAPE(c, m)	((m)[(c) >> 5] & (1 << ((c) & 0x1f)))

static void
stuff_frame(q, mp)
    queue_t *q;
    mblk_t *mp;
{
    ahdlc_state_t *state;
    int ilen, olen, c, extra, code;
    mblk_t *omsg, *op, *np;
    uchar_t *sp, *sp0, *dp, *dp0, *spend;
    ushort_t fcs;
    u_int32_t *xaccm, lcp_xaccm[8];

    state = (ahdlc_state_t *) q->q_ptr;
    ilen = msgdsize(mp);

    /*
     * We estimate the length of the output packet as
     * 1.25 * input length + 16 (for initial flag, FCS, final flag, slop).
     */
    olen = ilen + (ilen >> 2) + 16;
    if (olen > OFRAME_BSIZE)
	olen = OFRAME_BSIZE;
    omsg = op = allocb(olen, BPRI_MED);
    if (omsg == 0)
	goto bomb;

    /*
     * Put in an initial flag, unless the serial driver currently has
     * packets still to be transmitted in its queue.
     */
    dp = op->b_wptr;
    if (qsize(q->q_next) == 0) {
	*dp++ = PPP_FLAG;
	--olen;
    }

    /*
     * For LCP packets with code values between 1 and 7 (Conf-Req
     * to Code-Rej), we must escape all control characters.
     */
    xaccm = state->xaccm;
    if (MSG_BYTE(mp, 0) == PPP_ALLSTATIONS
	&& MSG_BYTE(mp, 1) == PPP_UI
	&& MSG_BYTE(mp, 2) == (PPP_LCP >> 8)
	&& MSG_BYTE(mp, 3) == (PPP_LCP & 0xFF)
	&& LCP_USE_DFLT(mp)) {
	bcopy((caddr_t) state->xaccm, (caddr_t) lcp_xaccm, sizeof(lcp_xaccm));
	lcp_xaccm[0] = ~0;
	xaccm = lcp_xaccm;
    }

    sp = mp->b_rptr;
    fcs = PPP_INITFCS;
    for (;;) {
	spend = mp->b_wptr;
	extra = sp + olen - spend;
	if (extra < 0) {
	    spend = sp + olen;
	    extra = 0;
	}
	/*
	 * We can safely process the input up to `spend'
	 * without overrunning the output, provided we don't
	 * hit more than `extra' characters which need to be escaped.
	 */
	sp0 = sp;
	dp0 = dp;
	while (sp < spend) {
	    c = *sp;
	    if (ESCAPE(c, xaccm)) {
		if (extra > 0)
		    --extra;
		else if (sp < spend - 1)
		    --spend;
		else
		    break;
		fcs = PPP_FCS(fcs, c);
		*dp++ = PPP_ESCAPE;
		c ^= PPP_TRANS;
	    } else
		fcs = PPP_FCS(fcs, c);
	    *dp++ = c;
	    ++sp;
	}
	ilen -= sp - sp0;
	olen -= dp - dp0;

	/*
	 * At this point, we have emptied an input block
	 * and/or filled an output block.
	 */
	if (sp >= mp->b_wptr) {
	    /*
	     * We've emptied an input block.  Advance to the next.
	     */
	    mp = mp->b_cont;
	    if (mp == 0)
		break;		/* all done */
	    sp = mp->b_rptr;
	}
	if (olen < 2) {
	    /*
	     * The output block is full.  Allocate a new one.
	     */
	    op->b_wptr = dp;
	    olen = 2 * ilen + 5;
	    if (olen > OFRAME_BSIZE)
		olen = OFRAME_BSIZE;
	    np = allocb(olen, BPRI_MED);
	    if (np == 0)
		goto bomb;
	    op->b_cont = np;
	    op = np;
	    dp = op->b_wptr;
	}
    }

    /*
     * Append the FCS and closing flag.
     * This could require up to 5 characters.
     */
    if (olen < 5) {
	/* Sigh.  Need another block. */
	op->b_wptr = dp;
	np = allocb(5, BPRI_MED);
	if (np == 0)
	    goto bomb;
	op->b_cont = np;
	op = np;
	dp = op->b_wptr;
    }
    c = ~fcs & 0xff;
    if (ESCAPE(c, xaccm)) {
	*dp++ = PPP_ESCAPE;
	c ^= PPP_TRANS;
    }
    *dp++ = c;
    c = (~fcs >> 8) & 0xff;
    if (ESCAPE(c, xaccm)) {
	*dp++ = PPP_ESCAPE;
	c ^= PPP_TRANS;
    }
    *dp++ = c;
    *dp++ = PPP_FLAG;
    op->b_wptr = dp;

    /*
     * Update statistics.
     */
    state->stats.ppp_obytes += msgdsize(omsg);
    state->stats.ppp_opackets++;

    /*
     * Send it on.
     */
    putnext(q, omsg);
    return;

 bomb:
    if (omsg != 0)
	freemsg(omsg);
    state->stats.ppp_oerrors++;
    putctl1(RD(q)->q_next, M_CTL, PPPCTL_OERROR);
}

#define UPDATE_FLAGS(c)	{				\
    if ((c) & 0x80)					\
	state->flags |= RCV_B7_1;			\
    else						\
	state->flags |= RCV_B7_0;			\
    if (0x6996 & (1 << ((((c) >> 4) ^ (c)) & 0xf)))	\
	state->flags |= RCV_ODDP;			\
    else						\
	state->flags |= RCV_EVNP;			\
}

/*
 * Process received characters.
 */
static void
unstuff_chars(q, mp)
    queue_t *q;
    mblk_t *mp;
{
    ahdlc_state_t *state;
    mblk_t *om;
    uchar_t *cp, *cpend, *dp, *dp0;
    int c, len, extra;
    ushort_t fcs;

    state = (ahdlc_state_t *) q->q_ptr;
    state->stats.ppp_ibytes += msgdsize(mp);
    cp = mp->b_rptr;
    for (;;) {
	/*
	 * Advance to next input block if necessary.
	 */
	if (cp >= mp->b_wptr) {
	    mp = mp->b_cont;
	    if (mp == 0)
		break;
	    cp = mp->b_rptr;
	    continue;
	}

	if ((state->flags & (IFLUSH|ESCAPED)) == 0
	    && state->inlen > 0 && (om = state->cur_blk) != 0) {
	    /*
	     * Process bulk chars as quickly as possible.
	     */
	    dp = om->b_wptr;
	    len = om->b_datap->db_lim - dp; /* max # output bytes */
	    extra = (mp->b_wptr - cp) - len;/* #input chars - #output bytes */
	    if (extra < 0) {
		len += extra;		    /* we'll run out of input first */
		extra = 0;
	    }
	    cpend = cp + len;
	    dp0 = dp;
	    fcs = state->infcs;
	    while (cp < cpend) {
		c = *cp;
		if (c == PPP_FLAG)
		    break;
		++cp;
		UPDATE_FLAGS(c);
		if (c == PPP_ESCAPE) {
		    if (extra > 0) {
			--extra;
			++cpend;
		    }
		    if (cp >= cpend || (c = *cp) == PPP_FLAG) {
			state->flags |= ESCAPED;
			break;
		    }
		    ++cp;
		    UPDATE_FLAGS(c);
		    c ^= PPP_TRANS;
		}
		*dp++ = c;
		fcs = PPP_FCS(fcs, c);
	    }
	    state->inlen += dp - dp0;
	    state->infcs = fcs;
	    om->b_wptr = dp;
	    if (cp >= mp->b_wptr)
		continue;	/* advance to the next mblk */
	}

	c = *cp++;
	UPDATE_FLAGS(c);
	if (c == PPP_FLAG) {
	    /*
	     * End of a frame.
	     * If the ESCAPE flag is set, the frame ended with
	     * the frame abort sequence "}~".
	     */
	    om = state->cur_frame;
	    len = state->inlen;
	    state->cur_frame = 0;
	    state->inlen = 0;
	    if (len == 0 && (state->flags & IFLUSH) == 0)
		continue;
	    state->stats.ppp_ipackets++;
	    if (om != 0 && (state->flags & (IFLUSH|ESCAPED)) == 0
		&& len > PPP_FCSLEN) {
		if (state->infcs == PPP_GOODFCS) {
		    adjmsg(om, -PPP_FCSLEN);	/* chop off fcs */
		    putnext(q, om);		/* bombs away! */
		    continue;
		}
		DPRINT2("ppp%d: bad fcs (len=%d)\n", state->unit, len);
	    }
	    if (om != 0)
		freemsg(om);
	    state->flags &= ~(IFLUSH|ESCAPED);
	    state->stats.ppp_ierrors++;
	    putctl1(q->q_next, M_CTL, PPPCTL_IERROR);
	    continue;
	}

	if (state->flags & IFLUSH)
	    continue;
	if (state->flags & ESCAPED) {
	    c ^= PPP_TRANS;
	    state->flags &= ~ESCAPED;
	} else if (c == PPP_ESCAPE) {
	    state->flags |= ESCAPED;
	    continue;
	}
	if (state->inlen == 0) {
	    /*
	     * First byte of the frame: allocate the first message block.
	     */
	    om = allocb(IFRAME_BSIZE, BPRI_MED);
	    if (om == 0) {
		state->flags |= IFLUSH;
		continue;
	    }
	    state->cur_frame = om;
	    state->cur_blk = om;
	    state->infcs = PPP_INITFCS;
	} else {
	    om = state->cur_blk;
	    if (om->b_wptr >= om->b_datap->db_lim) {
		/*
		 * Current message block is full.  Allocate another one,
		 * unless we have run out of MRU.
		 */
		if (state->inlen >= state->mru + PPP_HDRLEN + PPP_FCSLEN) {
		    state->flags |= IFLUSH;
		    DPRINT2("ppp%d: frame too long (%d)\n",
			    state->unit, state->inlen);
		    continue;
		}
		om = allocb(IFRAME_BSIZE, BPRI_MED);
		if (om == 0) {
		    state->flags |= IFLUSH;
		    continue;
		}
		state->cur_blk->b_cont = om;
		state->cur_blk = om;
	    }
	}

	if (state->inlen == 0) {
	    /*
	     * We don't do address/control & protocol decompression here,
	     * but we try to put the first byte at an offset such that
	     * the info field starts on a word boundary.  The code here
	     * will do this except for packets with protocol compression
	     * but not address/control compression.
	     */
	    if (c != PPP_ALLSTATIONS) {
		om->b_wptr += 2;
		if (c & 1)
		    ++om->b_wptr;
		om->b_rptr = om->b_wptr;
	    }
	}

	*om->b_wptr++ = c;
	++state->inlen;
	state->infcs = PPP_FCS(state->infcs, c);
    }
}

static int
msg_byte(mp, i)
    mblk_t *mp;
    unsigned int i;
{
    while (mp != 0 && i >= mp->b_wptr - mp->b_rptr)
	mp = mp->b_cont;
    if (mp == 0)
	return -1;
    return mp->b_rptr[i];
}