[mirror_qemu.git] / net / l2tpv3.c

/*
 * QEMU System Emulator
 *
 * Copyright (c) 2003-2008 Fabrice Bellard
 * Copyright (c) 2012-2014 Cisco Systems
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "qemu/osdep.h"
#include <linux/ip.h>
#include <netdb.h>
#include "net/net.h"
#include "clients.h"
#include "qapi/error.h"
#include "qemu/error-report.h"
#include "qemu/option.h"
#include "qemu/sockets.h"
#include "qemu/iov.h"
#include "qemu/main-loop.h"
#include "qemu/memalign.h"

/* The buffer size needs to be investigated for optimum numbers and
 * optimum means of paging in on different systems. This size is
 * chosen to be sufficient to accommodate one packet with some headers
 */

#define BUFFER_ALIGN sysconf(_SC_PAGESIZE)
#define BUFFER_SIZE 16384
#define IOVSIZE 2
#define MAX_L2TPV3_MSGCNT 64
#define MAX_L2TPV3_IOVCNT (MAX_L2TPV3_MSGCNT * IOVSIZE)

/* Header set to 0x30000 signifies a data packet */

#define L2TPV3_DATA_PACKET 0x30000

/* IANA-assigned IP protocol ID for L2TPv3 */

#ifndef IPPROTO_L2TP
#define IPPROTO_L2TP 0x73
#endif

typedef struct NetL2TPV3State {
    NetClientState nc;
    int fd;

    /*
     * these are used for xmit - that happens packet a time
     * and for first sign of life packet (easier to parse that once)
     */

    uint8_t *header_buf;
    struct iovec *vec;

    /*
     * these are used for receive - try to "eat" up to 32 packets at a time
     */

    struct mmsghdr *msgvec;

    /*
     * peer address
     */

    struct sockaddr_storage *dgram_dst;
    uint32_t dst_size;

    /*
     * L2TPv3 parameters
     */

    uint64_t rx_cookie;
    uint64_t tx_cookie;
    uint32_t rx_session;
    uint32_t tx_session;
    uint32_t header_size;
    uint32_t counter;

    /*
    * DOS avoidance in error handling
    */

    bool header_mismatch;

    /*
     * Ring buffer handling
     */

    int queue_head;
    int queue_tail;
    int queue_depth;

    /*
     * Precomputed offsets
     */

    uint32_t offset;
    uint32_t cookie_offset;
    uint32_t counter_offset;
    uint32_t session_offset;

    /* Poll Control */

    bool read_poll;
    bool write_poll;

    /* Flags */

    bool ipv6;
    bool udp;
    bool has_counter;
    bool pin_counter;
    bool cookie;
    bool cookie_is_64;

} NetL2TPV3State;

static void net_l2tpv3_send(void *opaque);
static void l2tpv3_writable(void *opaque);

static void l2tpv3_update_fd_handler(NetL2TPV3State *s)
{
    qemu_set_fd_handler(s->fd,
                        s->read_poll ? net_l2tpv3_send : NULL,
                        s->write_poll ? l2tpv3_writable : NULL,
                        s);
}

static void l2tpv3_read_poll(NetL2TPV3State *s, bool enable)
{
    if (s->read_poll != enable) {
        s->read_poll = enable;
        l2tpv3_update_fd_handler(s);
    }
}

static void l2tpv3_write_poll(NetL2TPV3State *s, bool enable)
{
    if (s->write_poll != enable) {
        s->write_poll = enable;
        l2tpv3_update_fd_handler(s);
    }
}

static void l2tpv3_writable(void *opaque)
{
    NetL2TPV3State *s = opaque;
    l2tpv3_write_poll(s, false);
    qemu_flush_queued_packets(&s->nc);
}

static void l2tpv3_send_completed(NetClientState *nc, ssize_t len)
{
    NetL2TPV3State *s = DO_UPCAST(NetL2TPV3State, nc, nc);
    l2tpv3_read_poll(s, true);
}

static void l2tpv3_poll(NetClientState *nc, bool enable)
{
    NetL2TPV3State *s = DO_UPCAST(NetL2TPV3State, nc, nc);
    l2tpv3_write_poll(s, enable);
    l2tpv3_read_poll(s, enable);
}

static void l2tpv3_form_header(NetL2TPV3State *s)
{
    uint32_t *counter;

    if (s->udp) {
        stl_be_p((uint32_t *) s->header_buf, L2TPV3_DATA_PACKET);
    }
    stl_be_p(
            (uint32_t *) (s->header_buf + s->session_offset),
            s->tx_session
        );
    if (s->cookie) {
        if (s->cookie_is_64) {
            stq_be_p(
                (uint64_t *)(s->header_buf + s->cookie_offset),
                s->tx_cookie
            );
        } else {
            stl_be_p(
                (uint32_t *) (s->header_buf + s->cookie_offset),
                s->tx_cookie
            );
        }
    }
    if (s->has_counter) {
        counter = (uint32_t *)(s->header_buf + s->counter_offset);
        if (s->pin_counter) {
            *counter = 0;
        } else {
            stl_be_p(counter, ++s->counter);
        }
    }
}

static ssize_t net_l2tpv3_receive_dgram_iov(NetClientState *nc,
                    const struct iovec *iov,
                    int iovcnt)
{
    NetL2TPV3State *s = DO_UPCAST(NetL2TPV3State, nc, nc);

    struct msghdr message;
    int ret;

    if (iovcnt > MAX_L2TPV3_IOVCNT - 1) {
        error_report(
            "iovec too long %d > %d, change l2tpv3.h",
            iovcnt, MAX_L2TPV3_IOVCNT
        );
        return -1;
    }
    l2tpv3_form_header(s);
    memcpy(s->vec + 1, iov, iovcnt * sizeof(struct iovec));
    s->vec->iov_base = s->header_buf;
    s->vec->iov_len = s->offset;
    message.msg_name = s->dgram_dst;
    message.msg_namelen = s->dst_size;
    message.msg_iov = s->vec;
    message.msg_iovlen = iovcnt + 1;
    message.msg_control = NULL;
    message.msg_controllen = 0;
    message.msg_flags = 0;
    ret = RETRY_ON_EINTR(sendmsg(s->fd, &message, 0));
    if (ret > 0) {
        ret -= s->offset;
    } else if (ret == 0) {
        /* belt and braces - should not occur on DGRAM
        * we should get an error and never a 0 send
        */
        ret = iov_size(iov, iovcnt);
    } else {
        /* signal upper layer that socket buffer is full */
        ret = -errno;
        if (ret == -EAGAIN || ret == -ENOBUFS) {
            l2tpv3_write_poll(s, true);
            ret = 0;
        }
    }
    return ret;
}

static ssize_t net_l2tpv3_receive_dgram(NetClientState *nc,
                    const uint8_t *buf,
                    size_t size)
{
    NetL2TPV3State *s = DO_UPCAST(NetL2TPV3State, nc, nc);

    struct iovec *vec;
    struct msghdr message;
    ssize_t ret = 0;

    l2tpv3_form_header(s);
    vec = s->vec;
    vec->iov_base = s->header_buf;
    vec->iov_len = s->offset;
    vec++;
    vec->iov_base = (void *) buf;
    vec->iov_len = size;
    message.msg_name = s->dgram_dst;
    message.msg_namelen = s->dst_size;
    message.msg_iov = s->vec;
    message.msg_iovlen = 2;
    message.msg_control = NULL;
    message.msg_controllen = 0;
    message.msg_flags = 0;
    ret = RETRY_ON_EINTR(sendmsg(s->fd, &message, 0));
    if (ret > 0) {
        ret -= s->offset;
    } else if (ret == 0) {
        /* belt and braces - should not occur on DGRAM
        * we should get an error and never a 0 send
        */
        ret = size;
    } else {
        ret = -errno;
        if (ret == -EAGAIN || ret == -ENOBUFS) {
            /* signal upper layer that socket buffer is full */
            l2tpv3_write_poll(s, true);
            ret = 0;
        }
    }
    return ret;
}

static int l2tpv3_verify_header(NetL2TPV3State *s, uint8_t *buf)
{

    uint32_t *session;
    uint64_t cookie;

    if ((!s->udp) && (!s->ipv6)) {
        buf += sizeof(struct iphdr) /* fix for ipv4 raw */;
    }

    /* we do not do a strict check for "data" packets as per
    * the RFC spec because the pure IP spec does not have
    * that anyway.
    */

    if (s->cookie) {
        if (s->cookie_is_64) {
            cookie = ldq_be_p(buf + s->cookie_offset);
        } else {
            cookie = ldl_be_p(buf + s->cookie_offset) & 0xffffffffULL;
        }
        if (cookie != s->rx_cookie) {
            if (!s->header_mismatch) {
                error_report("unknown cookie id");
            }
            return -1;
        }
    }
    session = (uint32_t *) (buf + s->session_offset);
    if (ldl_be_p(session) != s->rx_session) {
        if (!s->header_mismatch) {
            error_report("session mismatch");
        }
        return -1;
    }
    return 0;
}

static void net_l2tpv3_process_queue(NetL2TPV3State *s)
{
    int size = 0;
    struct iovec *vec;
    bool bad_read;
    int data_size;
    struct mmsghdr *msgvec;

    /* go into ring mode only if there is a "pending" tail */
    if (s->queue_depth > 0) {
        do {
            msgvec = s->msgvec + s->queue_tail;
            if (msgvec->msg_len > 0) {
                data_size = msgvec->msg_len - s->header_size;
                vec = msgvec->msg_hdr.msg_iov;
                if ((data_size > 0) &&
                    (l2tpv3_verify_header(s, vec->iov_base) == 0)) {
                    vec++;
                    /* Use the legacy delivery for now, we will
                     * switch to using our own ring as a queueing mechanism
                     * at a later date
                     */
                    size = qemu_send_packet_async(
                            &s->nc,
                            vec->iov_base,
                            data_size,
                            l2tpv3_send_completed
                        );
                    if (size == 0) {
                        l2tpv3_read_poll(s, false);
                    }
                    bad_read = false;
                } else {
                    bad_read = true;
                    if (!s->header_mismatch) {
                        /* report error only once */
                        error_report("l2tpv3 header verification failed");
                        s->header_mismatch = true;
                    }
                }
            } else {
                bad_read = true;
            }
            s->queue_tail = (s->queue_tail + 1) % MAX_L2TPV3_MSGCNT;
            s->queue_depth--;
        } while (
                (s->queue_depth > 0) &&
                 qemu_can_send_packet(&s->nc) &&
                ((size > 0) || bad_read)
            );
    }
}

static void net_l2tpv3_send(void *opaque)
{
    NetL2TPV3State *s = opaque;
    int target_count, count;
    struct mmsghdr *msgvec;

    /* go into ring mode only if there is a "pending" tail */

    if (s->queue_depth) {

        /* The ring buffer we use has variable intake
         * count of how much we can read varies - adjust accordingly
         */

        target_count = MAX_L2TPV3_MSGCNT - s->queue_depth;

        /* Ensure we do not overrun the ring when we have
         * a lot of enqueued packets
         */

        if (s->queue_head + target_count > MAX_L2TPV3_MSGCNT) {
            target_count = MAX_L2TPV3_MSGCNT - s->queue_head;
        }
    } else {

        /* we do not have any pending packets - we can use
        * the whole message vector linearly instead of using
        * it as a ring
        */

        s->queue_head = 0;
        s->queue_tail = 0;
        target_count = MAX_L2TPV3_MSGCNT;
    }

    msgvec = s->msgvec + s->queue_head;
    if (target_count > 0) {
        count = RETRY_ON_EINTR(
                recvmmsg(s->fd, msgvec, target_count, MSG_DONTWAIT, NULL)
        );
        if (count < 0) {
            /* Recv error - we still need to flush packets here,
             * (re)set queue head to current position
             */
            count = 0;
        }
        s->queue_head = (s->queue_head + count) % MAX_L2TPV3_MSGCNT;
        s->queue_depth += count;
    }
    net_l2tpv3_process_queue(s);
}

static void destroy_vector(struct mmsghdr *msgvec, int count, int iovcount)
{
    int i, j;
    struct iovec *iov;
    struct mmsghdr *cleanup = msgvec;
    if (cleanup) {
        for (i = 0; i < count; i++) {
            if (cleanup->msg_hdr.msg_iov) {
                iov = cleanup->msg_hdr.msg_iov;
                for (j = 0; j < iovcount; j++) {
                    g_free(iov->iov_base);
                    iov++;
                }
                g_free(cleanup->msg_hdr.msg_iov);
            }
            cleanup++;
        }
        g_free(msgvec);
    }
}

static struct mmsghdr *build_l2tpv3_vector(NetL2TPV3State *s, int count)
{
    int i;
    struct iovec *iov;
    struct mmsghdr *msgvec, *result;

    msgvec = g_new(struct mmsghdr, count);
    result = msgvec;
    for (i = 0; i < count ; i++) {
        msgvec->msg_hdr.msg_name = NULL;
        msgvec->msg_hdr.msg_namelen = 0;
        iov =  g_new(struct iovec, IOVSIZE);
        msgvec->msg_hdr.msg_iov = iov;
        iov->iov_base = g_malloc(s->header_size);
        iov->iov_len = s->header_size;
        iov++ ;
        iov->iov_base = qemu_memalign(BUFFER_ALIGN, BUFFER_SIZE);
        iov->iov_len = BUFFER_SIZE;
        msgvec->msg_hdr.msg_iovlen = 2;
        msgvec->msg_hdr.msg_control = NULL;
        msgvec->msg_hdr.msg_controllen = 0;
        msgvec->msg_hdr.msg_flags = 0;
        msgvec++;
    }
    return result;
}

static void net_l2tpv3_cleanup(NetClientState *nc)
{
    NetL2TPV3State *s = DO_UPCAST(NetL2TPV3State, nc, nc);
    qemu_purge_queued_packets(nc);
    l2tpv3_read_poll(s, false);
    l2tpv3_write_poll(s, false);
    if (s->fd >= 0) {
        close(s->fd);
    }
    destroy_vector(s->msgvec, MAX_L2TPV3_MSGCNT, IOVSIZE);
    g_free(s->vec);
    g_free(s->header_buf);
    g_free(s->dgram_dst);
}

static NetClientInfo net_l2tpv3_info = {
    .type = NET_CLIENT_DRIVER_L2TPV3,
    .size = sizeof(NetL2TPV3State),
    .receive = net_l2tpv3_receive_dgram,
    .receive_iov = net_l2tpv3_receive_dgram_iov,
    .poll = l2tpv3_poll,
    .cleanup = net_l2tpv3_cleanup,
};

int net_init_l2tpv3(const Netdev *netdev,
                    const char *name,
                    NetClientState *peer, Error **errp)
{
    const NetdevL2TPv3Options *l2tpv3;
    NetL2TPV3State *s;
    NetClientState *nc;
    int fd = -1, gairet;
    struct addrinfo hints;
    struct addrinfo *result = NULL;
    char *srcport, *dstport;

    nc = qemu_new_net_client(&net_l2tpv3_info, peer, "l2tpv3", name);

    s = DO_UPCAST(NetL2TPV3State, nc, nc);

    s->queue_head = 0;
    s->queue_tail = 0;
    s->header_mismatch = false;

    assert(netdev->type == NET_CLIENT_DRIVER_L2TPV3);
    l2tpv3 = &netdev->u.l2tpv3;

    if (l2tpv3->has_ipv6 && l2tpv3->ipv6) {
        s->ipv6 = l2tpv3->ipv6;
    } else {
        s->ipv6 = false;
    }

    if ((l2tpv3->has_offset) && (l2tpv3->offset > 256)) {
        error_setg(errp, "offset must be less than 256 bytes");
        goto outerr;
    }

    if (l2tpv3->has_rxcookie || l2tpv3->has_txcookie) {
        if (l2tpv3->has_rxcookie && l2tpv3->has_txcookie) {
            s->cookie = true;
        } else {
            error_setg(errp,
                       "require both 'rxcookie' and 'txcookie' or neither");
            goto outerr;
        }
    } else {
        s->cookie = false;
    }

    if (l2tpv3->has_cookie64 || l2tpv3->cookie64) {
        s->cookie_is_64  = true;
    } else {
        s->cookie_is_64  = false;
    }

    if (l2tpv3->has_udp && l2tpv3->udp) {
        s->udp = true;
        if (!(l2tpv3->srcport && l2tpv3->dstport)) {
            error_setg(errp, "need both src and dst port for udp");
            goto outerr;
        } else {
            srcport = l2tpv3->srcport;
            dstport = l2tpv3->dstport;
        }
    } else {
        s->udp = false;
        srcport = NULL;
        dstport = NULL;
    }


    s->offset = 4;
    s->session_offset = 0;
    s->cookie_offset = 4;
    s->counter_offset = 4;

    s->tx_session = l2tpv3->txsession;
    if (l2tpv3->has_rxsession) {
        s->rx_session = l2tpv3->rxsession;
    } else {
        s->rx_session = s->tx_session;
    }

    if (s->cookie) {
        s->rx_cookie = l2tpv3->rxcookie;
        s->tx_cookie = l2tpv3->txcookie;
        if (s->cookie_is_64 == true) {
            /* 64 bit cookie */
            s->offset += 8;
            s->counter_offset += 8;
        } else {
            /* 32 bit cookie */
            s->offset += 4;
            s->counter_offset += 4;
        }
    }

    memset(&hints, 0, sizeof(hints));

    if (s->ipv6) {
        hints.ai_family = AF_INET6;
    } else {
        hints.ai_family = AF_INET;
    }
    if (s->udp) {
        hints.ai_socktype = SOCK_DGRAM;
        hints.ai_protocol = 0;
        s->offset += 4;
        s->counter_offset += 4;
        s->session_offset += 4;
        s->cookie_offset += 4;
    } else {
        hints.ai_socktype = SOCK_RAW;
        hints.ai_protocol = IPPROTO_L2TP;
    }

    gairet = getaddrinfo(l2tpv3->src, srcport, &hints, &result);

    if ((gairet != 0) || (result == NULL)) {
        error_setg(errp, "could not resolve src, errno = %s",
                   gai_strerror(gairet));
        goto outerr;
    }
    fd = socket(result->ai_family, result->ai_socktype, result->ai_protocol);
    if (fd == -1) {
        fd = -errno;
        error_setg(errp, "socket creation failed, errno = %d",
                   -fd);
        goto outerr;
    }
    if (bind(fd, (struct sockaddr *) result->ai_addr, result->ai_addrlen)) {
        error_setg(errp, "could not bind socket err=%i", errno);
        goto outerr;
    }

    freeaddrinfo(result);

    memset(&hints, 0, sizeof(hints));

    if (s->ipv6) {
        hints.ai_family = AF_INET6;
    } else {
        hints.ai_family = AF_INET;
    }
    if (s->udp) {
        hints.ai_socktype = SOCK_DGRAM;
        hints.ai_protocol = 0;
    } else {
        hints.ai_socktype = SOCK_RAW;
        hints.ai_protocol = IPPROTO_L2TP;
    }

    result = NULL;
    gairet = getaddrinfo(l2tpv3->dst, dstport, &hints, &result);
    if ((gairet != 0) || (result == NULL)) {
        error_setg(errp, "could not resolve dst, error = %s",
                   gai_strerror(gairet));
        goto outerr;
    }

    s->dgram_dst = g_new0(struct sockaddr_storage, 1);
    memcpy(s->dgram_dst, result->ai_addr, result->ai_addrlen);
    s->dst_size = result->ai_addrlen;

    freeaddrinfo(result);

    if (l2tpv3->has_counter && l2tpv3->counter) {
        s->has_counter = true;
        s->offset += 4;
    } else {
        s->has_counter = false;
    }

    if (l2tpv3->has_pincounter && l2tpv3->pincounter) {
        s->has_counter = true;  /* pin counter implies that there is counter */
        s->pin_counter = true;
    } else {
        s->pin_counter = false;
    }

    if (l2tpv3->has_offset) {
        /* extra offset */
        s->offset += l2tpv3->offset;
    }

    if ((s->ipv6) || (s->udp)) {
        s->header_size = s->offset;
    } else {
        s->header_size = s->offset + sizeof(struct iphdr);
    }

    s->msgvec = build_l2tpv3_vector(s, MAX_L2TPV3_MSGCNT);
    s->vec = g_new(struct iovec, MAX_L2TPV3_IOVCNT);
    s->header_buf = g_malloc(s->header_size);

    qemu_socket_set_nonblock(fd);

    s->fd = fd;
    s->counter = 0;

    l2tpv3_read_poll(s, true);

    qemu_set_info_str(&s->nc, "l2tpv3: connected");
    return 0;
outerr:
    qemu_del_net_client(nc);
    if (fd >= 0) {
        close(fd);
    }
    if (result) {
        freeaddrinfo(result);
    }
    return -1;
}
Commit	Line	Data
3fb69aa1 AI	1	/*
	2	* QEMU System Emulator
	3	*
	4	* Copyright (c) 2003-2008 Fabrice Bellard
	5	* Copyright (c) 2012-2014 Cisco Systems
	6	*
	7	* Permission is hereby granted, free of charge, to any person obtaining a copy
	8	* of this software and associated documentation files (the "Software"), to deal
	9	* in the Software without restriction, including without limitation the rights
	10	* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
	11	* copies of the Software, and to permit persons to whom the Software is
	12	* furnished to do so, subject to the following conditions:
	13	*
	14	* The above copyright notice and this permission notice shall be included in
	15	* all copies or substantial portions of the Software.
	16	*
	17	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
	18	* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
	19	* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
	20	* THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
	21	* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
	22	* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
	23	* THE SOFTWARE.
	24	*/
	25
2744d920	26	#include "qemu/osdep.h"
3fb69aa1 AI	27	#include <linux/ip.h>
3fb69aa1 AI	28	#include <netdb.h>
3fb69aa1 AI	29	#include "net/net.h"
3fb69aa1 AI	30	#include "clients.h"
007e5f87	31	#include "qapi/error.h"
3fb69aa1 AI	32	#include "qemu/error-report.h"
	33	#include "qemu/option.h"
	34	#include "qemu/sockets.h"
	35	#include "qemu/iov.h"
	36	#include "qemu/main-loop.h"
5df022cf	37	#include "qemu/memalign.h"
3fb69aa1 AI	38
	39	/* The buffer size needs to be investigated for optimum numbers and
	40	* optimum means of paging in on different systems. This size is
	41	* chosen to be sufficient to accommodate one packet with some headers
	42	*/
	43
	44	#define BUFFER_ALIGN sysconf(_SC_PAGESIZE)
0c65ef4f	45	#define BUFFER_SIZE 16384
3fb69aa1 AI	46	#define IOVSIZE 2
	47	#define MAX_L2TPV3_MSGCNT 64
	48	#define MAX_L2TPV3_IOVCNT (MAX_L2TPV3_MSGCNT * IOVSIZE)
	49
	50	/* Header set to 0x30000 signifies a data packet */
	51
	52	#define L2TPV3_DATA_PACKET 0x30000
	53
	54	/* IANA-assigned IP protocol ID for L2TPv3 */
	55
	56	#ifndef IPPROTO_L2TP
	57	#define IPPROTO_L2TP 0x73
	58	#endif
	59
	60	typedef struct NetL2TPV3State {
	61	NetClientState nc;
	62	int fd;
	63
	64	/*
	65	* these are used for xmit - that happens packet a time
	66	* and for first sign of life packet (easier to parse that once)
	67	*/
	68
	69	uint8_t *header_buf;
	70	struct iovec *vec;
	71
	72	/*
	73	* these are used for receive - try to "eat" up to 32 packets at a time
	74	*/
	75
	76	struct mmsghdr *msgvec;
	77
	78	/*
	79	* peer address
	80	*/
	81
	82	struct sockaddr_storage *dgram_dst;
	83	uint32_t dst_size;
	84
	85	/*
	86	* L2TPv3 parameters
	87	*/
	88
	89	uint64_t rx_cookie;
	90	uint64_t tx_cookie;
	91	uint32_t rx_session;
	92	uint32_t tx_session;
	93	uint32_t header_size;
	94	uint32_t counter;
	95
	96	/*
	97	* DOS avoidance in error handling
	98	*/
	99
	100	bool header_mismatch;
	101
	102	/*
	103	* Ring buffer handling
	104	*/
	105
	106	int queue_head;
	107	int queue_tail;
	108	int queue_depth;
	109
110	/*
111	* Precomputed offsets
112	*/
113
114	uint32_t offset;
115	uint32_t cookie_offset;
116	uint32_t counter_offset;
117	uint32_t session_offset;
118
119	/* Poll Control */
120
121	bool read_poll;
122	bool write_poll;
123
124	/* Flags */
125
126	bool ipv6;
127	bool udp;
128	bool has_counter;
129	bool pin_counter;
130	bool cookie;
131	bool cookie_is_64;
132
133	} NetL2TPV3State;
134
3fb69aa1 AI	135	static void net_l2tpv3_send(void *opaque);
	136	static void l2tpv3_writable(void *opaque);
	137
	138	static void l2tpv3_update_fd_handler(NetL2TPV3State *s)
	139	{
82e1cc4b FZ	140	qemu_set_fd_handler(s->fd,
	141	s->read_poll ? net_l2tpv3_send : NULL,
	142	s->write_poll ? l2tpv3_writable : NULL,
	143	s);
3fb69aa1 AI	144	}
	145
	146	static void l2tpv3_read_poll(NetL2TPV3State *s, bool enable)
	147	{
	148	if (s->read_poll != enable) {
	149	s->read_poll = enable;
	150	l2tpv3_update_fd_handler(s);
	151	}
	152	}
	153
	154	static void l2tpv3_write_poll(NetL2TPV3State *s, bool enable)
	155	{
	156	if (s->write_poll != enable) {
	157	s->write_poll = enable;
	158	l2tpv3_update_fd_handler(s);
	159	}
	160	}
	161
	162	static void l2tpv3_writable(void *opaque)
	163	{
	164	NetL2TPV3State *s = opaque;
	165	l2tpv3_write_poll(s, false);
	166	qemu_flush_queued_packets(&s->nc);
	167	}
	168
3fb69aa1 AI	169	static void l2tpv3_send_completed(NetClientState *nc, ssize_t len)
	170	{
	171	NetL2TPV3State *s = DO_UPCAST(NetL2TPV3State, nc, nc);
	172	l2tpv3_read_poll(s, true);
	173	}
	174
	175	static void l2tpv3_poll(NetClientState *nc, bool enable)
	176	{
	177	NetL2TPV3State *s = DO_UPCAST(NetL2TPV3State, nc, nc);
	178	l2tpv3_write_poll(s, enable);
	179	l2tpv3_read_poll(s, enable);
	180	}
	181
	182	static void l2tpv3_form_header(NetL2TPV3State *s)
	183	{
	184	uint32_t *counter;
	185
	186	if (s->udp) {
	187	stl_be_p((uint32_t *) s->header_buf, L2TPV3_DATA_PACKET);
	188	}
	189	stl_be_p(
	190	(uint32_t *) (s->header_buf + s->session_offset),
	191	s->tx_session
	192	);
	193	if (s->cookie) {
	194	if (s->cookie_is_64) {
	195	stq_be_p(
	196	(uint64_t *)(s->header_buf + s->cookie_offset),
	197	s->tx_cookie
	198	);
	199	} else {
	200	stl_be_p(
	201	(uint32_t *) (s->header_buf + s->cookie_offset),
	202	s->tx_cookie
	203	);
	204	}
	205	}
	206	if (s->has_counter) {
	207	counter = (uint32_t *)(s->header_buf + s->counter_offset);
	208	if (s->pin_counter) {
	209	*counter = 0;
	210	} else {
	211	stl_be_p(counter, ++s->counter);
	212	}
	213	}
	214	}
	215
	216	static ssize_t net_l2tpv3_receive_dgram_iov(NetClientState *nc,
	217	const struct iovec *iov,
	218	int iovcnt)
	219	{
	220	NetL2TPV3State *s = DO_UPCAST(NetL2TPV3State, nc, nc);
	221
	222	struct msghdr message;
	223	int ret;
	224
	225	if (iovcnt > MAX_L2TPV3_IOVCNT - 1) {
	226	error_report(
	227	"iovec too long %d > %d, change l2tpv3.h",
	228	iovcnt, MAX_L2TPV3_IOVCNT
	229	);
	230	return -1;
	231	}
	232	l2tpv3_form_header(s);
233	memcpy(s->vec + 1, iov, iovcnt * sizeof(struct iovec));
234	s->vec->iov_base = s->header_buf;
235	s->vec->iov_len = s->offset;
236	message.msg_name = s->dgram_dst;
237	message.msg_namelen = s->dst_size;
238	message.msg_iov = s->vec;
239	message.msg_iovlen = iovcnt + 1;
240	message.msg_control = NULL;
241	message.msg_controllen = 0;
242	message.msg_flags = 0;
37b0b24e	243	ret = RETRY_ON_EINTR(sendmsg(s->fd, &message, 0));
3fb69aa1 AI	244	if (ret > 0) {
	245	ret -= s->offset;
	246	} else if (ret == 0) {
	247	/* belt and braces - should not occur on DGRAM
	248	* we should get an error and never a 0 send
	249	*/
	250	ret = iov_size(iov, iovcnt);
	251	} else {
	252	/* signal upper layer that socket buffer is full */
	253	ret = -errno;
	254	if (ret == -EAGAIN \|\| ret == -ENOBUFS) {
	255	l2tpv3_write_poll(s, true);
	256	ret = 0;
	257	}
	258	}
	259	return ret;
	260	}
	261
	262	static ssize_t net_l2tpv3_receive_dgram(NetClientState *nc,
	263	const uint8_t *buf,
	264	size_t size)
	265	{
	266	NetL2TPV3State *s = DO_UPCAST(NetL2TPV3State, nc, nc);
	267
	268	struct iovec *vec;
	269	struct msghdr message;
	270	ssize_t ret = 0;
	271
	272	l2tpv3_form_header(s);
	273	vec = s->vec;
	274	vec->iov_base = s->header_buf;
	275	vec->iov_len = s->offset;
	276	vec++;
	277	vec->iov_base = (void *) buf;
	278	vec->iov_len = size;
	279	message.msg_name = s->dgram_dst;
	280	message.msg_namelen = s->dst_size;
	281	message.msg_iov = s->vec;
	282	message.msg_iovlen = 2;
	283	message.msg_control = NULL;
	284	message.msg_controllen = 0;
	285	message.msg_flags = 0;
37b0b24e	286	ret = RETRY_ON_EINTR(sendmsg(s->fd, &message, 0));
3fb69aa1 AI	287	if (ret > 0) {
	288	ret -= s->offset;
	289	} else if (ret == 0) {
	290	/* belt and braces - should not occur on DGRAM
	291	* we should get an error and never a 0 send
	292	*/
	293	ret = size;
	294	} else {
	295	ret = -errno;
	296	if (ret == -EAGAIN \|\| ret == -ENOBUFS) {
	297	/* signal upper layer that socket buffer is full */
	298	l2tpv3_write_poll(s, true);
	299	ret = 0;
	300	}
	301	}
	302	return ret;
	303	}
	304
	305	static int l2tpv3_verify_header(NetL2TPV3State s, uint8_t buf)
	306	{
	307
	308	uint32_t *session;
	309	uint64_t cookie;
	310
	311	if ((!s->udp) && (!s->ipv6)) {
	312	buf += sizeof(struct iphdr) /* fix for ipv4 raw */;
	313	}
	314
	315	/* we do not do a strict check for "data" packets as per
	316	* the RFC spec because the pure IP spec does not have
	317	* that anyway.
	318	*/
	319
	320	if (s->cookie) {
	321	if (s->cookie_is_64) {
	322	cookie = ldq_be_p(buf + s->cookie_offset);
	323	} else {
3be9b352	324	cookie = ldl_be_p(buf + s->cookie_offset) & 0xffffffffULL;
3fb69aa1 AI	325	}
	326	if (cookie != s->rx_cookie) {
	327	if (!s->header_mismatch) {
	328	error_report("unknown cookie id");
	329	}
	330	return -1;
	331	}
	332	}
	333	session = (uint32_t *) (buf + s->session_offset);
	334	if (ldl_be_p(session) != s->rx_session) {
	335	if (!s->header_mismatch) {
	336	error_report("session mismatch");
	337	}
	338	return -1;
	339	}
	340	return 0;
	341	}
	342
	343	static void net_l2tpv3_process_queue(NetL2TPV3State *s)
	344	{
	345	int size = 0;
	346	struct iovec *vec;
	347	bool bad_read;
	348	int data_size;
	349	struct mmsghdr *msgvec;
	350
	351	/* go into ring mode only if there is a "pending" tail */
	352	if (s->queue_depth > 0) {
	353	do {
	354	msgvec = s->msgvec + s->queue_tail;
	355	if (msgvec->msg_len > 0) {
	356	data_size = msgvec->msg_len - s->header_size;
	357	vec = msgvec->msg_hdr.msg_iov;
	358	if ((data_size > 0) &&
	359	(l2tpv3_verify_header(s, vec->iov_base) == 0)) {
	360	vec++;
	361	/* Use the legacy delivery for now, we will
	362	* switch to using our own ring as a queueing mechanism
	363	* at a later date
	364	*/
	365	size = qemu_send_packet_async(
	366	&s->nc,
	367	vec->iov_base,
	368	data_size,
	369	l2tpv3_send_completed
	370	);
	371	if (size == 0) {
	372	l2tpv3_read_poll(s, false);
	373	}
	374	bad_read = false;
	375	} else {
	376	bad_read = true;
	377	if (!s->header_mismatch) {
	378	/* report error only once */
	379	error_report("l2tpv3 header verification failed");
	380	s->header_mismatch = true;
	381	}
	382	}
	383	} else {
	384	bad_read = true;
	385	}
	386	s->queue_tail = (s->queue_tail + 1) % MAX_L2TPV3_MSGCNT;
	387	s->queue_depth--;
	388	} while (
389	(s->queue_depth > 0) &&
390	qemu_can_send_packet(&s->nc) &&
391	((size > 0) \|\| bad_read)
392	);
393	}
394	}
395
396	static void net_l2tpv3_send(void *opaque)
397	{
398	NetL2TPV3State *s = opaque;
399	int target_count, count;
400	struct mmsghdr *msgvec;
401
402	/* go into ring mode only if there is a "pending" tail */
403
404	if (s->queue_depth) {
405
406	/* The ring buffer we use has variable intake
407	* count of how much we can read varies - adjust accordingly
408	*/
409
410	target_count = MAX_L2TPV3_MSGCNT - s->queue_depth;
411
412	/* Ensure we do not overrun the ring when we have
413	* a lot of enqueued packets
414	*/
415
416	if (s->queue_head + target_count > MAX_L2TPV3_MSGCNT) {
417	target_count = MAX_L2TPV3_MSGCNT - s->queue_head;
418	}
419	} else {
420
421	/* we do not have any pending packets - we can use
422	* the whole message vector linearly instead of using
423	* it as a ring
424	*/
425
426	s->queue_head = 0;
427	s->queue_tail = 0;
428	target_count = MAX_L2TPV3_MSGCNT;
429	}
430
431	msgvec = s->msgvec + s->queue_head;
432	if (target_count > 0) {
37b0b24e NI	433	count = RETRY_ON_EINTR(
	434	recvmmsg(s->fd, msgvec, target_count, MSG_DONTWAIT, NULL)
	435	);
3fb69aa1 AI	436	if (count < 0) {
	437	/* Recv error - we still need to flush packets here,
	438	* (re)set queue head to current position
	439	*/
	440	count = 0;
	441	}
	442	s->queue_head = (s->queue_head + count) % MAX_L2TPV3_MSGCNT;
	443	s->queue_depth += count;
	444	}
	445	net_l2tpv3_process_queue(s);
	446	}
	447
	448	static void destroy_vector(struct mmsghdr *msgvec, int count, int iovcount)
	449	{
	450	int i, j;
	451	struct iovec *iov;
	452	struct mmsghdr *cleanup = msgvec;
	453	if (cleanup) {
	454	for (i = 0; i < count; i++) {
	455	if (cleanup->msg_hdr.msg_iov) {
	456	iov = cleanup->msg_hdr.msg_iov;
	457	for (j = 0; j < iovcount; j++) {
	458	g_free(iov->iov_base);
	459	iov++;
	460	}
	461	g_free(cleanup->msg_hdr.msg_iov);
	462	}
	463	cleanup++;
	464	}
	465	g_free(msgvec);
	466	}
	467	}
	468
	469	static struct mmsghdr build_l2tpv3_vector(NetL2TPV3State s, int count)
	470	{
	471	int i;
	472	struct iovec *iov;
	473	struct mmsghdr msgvec, result;
	474
58889fe5	475	msgvec = g_new(struct mmsghdr, count);
3fb69aa1 AI	476	result = msgvec;
	477	for (i = 0; i < count ; i++) {
	478	msgvec->msg_hdr.msg_name = NULL;
	479	msgvec->msg_hdr.msg_namelen = 0;
58889fe5	480	iov = g_new(struct iovec, IOVSIZE);
3fb69aa1 AI	481	msgvec->msg_hdr.msg_iov = iov;
	482	iov->iov_base = g_malloc(s->header_size);
	483	iov->iov_len = s->header_size;
	484	iov++ ;
	485	iov->iov_base = qemu_memalign(BUFFER_ALIGN, BUFFER_SIZE);
	486	iov->iov_len = BUFFER_SIZE;
	487	msgvec->msg_hdr.msg_iovlen = 2;
	488	msgvec->msg_hdr.msg_control = NULL;
	489	msgvec->msg_hdr.msg_controllen = 0;
	490	msgvec->msg_hdr.msg_flags = 0;
	491	msgvec++;
	492	}
	493	return result;
	494	}
	495
	496	static void net_l2tpv3_cleanup(NetClientState *nc)
	497	{
	498	NetL2TPV3State *s = DO_UPCAST(NetL2TPV3State, nc, nc);
	499	qemu_purge_queued_packets(nc);
	500	l2tpv3_read_poll(s, false);
	501	l2tpv3_write_poll(s, false);
d4754a95	502	if (s->fd >= 0) {
3fb69aa1 AI	503	close(s->fd);
	504	}
	505	destroy_vector(s->msgvec, MAX_L2TPV3_MSGCNT, IOVSIZE);
	506	g_free(s->vec);
	507	g_free(s->header_buf);
	508	g_free(s->dgram_dst);
	509	}
	510
	511	static NetClientInfo net_l2tpv3_info = {
f394b2e2	512	.type = NET_CLIENT_DRIVER_L2TPV3,
3fb69aa1 AI	513	.size = sizeof(NetL2TPV3State),
	514	.receive = net_l2tpv3_receive_dgram,
	515	.receive_iov = net_l2tpv3_receive_dgram_iov,
	516	.poll = l2tpv3_poll,
	517	.cleanup = net_l2tpv3_cleanup,
	518	};
	519
cebea510	520	int net_init_l2tpv3(const Netdev *netdev,
3fb69aa1	521	const char *name,
a30ecde6	522	NetClientState peer, Error *errp)
3fb69aa1	523	{
3fb69aa1 AI	524	const NetdevL2TPv3Options *l2tpv3;
	525	NetL2TPV3State *s;
	526	NetClientState *nc;
	527	int fd = -1, gairet;
	528	struct addrinfo hints;
	529	struct addrinfo *result = NULL;
	530	char srcport, dstport;
	531
	532	nc = qemu_new_net_client(&net_l2tpv3_info, peer, "l2tpv3", name);
	533
	534	s = DO_UPCAST(NetL2TPV3State, nc, nc);
	535
	536	s->queue_head = 0;
	537	s->queue_tail = 0;
	538	s->header_mismatch = false;
	539
f394b2e2 EB	540	assert(netdev->type == NET_CLIENT_DRIVER_L2TPV3);
f394b2e2 EB	541	l2tpv3 = &netdev->u.l2tpv3;
3fb69aa1 AI	542
	543	if (l2tpv3->has_ipv6 && l2tpv3->ipv6) {
	544	s->ipv6 = l2tpv3->ipv6;
	545	} else {
	546	s->ipv6 = false;
	547	}
	548
	549	if ((l2tpv3->has_offset) && (l2tpv3->offset > 256)) {
007e5f87	550	error_setg(errp, "offset must be less than 256 bytes");
3fb69aa1 AI	551	goto outerr;
	552	}
	553
	554	if (l2tpv3->has_rxcookie \|\| l2tpv3->has_txcookie) {
	555	if (l2tpv3->has_rxcookie && l2tpv3->has_txcookie) {
	556	s->cookie = true;
	557	} else {
007e5f87 MA	558	error_setg(errp,
007e5f87 MA	559	"require both 'rxcookie' and 'txcookie' or neither");
3fb69aa1 AI	560	goto outerr;
	561	}
	562	} else {
	563	s->cookie = false;
	564	}
	565
	566	if (l2tpv3->has_cookie64 \|\| l2tpv3->cookie64) {
	567	s->cookie_is_64 = true;
	568	} else {
	569	s->cookie_is_64 = false;
	570	}
	571
	572	if (l2tpv3->has_udp && l2tpv3->udp) {
	573	s->udp = true;
7480874a	574	if (!(l2tpv3->srcport && l2tpv3->dstport)) {
007e5f87	575	error_setg(errp, "need both src and dst port for udp");
3fb69aa1 AI	576	goto outerr;
	577	} else {
	578	srcport = l2tpv3->srcport;
	579	dstport = l2tpv3->dstport;
	580	}
	581	} else {
	582	s->udp = false;
	583	srcport = NULL;
	584	dstport = NULL;
	585	}
	586
	587
	588	s->offset = 4;
	589	s->session_offset = 0;
	590	s->cookie_offset = 4;
	591	s->counter_offset = 4;
	592
	593	s->tx_session = l2tpv3->txsession;
	594	if (l2tpv3->has_rxsession) {
	595	s->rx_session = l2tpv3->rxsession;
	596	} else {
	597	s->rx_session = s->tx_session;
	598	}
	599
	600	if (s->cookie) {
	601	s->rx_cookie = l2tpv3->rxcookie;
	602	s->tx_cookie = l2tpv3->txcookie;
	603	if (s->cookie_is_64 == true) {
	604	/* 64 bit cookie */
	605	s->offset += 8;
	606	s->counter_offset += 8;
	607	} else {
	608	/* 32 bit cookie */
	609	s->offset += 4;
	610	s->counter_offset += 4;
	611	}
	612	}
	613
	614	memset(&hints, 0, sizeof(hints));
	615
	616	if (s->ipv6) {
	617	hints.ai_family = AF_INET6;
	618	} else {
	619	hints.ai_family = AF_INET;
	620	}
	621	if (s->udp) {
	622	hints.ai_socktype = SOCK_DGRAM;
	623	hints.ai_protocol = 0;
	624	s->offset += 4;
	625	s->counter_offset += 4;
	626	s->session_offset += 4;
	627	s->cookie_offset += 4;
	628	} else {
	629	hints.ai_socktype = SOCK_RAW;
	630	hints.ai_protocol = IPPROTO_L2TP;
	631	}
	632
	633	gairet = getaddrinfo(l2tpv3->src, srcport, &hints, &result);
	634
	635	if ((gairet != 0) \|\| (result == NULL)) {
007e5f87 MA	636	error_setg(errp, "could not resolve src, errno = %s",
007e5f87 MA	637	gai_strerror(gairet));
3fb69aa1 AI	638	goto outerr;
	639	}
	640	fd = socket(result->ai_family, result->ai_socktype, result->ai_protocol);
	641	if (fd == -1) {
	642	fd = -errno;
007e5f87 MA	643	error_setg(errp, "socket creation failed, errno = %d",
007e5f87 MA	644	-fd);
3fb69aa1 AI	645	goto outerr;
	646	}
	647	if (bind(fd, (struct sockaddr *) result->ai_addr, result->ai_addrlen)) {
007e5f87	648	error_setg(errp, "could not bind socket err=%i", errno);
3fb69aa1 AI	649	goto outerr;
3fb69aa1 AI	650	}
d949fe64 AC	651
d949fe64 AC	652	freeaddrinfo(result);
3fb69aa1 AI	653
	654	memset(&hints, 0, sizeof(hints));
	655
	656	if (s->ipv6) {
	657	hints.ai_family = AF_INET6;
	658	} else {
	659	hints.ai_family = AF_INET;
	660	}
	661	if (s->udp) {
	662	hints.ai_socktype = SOCK_DGRAM;
	663	hints.ai_protocol = 0;
	664	} else {
	665	hints.ai_socktype = SOCK_RAW;
	666	hints.ai_protocol = IPPROTO_L2TP;
	667	}
	668
	669	result = NULL;
	670	gairet = getaddrinfo(l2tpv3->dst, dstport, &hints, &result);
	671	if ((gairet != 0) \|\| (result == NULL)) {
007e5f87 MA	672	error_setg(errp, "could not resolve dst, error = %s",
007e5f87 MA	673	gai_strerror(gairet));
3fb69aa1 AI	674	goto outerr;
	675	}
	676
71e28e3c	677	s->dgram_dst = g_new0(struct sockaddr_storage, 1);
3fb69aa1 AI	678	memcpy(s->dgram_dst, result->ai_addr, result->ai_addrlen);
	679	s->dst_size = result->ai_addrlen;
	680
d949fe64	681	freeaddrinfo(result);
3fb69aa1 AI	682
	683	if (l2tpv3->has_counter && l2tpv3->counter) {
	684	s->has_counter = true;
	685	s->offset += 4;
	686	} else {
	687	s->has_counter = false;
	688	}
	689
	690	if (l2tpv3->has_pincounter && l2tpv3->pincounter) {
	691	s->has_counter = true; /* pin counter implies that there is counter */
	692	s->pin_counter = true;
	693	} else {
	694	s->pin_counter = false;
	695	}
	696
	697	if (l2tpv3->has_offset) {
	698	/* extra offset */
	699	s->offset += l2tpv3->offset;
	700	}
	701
	702	if ((s->ipv6) \|\| (s->udp)) {
	703	s->header_size = s->offset;
	704	} else {
	705	s->header_size = s->offset + sizeof(struct iphdr);
	706	}
	707
	708	s->msgvec = build_l2tpv3_vector(s, MAX_L2TPV3_MSGCNT);
58889fe5	709	s->vec = g_new(struct iovec, MAX_L2TPV3_IOVCNT);
3fb69aa1 AI	710	s->header_buf = g_malloc(s->header_size);
3fb69aa1 AI	711
ff5927ba	712	qemu_socket_set_nonblock(fd);
3fb69aa1 AI	713
	714	s->fd = fd;
	715	s->counter = 0;
	716
	717	l2tpv3_read_poll(s, true);
	718
53b85d95	719	qemu_set_info_str(&s->nc, "l2tpv3: connected");
3fb69aa1 AI	720	return 0;
	721	outerr:
	722	qemu_del_net_client(nc);
d4754a95	723	if (fd >= 0) {
3fb69aa1 AI	724	close(fd);
	725	}
	726	if (result) {
	727	freeaddrinfo(result);
	728	}
	729	return -1;
	730	}
	731