int ng_l2cap_lp_con_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg) { ng_hci_lp_con_cfm_ep *ep = NULL; ng_l2cap_con_p con = NULL; int error = 0; /* Check message */ if (msg->header.arglen != sizeof(*ep)) { NG_L2CAP_ALERT( "%s: %s - invalid LP_ConnectCfm[Neg] message size\n", __func__, NG_NODE_NAME(l2cap->node)); error = EMSGSIZE; goto out; } ep = (ng_hci_lp_con_cfm_ep *) (msg->data); /* Check if we have requested/accepted this connection */ con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr); if (con == NULL) { NG_L2CAP_ERR( "%s: %s - unexpected LP_ConnectCfm event. Connection does not exist\n", __func__, NG_NODE_NAME(l2cap->node)); error = ENOENT; goto out; } /* Check connection state */ if (con->state != NG_L2CAP_W4_LP_CON_CFM) { NG_L2CAP_ALERT( "%s: %s - unexpected LP_ConnectCfm event. " \ "Invalid connection state, state=%d, con_handle=%d\n", __func__, NG_NODE_NAME(l2cap->node), con->state, con->con_handle); error = EINVAL; goto out; } /* * Looks like it is our confirmation. It is safe now to cancel * connection timer and notify upper layer. If timeout already * happened then ignore connection confirmation and let timeout * handle that. */ if ((error = ng_l2cap_lp_untimeout(con)) != 0) goto out; if (ep->status == 0) { con->state = NG_L2CAP_CON_OPEN; con->con_handle = ep->con_handle; ng_l2cap_lp_deliver(con); } else /* Negative confirmation - remove connection descriptor */ ng_l2cap_con_fail(con, ep->status); out: return (error); } /* ng_l2cap_lp_con_cfm */
int ng_l2cap_l2ca_write_req(ng_l2cap_p l2cap, struct mbuf *m) { ng_l2cap_l2ca_hdr_t *l2ca_hdr = NULL; ng_l2cap_chan_p ch = NULL; ng_l2cap_cmd_p cmd = NULL; int error = 0; u_int32_t token = 0; /* Make sure we can access L2CA data packet header */ if (m->m_pkthdr.len < sizeof(*l2ca_hdr)) { NG_L2CAP_ERR( "%s: %s - L2CA Data packet too small, len=%d\n", __func__,NG_NODE_NAME(l2cap->node),m->m_pkthdr.len); error = EMSGSIZE; goto drop; } /* Get L2CA data packet header */ NG_L2CAP_M_PULLUP(m, sizeof(*l2ca_hdr)); if (m == NULL) return (ENOBUFS); l2ca_hdr = mtod(m, ng_l2cap_l2ca_hdr_t *); token = l2ca_hdr->token; m_adj(m, sizeof(*l2ca_hdr)); /* Verify payload size */ if (l2ca_hdr->length != m->m_pkthdr.len) { NG_L2CAP_ERR( "%s: %s - invalid L2CA Data packet. " \ "Payload length does not match, length=%d, len=%d\n", __func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->length, m->m_pkthdr.len); error = EMSGSIZE; goto drop; } /* Check channel ID */ if (l2ca_hdr->lcid < NG_L2CAP_FIRST_CID) { NG_L2CAP_ERR( "%s: %s - invalid L2CA Data packet. Inavlid channel ID, cid=%d\n", __func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid); error = EINVAL; goto drop; } /* Verify that we have the channel and make sure it is open */ ch = ng_l2cap_chan_by_scid(l2cap, l2ca_hdr->lcid); if (ch == NULL) { NG_L2CAP_ERR( "%s: %s - invalid L2CA Data packet. Channel does not exist, cid=%d\n", __func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid); error = ENOENT; goto drop; } if (ch->state != NG_L2CAP_OPEN) { NG_L2CAP_ERR( "%s: %s - invalid L2CA Data packet. Invalid channel state, scid=%d, state=%d\n", __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->state); error = EHOSTDOWN; goto drop; /* XXX not always - re-configure */ } /* Create L2CAP command descriptor */ cmd = ng_l2cap_new_cmd(ch->con, ch, 0, NGM_L2CAP_L2CA_WRITE, token); if (cmd == NULL) { error = ENOMEM; goto drop; } /* Attach data packet and link command to the queue */ cmd->aux = m; ng_l2cap_link_cmd(ch->con, cmd); ng_l2cap_lp_deliver(ch->con); return (error); drop: NG_FREE_M(m); return (error); } /* ng_l2cap_l2ca_write_req */
int ng_l2cap_l2ca_con_req(ng_l2cap_p l2cap, struct ng_mesg *msg) { ng_l2cap_l2ca_con_ip *ip = NULL; ng_l2cap_con_p con = NULL; ng_l2cap_chan_p ch = NULL; ng_l2cap_cmd_p cmd = NULL; int error = 0; /* Check message */ if (msg->header.arglen != sizeof(*ip)) { NG_L2CAP_ALERT( "%s: %s - invalid L2CA_Connect request message size, size=%d\n", __func__, NG_NODE_NAME(l2cap->node), msg->header.arglen); error = EMSGSIZE; goto out; } ip = (ng_l2cap_l2ca_con_ip *)(msg->data); /* Check if we have connection to the remote unit */ con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); if (con == NULL) { /* Submit LP_ConnectReq to the lower layer */ error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr); if (error != 0) { NG_L2CAP_ERR( "%s: %s - unable to send LP_ConnectReq message, error=%d\n", __func__, NG_NODE_NAME(l2cap->node), error); goto out; } /* This should not fail */ con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); KASSERT((con != NULL), ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node))); } /* * Create new empty channel descriptor. In case of any failure do * not touch connection descriptor. */ ch = ng_l2cap_new_chan(l2cap, con, ip->psm); if (ch == NULL) { error = ENOMEM; goto out; } /* Now create L2CAP_ConnectReq command */ cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(con), NG_L2CAP_CON_REQ, msg->header.token); if (cmd == NULL) { ng_l2cap_free_chan(ch); error = ENOMEM; goto out; } if (cmd->ident == NG_L2CAP_NULL_IDENT) { ng_l2cap_free_cmd(cmd); ng_l2cap_free_chan(ch); error = EIO; goto out; } /* Create L2CAP command packet */ _ng_l2cap_con_req(cmd->aux, cmd->ident, ch->psm, ch->scid); if (cmd->aux == NULL) { ng_l2cap_free_cmd(cmd); ng_l2cap_free_chan(ch); error = ENOBUFS; goto out; } ch->state = NG_L2CAP_W4_L2CAP_CON_RSP; /* Link command to the queue */ ng_l2cap_link_cmd(ch->con, cmd); ng_l2cap_lp_deliver(ch->con); out: return (error); } /* ng_l2cap_l2ca_con_req */
int ng_l2cap_l2ca_cfg_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg) { ng_l2cap_l2ca_cfg_rsp_ip *ip = NULL; ng_l2cap_chan_p ch = NULL; ng_l2cap_cmd_p cmd = NULL; struct mbuf *opt = NULL; u_int16_t *mtu = NULL; ng_l2cap_flow_p flow = NULL; int error = 0; /* Check message */ if (msg->header.arglen != sizeof(*ip)) { NG_L2CAP_ALERT( "%s: %s - invalid L2CA_ConfigRsp request message size, size=%d\n", __func__, NG_NODE_NAME(l2cap->node), msg->header.arglen); error = EMSGSIZE; goto out; } ip = (ng_l2cap_l2ca_cfg_rsp_ip *)(msg->data); /* Check if we have this channel */ ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); if (ch == NULL) { NG_L2CAP_ERR( "%s: %s - unexpected L2CA_ConfigRsp request message. " \ "Channel does not exist, lcid=%d\n", __func__, NG_NODE_NAME(l2cap->node), ip->lcid); error = ENOENT; goto out; } /* Check channel state */ if (ch->state != NG_L2CAP_CONFIG) { NG_L2CAP_ERR( "%s: %s - unexpected L2CA_ConfigRsp request message. " \ "Invalid channel state, state=%d, lcid=%d\n", __func__, NG_NODE_NAME(l2cap->node), ch->state, ch->scid); error = EINVAL; goto out; } /* Set channel settings */ if (ip->omtu != ch->omtu) { ch->omtu = ip->omtu; mtu = &ch->omtu; } if (bcmp(&ip->iflow, &ch->iflow, sizeof(ch->iflow)) != 0) { bcopy(&ip->iflow, &ch->iflow, sizeof(ch->iflow)); flow = &ch->iflow; } if (mtu != NULL || flow != NULL) { _ng_l2cap_build_cfg_options(opt, mtu, NULL, flow); if (opt == NULL) { error = ENOBUFS; goto out; } } /* Create L2CAP command */ cmd = ng_l2cap_new_cmd(ch->con, ch, ch->ident, NG_L2CAP_CFG_RSP, msg->header.token); if (cmd == NULL) { NG_FREE_M(opt); error = ENOMEM; goto out; } _ng_l2cap_cfg_rsp(cmd->aux,cmd->ident,ch->dcid,0,NG_L2CAP_SUCCESS,opt); if (cmd->aux == NULL) { ng_l2cap_free_cmd(cmd); error = ENOBUFS; goto out; } /* XXX FIXME - not here ??? */ ch->cfg_state |= NG_L2CAP_CFG_OUT; if (ch->cfg_state == NG_L2CAP_CFG_BOTH) ch->state = NG_L2CAP_OPEN; /* Link command to the queue */ ng_l2cap_link_cmd(ch->con, cmd); ng_l2cap_lp_deliver(ch->con); out: return (error); } /* ng_l2cap_l2ca_cfg_rsp_req */
int ng_l2cap_l2ca_cfg_req(ng_l2cap_p l2cap, struct ng_mesg *msg) { ng_l2cap_l2ca_cfg_ip *ip = NULL; ng_l2cap_chan_p ch = NULL; ng_l2cap_cmd_p cmd = NULL; struct mbuf *opt = NULL; u_int16_t *mtu = NULL, *flush_timo = NULL; ng_l2cap_flow_p flow = NULL; int error = 0; /* Check message */ if (msg->header.arglen != sizeof(*ip)) { NG_L2CAP_ALERT( "%s: %s - Invalid L2CA_Config request message size, size=%d\n", __func__, NG_NODE_NAME(l2cap->node), msg->header.arglen); error = EMSGSIZE; goto out; } ip = (ng_l2cap_l2ca_cfg_ip *)(msg->data); /* Check if we have this channel */ ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); if (ch == NULL) { NG_L2CAP_ERR( "%s: %s - unexpected L2CA_Config request message. " \ "Channel does not exist, lcid=%d\n", __func__, NG_NODE_NAME(l2cap->node), ip->lcid); error = ENOENT; goto out; } /* Check channel state */ if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG) { NG_L2CAP_ERR( "%s: %s - unexpected L2CA_Config request message. " \ "Invalid channel state, state=%d, lcid=%d\n", __func__, NG_NODE_NAME(l2cap->node), ch->state, ch->scid); error = EINVAL; goto out; } /* Set requested channel configuration options */ ch->imtu = ip->imtu; bcopy(&ip->oflow, &ch->oflow, sizeof(ch->oflow)); ch->flush_timo = ip->flush_timo; ch->link_timo = ip->link_timo; /* Compare channel settings with defaults */ if (ch->imtu != NG_L2CAP_MTU_DEFAULT) mtu = &ch->imtu; if (ch->flush_timo != NG_L2CAP_FLUSH_TIMO_DEFAULT) flush_timo = &ch->flush_timo; if (bcmp(ng_l2cap_default_flow(), &ch->oflow, sizeof(ch->oflow)) != 0) flow = &ch->oflow; /* Create configuration options */ _ng_l2cap_build_cfg_options(opt, mtu, flush_timo, flow); if (opt == NULL) { error = ENOBUFS; goto out; } /* Create L2CAP command descriptor */ cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con), NG_L2CAP_CFG_REQ, msg->header.token); if (cmd == NULL) { NG_FREE_M(opt); error = ENOMEM; goto out; } if (cmd->ident == NG_L2CAP_NULL_IDENT) { ng_l2cap_free_cmd(cmd); NG_FREE_M(opt); error = EIO; goto out; } /* Create L2CAP command packet */ _ng_l2cap_cfg_req(cmd->aux, cmd->ident, ch->dcid, 0, opt); if (cmd->aux == NULL) { ng_l2cap_free_cmd(cmd); error = ENOBUFS; goto out; } /* Adjust channel state for re-configuration */ if (ch->state == NG_L2CAP_OPEN) { ch->state = NG_L2CAP_CONFIG; ch->cfg_state = 0; } /* Link command to the queue */ ng_l2cap_link_cmd(ch->con, cmd); ng_l2cap_lp_deliver(ch->con); out: return (error); } /* ng_l2cap_l2ca_cfg_req */
int ng_l2cap_l2ca_con_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg) { ng_l2cap_l2ca_con_rsp_ip *ip = NULL; ng_l2cap_con_p con = NULL; ng_l2cap_chan_p ch = NULL; ng_l2cap_cmd_p cmd = NULL; u_int16_t dcid; int error = 0; /* Check message */ if (msg->header.arglen != sizeof(*ip)) { NG_L2CAP_ALERT( "%s: %s - invalid L2CA_ConnectRsp request message size, size=%d\n", __func__, NG_NODE_NAME(l2cap->node), msg->header.arglen); error = EMSGSIZE; goto out; } ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data); /* Check if we have this channel */ ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); if (ch == NULL) { NG_L2CAP_ALERT( "%s: %s - unexpected L2CA_ConnectRsp request message. " \ "Channel does not exist, lcid=%d\n", __func__, NG_NODE_NAME(l2cap->node), ip->lcid); error = ENOENT; goto out; } /* Check channel state */ if (ch->state != NG_L2CAP_W4_L2CA_CON_RSP) { NG_L2CAP_ERR( "%s: %s - unexpected L2CA_ConnectRsp request message. " \ "Invalid channel state, state=%d, lcid=%d\n", __func__, NG_NODE_NAME(l2cap->node), ch->state, ip->lcid); error = EINVAL; goto out; } dcid = ch->dcid; con = ch->con; /* * Now we are pretty much sure it is our response. So create and send * L2CAP_ConnectRsp message to our peer. */ if (ch->ident != ip->ident) NG_L2CAP_WARN( "%s: %s - channel ident and response ident do not match, scid=%d, ident=%d. " \ "Will use response ident=%d\n", __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->ident, ip->ident); /* Check result */ switch (ip->result) { case NG_L2CAP_SUCCESS: ch->state = NG_L2CAP_CONFIG; ch->cfg_state = 0; break; case NG_L2CAP_PENDING: break; default: ng_l2cap_free_chan(ch); ch = NULL; break; } /* Create L2CAP command */ cmd = ng_l2cap_new_cmd(con, ch, ip->ident, NG_L2CAP_CON_RSP, msg->header.token); if (cmd == NULL) { if (ch != NULL) ng_l2cap_free_chan(ch); error = ENOMEM; goto out; } _ng_l2cap_con_rsp(cmd->aux, cmd->ident, ip->lcid, dcid, ip->result, ip->status); if (cmd->aux == NULL) { if (ch != NULL) ng_l2cap_free_chan(ch); ng_l2cap_free_cmd(cmd); error = ENOBUFS; goto out; } /* Link command to the queue */ ng_l2cap_link_cmd(con, cmd); ng_l2cap_lp_deliver(con); out: return (error); } /* ng_l2cap_l2ca_con_rsp_req */
int ng_l2cap_l2ca_get_info_req(ng_l2cap_p l2cap, struct ng_mesg *msg) { ng_l2cap_l2ca_get_info_ip *ip = NULL; ng_l2cap_con_p con = NULL; ng_l2cap_cmd_p cmd = NULL; int error = 0; /* Verify message */ if (msg->header.arglen != sizeof(*ip)) { NG_L2CAP_ALERT( "%s: %s - invalid L2CA_GetInfo request message size, size=%d\n", __func__, NG_NODE_NAME(l2cap->node), msg->header.arglen); error = EMSGSIZE; goto out; } ip = (ng_l2cap_l2ca_get_info_ip *)(msg->data); /* Check if we have connection to the unit */ con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); if (con == NULL) { /* Submit LP_ConnectReq to the lower layer */ error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr); if (error != 0) { NG_L2CAP_ERR( "%s: %s - unable to send LP_ConnectReq message, error=%d\n", __func__, NG_NODE_NAME(l2cap->node), error); goto out; } /* This should not fail */ con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); KASSERT((con != NULL), ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node))); } /* Create L2CAP command descriptor */ cmd = ng_l2cap_new_cmd(con, NULL, ng_l2cap_get_ident(con), NG_L2CAP_INFO_REQ, msg->header.token); if (cmd == NULL) { error = ENOMEM; goto out; } if (cmd->ident == NG_L2CAP_NULL_IDENT) { ng_l2cap_free_cmd(cmd); error = EIO; goto out; } /* Create L2CAP command packet */ _ng_l2cap_info_req(cmd->aux, cmd->ident, ip->info_type); if (cmd->aux == NULL) { ng_l2cap_free_cmd(cmd); error = ENOBUFS; goto out; } /* Link command to the queue */ ng_l2cap_link_cmd(con, cmd); ng_l2cap_lp_deliver(con); out: return (error); } /* ng_l2cap_l2ca_get_info_req */
int ng_l2cap_l2ca_discon_req(ng_l2cap_p l2cap, struct ng_mesg *msg) { ng_l2cap_l2ca_discon_ip *ip = NULL; ng_l2cap_chan_p ch = NULL; ng_l2cap_cmd_p cmd = NULL; int error = 0; /* Check message */ if (msg->header.arglen != sizeof(*ip)) { NG_L2CAP_ALERT( "%s: %s - invalid L2CA_Disconnect request message size, size=%d\n", __func__, NG_NODE_NAME(l2cap->node), msg->header.arglen); error = EMSGSIZE; goto out; } ip = (ng_l2cap_l2ca_discon_ip *)(msg->data); /* Check if we have this channel */ ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); if (ch == NULL) { NG_L2CAP_ERR( "%s: %s - unexpected L2CA_Disconnect request message. " \ "Channel does not exist, lcid=%d\n", __func__, NG_NODE_NAME(l2cap->node), ip->lcid); error = ENOENT; goto out; } /* Check channel state */ if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { NG_L2CAP_ERR( "%s: %s - unexpected L2CA_Disconnect request message. " \ "Invalid channel state, state=%d, lcid=%d\n", __func__, NG_NODE_NAME(l2cap->node), ch->state, ch->scid); error = EINVAL; goto out; } /* Create and send L2CAP_DisconReq message */ cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con), NG_L2CAP_DISCON_REQ, msg->header.token); if (cmd == NULL) { ng_l2cap_free_chan(ch); error = ENOMEM; goto out; } if (cmd->ident == NG_L2CAP_NULL_IDENT) { ng_l2cap_free_chan(ch); ng_l2cap_free_cmd(cmd); error = EIO; goto out; } _ng_l2cap_discon_req(cmd->aux, cmd->ident, ch->dcid, ch->scid); if (cmd->aux == NULL) { ng_l2cap_free_chan(ch); ng_l2cap_free_cmd(cmd); error = ENOBUFS; goto out; } ch->state = NG_L2CAP_W4_L2CAP_DISCON_RSP; /* Link command to the queue */ ng_l2cap_link_cmd(ch->con, cmd); ng_l2cap_lp_deliver(ch->con); out: return (error); } /* ng_l2cap_l2ca_discon_req */