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->idtype == NG_L2CAP_L2CA_IDTYPE_ATT){
ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_ATT_CID,
l2ca_hdr->lcid);
} else if (l2ca_hdr->idtype == NG_L2CAP_L2CA_IDTYPE_SMP){
ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_SMP_CID,
l2ca_hdr->lcid);
}else{
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,
l2ca_hdr->idtype);
}
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_clt_receive(ng_l2cap_con_p con)
{
struct _clt_pkt {
ng_l2cap_hdr_t h;
ng_l2cap_clt_hdr_t c_h;
} __attribute__ ((packed)) *hdr = NULL;
ng_l2cap_p l2cap = con->l2cap;
int length, error = 0;
NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
if (con->rx_pkt == NULL)
return (ENOBUFS);
hdr = mtod(con->rx_pkt, struct _clt_pkt *);
/* Check packet */
length = con->rx_pkt->m_pkthdr.len - sizeof(*hdr);
if (length < 0) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP CLT data packet. Packet too small, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node), length);
error = EMSGSIZE;
goto drop;
}
/* Check payload size against CLT MTU */
if (length > NG_L2CAP_MTU_DEFAULT) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP CLT data packet. Packet too big, length=%d, mtu=%d\n",
__func__, NG_NODE_NAME(l2cap->node), length,
NG_L2CAP_MTU_DEFAULT);
error = EMSGSIZE;
goto drop;
}
hdr->c_h.psm = le16toh(hdr->c_h.psm);
/*
* If we got here then everything looks good and we can sent packet
* to the upper layer protocol.
*/
/* Select upstream hook based on PSM */
switch (hdr->c_h.psm) {
case NG_L2CAP_PSM_SDP:
if (l2cap->flags & NG_L2CAP_CLT_SDP_DISABLED)
goto drop;
break;
case NG_L2CAP_PSM_RFCOMM:
if (l2cap->flags & NG_L2CAP_CLT_RFCOMM_DISABLED)
goto drop;
break;
case NG_L2CAP_PSM_TCP:
if (l2cap->flags & NG_L2CAP_CLT_TCP_DISABLED)
goto drop;
break;
}
/* 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 CLT data packet. " \
"Hook is not connected or valid, psm=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->c_h.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_l2ca_clt_receive */
int
ng_l2cap_lp_receive(ng_l2cap_p l2cap, struct mbuf *m)
{
ng_hci_acldata_pkt_t *acl_hdr = NULL;
ng_l2cap_hdr_t *l2cap_hdr = NULL;
ng_l2cap_con_p con = NULL;
u_int16_t con_handle, length, pb;
int error = 0;
/* Check ACL data packet */
if (m->m_pkthdr.len < sizeof(*acl_hdr)) {
NG_L2CAP_ERR(
"%s: %s - invalid ACL data packet. Packet too small, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node), m->m_pkthdr.len);
error = EMSGSIZE;
goto drop;
}
/* Strip ACL data packet header */
NG_L2CAP_M_PULLUP(m, sizeof(*acl_hdr));
if (m == NULL)
return (ENOBUFS);
acl_hdr = mtod(m, ng_hci_acldata_pkt_t *);
m_adj(m, sizeof(*acl_hdr));
/* Get ACL connection handle, PB flag and payload length */
acl_hdr->con_handle = le16toh(acl_hdr->con_handle);
con_handle = NG_HCI_CON_HANDLE(acl_hdr->con_handle);
pb = NG_HCI_PB_FLAG(acl_hdr->con_handle);
length = le16toh(acl_hdr->length);
NG_L2CAP_INFO(
"%s: %s - got ACL data packet, con_handle=%d, PB=%#x, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con_handle, pb, length);
/* Get connection descriptor */
con = ng_l2cap_con_by_handle(l2cap, con_handle);
if (con == NULL) {
NG_L2CAP_ERR(
"%s: %s - unexpected ACL data packet. " \
"Connection does not exist, con_handle=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con_handle);
error = ENOENT;
goto drop;
}
/* Verify connection state */
if (con->state != NG_L2CAP_CON_OPEN) {
NG_L2CAP_ERR(
"%s: %s - unexpected ACL data packet. Invalid connection state=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con->state);
error = EHOSTDOWN;
goto drop;
}
/* Process packet */
if (pb == NG_HCI_PACKET_START) {
if (con->rx_pkt != NULL) {
NG_L2CAP_ERR(
"%s: %s - dropping incomplete L2CAP packet, got %d bytes, want %d bytes\n",
__func__, NG_NODE_NAME(l2cap->node),
con->rx_pkt->m_pkthdr.len, con->rx_pkt_len);
NG_FREE_M(con->rx_pkt);
con->rx_pkt_len = 0;
}
/* Get L2CAP header */
if (m->m_pkthdr.len < sizeof(*l2cap_hdr)) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP packet start fragment. Packet too small, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node),
m->m_pkthdr.len);
error = EMSGSIZE;
goto drop;
}
NG_L2CAP_M_PULLUP(m, sizeof(*l2cap_hdr));
if (m == NULL)
return (ENOBUFS);
l2cap_hdr = mtod(m, ng_l2cap_hdr_t *);
NG_L2CAP_INFO(
"%s: %s - staring new L2CAP packet, con_handle=%d, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con_handle,
le16toh(l2cap_hdr->length));
/* Start new L2CAP packet */
con->rx_pkt = m;
con->rx_pkt_len = le16toh(l2cap_hdr->length)+sizeof(*l2cap_hdr);
} else if (pb == NG_HCI_PACKET_FRAGMENT) {
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;
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;
}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) {
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 */
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 */
