int
ng_l2cap_l2ca_ping_rsp(ng_l2cap_con_p con, u_int32_t token, u_int16_t result,
struct mbuf *data)
{
ng_l2cap_p l2cap = con->l2cap;
struct ng_mesg *msg = NULL;
ng_l2cap_l2ca_ping_op *op = NULL;
int error = 0, size = 0;
/* Check if control hook is connected and valid */
if (l2cap->ctl == NULL || NG_HOOK_NOT_VALID(l2cap->ctl)) {
NG_L2CAP_WARN(
"%s: %s - unable to send L2CA_Ping response message. " \
"Hook is not connected or valid\n",
__func__, NG_NODE_NAME(l2cap->node));
error = ENOTCONN;
goto out;
}
size = (data == NULL)? 0 : data->m_pkthdr.len;
/* Create and send L2CA_Ping response message */
NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_PING,
sizeof(*op) + size, M_NOWAIT);
if (msg == NULL)
error = ENOMEM;
else {
msg->header.token = token;
msg->header.flags |= NGF_RESP;
op = (ng_l2cap_l2ca_ping_op *)(msg->data);
op->result = result;
bcopy(&con->remote, &op->bdaddr, sizeof(op->bdaddr));
if (data != NULL && size > 0) {
op->echo_size = size;
m_copydata(data, 0, size, (caddr_t) op + sizeof(*op));
}
NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->ctl, 0);
}
out:
NG_FREE_M(data);
return (error);
} /* ng_l2cap_l2ca_ping_rsp */
int
ng_l2cap_l2ca_receive(ng_l2cap_con_p con)
{
ng_l2cap_p l2cap = con->l2cap;
ng_l2cap_hdr_t *hdr = NULL;
ng_l2cap_chan_p ch = NULL;
int error = 0;
NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
if (con->rx_pkt == NULL)
return (ENOBUFS);
hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *);
/* Check channel */
ch = ng_l2cap_chan_by_scid(l2cap, hdr->dcid);
if (ch == NULL) {
NG_L2CAP_ERR(
"%s: %s - unexpected L2CAP data packet. Channel does not exist, cid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->dcid);
error = ENOENT;
goto drop;
}
/* Check channel state */
if (ch->state != NG_L2CAP_OPEN) {
NG_L2CAP_WARN(
"%s: %s - unexpected L2CAP data packet. " \
"Invalid channel state, cid=%d, state=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->scid,
ch->state);
error = EHOSTDOWN; /* XXX not always - re-configuration */
goto drop;
}
/* Check payload size and channel's MTU */
if (hdr->length > ch->imtu) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP data packet. " \
"Packet too big, length=%d, imtu=%d, cid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->length,
ch->imtu, ch->scid);
error = EMSGSIZE;
goto drop;
}
/*
* If we got here then everything looks good and we can sent packet
* to the upper layer protocol.
*/
/* Check if upstream hook is connected and valid */
if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
NG_L2CAP_ERR(
"%s: %s - unable to send L2CAP data packet. " \
"Hook is not connected or valid, psm=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->psm);
error = ENOTCONN;
goto drop;
}
NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt);
con->rx_pkt = NULL;
drop:
NG_FREE_M(con->rx_pkt); /* checks for != NULL */
return (error);
} /* ng_l2cap_receive */
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_ping_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
ng_l2cap_l2ca_ping_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_Ping request message size, size=%d\n",
__func__, NG_NODE_NAME(l2cap->node),
msg->header.arglen);
error = EMSGSIZE;
goto out;
}
ip = (ng_l2cap_l2ca_ping_ip *)(msg->data);
if (ip->echo_size > NG_L2CAP_MAX_ECHO_SIZE) {
NG_L2CAP_WARN(
"%s: %s - invalid L2CA_Ping request. Echo size is too big, echo_size=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ip->echo_size);
error = EMSGSIZE;
goto out;
}
/* 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_ECHO_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_echo_req(cmd->aux, cmd->ident,
msg->data + sizeof(*ip), ip->echo_size);
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_ping_req */
int
ng_l2cap_l2ca_receive(ng_l2cap_con_p con)
{
ng_l2cap_p l2cap = con->l2cap;
ng_l2cap_hdr_t *hdr = NULL;
ng_l2cap_chan_p ch = NULL;
int error = 0;
int idtype;
uint16_t *idp;
int silent = 0;
NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
if (con->rx_pkt == NULL)
return (ENOBUFS);
hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *);
/* Check channel */
if(hdr->dcid == NG_L2CAP_ATT_CID){
idtype = NG_L2CAP_L2CA_IDTYPE_ATT;
ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_ATT_CID,
con->con_handle);
/*
* Here,ATT channel is distinguished by
* connection handle
*/
hdr->dcid = con->con_handle;
silent = 1;
}else if(hdr->dcid == NG_L2CAP_SMP_CID){
idtype = NG_L2CAP_L2CA_IDTYPE_SMP;
ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_SMP_CID,
con->con_handle);
/*
* Here,SMP channel is distinguished by
* connection handle
*/
silent = 1;
hdr->dcid = con->con_handle;
}else{
idtype = (con->linktype==NG_HCI_LINK_ACL)?
NG_L2CAP_L2CA_IDTYPE_BREDR:
NG_L2CAP_L2CA_IDTYPE_LE;
ch = ng_l2cap_chan_by_scid(l2cap, hdr->dcid, idtype);
}
if (ch == NULL) {
if(!silent)
NG_L2CAP_ERR(
"%s: %s - unexpected L2CAP data packet. Channel does not exist, cid=%d, idtype=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->dcid, idtype);
error = ENOENT;
goto drop;
}
/* Check channel state */
if (ch->state != NG_L2CAP_OPEN) {
NG_L2CAP_WARN(
"%s: %s - unexpected L2CAP data packet. " \
"Invalid channel state, cid=%d, state=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->scid,
ch->state);
error = EHOSTDOWN; /* XXX not always - re-configuration */
goto drop;
}
/* Check payload size and channel's MTU */
if (hdr->length > ch->imtu) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP data packet. " \
"Packet too big, length=%d, imtu=%d, cid=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->length,
ch->imtu, ch->scid);
error = EMSGSIZE;
goto drop;
}
/*
* If we got here then everything looks good and we can sent packet
* to the upper layer protocol.
*/
/* Check if upstream hook is connected and valid */
if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
NG_L2CAP_ERR(
"%s: %s - unable to send L2CAP data packet. " \
"Hook is not connected or valid, psm=%d\n",
__func__, NG_NODE_NAME(l2cap->node), ch->psm);
error = ENOTCONN;
goto drop;
}
M_PREPEND(con->rx_pkt, sizeof(uint16_t), M_NOWAIT);
if(con->rx_pkt == NULL)
goto drop;
idp = mtod(con->rx_pkt, uint16_t *);
*idp = idtype;
NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt);
con->rx_pkt = NULL;
drop:
NG_FREE_M(con->rx_pkt); /* checks for != NULL */
return (error);
} /* ng_l2cap_receive */
