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 */
static int
ng_vlan_rcvmsg(node_p node, item_p item, hook_p lasthook)
{
const priv_p priv = NG_NODE_PRIVATE(node);
struct ng_mesg *msg, *resp = NULL;
struct ng_vlan_filter *vf;
hook_p hook;
struct ng_vlan_table *t;
uintptr_t hook_data;
int i, vlan_count;
uint16_t vid;
int error = 0;
NGI_GET_MSG(item, msg);
/* Deal with message according to cookie and command. */
switch (msg->header.typecookie) {
case NGM_VLAN_COOKIE:
switch (msg->header.cmd) {
case NGM_VLAN_ADD_FILTER:
/* Check that message is long enough. */
if (msg->header.arglen != sizeof(*vf)) {
error = EINVAL;
break;
}
vf = (struct ng_vlan_filter *)msg->data;
/* Sanity check the VLAN ID value. */
#ifdef NG_VLAN_USE_OLD_VLAN_NAME
if (vf->vid == 0 && vf->vid != vf->vlan) {
vf->vid = vf->vlan;
} else if (vf->vid != 0 && vf->vlan != 0 &&
vf->vid != vf->vlan) {
error = EINVAL;
break;
}
#endif
if (vf->vid & ~EVL_VLID_MASK ||
vf->pcp & ~7 ||
vf->cfi & ~1) {
error = EINVAL;
break;
}
/* Check that a referenced hook exists. */
hook = ng_findhook(node, vf->hook_name);
if (hook == NULL) {
error = ENOENT;
break;
}
/* And is not one of the special hooks. */
if (hook == priv->downstream_hook ||
hook == priv->nomatch_hook) {
error = EINVAL;
break;
}
/* And is not already in service. */
if (IS_HOOK_VLAN_SET(NG_HOOK_PRIVATE(hook))) {
error = EEXIST;
break;
}
/* Check we don't already trap this VLAN. */
if (priv->vlan_hook[vf->vid] != NULL) {
error = EEXIST;
break;
}
/* Link vlan and hook together. */
NG_HOOK_SET_PRIVATE(hook,
(void *)(HOOK_VLAN_TAG_SET_MASK |
EVL_MAKETAG(vf->vid, vf->pcp, vf->cfi)));
priv->vlan_hook[vf->vid] = hook;
break;
case NGM_VLAN_DEL_FILTER:
/* Check that message is long enough. */
if (msg->header.arglen != NG_HOOKSIZ) {
error = EINVAL;
break;
}
/* Check that hook exists and is active. */
hook = ng_findhook(node, (char *)msg->data);
if (hook == NULL) {
error = ENOENT;
break;
}
hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
if (IS_HOOK_VLAN_SET(hook_data) == 0) {
error = ENOENT;
break;
}
KASSERT(priv->vlan_hook[EVL_VLANOFTAG(hook_data)] == hook,
("%s: NGM_VLAN_DEL_FILTER: Invalid VID for Hook = %s\n",
__func__, (char *)msg->data));
/* Purge a rule that refers to this hook. */
priv->vlan_hook[EVL_VLANOFTAG(hook_data)] = NULL;
NG_HOOK_SET_PRIVATE(hook, NULL);
break;
case NGM_VLAN_DEL_VID_FLT:
/* Check that message is long enough. */
if (msg->header.arglen != sizeof(uint16_t)) {
error = EINVAL;
break;
}
vid = (*((uint16_t *)msg->data));
/* Sanity check the VLAN ID value. */
if (vid & ~EVL_VLID_MASK) {
error = EINVAL;
break;
}
/* Check that hook exists and is active. */
hook = priv->vlan_hook[vid];
if (hook == NULL) {
error = ENOENT;
break;
}
hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
if (IS_HOOK_VLAN_SET(hook_data) == 0) {
error = ENOENT;
break;
}
KASSERT(EVL_VLANOFTAG(hook_data) == vid,
("%s: NGM_VLAN_DEL_VID_FLT:"
" Invalid VID Hook = %us, must be: %us\n",
__func__, (uint16_t )EVL_VLANOFTAG(hook_data),
vid));
/* Purge a rule that refers to this hook. */
priv->vlan_hook[vid] = NULL;
NG_HOOK_SET_PRIVATE(hook, NULL);
break;
case NGM_VLAN_GET_TABLE:
/* Calculate vlans. */
vlan_count = 0;
for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
if (priv->vlan_hook[i] != NULL &&
NG_HOOK_IS_VALID(priv->vlan_hook[i]))
vlan_count ++;
}
/* Allocate memory for response. */
NG_MKRESPONSE(resp, msg, sizeof(*t) +
vlan_count * sizeof(*t->filter), M_NOWAIT);
if (resp == NULL) {
error = ENOMEM;
break;
}
/* Pack data to response. */
t = (struct ng_vlan_table *)resp->data;
t->n = 0;
vf = &t->filter[0];
for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
hook = priv->vlan_hook[i];
if (hook == NULL || NG_HOOK_NOT_VALID(hook))
continue;
hook_data = (uintptr_t)NG_HOOK_PRIVATE(hook);
if (IS_HOOK_VLAN_SET(hook_data) == 0)
continue;
KASSERT(EVL_VLANOFTAG(hook_data) == i,
("%s: NGM_VLAN_GET_TABLE:"
" hook %s VID = %us, must be: %i\n",
__func__, NG_HOOK_NAME(hook),
(uint16_t)EVL_VLANOFTAG(hook_data), i));
#ifdef NG_VLAN_USE_OLD_VLAN_NAME
vf->vlan = i;
#endif
vf->vid = i;
vf->pcp = EVL_PRIOFTAG(hook_data);
vf->cfi = EVL_CFIOFTAG(hook_data);
strncpy(vf->hook_name,
NG_HOOK_NAME(hook), NG_HOOKSIZ);
vf ++;
t->n ++;
}
break;
case NGM_VLAN_GET_DECAP:
NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
if (resp == NULL) {
error = ENOMEM;
break;
}
(*((uint32_t *)resp->data)) = priv->decap_enable;
break;
case NGM_VLAN_SET_DECAP:
if (msg->header.arglen != sizeof(uint32_t)) {
error = EINVAL;
break;
}
priv->decap_enable = (*((uint32_t *)msg->data));
break;
case NGM_VLAN_GET_ENCAP:
NG_MKRESPONSE(resp, msg, sizeof(uint32_t), M_NOWAIT);
if (resp == NULL) {
error = ENOMEM;
break;
}
(*((uint32_t *)resp->data)) = priv->encap_enable;
break;
case NGM_VLAN_SET_ENCAP:
if (msg->header.arglen != sizeof(uint32_t)) {
error = EINVAL;
break;
}
priv->encap_enable = (*((uint32_t *)msg->data));
break;
case NGM_VLAN_GET_ENCAP_PROTO:
NG_MKRESPONSE(resp, msg, sizeof(uint16_t), M_NOWAIT);
if (resp == NULL) {
error = ENOMEM;
break;
}
(*((uint16_t *)resp->data)) = ntohs(priv->encap_proto);
break;
case NGM_VLAN_SET_ENCAP_PROTO:
if (msg->header.arglen != sizeof(uint16_t)) {
error = EINVAL;
break;
}
priv->encap_proto = htons((*((uint16_t *)msg->data)));
break;
default: /* Unknown command. */
error = EINVAL;
break;
}
break;
case NGM_FLOW_COOKIE:
{
struct ng_mesg *copy;
/*
* Flow control messages should come only
* from downstream.
*/
if (lasthook == NULL)
break;
if (lasthook != priv->downstream_hook)
break;
/* Broadcast the event to all uplinks. */
for (i = 0; i < (EVL_VLID_MASK + 1); i ++) {
if (priv->vlan_hook[i] == NULL)
continue;
NG_COPYMESSAGE(copy, msg, M_NOWAIT);
if (copy == NULL)
continue;
NG_SEND_MSG_HOOK(error, node, copy,
priv->vlan_hook[i], 0);
}
break;
}
default: /* Unknown type cookie. */
error = EINVAL;
break;
}
NG_RESPOND_MSG(error, node, item, resp);
NG_FREE_MSG(msg);
return (error);
}
int
ng_l2cap_lp_con_req(ng_l2cap_p l2cap, bdaddr_p bdaddr)
{
struct ng_mesg *msg = NULL;
ng_hci_lp_con_req_ep *ep = NULL;
ng_l2cap_con_p con = NULL;
int error = 0;
/* Verify that we DO NOT have connection to the remote unit */
con = ng_l2cap_con_by_addr(l2cap, bdaddr);
if (con != NULL) {
NG_L2CAP_ALERT(
"%s: %s - unexpected LP_ConnectReq event. " \
"Connection already exists, state=%d, con_handle=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con->state,
con->con_handle);
return (EEXIST);
}
/* Check if lower layer protocol is still connected */
if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
NG_L2CAP_ERR(
"%s: %s - hook \"%s\" is not connected or valid\n",
__func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
return (ENOTCONN);
}
/* Create and intialize new connection descriptor */
con = ng_l2cap_new_con(l2cap, bdaddr);
if (con == NULL)
return (ENOMEM);
/* Create and send LP_ConnectReq event */
NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_REQ,
sizeof(*ep), M_WAITOK | M_NULLOK);
if (msg == NULL) {
ng_l2cap_free_con(con);
return (ENOMEM);
}
ep = (ng_hci_lp_con_req_ep *) (msg->data);
bcopy(bdaddr, &ep->bdaddr, sizeof(ep->bdaddr));
ep->link_type = NG_HCI_LINK_ACL;
con->flags |= NG_L2CAP_CON_OUTGOING;
con->state = NG_L2CAP_W4_LP_CON_CFM;
ng_l2cap_lp_timeout(con);
NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, 0);
if (error != 0) {
if ((error = ng_l2cap_lp_untimeout(con)) != 0)
return (error);
ng_l2cap_free_con(con);
}
return (error);
} /* ng_l2cap_lp_con_req */
int
ng_l2cap_lp_con_ind(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
ng_hci_lp_con_ind_ep *ep = NULL;
ng_hci_lp_con_rsp_ep *rp = NULL;
struct ng_mesg *rsp = 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_ConnectInd message size\n",
__func__, NG_NODE_NAME(l2cap->node));
error = EMSGSIZE;
goto out;
}
ep = (ng_hci_lp_con_ind_ep *) (msg->data);
/* Make sure we have only one connection to the remote unit */
con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr);
if (con != NULL) {
NG_L2CAP_ALERT(
"%s: %s - unexpected LP_ConnectInd event. " \
"Connection already exists, state=%d, con_handle=%d\n",
__func__, NG_NODE_NAME(l2cap->node), con->state,
con->con_handle);
error = EEXIST;
goto out;
}
/* Check if lower layer protocol is still connected */
if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
NG_L2CAP_ERR(
"%s: %s - hook \"%s\" is not connected or valid",
__func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
error = ENOTCONN;
goto out;
}
/* Create and intialize new connection descriptor */
con = ng_l2cap_new_con(l2cap, &ep->bdaddr);
if (con == NULL) {
error = ENOMEM;
goto out;
}
/* Create and send LP_ConnectRsp event */
NG_MKMESSAGE(rsp, NGM_HCI_COOKIE, NGM_HCI_LP_CON_RSP,
sizeof(*rp), M_WAITOK | M_NULLOK);
if (rsp == NULL) {
ng_l2cap_free_con(con);
error = ENOMEM;
goto out;
}
rp = (ng_hci_lp_con_rsp_ep *)(rsp->data);
rp->status = 0x00; /* accept connection */
rp->link_type = NG_HCI_LINK_ACL;
bcopy(&ep->bdaddr, &rp->bdaddr, sizeof(rp->bdaddr));
con->state = NG_L2CAP_W4_LP_CON_CFM;
ng_l2cap_lp_timeout(con);
NG_SEND_MSG_HOOK(error, l2cap->node, rsp, l2cap->hci, 0);
if (error != 0) {
if ((error = ng_l2cap_lp_untimeout(con)) != 0)
goto out;
ng_l2cap_free_con(con);
}
out:
return (error);
} /* ng_hci_lp_con_ind */
int
ng_hci_send_command(ng_hci_unit_p unit)
{
struct mbuf *m0 = NULL, *m = NULL;
int free, error = 0;
/* Check if other command is pending */
if (unit->state & NG_HCI_UNIT_COMMAND_PENDING)
return (0);
/* Check if unit can accept our command */
NG_HCI_BUFF_CMD_GET(unit->buffer, free);
if (free == 0)
return (0);
/* Check if driver hook is still ok */
if (unit->drv == NULL || NG_HOOK_NOT_VALID(unit->drv)) {
NG_HCI_WARN(
"%s: %s - hook \"%s\" is not connected or valid\n",
__func__, NG_NODE_NAME(unit->node), NG_HCI_HOOK_DRV);
NG_BT_MBUFQ_DRAIN(&unit->cmdq);
return (ENOTCONN);
}
/*
* Get first command from queue, give it to RAW hook then
* make copy of it and send it to the driver
*/
m0 = NG_BT_MBUFQ_FIRST(&unit->cmdq);
if (m0 == NULL)
return (0);
ng_hci_mtap(unit, m0);
m = m_dup(m0, MB_DONTWAIT);
if (m != NULL)
NG_SEND_DATA_ONLY(error, unit->drv, m);
else
error = ENOBUFS;
if (error != 0)
NG_HCI_ERR(
"%s: %s - could not send HCI command, error=%d\n",
__func__, NG_NODE_NAME(unit->node), error);
/*
* Even if we were not able to send command we still pretend
* that everything is OK and let timeout handle that.
*/
NG_HCI_BUFF_CMD_USE(unit->buffer, 1);
NG_HCI_STAT_CMD_SENT(unit->stat);
NG_HCI_STAT_BYTES_SENT(unit->stat, m0->m_pkthdr.len);
/*
* Note: ng_hci_command_timeout() will set
* NG_HCI_UNIT_COMMAND_PENDING flag
*/
ng_hci_command_timeout(unit);
return (0);
} /* ng_hci_send_command */
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 */
