static int
ng_sscfu_rcvlower(hook_p hook, item_p item)
{
node_p node = NG_HOOK_NODE(hook);
struct priv *priv = NG_NODE_PRIVATE(node);
struct mbuf *m;
struct sscop_arg a;
if (!priv->enabled || priv->upper == NULL) {
NG_FREE_ITEM(item);
return (0);
}
NGI_GET_M(item, m);
NG_FREE_ITEM(item);
if (!(m->m_flags & M_PKTHDR)) {
printf("no pkthdr\n");
m_freem(m);
return (EINVAL);
}
/*
* Strip of the SSCOP header.
*/
if (m->m_len < (int)sizeof(a) && (m = m_pullup(m, sizeof(a))) == NULL)
return (ENOMEM);
bcopy((caddr_t)mtod(m, struct sscop_arg *), &a, sizeof(a));
m_adj(m, sizeof(a));
sscfu_input(priv->sscf, a.sig, m, a.arg);
return (0);
}
static int
ng_sscop_rcvupper(hook_p hook, item_p item)
{
struct priv *priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
struct sscop_arg a;
struct mbuf *m;
if (!priv->enabled) {
NG_FREE_ITEM(item);
return (EINVAL);
}
/*
* If the lower layer is not connected allow to proceed.
* The lower layer sending function will drop outgoing frames,
* and the sscop will timeout any establish requests.
*/
NGI_GET_M(item, m);
NG_FREE_ITEM(item);
if (!(m->m_flags & M_PKTHDR)) {
printf("no pkthdr\n");
m_freem(m);
return (EINVAL);
}
if (m->m_len < (int)sizeof(a) && (m = m_pullup(m, sizeof(a))) == NULL)
return (ENOBUFS);
bcopy((caddr_t)mtod(m, struct sscop_arg *), &a, sizeof(a));
m_adj(m, sizeof(a));
return (sscop_aasig(priv->sscop, a.sig, m, a.arg));
}
/*
* DATA
*/
static int
ng_sscop_rcvlower(hook_p hook, item_p item)
{
struct priv *priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
struct mbuf *m;
if (!priv->enabled) {
NG_FREE_ITEM(item);
return EINVAL;
}
/*
* If we are disconnected at the upper layer and in the IDLE
* state, drop any incoming packet.
*/
if (priv->upper != NULL || sscop_getstate(priv->sscop) != SSCOP_IDLE) {
NGI_GET_M(item, m);
priv->stats.in_packets++;
sscop_input(priv->sscop, m);
} else {
priv->stats.in_dropped++;
}
NG_FREE_ITEM(item);
return (0);
}
/*
* Receive data, and do something with it.
* Actually we receive a queue item which holds the data.
* If we free the item it will also free the data unless we have
* previously disassociated it using the NGI_GET_M() macro.
* Possibly send it out on another link after processing.
* Possibly do something different if it comes from different
* hooks. The caller will never free m, so if we use up this data or
* abort we must free it.
*
* If we want, we may decide to force this data to be queued and reprocessed
* at the netgraph NETISR time.
* We would do that by setting the HK_QUEUE flag on our hook. We would do that
* in the connect() method.
*/
static int
ng_xxx_rcvdata(hook_p hook, item_p item )
{
const xxx_p xxxp = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
int chan = -2;
int dlci = -2;
int error;
struct mbuf *m;
NGI_GET_M(item, m);
if (NG_HOOK_PRIVATE(hook)) {
dlci = ((struct XXX_hookinfo *) NG_HOOK_PRIVATE(hook))->dlci;
chan = ((struct XXX_hookinfo *) NG_HOOK_PRIVATE(hook))->channel;
if (dlci != -1) {
/* If received on a DLCI hook process for this
* channel and pass it to the downstream module.
* Normally one would add a multiplexing header at
* the front here */
/* M_PREPEND(....) ; */
/* mtod(m, xxxxxx)->dlci = dlci; */
NG_FWD_NEW_DATA(error, item,
xxxp->downstream_hook.hook, m);
xxxp->packets_out++;
} else {
/* data came from the multiplexed link */
dlci = 1; /* get dlci from header */
/* madjust(....) *//* chop off header */
for (chan = 0; chan < XXX_NUM_DLCIS; chan++)
if (xxxp->channel[chan].dlci == dlci)
break;
if (chan == XXX_NUM_DLCIS) {
NG_FREE_ITEM(item);
NG_FREE_M(m);
return (ENETUNREACH);
}
/* If we were called at splnet, use the following:
* NG_SEND_DATA_ONLY(error, otherhook, m); if this
* node is running at some SPL other than SPLNET
* then you should use instead: error =
* ng_queueit(otherhook, m, NULL); m = NULL;
* This queues the data using the standard NETISR
* system and schedules the data to be picked
* up again once the system has moved to SPLNET and
* the processing of the data can continue. After
* these are run 'm' should be considered
* as invalid and NG_SEND_DATA actually zaps them. */
NG_FWD_NEW_DATA(error, item,
xxxp->channel[chan].hook, m);
xxxp->packets_in++;
}
} else {
/* It's the debug hook, throw it away.. */
if (hook == xxxp->downstream_hook.hook) {
NG_FREE_ITEM(item);
NG_FREE_M(m);
}
}
return 0;
}
/*
* Connect the data socket to a named control socket node.
*/
static int
ng_connect_data(struct sockaddr *nam, struct ngpcb *pcbp)
{
struct sockaddr_ng *sap;
node_p farnode;
struct ngsock *priv;
int error;
item_p item;
/* If we are already connected, don't do it again. */
if (pcbp->sockdata != NULL)
return (EISCONN);
/*
* Find the target (victim) and check it doesn't already have
* a data socket. Also check it is a 'socket' type node.
* Use ng_package_data() and ng_address_path() to do this.
*/
sap = (struct sockaddr_ng *) nam;
/* The item will hold the node reference. */
item = ng_package_data(NULL, NG_WAITOK);
if ((error = ng_address_path(NULL, item, sap->sg_data, 0)))
return (error); /* item is freed on failure */
/*
* Extract node from item and free item. Remember we now have
* a reference on the node. The item holds it for us.
* when we free the item we release the reference.
*/
farnode = item->el_dest; /* shortcut */
if (strcmp(farnode->nd_type->name, NG_SOCKET_NODE_TYPE) != 0) {
NG_FREE_ITEM(item); /* drop the reference to the node */
return (EINVAL);
}
priv = NG_NODE_PRIVATE(farnode);
if (priv->datasock != NULL) {
NG_FREE_ITEM(item); /* drop the reference to the node */
return (EADDRINUSE);
}
/*
* Link the PCB and the private data struct. and note the extra
* reference. Drop the extra reference on the node.
*/
mtx_lock(&priv->mtx);
priv->datasock = pcbp;
pcbp->sockdata = priv;
priv->refs++;
mtx_unlock(&priv->mtx);
NG_FREE_ITEM(item); /* drop the reference to the node */
return (0);
}
int
ng_hci_lp_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
{
int link_type;
if ((unit->state & NG_HCI_UNIT_READY) != NG_HCI_UNIT_READY) {
NG_HCI_WARN(
"%s: %s - unit is not ready, state=%#x\n",
__func__, NG_NODE_NAME(unit->node), unit->state);
NG_FREE_ITEM(item);
return (ENXIO);
}
if (NGI_MSG(item)->header.arglen != sizeof(ng_hci_lp_con_req_ep)) {
NG_HCI_ALERT(
"%s: %s - invalid LP_ConnectReq message size=%d\n",
__func__, NG_NODE_NAME(unit->node),
NGI_MSG(item)->header.arglen);
NG_FREE_ITEM(item);
return (EMSGSIZE);
}
link_type = ((ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data))->link_type;
switch(link_type){
case NG_HCI_LINK_ACL:
return (ng_hci_lp_acl_con_req(unit, item, hook));
case NG_HCI_LINK_SCO:
if (hook != unit->sco ) {
NG_HCI_WARN(
"%s: %s - LP_ConnectReq for SCO connection came from wrong hook=%p\n",
__func__, NG_NODE_NAME(unit->node), hook);
NG_FREE_ITEM(item);
return (EINVAL);
}
return (ng_hci_lp_sco_con_req(unit, item, hook));
case NG_HCI_LINK_LE_PUBLIC:
case NG_HCI_LINK_LE_RANDOM:
return (ng_hci_lp_le_con_req(unit, item, hook, link_type));
default:
panic("%s: link_type invalid.", __func__);
}
return (EINVAL);
} /* ng_hci_lp_con_req */
/*
* Receive incoming data on our hook. Send it out the socket.
*/
static int
ng_ksocket_rcvdata(hook_p hook, item_p item)
{
struct thread *td = curthread; /* XXX broken */
const node_p node = NG_HOOK_NODE(hook);
const priv_p priv = NG_NODE_PRIVATE(node);
struct socket *const so = priv->so;
struct sockaddr *sa = NULL;
int error;
struct mbuf *m;
struct sa_tag *stag;
/* Extract data */
NGI_GET_M(item, m);
NG_FREE_ITEM(item);
/*
* Look if socket address is stored in packet tags.
* If sockaddr is ours, or provided by a third party (zero id),
* then we accept it.
*/
if (((stag = (struct sa_tag *)m_tag_locate(m, NGM_KSOCKET_COOKIE,
NG_KSOCKET_TAG_SOCKADDR, NULL)) != NULL) &&
(stag->id == NG_NODE_ID(node) || stag->id == 0))
sa = &stag->sa;
/* Reset specific mbuf flags to prevent addressing problems. */
m->m_flags &= ~(M_BCAST|M_MCAST);
/* Send packet */
error = sosend(so, sa, 0, m, 0, 0, td);
return (error);
}
/*
* Receive data
*/
static int
ngee_rcvdata(hook_p hook, item_p item)
{
int error;
struct mbuf *m;
struct ether_header *eh;
struct ether_addr tmpaddr;
/* Make sure we have an entire header */
NGI_GET_M(item, m);
if (m->m_len < sizeof(*eh) ) {
m = m_pullup(m, sizeof(*eh));
if (m == NULL) {
NG_FREE_ITEM(item);
return(EINVAL);
}
}
eh = mtod(m, struct ether_header *);
/* swap the source and destination fields */
bcopy(eh->ether_dhost, &tmpaddr, ETHER_ADDR_LEN);
bcopy(eh->ether_shost, eh->ether_dhost, ETHER_ADDR_LEN);
bcopy(&tmpaddr, eh->ether_shost, ETHER_ADDR_LEN);
NG_FWD_NEW_DATA(error, item, hook, m);
return (error);
}
/*
* Decode an incoming frame coming from the switch
*/
static int
ngfrm_decode(node_p node, item_p item)
{
const sc_p sc = NG_NODE_PRIVATE(node);
char *data;
int alen;
u_int dlci = 0;
int error = 0;
int ctxnum;
struct mbuf *m;
NGI_GET_M(item, m);
if (m->m_len < 4 && (m = m_pullup(m, 4)) == NULL) {
error = ENOBUFS;
goto out;
}
data = mtod(m, char *);
if ((alen = sc->addrlen) == 0) {
sc->addrlen = alen = ngfrm_addrlen(data);
}
switch (alen) {
case 2:
SHIFTIN(makeup + 0, data[0], dlci);
SHIFTIN(makeup + 1, data[1], dlci);
break;
case 3:
SHIFTIN(makeup + 0, data[0], dlci);
SHIFTIN(makeup + 1, data[1], dlci);
SHIFTIN(makeup + 3, data[2], dlci); /* 3 and 2 is correct */
break;
case 4:
SHIFTIN(makeup + 0, data[0], dlci);
SHIFTIN(makeup + 1, data[1], dlci);
SHIFTIN(makeup + 2, data[2], dlci);
SHIFTIN(makeup + 3, data[3], dlci);
break;
default:
error = EINVAL;
goto out;
}
if (dlci > 1023) {
error = EINVAL;
goto out;
}
ctxnum = sc->ALT[dlci];
if ((ctxnum & CTX_VALID) && sc->channel[ctxnum &= CTX_VALUE].hook) {
/* Send it */
m_adj(m, alen);
NG_FWD_NEW_DATA(error, item, sc->channel[ctxnum].hook, m);
return (error);
} else {
error = ENETDOWN;
}
out:
NG_FREE_ITEM(item);
NG_FREE_M(m);
return (error);
}
static int
ng_btsocket_hci_raw_node_rcvmsg(node_p node, item_p item, hook_p lasthook)
{
struct ng_mesg *msg = NGI_MSG(item); /* item still has message */
int error = 0;
/*
* Check for empty sockets list creates LOR when both sender and
* receiver device are connected to the same host, so remove it
* for now
*/
if (msg != NULL &&
(msg->header.typecookie == NGM_HCI_COOKIE ||
msg->header.typecookie == NGM_GENERIC_COOKIE) &&
msg->header.flags & NGF_RESP) {
if (msg->header.token == 0) {
NG_FREE_ITEM(item);
return (0);
}
lockmgr(&ng_btsocket_hci_raw_queue_lock, LK_EXCLUSIVE);
if (NG_BT_ITEMQ_FULL(&ng_btsocket_hci_raw_queue)) {
NG_BTSOCKET_HCI_RAW_ERR(
"%s: Input queue is full\n", __func__);
NG_BT_ITEMQ_DROP(&ng_btsocket_hci_raw_queue);
NG_FREE_ITEM(item);
error = ENOBUFS;
} else {
ng_ref_item(item);
NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_hci_raw_queue, item);
error = ng_btsocket_hci_raw_wakeup_input_task();
}
lockmgr(&ng_btsocket_hci_raw_queue_lock, LK_RELEASE);
} else {
NG_FREE_ITEM(item);
error = EINVAL;
}
return (error);
} /* ng_btsocket_hci_raw_node_rcvmsg */
/*
* Recive data from a hook.
*/
static int
ng_split_rcvdata(hook_p hook, item_p item)
{
const priv_p priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
int error = 0;
if (hook == priv->out) {
printf("ng_split: got packet from out hook!\n");
NG_FREE_ITEM(item);
error = EINVAL;
} else if ((hook == priv->in) && (priv->mixed != NULL)) {
NG_FWD_ITEM_HOOK(error, item, priv->mixed);
} else if ((hook == priv->mixed) && (priv->out != NULL)) {
NG_FWD_ITEM_HOOK(error, item, priv->out);
}
if (item)
NG_FREE_ITEM(item);
return (error);
}
/*
* Receive data
*/
static int
ngipi_rcvdata(hook_p hook, item_p item)
{
struct mbuf *m;
NGI_GET_M(item, m);
NG_FREE_ITEM(item);
if (curthread->td_ng_outbound)
netisr_queue(NETISR_IP, m);
else
netisr_dispatch(NETISR_IP, m);
return 0;
}
int
ng_hci_lp_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
{
if ((unit->state & NG_HCI_UNIT_READY) != NG_HCI_UNIT_READY) {
NG_HCI_WARN(
"%s: %s - unit is not ready, state=%#x\n",
__func__, NG_NODE_NAME(unit->node), unit->state);
NG_FREE_ITEM(item);
return (ENXIO);
}
if (NGI_MSG(item)->header.arglen != sizeof(ng_hci_lp_con_req_ep)) {
NG_HCI_ALERT(
"%s: %s - invalid LP_ConnectReq message size=%d\n",
__func__, NG_NODE_NAME(unit->node),
NGI_MSG(item)->header.arglen);
NG_FREE_ITEM(item);
return (EMSGSIZE);
}
if (((ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data))->link_type == NG_HCI_LINK_ACL)
return (ng_hci_lp_acl_con_req(unit, item, hook));
if (hook != unit->sco) {
NG_HCI_WARN(
"%s: %s - LP_ConnectReq for SCO connection came from wrong hook=%p\n",
__func__, NG_NODE_NAME(unit->node), hook);
NG_FREE_ITEM(item);
return (EINVAL);
}
return (ng_hci_lp_sco_con_req(unit, item, hook));
} /* ng_hci_lp_con_req */
static int
ng_sscop_rcvmanage(hook_p hook, item_p item)
{
struct priv *priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
struct sscop_marg a;
struct mbuf *m;
if (!priv->enabled) {
NG_FREE_ITEM(item);
return (EINVAL);
}
NGI_GET_M(item, m);
NG_FREE_ITEM(item);
if (m->m_len < (int)sizeof(a) && (m = m_pullup(m, sizeof(a))) == NULL)
return (ENOBUFS);
bcopy((caddr_t)mtod(m, struct sscop_arg *), &a, sizeof(a));
m_adj(m, sizeof(a));
return (sscop_maasig(priv->sscop, a.sig, m));
}
/*
* Receive data
*/
static int
ng_vjc_rcvdata(hook_p hook, item_p item)
{
const node_p node = NG_HOOK_NODE(hook);
const priv_p priv = NG_NODE_PRIVATE(node);
int error = 0;
struct mbuf *m;
NGI_GET_M(item, m);
if (hook == priv->ip) { /* outgoing packet */
u_int type = TYPE_IP;
/* Compress packet if enabled and proto is TCP */
if (priv->conf.enableComp) {
struct ip *ip;
if ((m = ng_vjc_pulluphdrs(m, 0)) == NULL) {
NG_FREE_ITEM(item);
return (ENOBUFS);
}
ip = mtod(m, struct ip *);
if (ip->ip_p == IPPROTO_TCP) {
const int origLen = m->m_len;
type = sl_compress_tcp(m, ip,
&priv->slc, priv->conf.compressCID);
m->m_pkthdr.len += m->m_len - origLen;
}
}
/* Dispatch to the appropriate outgoing hook */
switch (type) {
case TYPE_IP:
hook = priv->vjip;
break;
case TYPE_UNCOMPRESSED_TCP:
hook = priv->vjuncomp;
break;
case TYPE_COMPRESSED_TCP:
hook = priv->vjcomp;
break;
default:
panic("%s: type=%d", __func__, type);
}
} else if (hook == priv->vjcomp) { /* incoming compressed packet */
/*
* Accept data from the hook and queue it for output.
*/
Static int
ng_udbp_rcvdata(hook_p hook, item_p item)
{
const udbp_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
int error;
struct ifqueue *xmitq_p;
int s;
struct mbuf *m;
meta_p meta;
NGI_GET_M(item, m);
NGI_GET_META(item, meta);
NG_FREE_ITEM(item);
/*
* Now queue the data for when it can be sent
*/
if (meta && meta->priority > 0) {
xmitq_p = (&sc->xmitq_hipri);
} else {
xmitq_p = (&sc->xmitq);
}
s = splusb();
IF_LOCK(xmitq_p);
if (_IF_QFULL(xmitq_p)) {
_IF_DROP(xmitq_p);
IF_UNLOCK(xmitq_p);
splx(s);
error = ENOBUFS;
goto bad;
}
_IF_ENQUEUE(xmitq_p, m);
IF_UNLOCK(xmitq_p);
if (!(sc->flags & OUT_BUSY))
udbp_setup_out_transfer(sc);
splx(s);
return (0);
bad: /*
* It was an error case.
* check if we need to free the mbuf, and then return the error
*/
NG_FREE_M(m);
NG_FREE_META(meta);
return (error);
}
static int
ng_btsocket_hci_raw_node_rcvdata(hook_p hook, item_p item)
{
struct mbuf *nam = NULL;
int error;
/*
* Check for empty sockets list creates LOR when both sender and
* receiver device are connected to the same host, so remove it
* for now
*/
MGET(nam, M_NOWAIT, MT_SONAME);
if (nam != NULL) {
struct sockaddr_hci *sa = mtod(nam, struct sockaddr_hci *);
nam->m_len = sizeof(struct sockaddr_hci);
sa->hci_len = sizeof(*sa);
sa->hci_family = AF_BLUETOOTH;
strlcpy(sa->hci_node, NG_PEER_NODE_NAME(hook),
sizeof(sa->hci_node));
NGI_GET_M(item, nam->m_next);
NGI_M(item) = nam;
lockmgr(&ng_btsocket_hci_raw_queue_lock, LK_EXCLUSIVE);
if (NG_BT_ITEMQ_FULL(&ng_btsocket_hci_raw_queue)) {
NG_BTSOCKET_HCI_RAW_ERR(
"%s: Input queue is full\n", __func__);
NG_BT_ITEMQ_DROP(&ng_btsocket_hci_raw_queue);
NG_FREE_ITEM(item);
error = ENOBUFS;
} else {
ng_ref_item(item);
NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_hci_raw_queue, item);
error = ng_btsocket_hci_raw_wakeup_input_task();
}
lockmgr(&ng_btsocket_hci_raw_queue_lock, LK_RELEASE);
} else {
static int
ng_bt3c_rcvdata(hook_p hook, item_p item)
{
bt3c_softc_p sc = (bt3c_softc_p)NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
struct mbuf *m = NULL;
int error = 0;
if (sc == NULL) {
error = EHOSTDOWN;
goto out;
}
if (hook != sc->hook) {
error = EINVAL;
goto out;
}
NGI_GET_M(item, m);
IF_LOCK(&sc->outq);
if (_IF_QFULL(&sc->outq)) {
NG_BT3C_ERR(sc->dev,
"Outgoing queue is full. Dropping mbuf, len=%d\n", m->m_pkthdr.len);
_IF_DROP(&sc->outq);
NG_BT3C_STAT_OERROR(sc->stat);
NG_FREE_M(m);
} else
_IF_ENQUEUE(&sc->outq, m);
IF_UNLOCK(&sc->outq);
error = ng_send_fn(sc->node, NULL, bt3c_send, NULL, 0 /* new send */);
out:
NG_FREE_ITEM(item);
return (error);
} /* ng_bt3c_rcvdata */
/*
* Receive data on a hook
*/
static int
ngs_rcvdata(hook_p hook, item_p item)
{
struct ngsock *const priv = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
struct ngpcb *const pcbp = priv->datasock;
struct socket *so;
struct sockaddr_ng *addr;
char *addrbuf[NG_HOOKSIZ + 4];
int addrlen;
struct mbuf *m;
NGI_GET_M(item, m);
NG_FREE_ITEM(item);
/* If there is no data socket, black-hole it. */
if (pcbp == NULL) {
NG_FREE_M(m);
return (0);
}
so = pcbp->ng_socket;
/* Get the return address into a sockaddr. */
addrlen = strlen(NG_HOOK_NAME(hook)); /* <= NG_HOOKSIZ - 1 */
addr = (struct sockaddr_ng *) addrbuf;
addr->sg_len = addrlen + 3;
addr->sg_family = AF_NETGRAPH;
bcopy(NG_HOOK_NAME(hook), addr->sg_data, addrlen);
addr->sg_data[addrlen] = '\0';
/* Try to tell the socket which hook it came in on. */
if (sbappendaddr((struct sockbuf *)&so->so_rcv, (struct sockaddr *)addr, m, NULL) == 0) {
m_freem(m);
TRAP_ERROR;
return (ENOBUFS);
}
sorwakeup(so);
return (0);
}
int
ng_hci_lp_discon_req(ng_hci_unit_p unit, item_p item, hook_p hook)
{
struct discon_req {
ng_hci_cmd_pkt_t hdr;
ng_hci_discon_cp cp;
} __attribute__ ((packed)) *req = NULL;
ng_hci_lp_discon_req_ep *ep = NULL;
ng_hci_unit_con_p con = NULL;
struct mbuf *m = NULL;
int error = 0;
/* Check if unit is ready */
if ((unit->state & NG_HCI_UNIT_READY) != NG_HCI_UNIT_READY) {
NG_HCI_WARN(
"%s: %s - unit is not ready, state=%#x\n",
__func__, NG_NODE_NAME(unit->node), unit->state);
error = ENXIO;
goto out;
}
if (NGI_MSG(item)->header.arglen != sizeof(*ep)) {
NG_HCI_ALERT(
"%s: %s - invalid LP_DisconnectReq message size=%d\n",
__func__, NG_NODE_NAME(unit->node),
NGI_MSG(item)->header.arglen);
error = EMSGSIZE;
goto out;
}
ep = (ng_hci_lp_discon_req_ep *)(NGI_MSG(item)->data);
con = ng_hci_con_by_handle(unit, ep->con_handle);
if (con == NULL) {
NG_HCI_ERR(
"%s: %s - invalid connection handle=%d\n",
__func__, NG_NODE_NAME(unit->node), ep->con_handle);
error = ENOENT;
goto out;
}
if (con->state != NG_HCI_CON_OPEN) {
NG_HCI_ERR(
"%s: %s - invalid connection state=%d, handle=%d\n",
__func__, NG_NODE_NAME(unit->node), con->state,
ep->con_handle);
error = EINVAL;
goto out;
}
/*
* Create HCI command
*/
MGETHDR(m, M_DONTWAIT, MT_DATA);
if (m == NULL) {
error = ENOBUFS;
goto out;
}
m->m_pkthdr.len = m->m_len = sizeof(*req);
req = mtod(m, struct discon_req *);
req->hdr.type = NG_HCI_CMD_PKT;
req->hdr.length = sizeof(req->cp);
req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_DISCON));
req->cp.con_handle = htole16(ep->con_handle);
req->cp.reason = ep->reason;
/*
* Queue and send HCI command
*/
NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
error = ng_hci_send_command(unit);
out:
NG_FREE_ITEM(item);
return (error);
} /* ng_hci_lp_discon_req */
static int
ng_hci_lp_sco_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
{
struct sco_con_req {
ng_hci_cmd_pkt_t hdr;
ng_hci_add_sco_con_cp cp;
} __attribute__ ((packed)) *req = NULL;
ng_hci_lp_con_req_ep *ep = NULL;
ng_hci_unit_con_p acl_con = NULL, sco_con = NULL;
struct mbuf *m = NULL;
int error = 0;
ep = (ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data);
/*
* SCO connection without ACL link
*
* If upper layer requests SCO connection and there is no open ACL
* connection to the desired remote unit, we will reject the request.
*/
LIST_FOREACH(acl_con, &unit->con_list, next)
if (acl_con->link_type == NG_HCI_LINK_ACL &&
acl_con->state == NG_HCI_CON_OPEN &&
bcmp(&acl_con->bdaddr, &ep->bdaddr, sizeof(bdaddr_t)) == 0)
break;
if (acl_con == NULL) {
NG_HCI_INFO(
"%s: %s - No open ACL connection to bdaddr=%x:%x:%x:%x:%x:%x\n",
__func__, NG_NODE_NAME(unit->node),
ep->bdaddr.b[5], ep->bdaddr.b[4], ep->bdaddr.b[3],
ep->bdaddr.b[2], ep->bdaddr.b[1], ep->bdaddr.b[0]);
error = ENOENT;
goto out;
}
/*
* Multiple SCO connections can exist between the same pair of units.
* We assume that multiple SCO connections have to be opened one after
* another.
*
* Try to find SCO connection descriptor that matches the following:
*
* 1) sco_con->link_type == NG_HCI_LINK_SCO
*
* 2) sco_con->state == NG_HCI_CON_W4_LP_CON_RSP ||
* sco_con->state == NG_HCI_CON_W4_CONN_COMPLETE
*
* 3) sco_con->bdaddr == ep->bdaddr
*
* Two cases:
*
* 1) We do not have connection descriptor. This is simple. Just
* create new connection and submit Add_SCO_Connection command.
*
* 2) We do have connection descriptor. We need to check the state.
*
* 2.1) NG_HCI_CON_W4_LP_CON_RSP means we in the middle of accepting
* connection from the remote unit. This is a race condition and
* we will ignore the request.
*
* 2.2) NG_HCI_CON_W4_CONN_COMPLETE means upper layer already requested
* connection or we just accepted it.
*/
LIST_FOREACH(sco_con, &unit->con_list, next)
if (sco_con->link_type == NG_HCI_LINK_SCO &&
(sco_con->state == NG_HCI_CON_W4_LP_CON_RSP ||
sco_con->state == NG_HCI_CON_W4_CONN_COMPLETE) &&
bcmp(&sco_con->bdaddr, &ep->bdaddr, sizeof(bdaddr_t)) == 0)
break;
if (sco_con != NULL) {
switch (sco_con->state) {
case NG_HCI_CON_W4_LP_CON_RSP: /* XXX */
error = EALREADY;
break;
case NG_HCI_CON_W4_CONN_COMPLETE:
sco_con->flags |= NG_HCI_CON_NOTIFY_SCO;
break;
default:
panic(
"%s: %s - Inavalid connection state=%d\n",
__func__, NG_NODE_NAME(unit->node),
sco_con->state);
break;
}
goto out;
}
/*
* If we got here then we need to create new SCO connection descriptor
* and submit HCI command.
*/
sco_con = ng_hci_new_con(unit, NG_HCI_LINK_SCO);
if (sco_con == NULL) {
error = ENOMEM;
goto out;
}
bcopy(&ep->bdaddr, &sco_con->bdaddr, sizeof(sco_con->bdaddr));
/*
* Create HCI command
*/
MGETHDR(m, M_DONTWAIT, MT_DATA);
if (m == NULL) {
ng_hci_free_con(sco_con);
error = ENOBUFS;
goto out;
}
m->m_pkthdr.len = m->m_len = sizeof(*req);
req = mtod(m, struct sco_con_req *);
req->hdr.type = NG_HCI_CMD_PKT;
req->hdr.length = sizeof(req->cp);
req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_ADD_SCO_CON));
req->cp.con_handle = htole16(acl_con->con_handle);
req->cp.pkt_type = NG_HCI_PKT_HV1;
if (unit->features[1] & NG_HCI_LMP_HV2_PKT)
req->cp.pkt_type |= NG_HCI_PKT_HV2;
if (unit->features[1] & NG_HCI_LMP_HV3_PKT)
req->cp.pkt_type |= NG_HCI_PKT_HV3;
req->cp.pkt_type &= unit->packet_mask;
if ((req->cp.pkt_type & (NG_HCI_PKT_HV1|
NG_HCI_PKT_HV2|
NG_HCI_PKT_HV3)) == 0)
req->cp.pkt_type = NG_HCI_PKT_HV1;
req->cp.pkt_type = htole16(req->cp.pkt_type);
/*
* Adust connection state
*/
sco_con->flags |= NG_HCI_CON_NOTIFY_SCO;
sco_con->state = NG_HCI_CON_W4_CONN_COMPLETE;
ng_hci_con_timeout(sco_con);
/*
* Queue and send HCI command
*/
NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
error = ng_hci_send_command(unit);
out:
NG_FREE_ITEM(item);
return (error);
} /* ng_hci_lp_sco_con_req */
static int
ng_hci_lp_acl_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
{
struct acl_con_req {
ng_hci_cmd_pkt_t hdr;
ng_hci_create_con_cp cp;
} __attribute__ ((packed)) *req = NULL;
ng_hci_lp_con_req_ep *ep = NULL;
ng_hci_unit_con_p con = NULL;
ng_hci_neighbor_t *n = NULL;
struct mbuf *m = NULL;
int error = 0;
ep = (ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data);
/*
* Only one ACL connection can exist between each pair of units.
* So try to find ACL connection descriptor (in any state) that
* has requested remote BD_ADDR.
*
* Two cases:
*
* 1) We do not have connection to the remote unit. This is simple.
* Just create new connection descriptor and send HCI command to
* create new connection.
*
* 2) We do have connection descriptor. We need to check connection
* state:
*
* 2.1) NG_HCI_CON_W4_LP_CON_RSP means that we are in the middle of
* accepting connection from the remote unit. This is a race
* condition. We will ignore this message.
*
* 2.2) NG_HCI_CON_W4_CONN_COMPLETE means that upper layer already
* requested connection or we just accepted it. In any case
* all we need to do here is set appropriate notification bit
* and wait.
*
* 2.3) NG_HCI_CON_OPEN means connection is open. Just reply back
* and let upper layer know that we have connection already.
*/
con = ng_hci_con_by_bdaddr(unit, &ep->bdaddr, NG_HCI_LINK_ACL);
if (con != NULL) {
switch (con->state) {
case NG_HCI_CON_W4_LP_CON_RSP: /* XXX */
error = EALREADY;
break;
case NG_HCI_CON_W4_CONN_COMPLETE:
if (hook == unit->acl)
con->flags |= NG_HCI_CON_NOTIFY_ACL;
else
con->flags |= NG_HCI_CON_NOTIFY_SCO;
break;
case NG_HCI_CON_OPEN: {
struct ng_mesg *msg = NULL;
ng_hci_lp_con_cfm_ep *cfm = NULL;
if (hook != NULL && NG_HOOK_IS_VALID(hook)) {
NGI_GET_MSG(item, msg);
NG_FREE_MSG(msg);
NG_MKMESSAGE(msg, NGM_HCI_COOKIE,
NGM_HCI_LP_CON_CFM, sizeof(*cfm),
M_NOWAIT);
if (msg != NULL) {
cfm = (ng_hci_lp_con_cfm_ep *)msg->data;
cfm->status = 0;
cfm->link_type = con->link_type;
cfm->con_handle = con->con_handle;
bcopy(&con->bdaddr, &cfm->bdaddr,
sizeof(cfm->bdaddr));
/*
* This will forward item back to
* sender and set item to NULL
*/
_NGI_MSG(item) = msg;
NG_FWD_ITEM_HOOK(error, item, hook);
} else
error = ENOMEM;
} else
NG_HCI_INFO(
"%s: %s - Source hook is not valid, hook=%p\n",
__func__, NG_NODE_NAME(unit->node),
hook);
} break;
default:
panic(
"%s: %s - Invalid connection state=%d\n",
__func__, NG_NODE_NAME(unit->node), con->state);
break;
}
goto out;
}
/*
* If we got here then we need to create new ACL connection descriptor
* and submit HCI command. First create new connection desriptor, set
* bdaddr and notification flags.
*/
con = ng_hci_new_con(unit, NG_HCI_LINK_ACL);
if (con == NULL) {
error = ENOMEM;
goto out;
}
bcopy(&ep->bdaddr, &con->bdaddr, sizeof(con->bdaddr));
/*
* Create HCI command
*/
MGETHDR(m, M_DONTWAIT, MT_DATA);
if (m == NULL) {
ng_hci_free_con(con);
error = ENOBUFS;
goto out;
}
m->m_pkthdr.len = m->m_len = sizeof(*req);
req = mtod(m, struct acl_con_req *);
req->hdr.type = NG_HCI_CMD_PKT;
req->hdr.length = sizeof(req->cp);
req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_CREATE_CON));
bcopy(&ep->bdaddr, &req->cp.bdaddr, sizeof(req->cp.bdaddr));
req->cp.pkt_type = (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1);
if (unit->features[0] & NG_HCI_LMP_3SLOT)
req->cp.pkt_type |= (NG_HCI_PKT_DM3|NG_HCI_PKT_DH3);
if (unit->features[0] & NG_HCI_LMP_5SLOT)
req->cp.pkt_type |= (NG_HCI_PKT_DM5|NG_HCI_PKT_DH5);
req->cp.pkt_type &= unit->packet_mask;
if ((req->cp.pkt_type & (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1|
NG_HCI_PKT_DM3|NG_HCI_PKT_DH3|
NG_HCI_PKT_DM5|NG_HCI_PKT_DH5)) == 0)
req->cp.pkt_type = (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1);
req->cp.pkt_type = htole16(req->cp.pkt_type);
if ((unit->features[0] & NG_HCI_LMP_SWITCH) && unit->role_switch)
req->cp.accept_role_switch = 1;
else
req->cp.accept_role_switch = 0;
/*
* We may speed up connect by specifying valid parameters.
* So check the neighbor cache.
*/
n = ng_hci_get_neighbor(unit, &ep->bdaddr);
if (n == NULL) {
req->cp.page_scan_rep_mode = 0;
req->cp.page_scan_mode = 0;
req->cp.clock_offset = 0;
} else {
req->cp.page_scan_rep_mode = n->page_scan_rep_mode;
req->cp.page_scan_mode = n->page_scan_mode;
req->cp.clock_offset = htole16(n->clock_offset);
}
/*
* Adust connection state
*/
if (hook == unit->acl)
con->flags |= NG_HCI_CON_NOTIFY_ACL;
else
con->flags |= NG_HCI_CON_NOTIFY_SCO;
con->state = NG_HCI_CON_W4_CONN_COMPLETE;
ng_hci_con_timeout(con);
/*
* Queue and send HCI command
*/
NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
error = ng_hci_send_command(unit);
out:
if (item != NULL)
NG_FREE_ITEM(item);
return (error);
} /* ng_hci_lp_acl_con_req */
static int
ng_hci_lp_le_con_req(ng_hci_unit_p unit, item_p item, hook_p hook, int link_type)
{
struct acl_con_req {
ng_hci_cmd_pkt_t hdr;
ng_hci_le_create_connection_cp cp;
} __attribute__ ((packed)) *req = NULL;
ng_hci_lp_con_req_ep *ep = NULL;
ng_hci_unit_con_p con = NULL;
struct mbuf *m = NULL;
int error = 0;
ep = (ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data);
if((link_type != NG_HCI_LINK_LE_PUBLIC)&&
(link_type != NG_HCI_LINK_LE_RANDOM)){
printf("%s: Link type %d Cannot be here \n", __func__,
link_type);
}
/*
* Only one ACL connection can exist between each pair of units.
* So try to find ACL connection descriptor (in any state) that
* has requested remote BD_ADDR.
*
* Two cases:
*
* 1) We do not have connection to the remote unit. This is simple.
* Just create new connection descriptor and send HCI command to
* create new connection.
*
* 2) We do have connection descriptor. We need to check connection
* state:
*
* 2.1) NG_HCI_CON_W4_LP_CON_RSP means that we are in the middle of
* accepting connection from the remote unit. This is a race
* condition. We will ignore this message.
*
* 2.2) NG_HCI_CON_W4_CONN_COMPLETE means that upper layer already
* requested connection or we just accepted it. In any case
* all we need to do here is set appropriate notification bit
* and wait.
*
* 2.3) NG_HCI_CON_OPEN means connection is open. Just reply back
* and let upper layer know that we have connection already.
*/
con = ng_hci_con_by_bdaddr(unit, &ep->bdaddr, link_type);
if (con != NULL) {
switch (con->state) {
case NG_HCI_CON_W4_LP_CON_RSP: /* XXX */
error = EALREADY;
break;
case NG_HCI_CON_W4_CONN_COMPLETE:
if (hook != unit->sco)
con->flags |= NG_HCI_CON_NOTIFY_ACL;
else
con->flags |= NG_HCI_CON_NOTIFY_SCO;
break;
case NG_HCI_CON_OPEN: {
struct ng_mesg *msg = NULL;
ng_hci_lp_con_cfm_ep *cfm = NULL;
if (hook != NULL && NG_HOOK_IS_VALID(hook)) {
NGI_GET_MSG(item, msg);
NG_FREE_MSG(msg);
NG_MKMESSAGE(msg, NGM_HCI_COOKIE,
NGM_HCI_LP_CON_CFM, sizeof(*cfm),
M_NOWAIT);
if (msg != NULL) {
cfm = (ng_hci_lp_con_cfm_ep *)msg->data;
cfm->status = 0;
cfm->link_type = con->link_type;
cfm->con_handle = con->con_handle;
bcopy(&con->bdaddr, &cfm->bdaddr,
sizeof(cfm->bdaddr));
/*
* This will forward item back to
* sender and set item to NULL
*/
_NGI_MSG(item) = msg;
NG_FWD_ITEM_HOOK(error, item, hook);
} else
error = ENOMEM;
} else
NG_HCI_INFO(
"%s: %s - Source hook is not valid, hook=%p\n",
__func__, NG_NODE_NAME(unit->node),
hook);
} break;
default:
panic(
"%s: %s - Invalid connection state=%d\n",
__func__, NG_NODE_NAME(unit->node), con->state);
break;
}
goto out;
}
/*
* If we got here then we need to create new ACL connection descriptor
* and submit HCI command. First create new connection desriptor, set
* bdaddr and notification flags.
*/
con = ng_hci_new_con(unit, link_type);
if (con == NULL) {
error = ENOMEM;
goto out;
}
bcopy(&ep->bdaddr, &con->bdaddr, sizeof(con->bdaddr));
/*
* Create HCI command
*/
MGETHDR(m, M_NOWAIT, MT_DATA);
if (m == NULL) {
ng_hci_free_con(con);
error = ENOBUFS;
goto out;
}
m->m_pkthdr.len = m->m_len = sizeof(*req);
req = mtod(m, struct acl_con_req *);
req->hdr.type = NG_HCI_CMD_PKT;
req->hdr.length = sizeof(req->cp);
req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_CREATE_CONNECTION));
bcopy(&ep->bdaddr, &req->cp.peer_addr, sizeof(req->cp.peer_addr));
req->cp.own_address_type = 0;
req->cp.peer_addr_type = (link_type == NG_HCI_LINK_LE_RANDOM)? 1:0;
req->cp.scan_interval = htole16(4);
req->cp.scan_window = htole16(4);
req->cp.filter_policy = 0;
req->cp.conn_interval_min = htole16(0xf);
req->cp.conn_interval_max = htole16(0xf);
req->cp.conn_latency = htole16(0);
req->cp.supervision_timeout = htole16(0xc80);
req->cp.min_ce_length = htole16(1);
req->cp.max_ce_length = htole16(1);
/*
* Adust connection state
*/
if (hook != unit->sco)
con->flags |= NG_HCI_CON_NOTIFY_ACL;
else
con->flags |= NG_HCI_CON_NOTIFY_SCO;
con->state = NG_HCI_CON_W4_CONN_COMPLETE;
ng_hci_con_timeout(con);
/*
* Queue and send HCI command
*/
NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
error = ng_hci_send_command(unit);
out:
if (item != NULL)
NG_FREE_ITEM(item);
return (error);
} /* ng_hci_lp_acl_con_req */
/*
* Incoming messages get passed up to the control socket.
* Unless they are for us specifically (socket_type)
*/
static int
ngs_rcvmsg(node_p node, item_p item, hook_p lasthook)
{
struct ngsock *const priv = NG_NODE_PRIVATE(node);
struct ngpcb *const pcbp = priv->ctlsock;
struct socket *so;
struct sockaddr_ng addr;
struct ng_mesg *msg;
struct mbuf *m;
ng_ID_t retaddr = NGI_RETADDR(item);
int addrlen;
int error = 0;
NGI_GET_MSG(item, msg);
NG_FREE_ITEM(item);
/*
* Only allow mesgs to be passed if we have the control socket.
* Data sockets can only support the generic messages.
*/
if (pcbp == NULL) {
TRAP_ERROR;
NG_FREE_MSG(msg);
return (EINVAL);
}
so = pcbp->ng_socket;
#ifdef TRACE_MESSAGES
kprintf("[%x]:---------->[socket]: c=<%d>cmd=%x(%s) f=%x #%d\n",
retaddr,
msg->header.typecookie,
msg->header.cmd,
msg->header.cmdstr,
msg->header.flags,
msg->header.token);
#endif
if (msg->header.typecookie == NGM_SOCKET_COOKIE) {
switch (msg->header.cmd) {
case NGM_SOCK_CMD_NOLINGER:
priv->flags |= NGS_FLAG_NOLINGER;
break;
case NGM_SOCK_CMD_LINGER:
priv->flags &= ~NGS_FLAG_NOLINGER;
break;
default:
error = EINVAL; /* unknown command */
}
/* Free the message and return. */
NG_FREE_MSG(msg);
return (error);
}
/* Get the return address into a sockaddr. */
bzero(&addr, sizeof(addr));
addr.sg_len = sizeof(addr);
addr.sg_family = AF_NETGRAPH;
addrlen = ksnprintf((char *)&addr.sg_data, sizeof(addr.sg_data),
"[%x]:", retaddr);
if (addrlen < 0 || addrlen > sizeof(addr.sg_data)) {
kprintf("%s: ksnprintf([%x]) failed - %d\n", __func__, retaddr,
addrlen);
NG_FREE_MSG(msg);
return (EINVAL);
}
/* Copy the message itself into an mbuf chain. */
m = m_devget((caddr_t)msg, sizeof(struct ng_mesg) + msg->header.arglen,
0, NULL, NULL);
/*
* Here we free the message. We need to do that
* regardless of whether we got mbufs.
*/
NG_FREE_MSG(msg);
if (m == NULL) {
TRAP_ERROR;
return (ENOBUFS);
}
/* Send it up to the socket. */
if (sbappendaddr((struct sockbuf *)&so->so_rcv, (struct sockaddr *)&addr, m, NULL) == 0) {
TRAP_ERROR;
m_freem(m);
return (ENOBUFS);
}
sorwakeup(so);
return (error);
}
/*
* Receive data on a hook
*
* If data comes in the right link send a copy out right2left, and then
* send the original onwards out through the left link.
* Do the opposite for data coming in from the left link.
* Data coming in right2left or left2right is forwarded
* on through the appropriate destination hook as if it had come
* from the other side.
*/
static int
ngt_rcvdata(hook_p hook, item_p item)
{
const sc_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
struct hookinfo *const hinfo = NG_HOOK_PRIVATE(hook);
struct hookinfo *dest;
struct hookinfo *dup;
int error = 0;
struct mbuf *m;
meta_p meta;
m = NGI_M(item);
meta = NGI_META(item); /* leave these owned by the item */
/* Which hook? */
if (hinfo == &sc->left) {
dup = &sc->left2right;
dest = &sc->right;
} else if (hinfo == &sc->right) {
dup = &sc->right2left;
dest = &sc->left;
} else if (hinfo == &sc->right2left) {
dup = NULL;
dest = &sc->right;
} else if (hinfo == &sc->left2right) {
dup = NULL;
dest = &sc->left;
} else {
panic("%s: no hook!", __func__);
#ifdef RESTARTABLE_PANICS
return(EINVAL);
#endif
}
/* Update stats on incoming hook */
hinfo->stats.inOctets += m->m_pkthdr.len;
hinfo->stats.inFrames++;
/*
* Don't make a copy if only the dup hook exists.
*/
if ((dup && dup->hook) && (dest->hook == NULL)) {
dest = dup;
dup = NULL;
}
/* Duplicate packet and meta info if requried */
if (dup != NULL) {
struct mbuf *m2;
meta_p meta2;
/* Copy packet (failure will not stop the original)*/
m2 = m_dup(m, M_NOWAIT);
if (m2) {
/* Copy meta info */
/* If we can't get a copy, tough.. */
if (meta != NULL) {
meta2 = ng_copy_meta(meta);
} else
meta2 = NULL;
/* Deliver duplicate */
dup->stats.outOctets += m->m_pkthdr.len;
dup->stats.outFrames++;
NG_SEND_DATA(error, dup->hook, m2, meta2);
}
}
/* Deliver frame out destination hook */
dest->stats.outOctets += m->m_pkthdr.len;
dest->stats.outFrames++;
if (dest->hook)
NG_FWD_ITEM_HOOK(error, item, dest->hook);
else
NG_FREE_ITEM(item);
return (0);
}
/*
* receive data, and use it to update our status.
* Anything coming in on the debug port is discarded.
*/
static int
nglmi_rcvdata(hook_p hook, item_p item)
{
sc_p sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
const u_char *data;
unsigned short dlci;
u_short packetlen;
int resptype_seen = 0;
struct mbuf *m;
NGI_GET_M(item, m);
NG_FREE_ITEM(item);
if (NG_HOOK_PRIVATE(hook) == NULL) {
goto drop;
}
packetlen = m->m_len;
/* XXX what if it's more than 1 mbuf? */
if ((packetlen > MHLEN) && !(m->m_flags & M_EXT)) {
log(LOG_WARNING, "nglmi: packetlen (%d) too big\n", packetlen);
goto drop;
}
if (m->m_len < packetlen && (m = m_pullup(m, packetlen)) == NULL) {
log(LOG_WARNING,
"nglmi: m_pullup failed for %d bytes\n", packetlen);
return (0);
}
if (nglmi_checkdata(hook, m) == 0)
return (0);
/* pass the first 4 bytes (already checked in the nglmi_checkdata()) */
data = mtod(m, const u_char *);
STEPBY(4);
/* Now check if there is a 'locking shift'. This is only seen in
* Annex D frames. don't bother checking, we already did that. Don't
* increment immediatly as it might not be there. */
if (ANNEXD(sc))
STEPBY(1);
/* If we get this far we should consider that it is a legitimate
* frame and we know what it is. */
if (sc->flags & SCF_AUTO) {
/* note the hook that this valid channel came from and drop
* out of auto probe mode. */
if (ANNEXA(sc))
sc->protoname = NAME_ANNEXA;
else if (ANNEXD(sc))
sc->protoname = NAME_ANNEXD;
else if (GROUP4(sc))
sc->protoname = NAME_GROUP4;
else {
log(LOG_ERR, "nglmi: No known type\n");
goto drop;
}
sc->lmi_channel = hook;
sc->flags &= ~SCF_AUTO;
log(LOG_INFO, "nglmi: auto-detected %s LMI on DLCI %d\n",
sc->protoname, hook == sc->lmi_channel0 ? 0 : 1023);
}
/* While there is more data in the status packet, keep processing
* status items. First make sure there is enough data for the
* segment descriptor's length field. */
while (packetlen >= 2) {
u_int segtype = data[0];
u_int segsize = data[1];
/* Now that we know how long it claims to be, make sure
* there is enough data for the next seg. */
if (packetlen < segsize + 2)
break;
switch (segtype) {
case 0x01:
case 0x51:
if (resptype_seen) {
log(LOG_WARNING, "nglmi: dup MSGTYPE\n");
goto nextIE;
}
resptype_seen++;
/* The remote end tells us what kind of response
* this is. Only expect a type 0 or 1. if we are a
* full status, invalidate a few DLCIs just to see
* that they are still ok. */
if (segsize != 1)
goto nextIE;
switch (data[2]) {
case 1:
/* partial status, do no extra processing */
break;
case 0:
{
int count = 0;
int idx = sc->invalidx;
for (count = 0; count < 10; count++) {
if (idx > MAXDLCI)
idx = 0;
if (sc->dlci_state[idx] == DLCI_UP)
sc->dlci_state[idx] = DLCI_DOWN;
idx++;
}
sc->invalidx = idx;
/* we got and we wanted one. relax
* now.. but don't reset to 0 if it
* was unrequested. */
if (sc->livs > sc->liv_per_full)
sc->livs = 0;
break;
}
}
break;
case 0x03:
case 0x53:
/* The remote tells us what it thinks the sequence
* numbers are. If it's not size 2, it must be a
* duplicate to have gotten this far, skip it. */
if (segsize != 2)
goto nextIE;
sc->remote_seq = data[2];
if (sc->local_seq == data[3]) {
sc->local_seq++;
sc->seq_retries = 0;
/* Note that all 3 Frame protocols seem to
* not like 0 as a sequence number. */
if (sc->local_seq == 0)
sc->local_seq = 1;
}
break;
case 0x07:
case 0x57:
/* The remote tells us about a DLCI that it knows
* about. There may be many of these in a single
* status response */
switch (segsize) {
case 6:/* only on 'group of 4' */
dlci = ((u_short) data[2] & 0xff) << 8;
dlci |= (data[3] & 0xff);
if ((dlci < 1024) && (dlci > 0)) {
/* XXX */
}
break;
case 3:
dlci = ((u_short) data[2] & 0x3f) << 4;
dlci |= ((data[3] & 0x78) >> 3);
if ((dlci < 1024) && (dlci > 0)) {
/* set up the bottom half of the
* support for that dlci if it's not
* already been done */
/* store this information somewhere */
}
break;
default:
goto nextIE;
}
if (sc->dlci_state[dlci] != DLCI_UP) {
/* bring new DLCI to life */
/* may do more here some day */
if (sc->dlci_state[dlci] != DLCI_DOWN)
log(LOG_INFO,
"nglmi: DLCI %d became active\n",
dlci);
sc->dlci_state[dlci] = DLCI_UP;
}
break;
}
nextIE:
STEPBY(segsize + 2);
}
NG_FREE_M(m);
return (0);
drop:
NG_FREE_M(m);
return (EINVAL);
}
/**
* Handle data on netgraph hooks.
* Frames processing is deferred to a taskqueue because this might
* be called with non-sleepable locks held and code paths inside
* the virtual switch might sleep.
*/
static int ng_vboxnetflt_rcvdata(hook_p hook, item_p item)
{
const node_p node = NG_HOOK_NODE(hook);
PVBOXNETFLTINS pThis = NG_NODE_PRIVATE(node);
struct ifnet *ifp = pThis->u.s.ifp;
struct mbuf *m;
struct m_tag *mtag;
bool fActive;
VBOXCURVNET_SET(ifp->if_vnet);
fActive = vboxNetFltTryRetainBusyActive(pThis);
NGI_GET_M(item, m);
NG_FREE_ITEM(item);
/* Locate tag to see if processing should be skipped for this frame */
mtag = m_tag_locate(m, MTAG_VBOX, PACKET_TAG_VBOX, NULL);
if (mtag != NULL)
{
m_tag_unlink(m, mtag);
m_tag_free(mtag);
}
/*
* Handle incoming hook. This is connected to the
* input path of the interface, thus handling incoming frames.
*/
if (pThis->u.s.input == hook)
{
if (mtag != NULL || !fActive)
{
ether_demux(ifp, m);
if (fActive)
vboxNetFltRelease(pThis, true /*fBusy*/);
VBOXCURVNET_RESTORE();
return (0);
}
mtx_lock_spin(&pThis->u.s.inq.ifq_mtx);
_IF_ENQUEUE(&pThis->u.s.inq, m);
mtx_unlock_spin(&pThis->u.s.inq.ifq_mtx);
taskqueue_enqueue_fast(taskqueue_fast, &pThis->u.s.tskin);
}
/*
* Handle mbufs on the outgoing hook, frames going to the interface
*/
else if (pThis->u.s.output == hook)
{
if (mtag != NULL || !fActive)
{
int rc = ether_output_frame(ifp, m);
if (fActive)
vboxNetFltRelease(pThis, true /*fBusy*/);
VBOXCURVNET_RESTORE();
return rc;
}
mtx_lock_spin(&pThis->u.s.outq.ifq_mtx);
_IF_ENQUEUE(&pThis->u.s.outq, m);
mtx_unlock_spin(&pThis->u.s.outq.ifq_mtx);
taskqueue_enqueue_fast(taskqueue_fast, &pThis->u.s.tskout);
}
else
{
m_freem(m);
}
if (fActive)
vboxNetFltRelease(pThis, true /*fBusy*/);
VBOXCURVNET_RESTORE();
return (0);
}
/*
* Receive data packet
*/
static int
ngfrm_rcvdata(hook_p hook, item_p item)
{
struct ctxinfo *const ctxp = NG_HOOK_PRIVATE(hook);
int error = 0;
int dlci;
sc_p sc;
int alen;
char *data;
struct mbuf *m;
/* Data doesn't come in from just anywhere (e.g debug hook) */
if (ctxp == NULL) {
error = ENETDOWN;
goto bad;
}
/* If coming from downstream, decode it to a channel */
dlci = ctxp->dlci;
if (dlci == -1)
return (ngfrm_decode(NG_HOOK_NODE(hook), item));
NGI_GET_M(item, m);
/* Derive the softc we will need */
sc = NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
/* If there is no live channel, throw it away */
if ((sc->downstream.hook == NULL)
|| ((ctxp->flags & CHAN_ACTIVE) == 0)) {
error = ENETDOWN;
goto bad;
}
/* Store the DLCI on the front of the packet */
alen = sc->addrlen;
if (alen == 0)
alen = 2; /* default value for transmit */
M_PREPEND(m, alen, M_DONTWAIT);
if (m == NULL) {
error = ENOBUFS;
goto bad;
}
data = mtod(m, char *);
/*
* Shift the lowest bits into the address field untill we are done.
* First byte is MSBits of addr so work backwards.
*/
switch (alen) {
case 2:
data[0] = data[1] = '\0';
SHIFTOUT(makeup + 1, data[1], dlci);
SHIFTOUT(makeup + 0, data[0], dlci);
data[1] |= BYTEX_EA;
break;
case 3:
data[0] = data[1] = data[2] = '\0';
SHIFTOUT(makeup + 3, data[2], dlci); /* 3 and 2 is correct */
SHIFTOUT(makeup + 1, data[1], dlci);
SHIFTOUT(makeup + 0, data[0], dlci);
data[2] |= BYTEX_EA;
break;
case 4:
data[0] = data[1] = data[2] = data[3] = '\0';
SHIFTOUT(makeup + 3, data[3], dlci);
SHIFTOUT(makeup + 2, data[2], dlci);
SHIFTOUT(makeup + 1, data[1], dlci);
SHIFTOUT(makeup + 0, data[0], dlci);
data[3] |= BYTEX_EA;
break;
default:
panic(__func__);
}
/* Send it */
NG_FWD_NEW_DATA(error, item, sc->downstream.hook, m);
return (error);
bad:
NG_FREE_ITEM(item);
NG_FREE_M(m);
return (error);
}
static int
ng_bt3c_rcvmsg(node_p node, item_p item, hook_p lasthook)
{
bt3c_softc_p sc = (bt3c_softc_p) NG_NODE_PRIVATE(node);
struct ng_mesg *msg = NULL, *rsp = NULL;
int error = 0;
if (sc == NULL) {
NG_FREE_ITEM(item);
return (EHOSTDOWN);
}
NGI_GET_MSG(item, msg);
switch (msg->header.typecookie) {
case NGM_GENERIC_COOKIE:
switch (msg->header.cmd) {
case NGM_TEXT_STATUS:
NG_MKRESPONSE(rsp, msg, NG_TEXTRESPONSE, M_NOWAIT);
if (rsp == NULL)
error = ENOMEM;
else
snprintf(rsp->data, NG_TEXTRESPONSE,
"Hook: %s\n" \
"Flags: %#x\n" \
"Debug: %d\n" \
"State: %d\n" \
"IncmQ: [len:%d,max:%d]\n" \
"OutgQ: [len:%d,max:%d]\n",
(sc->hook != NULL)? NG_BT3C_HOOK : "",
sc->flags,
sc->debug,
sc->state,
_IF_QLEN(&sc->inq), /* XXX */
sc->inq.ifq_maxlen, /* XXX */
_IF_QLEN(&sc->outq), /* XXX */
sc->outq.ifq_maxlen /* XXX */
);
break;
default:
error = EINVAL;
break;
}
break;
case NGM_BT3C_COOKIE:
switch (msg->header.cmd) {
case NGM_BT3C_NODE_GET_STATE:
NG_MKRESPONSE(rsp, msg, sizeof(ng_bt3c_node_state_ep),
M_NOWAIT);
if (rsp == NULL)
error = ENOMEM;
else
*((ng_bt3c_node_state_ep *)(rsp->data)) =
sc->state;
break;
case NGM_BT3C_NODE_SET_DEBUG:
if (msg->header.arglen != sizeof(ng_bt3c_node_debug_ep))
error = EMSGSIZE;
else
sc->debug =
*((ng_bt3c_node_debug_ep *)(msg->data));
break;
case NGM_BT3C_NODE_GET_DEBUG:
NG_MKRESPONSE(rsp, msg, sizeof(ng_bt3c_node_debug_ep),
M_NOWAIT);
if (rsp == NULL)
error = ENOMEM;
else
*((ng_bt3c_node_debug_ep *)(rsp->data)) =
sc->debug;
break;
case NGM_BT3C_NODE_GET_QLEN:
NG_MKRESPONSE(rsp, msg, sizeof(ng_bt3c_node_qlen_ep),
M_NOWAIT);
if (rsp == NULL) {
error = ENOMEM;
break;
}
switch (((ng_bt3c_node_qlen_ep *)(msg->data))->queue) {
case NGM_BT3C_NODE_IN_QUEUE:
((ng_bt3c_node_qlen_ep *)(rsp->data))->queue =
NGM_BT3C_NODE_IN_QUEUE;
((ng_bt3c_node_qlen_ep *)(rsp->data))->qlen =
sc->inq.ifq_maxlen;
break;
case NGM_BT3C_NODE_OUT_QUEUE:
((ng_bt3c_node_qlen_ep *)(rsp->data))->queue =
NGM_BT3C_NODE_OUT_QUEUE;
((ng_bt3c_node_qlen_ep *)(rsp->data))->qlen =
sc->outq.ifq_maxlen;
break;
default:
NG_FREE_MSG(rsp);
error = EINVAL;
break;
}
break;
case NGM_BT3C_NODE_SET_QLEN:
if (msg->header.arglen != sizeof(ng_bt3c_node_qlen_ep)){
error = EMSGSIZE;
break;
}
if (((ng_bt3c_node_qlen_ep *)(msg->data))->qlen <= 0) {
error = EINVAL;
break;
}
switch (((ng_bt3c_node_qlen_ep *)(msg->data))->queue) {
case NGM_BT3C_NODE_IN_QUEUE:
sc->inq.ifq_maxlen = ((ng_bt3c_node_qlen_ep *)
(msg->data))->qlen; /* XXX */
break;
case NGM_BT3C_NODE_OUT_QUEUE:
sc->outq.ifq_maxlen = ((ng_bt3c_node_qlen_ep *)
(msg->data))->qlen; /* XXX */
break;
default:
error = EINVAL;
break;
}
break;
case NGM_BT3C_NODE_GET_STAT:
NG_MKRESPONSE(rsp, msg, sizeof(ng_bt3c_node_stat_ep),
M_NOWAIT);
if (rsp == NULL)
error = ENOMEM;
else
bcopy(&sc->stat, rsp->data,
sizeof(ng_bt3c_node_stat_ep));
break;
case NGM_BT3C_NODE_RESET_STAT:
NG_BT3C_STAT_RESET(sc->stat);
break;
case NGM_BT3C_NODE_DOWNLOAD_FIRMWARE:
if (msg->header.arglen <
sizeof(ng_bt3c_firmware_block_ep))
error = EMSGSIZE;
else
bt3c_download_firmware(sc, msg->data,
msg->header.arglen);
break;
default:
error = EINVAL;
break;
}
break;
default:
error = EINVAL;
break;
}
NG_RESPOND_MSG(error, node, item, rsp);
NG_FREE_MSG(msg);
return (error);
} /* ng_bt3c_rcvmsg */
