/* * Receive data */ static int nge_rcvdata(hook_p hook, item_p item) { int error; NG_FWD_ITEM_HOOK(error, item, hook); return (error); }
/* * 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); }
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 */
/* * 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 a control message */ static int ngt_rcvmsg(node_p node, item_p item, hook_p lasthook) { const sc_p sc = NG_NODE_PRIVATE(node); struct ng_mesg *resp = NULL; int error = 0; struct ng_mesg *msg; NGI_GET_MSG(item, msg); switch (msg->header.typecookie) { case NGM_TEE_COOKIE: switch (msg->header.cmd) { case NGM_TEE_GET_STATS: case NGM_TEE_CLR_STATS: case NGM_TEE_GETCLR_STATS: { struct ng_tee_stats *stats; if (msg->header.cmd != NGM_TEE_CLR_STATS) { NG_MKRESPONSE(resp, msg, sizeof(*stats), M_NOWAIT); if (resp == NULL) { error = ENOMEM; goto done; } stats = (struct ng_tee_stats *)resp->data; bcopy(&sc->right.stats, &stats->right, sizeof(stats->right)); bcopy(&sc->left.stats, &stats->left, sizeof(stats->left)); bcopy(&sc->right2left.stats, &stats->right2left, sizeof(stats->right2left)); bcopy(&sc->left2right.stats, &stats->left2right, sizeof(stats->left2right)); } if (msg->header.cmd != NGM_TEE_GET_STATS) { bzero(&sc->right.stats, sizeof(sc->right.stats)); bzero(&sc->left.stats, sizeof(sc->left.stats)); bzero(&sc->right2left.stats, sizeof(sc->right2left.stats)); bzero(&sc->left2right.stats, sizeof(sc->left2right.stats)); } break; } default: error = EINVAL; break; } break; case NGM_FLOW_COOKIE: if (lasthook) { if (lasthook == sc->left.hook) { if (sc->right.hook) { NGI_MSG(item) = msg; NG_FWD_ITEM_HOOK(error, item, sc->right.hook); return (error); } } else { if (sc->left.hook) { NGI_MSG(item) = msg; NG_FWD_ITEM_HOOK(error, item, sc->left.hook); return (error); } } } break; default: error = EINVAL; break; } done: NG_RESPOND_MSG(error, node, item, resp); NG_FREE_MSG(msg); return (error); }
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 */