Tomato 1.28

[tomato/tomato-null.git] / release / src / router / rp-l2tp / dgram.c

/***********************************************************************
*
* dgram.c
*
* Routines for manipulating L2TP datagrams.
*
* Copyright (C) 2002 by Roaring Penguin Software Inc.
*
* This software may be distributed under the terms of the GNU General
* Public License, Version 2, or (at your option) any later version.
*
* LIC: GPL
*
***********************************************************************/

static char const RCSID[] =
"$Id: dgram.c,v 1.1.48.1 2005/08/08 12:05:25 honor Exp $";

#include "l2tp.h"
#include "md5.h"
#include <sys/types.h>
#include <sys/socket.h>
#include <string.h>
#include <errno.h>
#include <stdio.h>

#define PULL_UINT16(buf, cursor, val) \
do { \
    val = ((uint16_t) buf[cursor]) * 256 + (uint16_t) buf[cursor+1]; \
    cursor += 2; \
} while(0)

#define PUSH_UINT16(buf, cursor, val) \
do { \
    buf[cursor] = val / 256; \
    buf[cursor+1] = val & 0xFF; \
    cursor += 2; \
} while (0)

#define GET_AVP_LEN(x) ((((x)[0] & 3) * 256) + (x)[1])

static int dgram_add_random_vector_avp(l2tp_dgram *dgram);

static void dgram_do_hide(uint16_t type,
                          uint16_t len,
                          unsigned char *value,
                          unsigned char const *secret,
                          size_t secret_len,
                          unsigned char const *random,
                          size_t random_len,
                          unsigned char *output);

static unsigned char *dgram_do_unhide(uint16_t type,
                                      uint16_t *len,
                                      unsigned char *value,
                                      unsigned char const *secret,
                                      size_t secret_len,
                                      unsigned char const *random,
                                      size_t random_len,
                                      unsigned char *output);

/* Description of AVP's indexed by AVP type */
struct avp_descrip {
    char const *name;           /* AVP name */
    int can_hide;               /* Can AVP be hidden? */
    uint16_t min_len;           /* Minimum PAYLOAD length */
    uint16_t max_len;           /* Maximum PAYLOAD length (9999 = no limit) */
};

static struct avp_descrip avp_table[] = {
    /* Name                     can_hide min_len max_len */
    {"Message Type",            0,       2,      2}, /* 0 */
    {"Result Code",             0,       2,   9999}, /* 1 */
    {"Protocol Version",        0,       2,      2}, /* 2 */
    {"Framing Capabilities",    1,       4,      4}, /* 3 */
    {"Bearer Capabilities",     1,       4,      4}, /* 4 */
    {"Tie Breaker",             0,       8,      8}, /* 5 */
    {"Firmware Revision",       1,       2,      2}, /* 6 */
    {"Host Name",               0,       0,   9999}, /* 7 */
    {"Vendor Name",             1,       0,   9999}, /* 8 */
    {"Assigned Tunnel ID",      1,       2,      2}, /* 9 */
    {"Receive Window Size",     0,       2,      2}, /* 10 */
    {"Challenge",               1,       0,   9999}, /* 11 */
    {"Q.931 Cause Code",        0,       3,   9999}, /* 12 */
    {"Challenge Response",      1,      16,     16}, /* 13 */
    {"Assigned Session ID",     1,       2,      2}, /* 14 */
    {"Call Serial Number",      1,       4,      4}, /* 15 */
    {"Minimum BPS",             1,       4,      4}, /* 16 */
    {"Maximum BPS",             1,       4,      4}, /* 17 */
    {"Bearer Type",             1,       4,      4}, /* 18 */
    {"Framing Type",            1,       4,      4}, /* 19 */
    {"Unknown",                 0,       0,   9999}, /* 20 */
    {"Called Number",           1,       0,   9999}, /* 21 */
    {"Calling Number",          1,       0,   9999}, /* 22 */
    {"Sub-Address",             1,       0,   9999}, /* 23 */
    {"TX Connect Speed",        1,       4,      4}, /* 24 */
    {"Physical Channel ID",     1,       4,      4}, /* 25 */
    {"Intial Received Confreq", 1,       0,   9999}, /* 26 */
    {"Last Sent Confreq",       1,       0,   9999}, /* 27 */
    {"Last Received Confreq",   1,       0,   9999}, /* 28 */
    {"Proxy Authen Type",       1,       2,      2}, /* 29 */
    {"Proxy Authen Name",       1,       0,   9999}, /* 30 */
    {"Proxy Authen Challenge",  1,       0,   9999}, /* 31 */
    {"Proxy Authen ID",         1,       2,      2}, /* 32 */
    {"Proxy Authen Response",   1,       0,   9999}, /* 33 */
    {"Call Errors",             1,      26,     26}, /* 34 */
    {"ACCM",                    1,      10,     10}, /* 35 */
    {"Random Vector",           0,       0,   9999}, /* 36 */
    {"Private Group ID",        1,       0,   9999}, /* 37 */
    {"RX Connect Speed",        1,       4,      4}, /* 38 */
    {"Sequencing Required",     0,       0,      0}  /* 39 */
};

/* A free list of L2TP dgram strucures.  Allocation of L2TP dgrams
   must be fast... */
static l2tp_dgram *dgram_free_list = NULL;

static void
describe_pulled_avp(uint16_t vendor,
                    uint16_t type,
                    uint16_t len,
                    int hidden,
                    int mandatory,
                    void *val)
{
    unsigned char *buf = val;
    uint16_t i;
    int ascii = 1;

    fprintf(stderr, "Pulled avp: len=%d ", (int) len);
    if (vendor == VENDOR_IETF) {
        fprintf(stderr, "type='%s' ",avp_table[type].name);
    } else {
        fprintf(stderr, "type=%d/%d ", (int) vendor, (int) type);
    }
    fprintf(stderr, "M=%d H=%d ", mandatory ? 1 : 0, hidden ? 1 : 0);

    fprintf(stderr, "val='");
    for (i=0; i<len; i++) {
        if (buf[i] < 32 || buf[i] > 126) {
            ascii = 0;
            break;
        }
    }
    for (i=0; i<len; i++) {
        if (ascii) {
            fprintf(stderr, "%c", buf[i]);
        } else {
            fprintf(stderr, "%02X ", (unsigned int) buf[i]);
        }
    }
    fprintf(stderr, "'\n");
}

/**********************************************************************
* %FUNCTION: dgram_validate_avp
* %ARGUMENTS:
*  vendor -- vendor code
*  type -- AVP type
*  len -- PAYLOAD length
*  mandatory -- non-zero if mandatory bit is set
* %RETURNS:
*  1 if len is valid for type, 0 otherwise.
***********************************************************************/
int
l2tp_dgram_validate_avp(uint16_t vendor,
                   uint16_t type,
                   uint16_t len,
                   int mandatory)
{
    if (vendor != VENDOR_IETF) {
        if (mandatory) {
            l2tp_set_errmsg("Unknown mandatory AVP (vendor %d, type %d)",
                       (int) vendor, (int) type);
            return 0;
        }
        /* I suppose... */
        return 1;
    }

    if (type > HIGHEST_AVP) {
        if (mandatory) {
            l2tp_set_errmsg("Unknown mandatory AVP of type %d",
                       (int) type);
            return 0;
        }
        return 1;
    }

    return (len >= avp_table[type].min_len &&
            len <= avp_table[type].max_len);
}

/**********************************************************************
* %FUNCTION: dgram_new
* %ARGUMENTS:
*  len -- payload length to allocate (not currently used...)
* %RETURNS:
*  A newly-allocated l2tp_dgram structure or NULL
* %DESCRIPTION:
*  Allocates and initializes an datagram structure.
***********************************************************************/
l2tp_dgram *
l2tp_dgram_new(size_t len)
{
    l2tp_dgram *dgram;

    if (len > MAX_PACKET_LEN) {
        l2tp_set_errmsg("dgram_new: cannot allocate datagram with payload length %d",
                   (int) len);
        return NULL;
    }

    if (dgram_free_list) {
        dgram = dgram_free_list;
        dgram_free_list = dgram->next;
    } else {
        dgram = malloc(sizeof(l2tp_dgram));
    }
    if (!dgram) {
        l2tp_set_errmsg("dgram_new: Out of memory");
        return NULL;
    }
    dgram->bits = 0;
    dgram->version = 2;
    dgram->length = 0;
    dgram->tid = 0;
    dgram->sid = 0;
    dgram->Ns = 0;
    dgram->Nr = 0;
    dgram->off_size = 0;
    dgram->last_random = 0;
    dgram->payload_len = 0;
    dgram->cursor = 0;

    /* We maintain this in case it becomes dynamic later on */
    dgram->alloc_len = MAX_PACKET_LEN;
    dgram->next = NULL;

    return dgram;
}

/**********************************************************************
* %FUNCTION: dgram_new_control
* %ARGUMENTS:
*  msg_type -- message type
*  tid -- tunnel ID
*  sid -- session ID
* %RETURNS:
*  A newly-allocated control datagram
* %DESCRIPTION:
*  Allocates datagram; sets tid and sid fields; adds message_type AVP
***********************************************************************/
l2tp_dgram *
l2tp_dgram_new_control(uint16_t msg_type,
                       uint16_t tid,
                       uint16_t sid)
{
    l2tp_dgram *dgram = l2tp_dgram_new(MAX_PACKET_LEN);
    uint16_t u16;

    if (!dgram) return NULL;

    dgram->bits = TYPE_BIT | LENGTH_BIT | SEQUENCE_BIT;
    dgram->tid = tid;
    dgram->sid = sid;
    dgram->msg_type = msg_type;
    if (msg_type != MESSAGE_ZLB) {
        u16 = htons(msg_type);

        l2tp_dgram_add_avp(dgram, NULL, MANDATORY,
                           sizeof(u16), VENDOR_IETF, AVP_MESSAGE_TYPE, &u16);
    }
    return dgram;
}

/**********************************************************************
* %FUNCTION: dgram_free
* %ARGUMENTS:
*  dgram -- datagram to free
* %RETURNS:
*  Nothing
* %DESCRIPTION:
*  Frees a datagram structure.
***********************************************************************/
void
l2tp_dgram_free(l2tp_dgram *dgram)
{
    if (!dgram) return;
    dgram->next = dgram_free_list;
    dgram_free_list = dgram;
}

/**********************************************************************
* %FUNCTION: dgram_take_from_wire
* %ARGUMENTS:
*  from -- set to address of peer.
* %RETURNS:
*  NULL on error, allocated datagram otherwise.
* %DESCRIPTION:
*  Reads an L2TP datagram off the wire and puts it in dgram.  Adjusts
*  header fields to host byte order.  Keeps reading in a loop unless
*  we hit a control frame or EAGAIN.  This is more efficient than
*  returning to select loop each time if there's lots of traffic.
***********************************************************************/
l2tp_dgram *
l2tp_dgram_take_from_wire(struct sockaddr_in *from)
{
    /* EXTRA_HEADER_ROOM bytes for other headers like PPPoE, etc. */

    unsigned char inbuf[MAX_PACKET_LEN+EXTRA_HEADER_ROOM];
    unsigned char *buf;
    socklen_t len = sizeof(struct sockaddr_in);
    int cursor;
    l2tp_dgram *dgram;
    unsigned char *payload;
    unsigned char *tidptr;
    uint16_t tid, sid;
    uint16_t cache_tid, cache_sid;
    l2tp_tunnel *tunnel;
    l2tp_session *ses = NULL;
    int mandatory, hidden, err;
    uint16_t vendor, type;
    unsigned char *msg;
    int r;

    /* Limit iterations before bailing back to select look.  Otherwise,
       we have a nice DoS possibility */
    int iters = 5;

    uint16_t off;
    int framelen;

    /* Active part of buffer */
    buf = inbuf + EXTRA_HEADER_ROOM;

    while(1) {
        if (--iters <= 0) return NULL;
        framelen = -1;
        r = recvfrom(Sock, buf, MAX_PACKET_LEN, 0,
                     (struct sockaddr *) from, &len);
        if (r <= 0) {
            return NULL;
        }

        /* Check version; drop frame if not L2TP (ver = 2) */
        if ((buf[1] & VERSION_MASK) != 2) continue;

        /* Not a data frame -- break out of loop and handle control frame */
        if (buf[0] & TYPE_BIT) break;

        /* If it's a data frame, we need to be FAST, so do not allocate
           a dgram structure, etc; just call into handler */
        payload = buf + 6;
        tidptr = buf + 2;
        if (buf[0] & LENGTH_BIT) {
            framelen = (((uint16_t) buf[2]) << 8) + buf[3];
            payload += 2;
            tidptr += 2;
        }
        if (buf[0] & SEQUENCE_BIT) {
            payload += 4;
        }
        if (buf[0] & OFFSET_BIT) {
            payload += 2;
            off = (((uint16_t) *(payload-2)) << 8) + *(payload-1);
            payload += off;
        }
        off = (payload - buf);
        if (framelen == -1) {
            framelen = r - off;
        } else {
            if (framelen < off || framelen > r) {
                continue;
            }
            /* Only count payload length */
            framelen -= off;
        }

        /* Forget the 0xFF, 0x03 HDLC markers */
        payload += 2;
        framelen -= 2;

        if (framelen < 0) {
            continue;
        }
        /* Get tunnel, session ID */
        tid = (((uint16_t) tidptr[0]) << 8) + (uint16_t) tidptr[1];
        sid = (((uint16_t) tidptr[2]) << 8) + (uint16_t) tidptr[3];

        /* Only do hash lookup if it's not cached */
        /* TODO: Is this optimization really worthwhile? */
        if (!ses || tid != cache_tid || sid != cache_sid) {
            ses = NULL;
            tunnel = l2tp_tunnel_find_by_my_id(tid);
            if (!tunnel) {
                l2tp_set_errmsg("Unknown tunnel %d", (int) tid);
                continue;
            }
            if (tunnel->state != TUNNEL_ESTABLISHED) {
                /* Drop frame if tunnel in wrong state */
                continue;
            }
            /* TODO: Verify source address */
            ses = l2tp_tunnel_find_session(tunnel, sid);
            if (!ses) {
                l2tp_set_errmsg("Unknown session %d in tunnel %d",
                                (int) sid, (int) tid);
                continue;
            }
            cache_tid = tid;
            cache_sid = sid;
            if (ses->state != SESSION_ESTABLISHED) {
                /* Drop frame if session in wrong state */
                continue;
            }
        }
        ses->call_ops->handle_ppp_frame(ses, payload, framelen);
        /* Snoop, if necessary */
        if (ses->snooping) {
            l2tp_session_lcp_snoop(ses, payload, framelen, 1);
        }
    }


    /* A bit of slop on dgram size */
    dgram = l2tp_dgram_new(r);
    if (!dgram) return NULL;

    dgram->bits = buf[0];
    dgram->version = buf[1];
    cursor = 2;

    if (dgram->bits & LENGTH_BIT) {
        PULL_UINT16(buf, cursor, dgram->length);
        if (dgram->length > r) {
            l2tp_set_errmsg("Invalid length field %d greater than received datagram size %d", (int) dgram->length, r);
            l2tp_dgram_free(dgram);
            return NULL;
        }
    } else {
        dgram->length = r;
    }

    PULL_UINT16(buf, cursor, dgram->tid);
    PULL_UINT16(buf, cursor, dgram->sid);
    if (dgram->bits & SEQUENCE_BIT) {
        PULL_UINT16(buf, cursor, dgram->Ns);
        PULL_UINT16(buf, cursor, dgram->Nr);
    } else {
        dgram->Ns = 0;
        dgram->Nr = 0;
    }

    if (dgram->bits & OFFSET_BIT) {
        PULL_UINT16(buf, cursor, dgram->off_size);
    } else {
        dgram->off_size = 0;
    }
    if (cursor > dgram->length) {
        l2tp_set_errmsg("Invalid length of datagram %d", (int) dgram->length);
        l2tp_dgram_free(dgram);
        return NULL;
    }

    dgram->payload_len = dgram->length - cursor;
    memcpy(dgram->data, buf+cursor, dgram->payload_len);

    /* Pull off the message type */
    if (dgram->bits & OFFSET_BIT) {
        l2tp_set_errmsg("Invalid control frame: O bit is set");
        l2tp_dgram_free(dgram);
        return NULL;
    }

    /* Handle ZLB */
    if (dgram->payload_len == 0) {
        dgram->msg_type = MESSAGE_ZLB;
    } else {
        uint16_t avp_len;
        msg = l2tp_dgram_pull_avp(dgram, NULL, &mandatory, &hidden,
                                  &avp_len, &vendor, &type, &err);
        if (!msg) {
            l2tp_dgram_free(dgram);
            return NULL;
        }
        if (type != AVP_MESSAGE_TYPE || vendor != VENDOR_IETF) {
            l2tp_set_errmsg("Invalid control message: First AVP must be message type");
            l2tp_dgram_free(dgram);
            return NULL;
        }
        if (avp_len != 2) {
            l2tp_set_errmsg("Invalid length %d for message-type AVP", (int) avp_len+6);
            l2tp_dgram_free(dgram);
            return NULL;
        }
        if (!mandatory) {
            l2tp_set_errmsg("Invalid control message: M bit not set on message-type AVP");
            l2tp_dgram_free(dgram);
            return NULL;
        }
        if (hidden) {
            l2tp_set_errmsg("Invalid control message: H bit set on message-type AVP");
            l2tp_dgram_free(dgram);
            return NULL;
        }

        dgram->msg_type = ((unsigned int) msg[0]) * 256 +
            (unsigned int) msg[1];
    }
    DBG(l2tp_db(DBG_XMIT_RCV,
           "dgram_take_from_wire() -> %s\n",
           l2tp_debug_describe_dgram(dgram)));
    return dgram;
}

/**********************************************************************
* %FUNCTION: dgram_send_to_wire
* %ARGUMENTS:
*  dgram -- datagram to send
*  to -- address to send datagram to
* %RETURNS:
*  Whatever sendto returns.  0 on success, -1 on failure.
* %DESCRIPTION:
*  Adjusts header fields from host byte order, then sends datagram
***********************************************************************/
int
l2tp_dgram_send_to_wire(l2tp_dgram const *dgram,
                        struct sockaddr_in const *to)
{
    unsigned char buf[MAX_PACKET_LEN+128];
    socklen_t len = sizeof(struct sockaddr_in);
    int cursor = 2;
    size_t total_len;
    unsigned char *len_ptr = NULL;

    DBG(l2tp_db(DBG_XMIT_RCV,
           "dgram_send_to_wire() -> %s\n",
           l2tp_debug_describe_dgram(dgram)));
    buf[0] = dgram->bits;
    buf[1] = dgram->version;

    if (dgram->bits & LENGTH_BIT) {
        len_ptr = buf + cursor;
        PUSH_UINT16(buf, cursor, dgram->length);
    }
    PUSH_UINT16(buf, cursor, dgram->tid);
    PUSH_UINT16(buf, cursor, dgram->sid);
    if (dgram->bits & SEQUENCE_BIT) {
        PUSH_UINT16(buf, cursor, dgram->Ns);
        PUSH_UINT16(buf, cursor, dgram->Nr);
    }
    if (dgram->bits & OFFSET_BIT) {
        PUSH_UINT16(buf, cursor, dgram->off_size);
    }
    total_len = cursor + dgram->payload_len;
    if (dgram->bits & LENGTH_BIT) {
        *len_ptr++ = total_len / 256;
        *len_ptr = total_len & 255;
    }
    memcpy(buf+cursor, dgram->data, dgram->payload_len);
    return sendto(Sock, buf, total_len, 0,
                  (struct sockaddr const *) to, len);
}

/**********************************************************************
* %FUNCTION: dgram_add_avp
* %ARGUMENTS:
*  dgram -- the L2TP datagram
*  tunnel -- the tunnel it will be sent on (for secret for hiding)
*  mandatory -- if true, set the M bit
*  len -- length of VALUE.  NOT length of entire attribute!
*  vendor -- vendor ID
*  type -- attribute type
*  val  -- attribute value
* %RETURNS:
*  0 on success, -1 on failure
* %DESCRIPTION:
*  Adds an AVP to the datagram.  If AVP may be hidden, and we have
*  a secret for it, then hide the AVP.
***********************************************************************/
int
l2tp_dgram_add_avp(l2tp_dgram *dgram,
                   l2tp_tunnel *tunnel,
                   int mandatory,
                   uint16_t len,
                   uint16_t vendor,
                   uint16_t type,
                   void *val)
{
    static unsigned char hidden_buffer[1024];
    unsigned char const *random_data;
    size_t random_len;
    int hidden = 0;

    /* max len is 1023 - 6 */
    if (len > 1023 - 6) {
        l2tp_set_errmsg("AVP length of %d too long", (int) len);
        return -1;
    }

    /* Do hiding where possible */
    if (tunnel                   &&
        tunnel->peer             &&
        tunnel->peer->hide_avps  &&
        vendor == VENDOR_IETF    &&
        tunnel->peer->secret_len &&
        len <= 1021 - 6          &&
        avp_table[type].can_hide) {
        if (!dgram->last_random) {
            /* Add a random vector */
            if (dgram_add_random_vector_avp(dgram) < 0) return -1;
        }
        /* Get pointer to random data */
        random_data = dgram->data + dgram->last_random + 6;
        random_len  = GET_AVP_LEN(dgram->data + dgram->last_random) - 6;

        /* Hide the value into the buffer */
        dgram_do_hide(type, len, val, (unsigned char *) tunnel->peer->secret,
                      tunnel->peer->secret_len,
                      random_data, random_len, hidden_buffer);

        /* Length is increased by 2 */
        len += 2;
        val = hidden_buffer;
        hidden = 1;
    }

    /* Adjust from payload len to actual len */
    len += 6;

    /* Does datagram have room? */
    if (dgram->cursor + len > dgram->alloc_len) {
        l2tp_set_errmsg("No room for AVP of length %d", (int) len);
        return -1;
    }

    dgram->data[dgram->cursor] = 0;
    if (mandatory) {
        dgram->data[dgram->cursor] |= AVP_MANDATORY_BIT;
    }
    if (hidden) {
        dgram->data[dgram->cursor] |= AVP_HIDDEN_BIT;
    }

    dgram->data[dgram->cursor] |= (len >> 8);
    dgram->data[dgram->cursor+1] = (len & 0xFF);
    dgram->data[dgram->cursor+2] = (vendor >> 8);
    dgram->data[dgram->cursor+3] = (vendor & 0xFF);
    dgram->data[dgram->cursor+4] = (type >> 8);
    dgram->data[dgram->cursor+5] = (type & 0xFF);

    if (len > 6) {
        memcpy(dgram->data + dgram->cursor + 6, val, len-6);
    }
    if (type == AVP_RANDOM_VECTOR) {
        /* Remember location of random vector */
        dgram->last_random = dgram->cursor;
    }
    dgram->cursor += len;
    dgram->payload_len = dgram->cursor;
    return 0;
}

/**********************************************************************
* %FUNCTION: dgram_pull_avp
* %ARGUMENTS:
*  dgram -- the L2TP datagram
*  tunnel -- the tunnel it was received on (for secret for hiding)
*  mandatory -- if set to true, the M bit was set.
*  hidden -- if set to true, the value was hidden
*  len -- set to length of value (after unhiding)
*  vendor -- set to vendor ID
*  type -- set to attribute type
*  err -- set to true if an error occurs, false if not.
* %RETURNS:
*  A pointer to a buffer containing the value, or NULL if an
*  error occurs or there are no more AVPs.
* %DESCRIPTION:
*  Pulls an AVP out of a received datagram.
***********************************************************************/
unsigned char *
l2tp_dgram_pull_avp(l2tp_dgram *dgram,
                    l2tp_tunnel *tunnel,
                    int *mandatory,
                    int *hidden,
                    uint16_t *len,
                    uint16_t *vendor,
                    uint16_t *type,
                    int *err)
{
    static unsigned char val[1024];
    unsigned char *ans;
    unsigned char *random_data;
    size_t random_len;
    int local_hidden;
    int local_mandatory;

    *err = 1;
    if (dgram->cursor >= dgram->payload_len) {
        /* EOF */
        *err = 0;
        return NULL;
    }

    /* Get bits */
    if (!mandatory) {
        mandatory = &local_mandatory;
    }
    *mandatory = dgram->data[dgram->cursor] & AVP_MANDATORY_BIT;

    if (!hidden) {
        hidden = &local_hidden;
    }
    *hidden = dgram->data[dgram->cursor] & AVP_HIDDEN_BIT;
    if (dgram->data[dgram->cursor] & AVP_RESERVED_BITS) {
        l2tp_set_errmsg("AVP with reserved bits set to non-zero");
        return NULL;
    }

    /* Get len */
    *len = ((unsigned int) (dgram->data[dgram->cursor] & 3)) * 256 +
        dgram->data[dgram->cursor+1];
    if (*len < 6) {
        l2tp_set_errmsg("Received AVP of length %d (too short)", (int) *len);
        return NULL;
    }

    if (dgram->cursor + *len > dgram->payload_len) {
        l2tp_set_errmsg("Received AVP of length %d too long for rest of datagram",
                   (int) *len);
        return NULL;
    }
    dgram->cursor += 2;

    PULL_UINT16(dgram->data, dgram->cursor, *vendor);
    PULL_UINT16(dgram->data, dgram->cursor, *type);

    /* If we see a random vector, remember it */
    if (*vendor == VENDOR_IETF && *type == AVP_RANDOM_VECTOR) {
        if (*hidden) {
            l2tp_set_errmsg("Invalid random vector AVP has H bit set");
            return NULL;
        }
        dgram->last_random = dgram->cursor - 6;
    }

    ans = dgram->data + dgram->cursor;
    dgram->cursor += *len - 6;
    if (*hidden) {
        if (!tunnel) {
            l2tp_set_errmsg("AVP cannot be hidden");
            return NULL;
        }
        if (*len < 8) {
            l2tp_set_errmsg("Received hidden AVP of length %d (too short)",
                       (int) *len);
            return NULL;
        }

        if (!tunnel->peer) {
            l2tp_set_errmsg("No peer??");
            return NULL;
        }
        if (!tunnel->peer->secret_len) {
            l2tp_set_errmsg("No shared secret to unhide AVP");
            return NULL;
        }
        if (!dgram->last_random) {
            l2tp_set_errmsg("Cannot unhide AVP unless Random Vector received first");
            return NULL;
        }

        if (*type <= HIGHEST_AVP) {
            if (!avp_table[*type].can_hide) {
                l2tp_set_errmsg("AVP of type %s cannot be hidden, but H bit set",
                           avp_table[*type].name);
                return NULL;
            }
        }

        /* Get pointer to random data */
        random_data = dgram->data + dgram->last_random + 6;
        random_len  = GET_AVP_LEN(dgram->data + dgram->last_random) - 6;

        /* Unhide value */
        ans = dgram_do_unhide(*type, len, ans,
                              (unsigned char *) tunnel->peer->secret,
                              tunnel->peer->secret_len,
                              random_data, random_len, val);
        if (!ans) return NULL;
    }

    /* Set len to length of value only */
    *len -= 6;

    *err = 0;
    /* DBG(describe_pulled_avp(*vendor, *type, *len, *hidden, *mandatory, ans)); */
    return ans;

}

/**********************************************************************
* %FUNCTION: dgram_do_hide
* %ARGUMENTS:
*  type -- attribute type
*  len -- attribute length
*  value -- attribute value
*  secret -- shared tunnel secret
*  secret_len -- length of secret
*  random -- random data
*  random_len -- length of random data
*  output -- where to put result
* %RETURNS:
*  Nothing
* %DESCRIPTION:
*  Hides AVP into output as described in RFC2661.  Does not do
*  padding (should we?)
***********************************************************************/
static void
dgram_do_hide(uint16_t type,
              uint16_t len,
              unsigned char *value,
              unsigned char const *secret,
              size_t secret_len,
              unsigned char const *random_data,
              size_t random_len,
              unsigned char *output)
{
    struct MD5Context ctx;
    unsigned char t[2];
    unsigned char digest[16];
    size_t done;
    size_t todo = len;

    /* Put type in network byte order */
    t[0] = type / 256;
    t[1] = type & 255;

    /* Compute initial pad */
    MD5Init(&ctx);
    MD5Update(&ctx, t, 2);
    MD5Update(&ctx, secret, secret_len);
    MD5Update(&ctx, random_data, random_len);
    MD5Final(digest, &ctx);

    /* Insert length */
    output[0] = digest[0] ^ (len / 256);
    output[1] = digest[1] ^ (len & 255);
    output += 2;
    done = 2;

    /* Insert value */
    while(todo) {
        *output = digest[done] ^ *value;
        ++output;
        ++value;
        --todo;
        ++done;
        if (done == 16 && todo) {
            /* Compute new digest */
            done = 0;
            MD5Init(&ctx);
            MD5Update(&ctx, secret, secret_len);
            MD5Update(&ctx, output-16, 16);
            MD5Final(digest, &ctx);
        }
    }
}

/**********************************************************************
* %FUNCTION: dgram_do_unhide
* %ARGUMENTS:
*  type -- attribute type
*  len -- attribute length (input = orig len; output = unhidden len)
*  value -- attribute value (the hidden characters)
*  secret -- shared tunnel secret
*  secret_len -- length of secret
*  random_data -- random data
*  random_len -- length of random data
*  output -- where to put result
* %RETURNS:
*  pointer to "output" on success; NULL on failure.
* %DESCRIPTION:
*  Un-hides AVP as in RFC2661
***********************************************************************/
static unsigned char *
dgram_do_unhide(uint16_t type,
                uint16_t *len,
                unsigned char *value,
                unsigned char const *secret,
                size_t secret_len,
                unsigned char const *random_data,
                size_t random_len,
                unsigned char *output)
{
    struct MD5Context ctx;
    unsigned char t[2];
    unsigned char digest[16];
    uint16_t tmplen;
    unsigned char *orig_output = output;
    size_t done, todo;

    /* Put type in network byte order */
    t[0] = type / 256;
    t[1] = type & 255;

    /* Compute initial pad */
    MD5Init(&ctx);
    MD5Update(&ctx, t, 2);
    MD5Update(&ctx, secret, secret_len);
    MD5Update(&ctx, random_data, random_len);
    MD5Final(digest, &ctx);

    /* Get hidden length */
    tmplen =
        ((uint16_t) (digest[0] ^ value[0])) * 256 +
        (uint16_t) (digest[1] ^ value[1]);

    value += 2;

    if (tmplen > *len-8) {
        l2tp_set_errmsg("Hidden length %d too long in AVP of length %d",
                   (int) tmplen, (int) *len);
        return NULL;
    }

    /* Adjust len.  Add 6 to compensate for pseudo-header */
    *len = tmplen + 6;

    /* Decrypt remainder */
    done = 2;
    todo = tmplen;
    while(todo) {
        *output = digest[done] ^ *value;
        ++output;
        ++value;
        --todo;
        ++done;
        if (done == 16 && todo) {
            /* Compute new digest */
            done = 0;
            MD5Init(&ctx);
            MD5Update(&ctx, secret, secret_len);
            MD5Update(&ctx, value-16, 16);
            MD5Final(digest, &ctx);
        }
    }
    return orig_output;
}

/**********************************************************************
* %FUNCTION: dgram_search_avp
* %ARGUMENTS:
*  dgram -- the datagram
*  tunnel -- associated tunnel
*  mandatory -- set to 1 if M bit was set
*  hidden -- set to 1 if H bit was set
*  len -- set to length of payload
*  vendor -- vendor ID we want
*  type -- AVP type we want
* %RETURNS:
*  A pointer to the AVP value if found, NULL if not
* %DESCRIPTION:
*  Searches dgram for specific AVP type.
***********************************************************************/
unsigned char *
l2tp_dgram_search_avp(l2tp_dgram *dgram,
                      l2tp_tunnel *tunnel,
                      int *mandatory,
                      int *hidden,
                      uint16_t *len,
                      uint16_t vendor,
                      uint16_t type)
{
    uint16_t svend, stype;
    int err;
    unsigned char *val;
    size_t cursor = dgram->cursor;
    size_t last_random = dgram->last_random;
    while(1) {
        val = l2tp_dgram_pull_avp(dgram, tunnel, mandatory, hidden, len,
                                  &svend, &stype, &err);
        if (!val) {
            if (!err) {
                l2tp_set_errmsg("AVP of vendor/type (%d, %d) not found",
                           (int) vendor, (int) type);
            }
            dgram->cursor = cursor;
            dgram->last_random = last_random;
            return NULL;
        }
        if (vendor == svend && type == stype) break;
    }
    dgram->cursor = cursor;
    dgram->last_random = last_random;
    return val;
}

/**********************************************************************
* %FUNCTION: dgram_send_ppp_frame
* %ARGUMENTS:
*  ses -- the session
*  buf -- PPP frame.  BUF MUST HAVE AT LEAST 16 BYTES OF SPACE AHEAD OF IT!
*  len -- length of PPP frame
* %RETURNS:
*  Whatever sendto returns
* %DESCRIPTION:
*  Sends a PPP frame.  TODO: Implement sequence numbers if required.
***********************************************************************/
int
l2tp_dgram_send_ppp_frame(l2tp_session *ses,
                          unsigned char const *buf,
                          int len)
{
    int r;
    unsigned char *real_buf = (unsigned char *) buf - 8;
    l2tp_tunnel *tunnel = ses->tunnel;

    /* Drop frame if tunnel and/or session in wrong state */
    if (!tunnel ||
        tunnel->state != TUNNEL_ESTABLISHED ||
        ses->state != SESSION_ESTABLISHED) {
        return 0;
    }

    /* If there is a pending ack on the tunnel, cancel and ack now */
    if (tunnel->ack_handler) {
        Event_DelHandler(tunnel->es, tunnel->ack_handler);
        tunnel->ack_handler = NULL;
        tunnel_send_ZLB(tunnel);
    }

    real_buf[0] = 0;
    real_buf[1] = 2;
    real_buf[2] = (tunnel->assigned_id >> 8);
    real_buf[3] = tunnel->assigned_id & 0xFF;
    real_buf[4] = ses->assigned_id >> 8;
    real_buf[5] = ses->assigned_id & 0xFF;
    real_buf[6] = 0xFF;         /* HDLC address */
    real_buf[7] = 0x03;         /* HDLC control */
    r = sendto(Sock, real_buf, len+8, 0,
               (struct sockaddr const *) &tunnel->peer_addr,
               sizeof(struct sockaddr_in));
    /* Snoop, if necessary */
    if (ses->snooping) {
        l2tp_session_lcp_snoop(ses, buf, len, 0);
    }
    return r;
}

/**********************************************************************
* %FUNCTION: dgram_add_random_vector_avp
* %ARGUMENTS:
*  dgram -- datagram
* %RETURNS:
*  0 on success; -1 on failure
* %DESCRIPTION:
*  Adds a random-vector AVP to the datagram
***********************************************************************/
static int
dgram_add_random_vector_avp(l2tp_dgram *dgram)
{
    unsigned char buf[16];

    l2tp_random_fill(buf, sizeof(buf));

    return l2tp_dgram_add_avp(dgram, NULL, MANDATORY, sizeof(buf),
                              VENDOR_IETF, AVP_RANDOM_VECTOR, buf);
}