control.c

第二层隧道模块l2tp源码,开发环境为linux

            new_hdr->length = buf->len + ehlen;
        };
        r++;
        new_hdr->tid = *r;
        r++;
        new_hdr->cid = *r;
        if (PFBIT (new_hdr->ver))
        {
            r++;
            new_hdr->Ns = *r;
            r++;
            new_hdr->Nr = *r;
        }
        else
        {
            new_hdr->Nr = c->data_seq_num;
            new_hdr->Ns = c->data_rec_seq_num;
        };
        if (PSBIT (new_hdr->ver))
        {
            r++;
            new_hdr->o_size = *r;
            r++;
            new_hdr->o_pad = *r;
        }
        else
        {
            new_hdr->o_size = 0;
            new_hdr->o_pad = 0;
        }
    }
    else
        new_hdr = h;
    /*
       * Handle sequence numbers
       *
     */
/*  JLM	if (PRBIT(new_hdr->ver)) {
		if (c->pSr > new_hdr->Ns) {
			log(LOG_DEBUG, "%s: R-bit set with Ns < pSr!\n",__FUNCTION__);
			return -EINVAL;
		}
#ifdef DEBUG_FLOW
		log(LOG_DEBUG, "%s: R-bit set on packet %d\n",__FUNCTION__,new_hdr->Ns);
#endif
		c->pSr=new_hdr->Ns;
	} */
#ifdef DEBUG_PAYLOAD
    log (LOG_DEBUG, "%s: payload, cid = %d, Ns = %d, Nr = %d\n", __FUNCTION__,
         c->cid, new_hdr->Ns, new_hdr->Nr);
#endif
    if (new_hdr->Ns != c->data_seq_num)
    {
        /* RFC1982-esque comparison of serial numbers */
        if (((new_hdr->Ns < c->data_rec_seq_num) && 
            ((c->data_rec_seq_num - new_hdr->Ns) < 32768)) ||
            ((new_hdr->Ns > c->data_rec_seq_num) && 
            ((c->data_rec_seq_num - new_hdr->Ns) > 32768)))
        {
#ifdef DEBUG_FLOW
            if (DEBUG)
                log (LOG_DEBUG,
                     "%s: Already seen this packet before (%d < %d)\n",
                     __FUNCTION__, new_hdr->Ns, c->pSr);
#endif
            return -EINVAL;
        }
        else if (new_hdr->Ns <= c->data_rec_seq_num + PAYLOAD_FUDGE)
        {
            /* FIXME: I should buffer for out of order packets */
#ifdef DEBUG_FLOW
            if (DEBUG)
                log (LOG_DEBUG,
                     "%s: Oops, lost a packet or two (%d != %d).  continuing...\n",
                     __FUNCTION__, new_hdr->Ns, c->pSr);
#endif
            c->data_rec_seq_num = new_hdr->Ns;
        }
        else
        {
#ifdef DEBUG_FLOW
            if (DEBUG)
                log (LOG_DEBUG,
                     "%s: Received out of order payload packet (%d != %d)\n",
                     __FUNCTION__, new_hdr->Ns, c->pSr);
#endif
            return -EINVAL;
        }
    }
    else
    {
        c->data_rec_seq_num++;
        c->pnu = -1;
    }
    /*
     * Check to see what the last thing
     * we got back was
     */
    c->pLr = new_hdr->Nr;
    buf->start = new_hdr;
    buf->len += ehlen;
    return 0;
}

void send_zlb (void *data)
{
    /*
     * Send a ZLB.  This procedure should be schedule()able
     */
    struct call *c;
    struct tunnel *t;
    struct buffer *buf;
    c = (struct call *) data;
    if (!c)
    {
        log (LOG_WARN, "%s: called on NULL call\n", __FUNCTION__);
        return;
    }
    t = c->container;
    if (!t)
    {
        log (LOG_WARN, "%s: called on call with NULL container\n",
             __FUNCTION__);
        return;
    }
    /* Update the counter so we know what Lr was when we last transmited a ZLB */
    c->prx = c->data_rec_seq_num;
    buf = new_payload (t->peer);
    add_payload_hdr (t, c, buf);
    c->data_seq_num--;                   /* We don't increment on ZLB's */
    c->zlb_xmit = NULL;
#ifdef DEBUG_ZLB
    log (LOG_DEBUG, "%s: sending payload ZLB\n", __FUNCTION__);
#endif
    udp_xmit (buf);
    toss (buf);
}

inline int write_packet (struct buffer *buf, struct tunnel *t, struct call *c,
                         int convert)
{
    /*
     * Write a packet, doing sync->async conversion if
     * necessary
     */
    int x;
    unsigned char e;
    int err;
    static unsigned char wbuf[MAX_RECV_SIZE];
    int pos = 0;

    if (c->fd < 0)
    {
        if (DEBUG)
            log (LOG_DEBUG, "%s: tty is not open yet.\n", __FUNCTION__);
        return -EIO;
    }
    /*
     * Skip over header 
     */
    buf->start += sizeof (struct payload_hdr);
    buf->len -= sizeof (struct payload_hdr);

    c->rx_pkts++;
    c->rx_bytes += buf->len;

    /*
     * FIXME:  What about offset?
     */

    while (!convert)
    {
        /* We are given async frames, so write them
           directly to the tty */
        err = write (c->fd, buf->start, buf->len);
        if (err == buf->len)
        {
            return 0;
        }
        else if (err == 0)
        {
            log (LOG_WARN, "%s: wrote no bytes of async packet\n",
                 __FUNCTION__);
            return -EINVAL;
        }
        else if (err < 0)
        {
            if ((errno == EAGAIN) || (errno == EINTR))
            {
                continue;
            }
            else
            {
                log (LOG_WARN, "%s: async write failed: %s\n", __FUNCTION__,
                     strerror (errno));
            }
        }
        else if (err < buf->len)
        {
            log (LOG_WARN, "%s: short write (%d of %d bytes)\n", __FUNCTION__,
                 err, buf->len);
            return -EINVAL;
        }
        else if (err > buf->len)
        {
            log (LOG_WARN, "%s: write returned LONGER than buffer length?\n",
                 __FUNCTION__);
            return -EINVAL;
        }
    }

    /*
     * sync->async conversion if we're doing sync frames
     * since the pppd driver will expect async frames
     * Write leading flag character
     */

    add_fcs (buf);
    e = PPP_FLAG;
    wbuf[pos++] = e;
    for (x = 0; x < buf->len; x++)
    {
        e = *((char *) buf->start + x);
        if ((e < 0x20) || (e == PPP_ESCAPE) || (e == PPP_FLAG))
        {
            /* Escape this */
            e = e ^ 0x20;
            wbuf[pos++] = PPP_ESCAPE;
        }
        wbuf[pos++] = e;

    }
    wbuf[pos++] = PPP_FLAG;
    x = write (c->fd, wbuf, pos);
    if (x < pos)
    {
        if (!(errno == EINTR) && !(errno == EAGAIN))
        {
            /*
               * I guess pppd died.  we'll pretend
               * everything ended normally
             */
            if (DEBUG)
                log (LOG_WARN, "%s: %s(%d)\n", __FUNCTION__, strerror (errno),
                     errno);
            c->needclose = -1;
            c->fd = -1;
            return -EIO;
        }
    }
    return 0;
}

void handle_special (struct buffer *buf, struct call *c, _u16 call)
{
    /*
       * This procedure is called when we have received a packet
       * on a call which doesn't exist in our tunnel.  We want to
       * send back a ZLB to keep the tunnel alive, on that particular
       * call if it was a CDN, otherwise, send a CDN to notify them
       * that this call has been terminated.
     */
    struct buffer *outgoing;
    struct tunnel *t = c->container;
    /* Don't do anything unless it's a control packet */
    if (!CTBIT (*((_u16 *) buf->start)))
        return;
    /* Temporarily, we make the tunnel have cid of call instead of 0,
       but we need to stop any scheduled events (like Hello's in
       particular) which might use this value */
    c->cid = call;
    if (!check_control (buf, t, c))
    {
        if (buf->len == sizeof (struct control_hdr))
        {
            /* If it's a ZLB, we ignore it */
            if (debug_tunnel)
                log (LOG_DEBUG, "%s: ZLB for closed call\n", __FUNCTION__);
            c->cid = 0;
            return;
        }
        /* Make a packet with the specified call number */
        outgoing = new_outgoing (t);
        /* FIXME: If I'm not a CDN, I need to send a CDN */
        control_zlb (buf, t, c);
        c->cid = 0;
        udp_xmit (buf);
        toss (buf);
    }
    else
    {
        c->cid = 0;
        if (debug_tunnel)
            log (LOG_DEBUG, "%s: invalid control packet\n", __FUNCTION__);
    }
}

inline int handle_packet (struct buffer *buf, struct tunnel *t,
                          struct call *c)
{
    int res;
    struct timeval tv;
    if (CTBIT (*((_u16 *) buf->start)))
    {
        /* We have a control packet */
        if (!check_control (buf, t, c))
        {
            c->msgtype = -1;
            if (buf->len == sizeof (struct control_hdr))
            {
#ifdef DEBUG_ZLB
                log (LOG_DEBUG, "%s: control ZLB received\n", __FUNCTION__);
#endif
                t->control_rec_seq_num--;
                c->cnu = 0;
                if (c->needclose && c->closing)
                {
                    if (c->container->cLr >= c->closeSs)
                    {
#ifdef DEBUG_ZLB
                        log (LOG_DEBUG, "%s: ZLB for closing message found\n",
                             __FUNCTION__);
#endif
                        c->needclose = 0;
                        /* Trigger final closing of call */
                    }
                }
                return 0;
            }
            else if (!handle_avps (buf, t, c))
            {
                return control_finish (t, c);
            }
            else
            {
                if (debug_tunnel)
                    log (LOG_DEBUG, "%s: bad AVP handling!\n", __FUNCTION__);
                return -EINVAL;
            }
        }
        else
        {
            log (LOG_DEBUG, "%s: bad control packet!\n", __FUNCTION__);
            return -EINVAL;
        }
    }
    else
    {
        if (!check_payload (buf, t, c))
        {
            if (!expand_payload (buf, t, c))
            {
                if (buf->len > sizeof (struct payload_hdr))
                {
/*					if (c->throttle) {
						if (c->pSs > c->pLr + c->rws) {
#ifdef DEBUG_FLOW
							log(LOG_DEBUG, "%s: not yet dethrottling call\n",__FUNCTION__);
#endif
						} else {
#ifdef DEBUG_FLOW
							log(LOG_DEBUG, "%s: dethrottling call\n",__FUNCTION__);
#endif
							if (c->dethrottle) deschedule(c->dethrottle);
							c->dethrottle=NULL;
							c->throttle = 0;
						}
					} */
/*	JLM				res = write_packet(buf,t,c, c->frame & SYNC_FRAMING); */
                    res = write_packet (buf, t, c, SYNC_FRAMING);
                    if (res)
                        return res;
                    /* 
                       * Assuming we wrote to the ppp driver okay, we should
                       * do something about ZLB's unless *we* requested no
                       * window size or if they we have turned off our fbit. 
                     */

/*					if (c->ourfbit && (c->ourrws > 0)) {
						if (c->pSr >= c->prx + c->ourrws - 2) {
						We've received enough to fill our receive window.  At
						this point, we should immediately send a ZLB!
#ifdef DEBUG_ZLB
							log(LOG_DEBUG, "%s: Sending immediate ZLB!\n",__FUNCTION__);
#endif
							if (c->zlb_xmit) {
							Deschedule any existing zlb_xmit's
								deschedule(c->zlb_xmit);
								c->zlb_xmit = NULL;
							}
							send_zlb((void *)c);
						} else {
						We need to schedule sending a ZLB.  FIXME:  Should
						be 1/4 RTT instead, when rate adaptive stuff is
						in place. Spec allows .5 seconds though
							tv.tv_sec = 0;
							tv.tv_usec = 500000;
							if (c->zlb_xmit)
								deschedule(c->zlb_xmit);
#ifdef DEBUG_ZLB
							log(LOG_DEBUG, "%s: scheduling ZLB\n",__FUNCTION__);
#endif
							c->zlb_xmit = schedule(tv, &send_zlb, (void *)c);
						}
					} */
                    return 0;
                }
                else if (buf->len == sizeof (struct payload_hdr))
                {
#ifdef DEBUG_ZLB
                    log (LOG_DEBUG, "%s: payload ZLB received\n",
                         __FUNCTION__);
#endif
/*					if (c->throttle) {
						if (c->pSs > c->pLr + c->rws) {
#ifdef DEBUG_FLOW
							log(LOG_DEBUG, "%s: not yet dethrottling call\n",__FUNCTION__);
#endif
						} else {
#ifdef DEBUG_FLOW
							log(LOG_DEBUG, "%s: dethrottling call\n",__FUNCTION__);
#endif
							if (c->dethrottle)
								deschedule(c->dethrottle);
							c->dethrottle=NULL;
							c->throttle = 0;
						}
					} */
                    c->data_rec_seq_num--;
                    return 0;
                }
                else
                {
                    log (LOG_DEBUG, "%s: payload too small!\n", __FUNCTION__);
                    return -EINVAL;
                }
            }
            else
            {
                if (debug_tunnel)
                    log (LOG_DEBUG, "%s: unable to expand payload!\n",
                         __FUNCTION__);
                return -EINVAL;
            }
        }
        else
        {
            log (LOG_DEBUG, "%s: invalid payload packet!\n", __FUNCTION__);
            return -EINVAL;
        }
    }
}