[BACK]Return to l2cap_signal.c CVS log [TXT][DIR] Up to [cvs.NetBSD.org] / src / sys / netbt

Please note that diffs are not public domain; they are subject to the copyright notices on the relevant files.

Diff for /src/sys/netbt/l2cap_signal.c between version 1.1.2.1 and 1.1.2.2

version 1.1.2.1, 2006/06/19 15:44:45 version 1.1.2.2, 2006/06/21 15:10:51
Line 0 
Line 1 
   /*      $NetBSD$        */
   
   /*-
    * 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/cdefs.h>
   __KERNEL_RCSID(0, "$NetBSD$");
   
   #include <sys/param.h>
   #include <sys/kernel.h>
   #include <sys/mbuf.h>
   #include <sys/proc.h>
   #include <sys/queue.h>
   #include <sys/systm.h>
   
   #include <machine/stdarg.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 = le16toh(cmd.length);
   
                   if (m->m_pkthdr.len < sizeof(cmd) + cmd.length)
                           goto reject;
   
                   DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
                           link->hl_unit->hci_devname,
                           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 = le16toh(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 (le16toh(cp.reason)) {
           case L2CAP_REJ_NOT_UNDERSTOOD:
                   /*
                    * I dont know what to do, just move up the timeout
                    */
                   callout_schedule(&req->lr_rtx, 0);
                   break;
   
           case L2CAP_REJ_MTU_EXCEEDED:
                   /*
                    * I didnt send any commands over L2CAP_MTU_MINIMUM size, but..
                    */
                   link->hl_mtu = le16toh(cp.data[0]);
                   callout_schedule(&req->lr_rtx, 0);  // XX maybe resend instead?
                   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(le16toh(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;
           l2cap_con_rsp_cp rp;
           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));
   
           /* init response */
           memset(&rp, 0, sizeof(rp));
           rp.scid = cp.scid;
   
           memset(&laddr, 0, sizeof(struct sockaddr_bt));
           laddr.bt_len = sizeof(struct sockaddr_bt);
           laddr.bt_family = AF_BLUETOOTH;
           laddr.bt_psm = le16toh(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 = le16toh(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) {
                           rp.result = htole16(L2CAP_NO_RESOURCES);
                           l2cap_send_signal(link, L2CAP_CONNECT_RSP,
                                                   cmd.ident, sizeof(rp), &rp);
                           (*new->lc_proto->disconnected)(new->lc_upper, err);
                           return;
                   }
   
                   new->lc_link = hci_acl_open(link->hl_unit, &link->hl_bdaddr);
                   KASSERT(new->lc_link == link);
   
                   new->lc_rcid = le16toh(cp.scid);
   
                   memcpy(&new->lc_laddr, &laddr, sizeof(struct sockaddr_bt));
                   memcpy(&new->lc_raddr, &raddr, sizeof(struct sockaddr_bt));
   
                   rp.dcid = htole16(new->lc_lcid);
                   rp.result = htole16(L2CAP_SUCCESS);
                   l2cap_send_signal(link, L2CAP_CONNECT_RSP, cmd.ident,
                                                   sizeof(rp), &rp);
   
                   new->lc_state = L2CAP_WAIT_CONFIG_REQ | L2CAP_WAIT_CONFIG_RSP;
                   l2cap_send_config_req(new);
                   return;
           }
   
           rp.result = htole16(L2CAP_PSM_NOT_SUPPORTED);
           l2cap_send_signal(link, L2CAP_CONNECT_RSP, cmd.ident, sizeof(rp), &rp);
   }
   
   /*
    * 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 = le16toh(cp.scid);
           cp.dcid = le16toh(cp.dcid);
           cp.result = le16toh(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_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_REQ | L2CAP_WAIT_CONFIG_RSP;
                   l2cap_send_config_req(chan);
                   break;
   
           case L2CAP_PENDING:
                   // 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 Reqest.
    */
   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 = le16toh(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 = le16toh(cp.dcid);
           cp.flags = le16toh(cp.flags);
   
           chan = l2cap_cid_lookup(cp.dcid);
           if (chan == NULL || (chan->lc_state & L2CAP_WAIT_CONFIG_REQ) == 0) {
                   l2cap_send_command_rej(link, cmd.ident, L2CAP_REJ_INVALID_CID,
                                           L2CAP_NULL_CID, cp.dcid);
                   goto out;
           }
   
           /* ready our response packet */
           rp = (l2cap_cfg_rsp_cp *)buf;
           rp->scid = 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
            * would make that happen. This could be if:
            *
            *      1. too many unknown option types (>40!)
            *      2. repeated options we found unacceptable
            *      3. corrupted packet looks like above
            */
           while (left > 0) {
                   if (left < sizeof(*opt) || len + sizeof(*opt) > sizeof(buf))
                           goto reject;
   
                   opt = (l2cap_cfg_opt_t *)&buf[len];
                   m_copydata(m, 0, sizeof(*opt), opt);
                   m_adj(m, sizeof(*opt));
                   left -= sizeof(*opt);
   
                   if (left < opt->length)
                           goto reject;
   
                   val = (l2cap_cfg_opt_val_t *)&buf[len + sizeof(*opt)];
   
                   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
                               || len + sizeof(*opt) + L2CAP_OPT_MTU_SIZE > sizeof(buf))
                                   goto reject;
   
                           m_copydata(m, 0, L2CAP_OPT_MTU_SIZE, val);
                           val->mtu = le16toh(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) {
                                   rp->result = L2CAP_UNACCEPTABLE_PARAMS;
                                   val->mtu = htole16(L2CAP_MTU_MINIMUM);
                                   len += sizeof(*opt) + 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
                               || len + sizeof(*opt) + L2CAP_OPT_FLUSH_TIMO_SIZE > sizeof(buf))
                                   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:
                   default:
                           /* ignore hints */
                           if (opt->type & L2CAP_OPT_HINT_BIT)
                                   break;
   
                           /* unknown options supercede all else */
                           if (rp->result != L2CAP_UNKNOWN_OPTION) {
                                   rp->result = L2CAP_UNKNOWN_OPTION;
                                   len = sizeof(*rp);
                           }
   
                           buf[len++] = opt->type;
                           buf[len++] = 0; // no need to return his data
                           break;
                   }
   
                   m_adj(m, opt->length);
                   left -= opt->length;
           }
   
           rp->result = htole16(rp->result);
           l2cap_send_signal(link, L2CAP_CONFIG_RSP, cmd.ident, len, buf);
   
           if ((cp.flags & L2CAP_OPT_CFLAG_BIT) == 0
               && rp->result == L2CAP_SUCCESS) {
   
                   chan->lc_state &= ~L2CAP_WAIT_CONFIG_REQ;
   
                   if ((chan->lc_state & 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 = le16toh(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 = le16toh(cp.scid);
           cp.flags = le16toh(cp.flags);
           cp.result = le16toh(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_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_state &= ~L2CAP_WAIT_CONFIG_RSP;
   
                           if ((chan->lc_state & 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 = le16toh(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 = le16toh(cp.scid);
           cp.dcid = le16toh(cp.dcid);
   
           chan = l2cap_cid_lookup(cp.dcid);
           if (chan == NULL || 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 = le16toh(cp.scid);
           cp.dcid = le16toh(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(le16toh(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;
   
   #ifdef DIAGNOSTIC
           if (link == NULL)
                   return ENETDOWN;
   
           if (sizeof(l2cap_cmd_hdr_t) + length > link->hl_mtu)
                   printf("(%s) exceeding L2CAP Signal MTU for link!\n",
                           link->hl_unit->hci_devname);
   #endif
   
           m = m_gethdr(M_DONTWAIT, MT_DATA);
           if (m == NULL)
                   return ENOMEM;
   
           hdr = mtod(m, l2cap_hdr_t *);
           cmd = (l2cap_cmd_hdr_t *)(hdr + 1);
   
           m->m_len = m->m_pkthdr.len = MHLEN;
   
           /* Command Data */
           if (length > 0)
                   m_copyback(m, sizeof(hdr) + sizeof(cmd), length, data);
   
           /* 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);
   
           if (m->m_pkthdr.len != MAX(MHLEN, length)) {
                   m_freem(m);
                   return ENOMEM;
           }
   
           m->m_pkthdr.len = length;
           m->m_len = MIN(length, MHLEN);
   
           DPRINTFN(2, "(%s) code %d, ident %d, len %d\n",
                   link->hl_unit->hci_devname, 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);
   }

Legend:
Removed from v.1.1.2.1  
changed lines
  Added in v.1.1.2.2

CVSweb <webmaster@jp.NetBSD.org>