/* $OpenBSD: src/sys/netbt/l2cap_signal.c,v 1.3 2008/02/24 21:34:48 uwe Exp $ */
/* $NetBSD: l2cap_signal.c,v 1.9 2007/11/10 23:12:23 plunky Exp $ */

/*-
 * Copyright (c) 2005 Iain Hibbert.
 * Copyright (c) 2006 Itronix Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of Itronix Inc. may not be used to endorse
 *    or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY ITRONIX INC. ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL ITRONIX INC. BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/param.h>
#include <sys/kernel.h>
#include <sys/malloc.h> /* for M_NOWAIT */
#include <sys/mbuf.h>
#include <sys/proc.h>
#include <sys/queue.h>
#include <sys/systm.h>
#include <sys/endian.h>

#include <netbt/bluetooth.h>
#include <netbt/hci.h>
#include <netbt/l2cap.h>

/*******************************************************************************
 *
 *      L2CAP Signal processing
 */

static void l2cap_recv_command_rej(struct mbuf *, struct hci_link *);
static void l2cap_recv_connect_req(struct mbuf *, struct hci_link *);
static void l2cap_recv_connect_rsp(struct mbuf *, struct hci_link *);
static void l2cap_recv_config_req(struct mbuf *, struct hci_link *);
static void l2cap_recv_config_rsp(struct mbuf *, struct hci_link *);
static void l2cap_recv_disconnect_req(struct mbuf *, struct hci_link *);
static void l2cap_recv_disconnect_rsp(struct mbuf *, struct hci_link *);
static void l2cap_recv_info_req(struct mbuf *, struct hci_link *);
static int l2cap_send_signal(struct hci_link *, uint8_t, uint8_t, uint16_t, void *);
static int l2cap_send_command_rej(struct hci_link *, uint8_t, uint16_t, ...);

/*
 * process incoming signal packets (CID 0x0001). Can contain multiple
 * requests/responses.
 */
void
l2cap_recv_signal(struct mbuf *m, struct hci_link *link)
{
        l2cap_cmd_hdr_t cmd;

        for(;;) {
                if (m->m_pkthdr.len == 0)
                        goto finish;

                if (m->m_pkthdr.len < sizeof(cmd))
                        goto reject;

                m_copydata(m, 0, sizeof(cmd), &cmd);
                cmd.length = letoh16(cmd.length);

                if (m->m_pkthdr.len < sizeof(cmd) + cmd.length)
                        goto reject;

                DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
                        device_get_nameunit(link->hl_unit->hci_dev),
                        cmd.code, cmd.ident, cmd.length);

                switch (cmd.code) {
                case L2CAP_COMMAND_REJ:
                        if (cmd.length > sizeof(l2cap_cmd_rej_cp))
                                goto finish;

                        l2cap_recv_command_rej(m, link);
                        break;

                case L2CAP_CONNECT_REQ:
                        if (cmd.length != sizeof(l2cap_con_req_cp))
                                goto reject;

                        l2cap_recv_connect_req(m, link);
                        break;

                case L2CAP_CONNECT_RSP:
                        if (cmd.length != sizeof(l2cap_con_rsp_cp))
                                goto finish;

                        l2cap_recv_connect_rsp(m, link);
                        break;

                case L2CAP_CONFIG_REQ:
                        l2cap_recv_config_req(m, link);
                        break;

                case L2CAP_CONFIG_RSP:
                        l2cap_recv_config_rsp(m, link);
                        break;

                case L2CAP_DISCONNECT_REQ:
                        if (cmd.length != sizeof(l2cap_discon_req_cp))
                                goto reject;

                        l2cap_recv_disconnect_req(m, link);
                        break;

                case L2CAP_DISCONNECT_RSP:
                        if (cmd.length != sizeof(l2cap_discon_rsp_cp))
                                goto finish;

                        l2cap_recv_disconnect_rsp(m, link);
                        break;

                case L2CAP_ECHO_REQ:
                        m_adj(m, sizeof(cmd) + cmd.length);
                        l2cap_send_signal(link, L2CAP_ECHO_RSP, cmd.ident,
                                        0, NULL);
                        break;

                case L2CAP_ECHO_RSP:
                        m_adj(m, sizeof(cmd) + cmd.length);
                        break;

                case L2CAP_INFO_REQ:
                        if (cmd.length != sizeof(l2cap_info_req_cp))
                                goto reject;

                        l2cap_recv_info_req(m, link);
                        break;

                case L2CAP_INFO_RSP:
                        m_adj(m, sizeof(cmd) + cmd.length);
                        break;

                default:
                        goto reject;
                }
        }

#ifdef DIAGNOSTIC
        panic("impossible!");
#endif

reject:
        l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
finish:
        m_freem(m);
}

/*
 * Process Received Command Reject. For now we dont try to recover gracefully
 * from this, it probably means that the link is garbled or the other end is
 * insufficiently capable of handling normal traffic. (not *my* fault, no way!)
 */
static void
l2cap_recv_command_rej(struct mbuf *m, struct hci_link *link)
{
        struct l2cap_req *req;
        struct l2cap_channel *chan;
        l2cap_cmd_hdr_t cmd;
        l2cap_cmd_rej_cp cp;

        m_copydata(m, 0, sizeof(cmd), &cmd);
        m_adj(m, sizeof(cmd));

        cmd.length = letoh16(cmd.length);

        m_copydata(m, 0, cmd.length, &cp);
        m_adj(m, cmd.length);

        req = l2cap_request_lookup(link, cmd.ident);
        if (req == NULL)
                return;

        switch (letoh16(cp.reason)) {
        case L2CAP_REJ_NOT_UNDERSTOOD:
                /*
                 * I dont know what to do, just move up the timeout
                 */
                callout_reset(&req->lr_rtx,0,l2cap_rtx,req);
                break;

        case L2CAP_REJ_MTU_EXCEEDED:
                /*
                 * I didnt send any commands over L2CAP_MTU_MINIMUM size, but..
                 *
                 * XXX maybe we should resend this, instead?
                 */
                link->hl_mtu = letoh16(cp.data[0]);
                callout_reset(&req->lr_rtx,0,l2cap_rtx,req);
                break;

        case L2CAP_REJ_INVALID_CID:
                /*
                 * Well, if they dont have such a channel then our channel is
                 * most likely closed. Make it so.
                 */
                chan = req->lr_chan;
                l2cap_request_free(req);
                if (chan != NULL && chan->lc_state != L2CAP_CLOSED)
                        l2cap_close(chan, ECONNABORTED);

                break;

        default:
                UNKNOWN(letoh16(cp.reason));
                break;
        }
}

/*
 * Process Received Connect Request. Find listening channel matching
 * psm & addr and ask upper layer for a new channel.
 */
static void
l2cap_recv_connect_req(struct mbuf *m, struct hci_link *link)
{
        struct sockaddr_bt laddr, raddr;
        struct l2cap_channel *chan, *new;
        l2cap_cmd_hdr_t cmd;
        l2cap_con_req_cp cp;
        int err;

        /* extract cmd */
        m_copydata(m, 0, sizeof(cmd), &cmd);
        m_adj(m, sizeof(cmd));

        /* extract request */
        m_copydata(m, 0, sizeof(cp), &cp);
        m_adj(m, sizeof(cp));

        cp.scid = letoh16(cp.scid);
        cp.psm = letoh16(cp.psm);

        memset(&laddr, 0, sizeof(struct sockaddr_bt));
        laddr.bt_len = sizeof(struct sockaddr_bt);
        laddr.bt_family = AF_BLUETOOTH;
        laddr.bt_psm = cp.psm;
        bdaddr_copy(&laddr.bt_bdaddr, &link->hl_unit->hci_bdaddr);

        memset(&raddr, 0, sizeof(struct sockaddr_bt));
        raddr.bt_len = sizeof(struct sockaddr_bt);
        raddr.bt_family = AF_BLUETOOTH;
        raddr.bt_psm = cp.psm;
        bdaddr_copy(&raddr.bt_bdaddr, &link->hl_bdaddr);

        LIST_FOREACH(chan, &l2cap_listen_list, lc_ncid) {
                if (chan->lc_laddr.bt_psm != laddr.bt_psm
                    && chan->lc_laddr.bt_psm != L2CAP_PSM_ANY)
                        continue;

                if (!bdaddr_same(&laddr.bt_bdaddr, &chan->lc_laddr.bt_bdaddr)
                    && bdaddr_any(&chan->lc_laddr.bt_bdaddr) == 0)
                        continue;

                new= (*chan->lc_proto->newconn)(chan->lc_upper, &laddr, &raddr);
                if (new == NULL)
                        continue;

                err = l2cap_cid_alloc(new);
                if (err) {
                        l2cap_send_connect_rsp(link, cmd.ident,
                                                0, cp.scid,
                                                L2CAP_NO_RESOURCES);

                        (*new->lc_proto->disconnected)(new->lc_upper, err);
                        return;
                }

                new->lc_link = hci_acl_open(link->hl_unit, &link->hl_bdaddr);
                KKASSERT(new->lc_link == link);

                new->lc_rcid = cp.scid;
                new->lc_ident = cmd.ident;

                memcpy(&new->lc_laddr, &laddr, sizeof(struct sockaddr_bt));
                memcpy(&new->lc_raddr, &raddr, sizeof(struct sockaddr_bt));

                new->lc_mode = chan->lc_mode;

                err = l2cap_setmode(new);
                if (err == EINPROGRESS) {
                        new->lc_state = L2CAP_WAIT_SEND_CONNECT_RSP;
                        (*new->lc_proto->connecting)(new->lc_upper);
                        return;
                }
                if (err) {
                        new->lc_state = L2CAP_CLOSED;
                        hci_acl_close(link, err);
                        new->lc_link = NULL;

                        l2cap_send_connect_rsp(link, cmd.ident,
                                                0, cp.scid,
                                                L2CAP_NO_RESOURCES);

                        (*new->lc_proto->disconnected)(new->lc_upper, err);
                        return;
                }

                err = l2cap_send_connect_rsp(link, cmd.ident,
                                              new->lc_lcid, new->lc_rcid,
                                              L2CAP_SUCCESS);
                if (err) {
                        l2cap_close(new, err);
                        return;
                }

                new->lc_state = L2CAP_WAIT_CONFIG;
                new->lc_flags |= (L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP);
                err = l2cap_send_config_req(new);
                if (err)
                        l2cap_close(new, err);

                return;
        }

        l2cap_send_connect_rsp(link, cmd.ident,
                                0, cp.scid,
                                L2CAP_PSM_NOT_SUPPORTED);
}

/*
 * Process Received Connect Response.
 */
static void
l2cap_recv_connect_rsp(struct mbuf *m, struct hci_link *link)
{
        l2cap_cmd_hdr_t cmd;
        l2cap_con_rsp_cp cp;
        struct l2cap_req *req;
        struct l2cap_channel *chan;

        m_copydata(m, 0, sizeof(cmd), &cmd);
        m_adj(m, sizeof(cmd));

        m_copydata(m, 0, sizeof(cp), &cp);
        m_adj(m, sizeof(cp));

        cp.scid = letoh16(cp.scid);
        cp.dcid = letoh16(cp.dcid);
        cp.result = letoh16(cp.result);

        req = l2cap_request_lookup(link, cmd.ident);
        if (req == NULL || req->lr_code != L2CAP_CONNECT_REQ)
                return;

        chan = req->lr_chan;
        if (chan != NULL && chan->lc_lcid != cp.scid)
                return;

        if (chan == NULL || chan->lc_state != L2CAP_WAIT_RECV_CONNECT_RSP) {
                l2cap_request_free(req);
                return;
        }

        switch (cp.result) {
        case L2CAP_SUCCESS:
                /*
                 * Ok, at this point we have a connection to the other party. We
                 * could indicate upstream that we are ready for business and
                 * wait for a "Configure Channel Request" but I'm not so sure
                 * that is required in our case - we will proceed directly to
                 * sending our config request. We set two state bits because in
                 * the config state we are waiting for requests and responses.
                 */
                l2cap_request_free(req);
                chan->lc_rcid = cp.dcid;
                chan->lc_state = L2CAP_WAIT_CONFIG;
                chan->lc_flags |= (L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP);
                l2cap_send_config_req(chan);
                break;

        case L2CAP_PENDING:
                /* XXX dont release request, should start eRTX timeout? */
                (*chan->lc_proto->connecting)(chan->lc_upper);
                break;

        case L2CAP_PSM_NOT_SUPPORTED:
        case L2CAP_SECURITY_BLOCK:
        case L2CAP_NO_RESOURCES:
        default:
                l2cap_request_free(req);
                l2cap_close(chan, ECONNREFUSED);
                break;
        }
}

/*
 * Process Received Config Request.
 */
static void
l2cap_recv_config_req(struct mbuf *m, struct hci_link *link)
{
        uint8_t buf[L2CAP_MTU_MINIMUM];
        l2cap_cmd_hdr_t cmd;
        l2cap_cfg_req_cp cp;
        l2cap_cfg_opt_t opt;
        l2cap_cfg_opt_val_t val;
        l2cap_cfg_rsp_cp rp;
        struct l2cap_channel *chan;
        int left, len;

        m_copydata(m, 0, sizeof(cmd), &cmd);
        m_adj(m, sizeof(cmd));
        left = letoh16(cmd.length);

        if (left < sizeof(cp))
                goto reject;

        m_copydata(m, 0, sizeof(cp), &cp);
        m_adj(m, sizeof(cp));
        left -= sizeof(cp);

        cp.dcid = letoh16(cp.dcid);
        cp.flags = letoh16(cp.flags);

        chan = l2cap_cid_lookup(cp.dcid);
        if (chan == NULL || chan->lc_link != link
            || chan->lc_state != L2CAP_WAIT_CONFIG
            || (chan->lc_flags & L2CAP_WAIT_CONFIG_REQ) == 0) {
                /* XXX we should really accept reconfiguration requests */
                l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
                                        L2CAP_NULL_CID, cp.dcid);
                goto out;
        }

        /* ready our response packet */
        rp.scid = htole16(chan->lc_rcid);
        rp.flags = 0;   /* "No Continuation" */
        rp.result = L2CAP_SUCCESS;
        len = sizeof(rp);

        /*
         * Process the packet. We build the return packet on the fly adding any
         * unacceptable parameters as we go. As we can only return one result,
         * unknown option takes precedence so we start our return packet anew
         * and ignore option values thereafter as they will be re-sent.
         *
         * Since we do not support enough options to make overflowing the min
         * MTU size an issue in normal use, we just reject config requests that
         * make that happen. This could be because options are repeated or the
         * packet is corrupted in some way.
         *
         * If unknown option types threaten to overflow the packet, we just
         * ignore them. We can deny them next time.
         */
        while (left > 0) {
                if (left < sizeof(opt))
                        goto reject;

                m_copydata(m, 0, sizeof(opt), &opt);
                m_adj(m, sizeof(opt));
                left -= sizeof(opt);

                if (left < opt.length)
                        goto reject;

                switch(opt.type & L2CAP_OPT_HINT_MASK) {
                case L2CAP_OPT_MTU:
                        if (rp.result == L2CAP_UNKNOWN_OPTION)
                                break;

                        if (opt.length != L2CAP_OPT_MTU_SIZE)
                                goto reject;

                        m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, &val);
                        val.mtu = letoh16(val.mtu);

                        /*
                         * XXX how do we know what the minimum acceptable MTU is
                         * for a channel? Spec says some profiles have a higher
                         * minimum but I have no way to find that out at this
                         * juncture..
                         */
                        if (val.mtu < L2CAP_MTU_MINIMUM) {
                                if (len + sizeof(opt) + L2CAP_OPT_MTU_SIZE > sizeof(buf))
                                        goto reject;

                                rp.result = L2CAP_UNACCEPTABLE_PARAMS;
                                memcpy(buf + len, &opt, sizeof(opt));
                                len += sizeof(opt);
                                val.mtu = htole16(L2CAP_MTU_MINIMUM);
                                memcpy(buf + len, &val, L2CAP_OPT_MTU_SIZE);
                                len += L2CAP_OPT_MTU_SIZE;
                        } else
                                chan->lc_omtu = val.mtu;

                        break;

                case L2CAP_OPT_FLUSH_TIMO:
                        if (rp.result == L2CAP_UNKNOWN_OPTION)
                                break;

                        if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE)
                                goto reject;

                        /*
                         * I think that this is informational only - he is
                         * informing us of the flush timeout he will be using.
                         * I dont think this affects us in any significant way,
                         * so just ignore this value for now.
                         */
                        break;

                case L2CAP_OPT_QOS:
                        if (rp.result == L2CAP_UNKNOWN_OPTION)
                                break;

                        if (opt.length != L2CAP_OPT_QOS_SIZE)
                                goto reject;

                        m_copydata(m, 0, L2CAP_OPT_QOS_SIZE, &val);
                        if (val.qos.service_type == L2CAP_QOS_NO_TRAFFIC ||
                            val.qos.service_type == L2CAP_QOS_BEST_EFFORT)
                                /*
                                 * In accordance with the spec, we choose to
                                 * ignore the fields an provide no response.
                                 */
                                break;

                        if (len + sizeof(opt) + L2CAP_OPT_QOS_SIZE > sizeof(buf))
                                goto reject;

                        if (val.qos.service_type != L2CAP_QOS_GUARANTEED) {
                                /*
                                 * Instead of sending an "unacceptable
                                 * parameters" response, treat this as an
                                 * unknown option and include the option
                                 * value in the response.
                                 */
                                rp.result = L2CAP_UNKNOWN_OPTION;
                        } else {
                                /*
                                 * According to the spec, we must return
                                 * specific values for wild card parameters.
                                 * I don't know what to return without lying,
                                 * so return "unacceptable parameters" and
                                 * specify the preferred service type as
                                 * "Best Effort".
                                 */
                                rp.result = L2CAP_UNACCEPTABLE_PARAMS;
                                val.qos.service_type = L2CAP_QOS_BEST_EFFORT;
                        }

                        memcpy(buf + len, &opt, sizeof(opt));
                        len += sizeof(opt);
                        memcpy(buf + len, &val, L2CAP_OPT_QOS_SIZE);
                        len += L2CAP_OPT_QOS_SIZE;
                        break;

                default:
                        /* ignore hints */
                        if (opt.type & L2CAP_OPT_HINT_BIT)
                                break;

                        /* unknown options supersede all else */
                        if (rp.result != L2CAP_UNKNOWN_OPTION) {
                                rp.result = L2CAP_UNKNOWN_OPTION;
                                len = sizeof(rp);
                        }

                        /* ignore if it doesn't fit */
                        if (len + sizeof(opt) > sizeof(buf))
                                break;

                        /* return unknown option type, but no data */
                        buf[len++] = opt.type;
                        buf[len++] = 0;
                        break;
                }

                m_adj(m, opt.length);
                left -= opt.length;
        }

        rp.result = htole16(rp.result);
        memcpy(buf, &rp, sizeof(rp));
        l2cap_send_signal(link, L2CAP_CONFIG_RSP, cmd.ident, len, buf);

        if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0
            && rp.result == letoh16(L2CAP_SUCCESS)) {

                chan->lc_flags &= ~L2CAP_WAIT_CONFIG_REQ;

                if ((chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0) {
                        chan->lc_state = L2CAP_OPEN;
                        /* XXX how to distinguish REconfiguration? */
                        (*chan->lc_proto->connected)(chan->lc_upper);
                }
        }
        return;

reject:
        l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_NOT_UNDERSTOOD);
out:
        m_adj(m, left);
}

/*
 * Process Received Config Response.
 */
static void
l2cap_recv_config_rsp(struct mbuf *m, struct hci_link *link)
{
        l2cap_cmd_hdr_t cmd;
        l2cap_cfg_rsp_cp cp;
        l2cap_cfg_opt_t opt;
        l2cap_cfg_opt_val_t val;
        struct l2cap_req *req;
        struct l2cap_channel *chan;
        int left;

        m_copydata(m, 0, sizeof(cmd), &cmd);
        m_adj(m, sizeof(cmd));
        left = letoh16(cmd.length);

        if (left < sizeof(cp))
                goto out;

        m_copydata(m, 0, sizeof(cp), &cp);
        m_adj(m, sizeof(cp));
        left -= sizeof(cp);

        cp.scid = letoh16(cp.scid);
        cp.flags = letoh16(cp.flags);
        cp.result = letoh16(cp.result);

        req = l2cap_request_lookup(link, cmd.ident);
        if (req == NULL || req->lr_code != L2CAP_CONFIG_REQ)
                goto out;

        chan = req->lr_chan;
        if (chan != NULL && chan->lc_lcid != cp.scid)
                goto out;

        l2cap_request_free(req);

        if (chan == NULL || chan->lc_state != L2CAP_WAIT_CONFIG
            || (chan->lc_flags & L2CAP_WAIT_CONFIG_RSP) == 0)
                goto out;

        if ((cp.flags & L2CAP_OPT_CFLAG_BIT)) {
                l2cap_cfg_req_cp rp;

                /*
                 * They have more to tell us and want another ID to
                 * use, so send an empty config request
                 */
                if (l2cap_request_alloc(chan, L2CAP_CONFIG_REQ))
                        goto discon;

                rp.dcid = htole16(cp.scid);
                rp.flags = 0;

                if (l2cap_send_signal(link, L2CAP_CONFIG_REQ, link->hl_lastid,
                                        sizeof(rp), &rp))
                        goto discon;
        }

        switch(cp.result) {
        case L2CAP_SUCCESS:
                /*
                 * If continuation flag was not set, our config request was
                 * accepted. We may have to wait for their config request to
                 * complete, so check that but otherwise we are open
                 *
                 * There may be 'advisory' values in the packet but we just
                 * ignore those..
                 */
                if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0) {
                        chan->lc_flags &= ~L2CAP_WAIT_CONFIG_RSP;

                        if ((chan->lc_flags & L2CAP_WAIT_CONFIG_REQ) == 0) {
                                chan->lc_state = L2CAP_OPEN;
                                /* XXX how to distinguish REconfiguration? */
                                (*chan->lc_proto->connected)(chan->lc_upper);
                        }
                }
                goto out;

        case L2CAP_UNACCEPTABLE_PARAMS:
                /*
                 * Packet contains unacceptable parameters with preferred values
                 */
                while (left > 0) {
                        if (left < sizeof(opt))
                                goto discon;

                        m_copydata(m, 0, sizeof(opt), &opt);
                        m_adj(m, sizeof(opt));
                        left -= sizeof(opt);

                        if (left < opt.length)
                                goto discon;

                        switch (opt.type) {
                        case L2CAP_OPT_MTU:
                                if (opt.length != L2CAP_OPT_MTU_SIZE)
                                        goto discon;

                                m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, &val);
                                chan->lc_imtu = letoh16(val.mtu);
                                if (chan->lc_imtu < L2CAP_MTU_MINIMUM)
                                        chan->lc_imtu = L2CAP_MTU_DEFAULT;
                                break;

                        case L2CAP_OPT_FLUSH_TIMO:
                                if (opt.length != L2CAP_OPT_FLUSH_TIMO_SIZE)
                                        goto discon;

                                /*
                                 * Spec says: If we cannot honor proposed value,
                                 * either disconnect or try again with original
                                 * value. I can't really see why they want to
                                 * interfere with OUR flush timeout in any case
                                 * so we just punt for now.
                                 */
                                goto discon;

                        case L2CAP_OPT_QOS:
                                break;

                        default:
                                UNKNOWN(opt.type);
                                goto discon;
                        }

                        m_adj(m, opt.length);
                        left -= opt.length;
                }

                if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
                        l2cap_send_config_req(chan);    /* no state change */

                goto out;

        case L2CAP_REJECT:
                goto discon;

        case L2CAP_UNKNOWN_OPTION:
                /*
                 * Packet contains options not understood. Turn off unknown
                 * options by setting them to default values (means they will
                 * not be requested again).
                 *
                 * If our option was already off then fail (paranoia?)
                 *
                 * XXX Should we consider that options were set for a reason?
                 */
                while (left > 0) {
                        if (left < sizeof(opt))
                                goto discon;

                        m_copydata(m, 0, sizeof(opt), &opt);
                        m_adj(m, sizeof(opt));
                        left -= sizeof(opt);

                        if (left < opt.length)
                                goto discon;

                        m_adj(m, opt.length);
                        left -= opt.length;

                        switch(opt.type) {
                        case L2CAP_OPT_MTU:
                                if (chan->lc_imtu == L2CAP_MTU_DEFAULT)
                                        goto discon;

                                chan->lc_imtu = L2CAP_MTU_DEFAULT;
                                break;

                        case L2CAP_OPT_FLUSH_TIMO:
                                if (chan->lc_flush == L2CAP_FLUSH_TIMO_DEFAULT)
                                        goto discon;

                                chan->lc_flush = L2CAP_FLUSH_TIMO_DEFAULT;
                                break;

                        case L2CAP_OPT_QOS:
                                break;

                        default:
                                UNKNOWN(opt.type);
                                goto discon;
                        }
                }

                if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0)
                        l2cap_send_config_req(chan);    /* no state change */

                goto out;

        default:
                UNKNOWN(cp.result);
                goto discon;
        }

        DPRINTF("how did I get here!?\n");

discon:
        l2cap_send_disconnect_req(chan);
        l2cap_close(chan, ECONNABORTED);

out:
        m_adj(m, left);
}

/*
 * Process Received Disconnect Request. We must validate scid and dcid
 * just in case but otherwise this connection is finished.
 */
static void
l2cap_recv_disconnect_req(struct mbuf *m, struct hci_link *link)
{
        l2cap_cmd_hdr_t cmd;
        l2cap_discon_req_cp cp;
        l2cap_discon_rsp_cp rp;
        struct l2cap_channel *chan;

        m_copydata(m, 0, sizeof(cmd), &cmd);
        m_adj(m, sizeof(cmd));

        m_copydata(m, 0, sizeof(cp), &cp);
        m_adj(m, sizeof(cp));

        cp.scid = letoh16(cp.scid);
        cp.dcid = letoh16(cp.dcid);

        chan = l2cap_cid_lookup(cp.dcid);
        if (chan == NULL || chan->lc_link != link || chan->lc_rcid != cp.scid) {
                l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
                                        cp.dcid, cp.scid);
                return;
        }

        rp.dcid = htole16(chan->lc_lcid);
        rp.scid = htole16(chan->lc_rcid);
        l2cap_send_signal(link, L2CAP_DISCONNECT_RSP, cmd.ident,
                                sizeof(rp), &rp);

        if (chan->lc_state != L2CAP_CLOSED)
                l2cap_close(chan, ECONNRESET);
}

/*
 * Process Received Disconnect Response. We must validate scid and dcid but
 * unless we were waiting for this signal, ignore it.
 */
static void
l2cap_recv_disconnect_rsp(struct mbuf *m, struct hci_link *link)
{
        l2cap_cmd_hdr_t cmd;
        l2cap_discon_rsp_cp cp;
        struct l2cap_req *req;
        struct l2cap_channel *chan;

        m_copydata(m, 0, sizeof(cmd), &cmd);
        m_adj(m, sizeof(cmd));

        m_copydata(m, 0, sizeof(cp), &cp);
        m_adj(m, sizeof(cp));

        cp.scid = letoh16(cp.scid);
        cp.dcid = letoh16(cp.dcid);

        req = l2cap_request_lookup(link, cmd.ident);
        if (req == NULL || req->lr_code != L2CAP_DISCONNECT_REQ)
                return;

        chan = req->lr_chan;
        if (chan == NULL
            || chan->lc_lcid != cp.scid
            || chan->lc_rcid != cp.dcid)
                return;

        l2cap_request_free(req);

        if (chan->lc_state != L2CAP_WAIT_DISCONNECT)
                return;

        l2cap_close(chan, 0);
}

/*
 * Process Received Info Request. We must respond but alas dont
 * support anything as yet so thats easy.
 */
static void
l2cap_recv_info_req(struct mbuf *m, struct hci_link *link)
{
        l2cap_cmd_hdr_t cmd;
        l2cap_info_req_cp cp;
        l2cap_info_rsp_cp rp;

        m_copydata(m, 0, sizeof(cmd), &cmd);
        m_adj(m, sizeof(cmd));

        m_copydata(m, 0, sizeof(cp), &cp);
        m_adj(m, sizeof(cp));

        switch(letoh16(cp.type)) {
        case L2CAP_CONNLESS_MTU:
        case L2CAP_EXTENDED_FEATURES:
        default:
                rp.type = cp.type;
                rp.result = htole16(L2CAP_NOT_SUPPORTED);

                l2cap_send_signal(link, L2CAP_INFO_RSP, cmd.ident,
                                        sizeof(rp), &rp);
                break;
        }
}

/*
 * Construct signal and wrap in C-Frame for link.
 */
static int
l2cap_send_signal(struct hci_link *link, uint8_t code, uint8_t ident,
                        uint16_t length, void *data)
{
        struct mbuf *m;
        l2cap_hdr_t *hdr;
        l2cap_cmd_hdr_t *cmd;
        int hlen;

#ifdef DIAGNOSTIC
        if (link == NULL)
                return ENETDOWN;

        if (sizeof(l2cap_cmd_hdr_t) + length > link->hl_mtu)
                kprintf("(%s) exceeding L2CAP Signal MTU for link!\n",
                    device_get_nameunit(link->hl_unit->hci_dev));
#endif

        m = m_gethdr(M_NOWAIT, MT_DATA);
        if (m == NULL)
                return ENOMEM;

        hdr = mtod(m, l2cap_hdr_t *);
        cmd = (l2cap_cmd_hdr_t *)(hdr + 1);
        hlen = (int)(sizeof(*hdr) + sizeof(*cmd));

        m->m_len = m->m_pkthdr.len = hlen; /* in case length == 0 */

        /* Command Data */
        if (length > 0 && m_copyback2(m, hlen, length, data, M_NOWAIT) != 0) {
                m_freem(m);
                return ENOMEM;
        }

        /* Command Header */
        cmd->code = code;
        cmd->ident = ident;
        cmd->length = htole16(length);
        length += sizeof(*cmd);

        /* C-Frame Header */
        hdr->length = htole16(length);
        hdr->dcid = htole16(L2CAP_SIGNAL_CID);
        length += sizeof(*hdr);

        DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
                device_get_nameunit(link->hl_unit->hci_dev), code, ident,
                length);

        return hci_acl_send(m, link, NULL);
}

/*
 * Send Command Reject packet.
 */
static int
l2cap_send_command_rej(struct hci_link *link, uint8_t ident,
                        uint16_t reason, ...)
{
        l2cap_cmd_rej_cp cp;
        int len = 0;
        __va_list ap;

        __va_start(ap, reason);

        cp.reason = htole16(reason);

        switch (reason) {
        case L2CAP_REJ_NOT_UNDERSTOOD:
                len = 2;
                break;

        case L2CAP_REJ_MTU_EXCEEDED:
                len = 4;
                cp.data[0] = __va_arg(ap, int);         /* SigMTU */
                cp.data[0] = htole16(cp.data[0]);
                break;

        case L2CAP_REJ_INVALID_CID:
                len = 6;
                cp.data[0] = __va_arg(ap, int);         /* dcid */
                cp.data[0] = htole16(cp.data[0]);
                cp.data[1] = __va_arg(ap, int);         /* scid */
                cp.data[1] = htole16(cp.data[1]);
                break;

        default:
                UNKNOWN(reason);
                return EINVAL;
        }

        __va_end(ap);

        return l2cap_send_signal(link, L2CAP_COMMAND_REJ, ident, len, &cp);
}

/*
 * Send Connect Request
 */
int
l2cap_send_connect_req(struct l2cap_channel *chan)
{
        l2cap_con_req_cp cp;
        int err;

        err = l2cap_request_alloc(chan, L2CAP_CONNECT_REQ);
        if (err)
                return err;

        cp.psm = htole16(chan->lc_raddr.bt_psm);
        cp.scid = htole16(chan->lc_lcid);

        return l2cap_send_signal(chan->lc_link, L2CAP_CONNECT_REQ,
                                chan->lc_link->hl_lastid, sizeof(cp), &cp);
}

/*
 * Send Config Request
 *
 * For outgoing config request, we only put options in the packet if they
 * differ from the default and would have to be actioned. We dont support
 * enough option types to make overflowing SigMTU an issue so it can all
 * go in one packet.
 */
int
l2cap_send_config_req(struct l2cap_channel *chan)
{
        l2cap_cfg_req_cp *cp;
        l2cap_cfg_opt_t *opt;
        l2cap_cfg_opt_val_t *val;
        uint8_t *next, buf[L2CAP_MTU_MINIMUM];
        int err;

        err = l2cap_request_alloc(chan, L2CAP_CONFIG_REQ);
        if (err)
                return err;

        /* Config Header (4 octets) */
        cp = (l2cap_cfg_req_cp *)buf;
        cp->dcid = htole16(chan->lc_rcid);
        cp->flags = 0;  /* "No Continuation" */

        next = buf + sizeof(l2cap_cfg_req_cp);

        /* Incoming MTU (4 octets) */
        if (chan->lc_imtu != L2CAP_MTU_DEFAULT) {
                opt = (l2cap_cfg_opt_t *)next;
                opt->type = L2CAP_OPT_MTU;
                opt->length = L2CAP_OPT_MTU_SIZE;

                val = (l2cap_cfg_opt_val_t *)(opt + 1);
                val->mtu = htole16(chan->lc_imtu);

                next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_MTU_SIZE;
        }

        /* Flush Timeout (4 octets) */
        if (chan->lc_flush != L2CAP_FLUSH_TIMO_DEFAULT) {
                opt = (l2cap_cfg_opt_t *)next;
                opt->type = L2CAP_OPT_FLUSH_TIMO;
                opt->length = L2CAP_OPT_FLUSH_TIMO_SIZE;

                val = (l2cap_cfg_opt_val_t *)(opt + 1);
                val->flush_timo = htole16(chan->lc_flush);

                next += sizeof(l2cap_cfg_opt_t) + L2CAP_OPT_FLUSH_TIMO_SIZE;
        }

        /* Outgoing QoS Flow (24 octets) */
        /* Retransmission & Flow Control (11 octets) */
        /*
         * From here we need to start paying attention to SigMTU as we have
         * possibly overflowed the minimum supported..
         */

        return l2cap_send_signal(chan->lc_link, L2CAP_CONFIG_REQ,
                                    chan->lc_link->hl_lastid, (int)(next - buf), buf);
}

/*
 * Send Disconnect Request
 */
int
l2cap_send_disconnect_req(struct l2cap_channel *chan)
{
        l2cap_discon_req_cp cp;
        int err;

        err = l2cap_request_alloc(chan, L2CAP_DISCONNECT_REQ);
        if (err)
                return err;

        cp.dcid = htole16(chan->lc_rcid);
        cp.scid = htole16(chan->lc_lcid);

        return l2cap_send_signal(chan->lc_link, L2CAP_DISCONNECT_REQ,
                                    chan->lc_link->hl_lastid, sizeof(cp), &cp);
}

/*
 * Send Connect Response
 */
int
l2cap_send_connect_rsp(struct hci_link *link, uint8_t ident, uint16_t dcid,
    uint16_t scid, uint16_t result)
{
        l2cap_con_rsp_cp cp;

        memset(&cp, 0, sizeof(cp));
        cp.dcid = htole16(dcid);
        cp.scid = htole16(scid);
        cp.result = htole16(result);

        return l2cap_send_signal(link, L2CAP_CONNECT_RSP, ident, sizeof(cp), &cp);
}