Index: head/sys/netgraph/bluetooth/hci/ng_hci_cmds.c =================================================================== --- head/sys/netgraph/bluetooth/hci/ng_hci_cmds.c +++ head/sys/netgraph/bluetooth/hci/ng_hci_cmds.c @@ -71,11 +71,15 @@ (ng_hci_unit_p, u_int16_t, struct mbuf *, struct mbuf *); static int process_testing_params (ng_hci_unit_p, u_int16_t, struct mbuf *, struct mbuf *); +static int process_le_params + (ng_hci_unit_p, u_int16_t, struct mbuf *, struct mbuf *); static int process_link_control_status (ng_hci_unit_p, ng_hci_command_status_ep *, struct mbuf *); static int process_link_policy_status (ng_hci_unit_p, ng_hci_command_status_ep *, struct mbuf *); +static int process_le_status + (ng_hci_unit_p, ng_hci_command_status_ep *, struct mbuf *); /* * Send HCI command to the driver. @@ -222,7 +226,10 @@ error = process_testing_params(unit, NG_HCI_OCF(ep->opcode), cp, e); break; - + case NG_HCI_OGF_LE: + error = process_le_params(unit, + NG_HCI_OCF(ep->opcode), cp, e); + break; case NG_HCI_OGF_BT_LOGO: case NG_HCI_OGF_VENDOR: NG_FREE_M(cp); @@ -294,7 +301,9 @@ case NG_HCI_OGF_LINK_POLICY: error = process_link_policy_status(unit, ep, cp); break; - + case NG_HCI_OGF_LE: + error = process_le_status(unit, ep, cp); + break; case NG_HCI_OGF_BT_LOGO: case NG_HCI_OGF_VENDOR: NG_FREE_M(cp); @@ -604,6 +613,8 @@ case NG_HCI_OCF_READ_LOCAL_NAME: case NG_HCI_OCF_READ_UNIT_CLASS: case NG_HCI_OCF_WRITE_UNIT_CLASS: + case NG_HCI_OCF_READ_LE_HOST_SUPPORTED: + case NG_HCI_OCF_WRITE_LE_HOST_SUPPORTED: /* These do not need post processing */ break; @@ -796,6 +807,132 @@ return (error); } /* process_testing_params */ +/* + * Process LE command return parameters + */ + +static int +process_le_params(ng_hci_unit_p unit, u_int16_t ocf, + struct mbuf *mcp, struct mbuf *mrp) +{ + int error = 0; + + switch (ocf){ + case NG_HCI_OCF_LE_SET_EVENT_MASK: + case NG_HCI_OCF_LE_READ_BUFFER_SIZE: + case NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES: + case NG_HCI_OCF_LE_SET_RANDOM_ADDRESS: + case NG_HCI_OCF_LE_SET_ADVERTISING_PARAMETERS: + case NG_HCI_OCF_LE_READ_ADVERTISING_CHANNEL_TX_POWER: + case NG_HCI_OCF_LE_SET_ADVERTISING_DATA: + case NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA: + case NG_HCI_OCF_LE_SET_ADVERTISE_ENABLE: + case NG_HCI_OCF_LE_SET_SCAN_PARAMETERS: + case NG_HCI_OCF_LE_SET_SCAN_ENABLE: + case NG_HCI_OCF_LE_CREATE_CONNECTION_CANCEL: + case NG_HCI_OCF_LE_CLEAR_WHITE_LIST: + case NG_HCI_OCF_LE_READ_WHITE_LIST_SIZE: + case NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST: + case NG_HCI_OCF_LE_REMOVE_DEVICE_FROM_WHITE_LIST: + case NG_HCI_OCF_LE_SET_HOST_CHANNEL_CLASSIFICATION: + case NG_HCI_OCF_LE_READ_CHANNEL_MAP: + case NG_HCI_OCF_LE_ENCRYPT: + case NG_HCI_OCF_LE_RAND: + case NG_HCI_OCF_LE_LONG_TERM_KEY_REQUEST_REPLY: + case NG_HCI_OCF_LE_LONG_TERM_KEY_REQUEST_NEGATIVE_REPLY: + case NG_HCI_OCF_LE_READ_SUPPORTED_STATUS: + case NG_HCI_OCF_LE_RECEIVER_TEST: + case NG_HCI_OCF_LE_TRANSMITTER_TEST: + case NG_HCI_OCF_LE_TEST_END: + + /* These do not need post processing */ + break; + case NG_HCI_OCF_LE_CREATE_CONNECTION: + case NG_HCI_OCF_LE_CONNECTION_UPDATE: + case NG_HCI_OCF_LE_READ_REMOTE_USED_FEATURES: + case NG_HCI_OCF_LE_START_ENCRYPTION: + + + default: + /* + * None of these command was supposed to generate + * Command_Complete event. Instead Command_Status event + * should have been generated and then appropriate event + * should have been sent to indicate the final result. + */ + + error = EINVAL; + break; + } + + NG_FREE_M(mcp); + NG_FREE_M(mrp); + + return (error); + +} + + + +static int +process_le_status(ng_hci_unit_p unit,ng_hci_command_status_ep *ep, + struct mbuf *mcp) +{ + int error = 0; + + switch (NG_HCI_OCF(ep->opcode)){ + case NG_HCI_OCF_LE_CREATE_CONNECTION: + case NG_HCI_OCF_LE_CONNECTION_UPDATE: + case NG_HCI_OCF_LE_READ_REMOTE_USED_FEATURES: + case NG_HCI_OCF_LE_START_ENCRYPTION: + + /* These do not need post processing */ + break; + + case NG_HCI_OCF_LE_SET_EVENT_MASK: + case NG_HCI_OCF_LE_READ_BUFFER_SIZE: + case NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES: + case NG_HCI_OCF_LE_SET_RANDOM_ADDRESS: + case NG_HCI_OCF_LE_SET_ADVERTISING_PARAMETERS: + case NG_HCI_OCF_LE_READ_ADVERTISING_CHANNEL_TX_POWER: + case NG_HCI_OCF_LE_SET_ADVERTISING_DATA: + case NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA: + case NG_HCI_OCF_LE_SET_ADVERTISE_ENABLE: + case NG_HCI_OCF_LE_SET_SCAN_PARAMETERS: + case NG_HCI_OCF_LE_SET_SCAN_ENABLE: + case NG_HCI_OCF_LE_CREATE_CONNECTION_CANCEL: + case NG_HCI_OCF_LE_CLEAR_WHITE_LIST: + case NG_HCI_OCF_LE_READ_WHITE_LIST_SIZE: + case NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST: + case NG_HCI_OCF_LE_REMOVE_DEVICE_FROM_WHITE_LIST: + case NG_HCI_OCF_LE_SET_HOST_CHANNEL_CLASSIFICATION: + case NG_HCI_OCF_LE_READ_CHANNEL_MAP: + case NG_HCI_OCF_LE_ENCRYPT: + case NG_HCI_OCF_LE_RAND: + case NG_HCI_OCF_LE_LONG_TERM_KEY_REQUEST_REPLY: + case NG_HCI_OCF_LE_LONG_TERM_KEY_REQUEST_NEGATIVE_REPLY: + case NG_HCI_OCF_LE_READ_SUPPORTED_STATUS: + case NG_HCI_OCF_LE_RECEIVER_TEST: + case NG_HCI_OCF_LE_TRANSMITTER_TEST: + case NG_HCI_OCF_LE_TEST_END: + + + default: + /* + * None of these command was supposed to generate + * Command_Stutus event. Command Complete instead. + */ + + error = EINVAL; + break; + } + + NG_FREE_M(mcp); + + return (error); + +} + /* * Process link control command status */ Index: head/sys/netgraph/bluetooth/hci/ng_hci_evnt.c =================================================================== --- head/sys/netgraph/bluetooth/hci/ng_hci_evnt.c +++ head/sys/netgraph/bluetooth/hci/ng_hci_evnt.c @@ -76,6 +76,7 @@ static int page_scan_rep_mode_change (ng_hci_unit_p, struct mbuf *); static int sync_con_queue (ng_hci_unit_p, ng_hci_unit_con_p, int); static int send_data_packets (ng_hci_unit_p, int, int); +static int le_event (ng_hci_unit_p, struct mbuf *); /* * Process HCI event packet @@ -121,6 +122,9 @@ /* These do not need post processing */ NG_FREE_M(event); break; + case NG_HCI_EVENT_LE: + error = le_event(unit, event); + break; case NG_HCI_EVENT_INQUIRY_RESULT: error = inquiry_result(unit, event); @@ -247,6 +251,7 @@ send_data_packets(ng_hci_unit_p unit, int link_type, int limit) { ng_hci_unit_con_p con = NULL, winner = NULL; + int reallink_type; item_p item = NULL; int min_pending, total_sent, sent, error, v; @@ -260,8 +265,11 @@ */ LIST_FOREACH(con, &unit->con_list, next) { - if (con->link_type != link_type) + reallink_type = (con->link_type == NG_HCI_LINK_SCO)? + NG_HCI_LINK_SCO: NG_HCI_LINK_ACL; + if (reallink_type != link_type){ continue; + } if (NG_BT_ITEMQ_LEN(&con->conq) == 0) continue; @@ -327,7 +335,6 @@ /* * Sync connection queue for the winner */ - sync_con_queue(unit, winner, sent); } @@ -346,7 +353,7 @@ ng_hci_sync_con_queue_ep *state = NULL; int error; - hook = (con->link_type == NG_HCI_LINK_ACL)? unit->acl : unit->sco; + hook = (con->link_type != NG_HCI_LINK_SCO)? unit->acl : unit->sco; if (hook == NULL || NG_HOOK_NOT_VALID(hook)) return (ENOTCONN); @@ -363,6 +370,223 @@ return (error); } /* sync_con_queue */ +/* le meta event */ +/* Inquiry result event */ +static int +le_advertizing_report(ng_hci_unit_p unit, struct mbuf *event) +{ + ng_hci_le_advertising_report_ep *ep = NULL; + ng_hci_neighbor_p n = NULL; + bdaddr_t bdaddr; + int error = 0; + u_int8_t event_type; + u_int8_t addr_type; + + NG_HCI_M_PULLUP(event, sizeof(*ep)); + if (event == NULL) + return (ENOBUFS); + + ep = mtod(event, ng_hci_le_advertising_report_ep *); + m_adj(event, sizeof(*ep)); + + for (; ep->num_reports > 0; ep->num_reports --) { + /* Get remote unit address */ + NG_HCI_M_PULLUP(event, sizeof(u_int8_t)); + event_type = *mtod(event, u_int8_t *); + m_adj(event, sizeof(u_int8_t)); + NG_HCI_M_PULLUP(event, sizeof(u_int8_t)); + addr_type = *mtod(event, u_int8_t *); + m_adj(event, sizeof(u_int8_t)); + + m_copydata(event, 0, sizeof(bdaddr), (caddr_t) &bdaddr); + m_adj(event, sizeof(bdaddr)); + + /* Lookup entry in the cache */ + n = ng_hci_get_neighbor(unit, &bdaddr, (addr_type) ? NG_HCI_LINK_LE_RANDOM:NG_HCI_LINK_LE_PUBLIC); + if (n == NULL) { + /* Create new entry */ + n = ng_hci_new_neighbor(unit); + if (n == NULL) { + error = ENOMEM; + break; + } + bcopy(&bdaddr, &n->bdaddr, sizeof(n->bdaddr)); + n->addrtype = (addr_type)? NG_HCI_LINK_LE_RANDOM : + NG_HCI_LINK_LE_PUBLIC; + + } else + getmicrotime(&n->updated); + +#if 0 + { + /* + * TODO: Make these information + * Available from userland. + */ + u_int8_t length_data; + + char *rssi; + + NG_HCI_M_PULLUP(event, sizeof(u_int8_t)); + length_data = *mtod(event, u_int8_t *); + m_adj(event, sizeof(u_int8_t)); + /*Advertizement data*/ + NG_HCI_M_PULLUP(event, length_data); + m_adj(event, length_data); + NG_HCI_M_PULLUP(event, sizeof(char )); + /*Get RSSI*/ + rssi = mtod(event, char *); + m_adj(event, sizeof(u_int8_t)); + } +#endif + } + NG_FREE_M(event); + + return (error); +} /* inquiry_result */ + +static int le_connection_complete(ng_hci_unit_p unit, struct mbuf *event) +{ + int error = 0; + + ng_hci_le_connection_complete_ep *ep = NULL; + ng_hci_unit_con_p con = NULL; + int link_type; + uint8_t uclass[3] = {0,0,0};//dummy uclass + + NG_HCI_M_PULLUP(event, sizeof(*ep)); + if (event == NULL) + return (ENOBUFS); + + ep = mtod(event, ng_hci_le_connection_complete_ep *); + link_type = (ep->address_type)? NG_HCI_LINK_LE_RANDOM : + NG_HCI_LINK_LE_PUBLIC; + /* + * Find the first connection descriptor that matches the following: + * + * 1) con->link_type == link_type + * 2) con->state == NG_HCI_CON_W4_CONN_COMPLETE + * 3) con->bdaddr == ep->address + */ + LIST_FOREACH(con, &unit->con_list, next) + if (con->link_type == link_type && + con->state == NG_HCI_CON_W4_CONN_COMPLETE && + bcmp(&con->bdaddr, &ep->address, sizeof(bdaddr_t)) == 0) + break; + + /* + * Two possible cases: + * + * 1) We have found connection descriptor. That means upper layer has + * requested this connection via LP_CON_REQ message. In this case + * connection must have timeout set. If ng_hci_con_untimeout() fails + * then timeout message already went into node's queue. In this case + * ignore Connection_Complete event and let timeout deal with it. + * + * 2) We do not have connection descriptor. That means upper layer + * nas not requested this connection , (less likely) we gave up + * on this connection (timeout) or as node act as slave role. + * The most likely scenario is that + * we have received LE_Create_Connection command + * from the RAW hook + */ + + if (con == NULL) { + if (ep->status != 0) + goto out; + + con = ng_hci_new_con(unit, link_type); + if (con == NULL) { + error = ENOMEM; + goto out; + } + + con->state = NG_HCI_CON_W4_LP_CON_RSP; + ng_hci_con_timeout(con); + + bcopy(&ep->address, &con->bdaddr, sizeof(con->bdaddr)); + error = ng_hci_lp_con_ind(con, uclass); + if (error != 0) { + ng_hci_con_untimeout(con); + ng_hci_free_con(con); + } + + } else if ((error = ng_hci_con_untimeout(con)) != 0) + goto out; + + /* + * Update connection descriptor and send notification + * to the upper layers. + */ + + con->con_handle = NG_HCI_CON_HANDLE(le16toh(ep->handle)); + con->encryption_mode = NG_HCI_ENCRYPTION_MODE_NONE; + + ng_hci_lp_con_cfm(con, ep->status); + + /* Adjust connection state */ + if (ep->status != 0) + ng_hci_free_con(con); + else { + con->state = NG_HCI_CON_OPEN; + + /* + * Change link policy for the ACL connections. Enable all + * supported link modes. Enable Role switch as well if + * device supports it. + */ + + } + +out: + NG_FREE_M(event); + + return (error); + +} + +static int le_connection_update(ng_hci_unit_p unit, struct mbuf *event) +{ + int error = 0; + /*TBD*/ + + NG_FREE_M(event); + return error; + +} +static int +le_event(ng_hci_unit_p unit, struct mbuf *event) +{ + int error = 0; + ng_hci_le_ep *lep; + + NG_HCI_M_PULLUP(event, sizeof(*lep)); + if(event ==NULL){ + return ENOBUFS; + } + lep = mtod(event, ng_hci_le_ep *); + m_adj(event, sizeof(*lep)); + switch(lep->subevent_code){ + case NG_HCI_LEEV_CON_COMPL: + le_connection_complete(unit, event); + break; + case NG_HCI_LEEV_ADVREP: + le_advertizing_report(unit, event); + break; + case NG_HCI_LEEV_CON_UPDATE_COMPL: + le_connection_update(unit, event); + break; + case NG_HCI_LEEV_READ_REMOTE_FEATURES_COMPL: + //TBD + /*FALLTHROUGH*/ + case NG_HCI_LEEV_LONG_TERM_KEY_REQUEST: + //TBD + /*FALLTHROUGH*/ + default: + NG_FREE_M(event); + } + return error; +} /* Inquiry result event */ static int @@ -386,7 +610,7 @@ m_adj(event, sizeof(bdaddr)); /* Lookup entry in the cache */ - n = ng_hci_get_neighbor(unit, &bdaddr); + n = ng_hci_get_neighbor(unit, &bdaddr, NG_HCI_LINK_ACL); if (n == NULL) { /* Create new entry */ n = ng_hci_new_neighbor(unit); @@ -398,6 +622,7 @@ getmicrotime(&n->updated); bcopy(&bdaddr, &n->bdaddr, sizeof(n->bdaddr)); + n->addrtype = NG_HCI_LINK_ACL; /* XXX call m_pullup here? */ @@ -754,7 +979,7 @@ } /* Update cache entry */ - n = ng_hci_get_neighbor(unit, &con->bdaddr); + n = ng_hci_get_neighbor(unit, &con->bdaddr, NG_HCI_LINK_ACL); if (n == NULL) { n = ng_hci_new_neighbor(unit); if (n == NULL) { @@ -763,6 +988,7 @@ } bcopy(&con->bdaddr, &n->bdaddr, sizeof(n->bdaddr)); + n->addrtype = NG_HCI_LINK_ACL; } else getmicrotime(&n->updated); @@ -909,7 +1135,7 @@ } /* Update buffer descriptor */ - if (con->link_type == NG_HCI_LINK_ACL) + if (con->link_type != NG_HCI_LINK_SCO) NG_HCI_BUFF_ACL_FREE(unit->buffer, p); else NG_HCI_BUFF_SCO_FREE(unit->buffer, p); @@ -1010,7 +1236,7 @@ } /* Update cache entry */ - n = ng_hci_get_neighbor(unit, &con->bdaddr); + n = ng_hci_get_neighbor(unit, &con->bdaddr, NG_HCI_LINK_ACL); if (n == NULL) { n = ng_hci_new_neighbor(unit); if (n == NULL) { @@ -1019,6 +1245,7 @@ } bcopy(&con->bdaddr, &n->bdaddr, sizeof(n->bdaddr)); + n->addrtype = NG_HCI_LINK_ACL; } else getmicrotime(&n->updated); @@ -1089,7 +1316,7 @@ ep = mtod(event, ng_hci_page_scan_mode_change_ep *); /* Update cache entry */ - n = ng_hci_get_neighbor(unit, &ep->bdaddr); + n = ng_hci_get_neighbor(unit, &ep->bdaddr, NG_HCI_LINK_ACL); if (n == NULL) { n = ng_hci_new_neighbor(unit); if (n == NULL) { @@ -1098,6 +1325,7 @@ } bcopy(&ep->bdaddr, &n->bdaddr, sizeof(n->bdaddr)); + n->addrtype = NG_HCI_LINK_ACL; } else getmicrotime(&n->updated); @@ -1123,7 +1351,7 @@ ep = mtod(event, ng_hci_page_scan_rep_mode_change_ep *); /* Update cache entry */ - n = ng_hci_get_neighbor(unit, &ep->bdaddr); + n = ng_hci_get_neighbor(unit, &ep->bdaddr, NG_HCI_LINK_ACL); if (n == NULL) { n = ng_hci_new_neighbor(unit); if (n == NULL) { @@ -1132,6 +1360,7 @@ } bcopy(&ep->bdaddr, &n->bdaddr, sizeof(n->bdaddr)); + n->addrtype = NG_HCI_LINK_ACL; } else getmicrotime(&n->updated); Index: head/sys/netgraph/bluetooth/hci/ng_hci_main.c =================================================================== --- head/sys/netgraph/bluetooth/hci/ng_hci_main.c +++ head/sys/netgraph/bluetooth/hci/ng_hci_main.c @@ -775,7 +775,6 @@ int size, error = 0; NG_HCI_BUFF_ACL_SIZE(unit->buffer, size); - /* Check packet */ NGI_GET_M(item, m); @@ -788,7 +787,6 @@ error = EINVAL; goto drop; } - if (m->m_pkthdr.len < sizeof(ng_hci_acldata_pkt_t) || m->m_pkthdr.len > sizeof(ng_hci_acldata_pkt_t) + size) { NG_HCI_ALERT( @@ -831,7 +829,7 @@ goto drop; } - if (con->link_type != NG_HCI_LINK_ACL) { + if (con->link_type == NG_HCI_LINK_SCO) { NG_HCI_ERR( "%s: %s - unexpected HCI ACL data packet. Not ACL link, con_handle=%d, " \ "link_type=%d\n", __func__, NG_NODE_NAME(unit->node), Index: head/sys/netgraph/bluetooth/hci/ng_hci_misc.h =================================================================== --- head/sys/netgraph/bluetooth/hci/ng_hci_misc.h +++ head/sys/netgraph/bluetooth/hci/ng_hci_misc.h @@ -41,7 +41,7 @@ ng_hci_neighbor_p ng_hci_new_neighbor (ng_hci_unit_p); void ng_hci_free_neighbor (ng_hci_neighbor_p); void ng_hci_flush_neighbor_cache (ng_hci_unit_p); -ng_hci_neighbor_p ng_hci_get_neighbor (ng_hci_unit_p, bdaddr_p); +ng_hci_neighbor_p ng_hci_get_neighbor (ng_hci_unit_p, bdaddr_p, int); int ng_hci_neighbor_stale (ng_hci_neighbor_p); ng_hci_unit_con_p ng_hci_new_con (ng_hci_unit_p, int); Index: head/sys/netgraph/bluetooth/hci/ng_hci_misc.c =================================================================== --- head/sys/netgraph/bluetooth/hci/ng_hci_misc.c +++ head/sys/netgraph/bluetooth/hci/ng_hci_misc.c @@ -214,7 +214,7 @@ */ ng_hci_neighbor_p -ng_hci_get_neighbor(ng_hci_unit_p unit, bdaddr_p bdaddr) +ng_hci_get_neighbor(ng_hci_unit_p unit, bdaddr_p bdaddr,int link_type) { ng_hci_neighbor_p n = NULL; @@ -222,7 +222,8 @@ ng_hci_neighbor_p nn = LIST_NEXT(n, next); if (!ng_hci_neighbor_stale(n)) { - if (bcmp(&n->bdaddr, bdaddr, sizeof(*bdaddr)) == 0) + if (n->addrtype == link_type && + bcmp(&n->bdaddr, bdaddr, sizeof(*bdaddr)) == 0) break; } else ng_hci_free_neighbor(n); /* remove old entry */ @@ -284,7 +285,7 @@ con->link_type = link_type; - if (con->link_type == NG_HCI_LINK_ACL) + if (con->link_type != NG_HCI_LINK_SCO) NG_HCI_BUFF_ACL_TOTAL(unit->buffer, num_pkts); else NG_HCI_BUFF_SCO_TOTAL(unit->buffer, num_pkts); @@ -313,7 +314,7 @@ * flushed these packets and we can free them too */ - if (con->link_type == NG_HCI_LINK_ACL) + if (con->link_type != NG_HCI_LINK_SCO) NG_HCI_BUFF_ACL_FREE(con->unit->buffer, con->pending); else NG_HCI_BUFF_SCO_FREE(con->unit->buffer, con->pending); Index: head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c =================================================================== --- head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c +++ head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c @@ -56,6 +56,7 @@ static int ng_hci_lp_acl_con_req (ng_hci_unit_p, item_p, hook_p); static int ng_hci_lp_sco_con_req (ng_hci_unit_p, item_p, hook_p); +static int ng_hci_lp_le_con_req (ng_hci_unit_p, item_p, hook_p, int); /* * Process LP_ConnectReq event from the upper layer protocol @@ -64,6 +65,8 @@ 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", @@ -84,21 +87,30 @@ return (EMSGSIZE); } - - if (((ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data))->link_type == NG_HCI_LINK_ACL) + 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)); - - 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); + 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 (ng_hci_lp_sco_con_req(unit, item, hook)); + + return (EINVAL); } /* ng_hci_lp_con_req */ /* @@ -264,7 +276,7 @@ * So check the neighbor cache. */ - n = ng_hci_get_neighbor(unit, &ep->bdaddr); + n = ng_hci_get_neighbor(unit, &ep->bdaddr, NG_HCI_LINK_ACL); if (n == NULL) { req->cp.page_scan_rep_mode = 0; req->cp.page_scan_mode = 0; @@ -469,6 +481,180 @@ return (error); } /* ng_hci_lp_sco_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 */ + /* * Process LP_DisconnectReq event from the upper layer protocol */ @@ -578,7 +764,7 @@ * only SCO upstream hook will receive notification */ - if (con->link_type == NG_HCI_LINK_ACL && + if (con->link_type != NG_HCI_LINK_SCO && con->flags & NG_HCI_CON_NOTIFY_ACL) { if (unit->acl != NULL && NG_HOOK_IS_VALID(unit->acl)) { NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_CFM, @@ -646,7 +832,7 @@ * Use link_type to select upstream hook. */ - if (con->link_type == NG_HCI_LINK_ACL) + if (con->link_type != NG_HCI_LINK_SCO) hook = unit->acl; else hook = unit->sco; @@ -887,7 +1073,7 @@ * only SCO upstream hook will receive notification. */ - if (con->link_type == NG_HCI_LINK_ACL) { + if (con->link_type != NG_HCI_LINK_SCO) { if (unit->acl != NULL && NG_HOOK_IS_VALID(unit->acl)) { NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_DISCON_IND, sizeof(*ep), M_NOWAIT); Index: head/sys/netgraph/bluetooth/hci/ng_hci_var.h =================================================================== --- head/sys/netgraph/bluetooth/hci/ng_hci_var.h +++ head/sys/netgraph/bluetooth/hci/ng_hci_var.h @@ -205,6 +205,7 @@ bdaddr_t bdaddr; /* address */ u_int8_t features[NG_HCI_FEATURES_SIZE]; /* LMP features */ + u_int8_t addrtype; /*Address Type*/ u_int8_t page_scan_rep_mode; /* PS rep. mode */ u_int8_t page_scan_mode; /* page scan mode */ Index: head/sys/netgraph/bluetooth/include/ng_btsocket.h =================================================================== --- head/sys/netgraph/bluetooth/include/ng_btsocket.h +++ head/sys/netgraph/bluetooth/include/ng_btsocket.h @@ -221,13 +221,32 @@ * Bluetooth version of struct sockaddr for L2CAP sockets (RAW and SEQPACKET) */ +struct sockaddr_l2cap_compat { + u_char l2cap_len; /* total length */ + u_char l2cap_family; /* address family */ + u_int16_t l2cap_psm; /* PSM (Protocol/Service Multiplexor) */ + bdaddr_t l2cap_bdaddr; /* address */ +}; + +#define BDADDR_BREDR 0 +#define BDADDR_LE_PUBLIC 1 +#define BDADDR_LE_RANDOM 2 + struct sockaddr_l2cap { u_char l2cap_len; /* total length */ u_char l2cap_family; /* address family */ u_int16_t l2cap_psm; /* PSM (Protocol/Service Multiplexor) */ bdaddr_t l2cap_bdaddr; /* address */ + u_int16_t l2cap_cid; /*cid*/ + u_int8_t l2cap_bdaddr_type; /*address type*/ }; + +#if !defined(L2CAP_SOCKET_CHECKED) && !defined(_KERNEL) +#warning "Make sure new member of socket address initialized" +#endif + + /* L2CAP socket options */ #define SOL_L2CAP 0x1609 /* socket option level */ Index: head/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h =================================================================== --- head/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h +++ head/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h @@ -70,6 +70,8 @@ bdaddr_t src; /* source address */ bdaddr_t dst; /* dest address */ + uint8_t srctype;/*source addr type*/ + uint8_t dsttype;/*source addr type*/ ng_btsocket_l2cap_rtentry_p rt; /* routing info */ u_int32_t token; /* message token */ @@ -129,6 +131,8 @@ bdaddr_t src; /* Source address */ bdaddr_t dst; /* Destination address */ + uint8_t srctype; /*source addr type*/ + uint8_t dsttype; /*source addr type*/ u_int16_t psm; /* PSM */ u_int16_t cid; /* Local channel ID */ Index: head/sys/netgraph/bluetooth/include/ng_hci.h =================================================================== --- head/sys/netgraph/bluetooth/include/ng_hci.h +++ head/sys/netgraph/bluetooth/include/ng_hci.h @@ -75,10 +75,11 @@ #define NG_HCI_KEY_SIZE 16 /* link key */ #define NG_HCI_PIN_SIZE 16 /* link PIN */ #define NG_HCI_EVENT_MASK_SIZE 8 /* event mask */ +#define NG_HCI_LE_EVENT_MASK_SIZE 8 /* event mask */ #define NG_HCI_CLASS_SIZE 3 /* unit class */ #define NG_HCI_FEATURES_SIZE 8 /* LMP features */ #define NG_HCI_UNIT_NAME_SIZE 248 /* unit name size */ - +#define NG_HCI_COMMANDS_SIZE 64 /*Command list BMP size*/ /* HCI specification */ #define NG_HCI_SPEC_V10 0x00 /* v1.0 */ #define NG_HCI_SPEC_V11 0x01 /* v1.1 */ @@ -115,6 +116,8 @@ /* Link types */ #define NG_HCI_LINK_SCO 0x00 /* Voice */ #define NG_HCI_LINK_ACL 0x01 /* Data */ +#define NG_HCI_LINK_LE_PUBLIC 0x02 /* LE Public*/ +#define NG_HCI_LINK_LE_RANDOM 0x03 /* LE Random*/ /* 0x02 - 0xFF - reserved for future use */ /* Packet types */ @@ -1265,6 +1268,7 @@ typedef ng_hci_status_rp ng_hci_write_iac_lap_rp; +/*0x003b-0x003e commands are depricated v2.0 or later*/ #define NG_HCI_OCF_READ_PAGE_SCAN_PERIOD 0x003b /* No command parameter(s) */ typedef struct { @@ -1293,6 +1297,21 @@ typedef ng_hci_status_rp ng_hci_write_page_scan_rp; +#define NG_HCI_OCF_READ_LE_HOST_SUPPORTED 0x6c +typedef struct { + u_int8_t status; /* 0x00 - success */ + u_int8_t le_supported_host ;/* LE host supported?*/ + u_int8_t simultaneous_le_host; /* BR/LE simulateneous? */ +} __attribute__ ((packed)) ng_hci_read_le_host_supported_rp; + +#define NG_HCI_OCF_WRITE_LE_HOST_SUPPORTED 0x6d +typedef struct { + u_int8_t le_supported_host; /* LE host supported?*/ + u_int8_t simultaneous_le_host; /* LE host supported?*/ +} __attribute__ ((packed)) ng_hci_write_le_host_supported_cp; + +typedef ng_hci_status_rp ng_hci_write_le_host_supported_rp; + /************************************************************************** ************************************************************************** ** Informational commands and return parameters @@ -1312,6 +1331,12 @@ u_int16_t lmp_subversion; /* LMP sub-version */ } __attribute__ ((packed)) ng_hci_read_local_ver_rp; +#define NG_HCI_OCF_READ_LOCAL_COMMANDS 0x0002 +typedef struct { + u_int8_t status; /* 0x00 - success */ + u_int8_t features[NG_HCI_COMMANDS_SIZE]; /* command bitmsk*/ +} __attribute__ ((packed)) ng_hci_read_local_commands_rp; + #define NG_HCI_OCF_READ_LOCAL_FEATURES 0x0003 typedef struct { u_int8_t status; /* 0x00 - success */ @@ -1418,6 +1443,251 @@ /************************************************************************** ************************************************************************** + ** LE OpCode group field + ************************************************************************** + **************************************************************************/ + +#define NG_HCI_OGF_LE 0x08 /* OpCode Group Field */ +#define NG_HCI_OCF_LE_SET_EVENT_MASK 0x0001 +typedef struct { + u_int8_t event_mask[NG_HCI_LE_EVENT_MASK_SIZE]; /* event_mask*/ + +} __attribute__ ((packed)) ng_hci_le_set_event_mask_cp; +typedef ng_hci_status_rp ng_hci_le_set_event_mask_rp; + +#define NG_HCI_OCF_LE_READ_BUFFER_SIZE 0x0002 +/*No command parameter */ +typedef struct { + u_int8_t status; /*status*/ + u_int16_t hc_le_data_packet_length; + u_int8_t hc_total_num_le_data_packets; +} __attribute__ ((packed)) ng_hci_le_read_buffer_size_rp; + + +#define NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES 0x0003 +/*No command parameter */ +typedef struct { + u_int8_t status; /*status*/ + u_int64_t le_features; +} __attribute__ ((packed)) ng_hci_le_read_local_supported_features_rp; + +#define NG_HCI_OCF_LE_SET_RANDOM_ADDRESS 0x0005 +typedef struct { + bdaddr_t random_address; +} __attribute__ ((packed)) ng_hci_le_set_random_address_cp_; +typedef ng_hci_status_rp ng_hci_le_set_random_address_rp; + +#define NG_HCI_OCF_LE_SET_ADVERTISING_PARAMETERS 0x0006 +typedef struct { + u_int16_t advertising_interval_min; + u_int16_t advertising_interval_max; + u_int8_t advertising_type; + u_int8_t own_address_type; + u_int8_t direct_address_type; + bdaddr_t direct_address; + u_int8_t advertising_channel_map; + u_int8_t advertising_filter_policy; +} __attribute__ ((packed)) ng_hci_le_set_advertising_parameters_cp; +typedef ng_hci_status_rp ng_hci_le_set_advertising_parameters_rp; + +#define NG_HCI_OCF_LE_READ_ADVERTISING_CHANNEL_TX_POWER 0x0007 +/*No command parameter*/ +typedef struct { + u_int8_t status; + u_int8_t transmit_power_level; +} __attribute__ ((packed)) ng_hci_le_read_advertising_channel_tx_power_rp; + +#define NG_HCI_OCF_LE_SET_ADVERTISING_DATA 0x0008 +#define NG_HCI_ADVERTISING_DATA_SIZE 31 +typedef struct { + u_int8_t advertising_data_length; + char advertising_data[NG_HCI_ADVERTISING_DATA_SIZE]; +} __attribute__ ((packed)) ng_hci_le_set_advertising_data_cp; +typedef ng_hci_status_rp ng_hci_le_set_advertising_data_rp; + +#define NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA 0x0009 + +typedef struct { + u_int8_t scan_response_data_length; + char scan_response_data[NG_HCI_ADVERTISING_DATA_SIZE]; +} __attribute__ ((packed)) ng_hci_le_set_scan_response_data_cp; +typedef ng_hci_status_rp ng_hci_le_set_scan_response_data_rp; + +#define NG_HCI_OCF_LE_SET_ADVERTISE_ENABLE 0x000a +typedef struct { + u_int8_t advertising_enable; +}__attribute__ ((packed)) ng_hci_le_set_advertise_enable_cp; +typedef ng_hci_status_rp ng_hci_le_set_advertise_enable_rp; + +#define NG_HCI_OCF_LE_SET_SCAN_PARAMETERS 0x000b +typedef struct { + u_int8_t le_scan_type; + u_int16_t le_scan_interval; + u_int16_t le_scan_window; + u_int8_t own_address_type; + u_int8_t scanning_filter_policy; +}__attribute__ ((packed)) ng_hci_le_set_scan_parameters_cp; +typedef ng_hci_status_rp ng_hci_le_set_scan_parameters_rp; + +#define NG_HCI_OCF_LE_SET_SCAN_ENABLE 0x000c +typedef struct { + u_int8_t le_scan_enable; + u_int8_t filter_duplicates; +}__attribute__ ((packed)) ng_hci_le_set_scan_enable_cp; +typedef ng_hci_status_rp ng_hci_le_set_scan_enable_rp; + +#define NG_HCI_OCF_LE_CREATE_CONNECTION 0x000d +typedef struct { + u_int16_t scan_interval; + u_int16_t scan_window; + u_int8_t filter_policy; + u_int8_t peer_addr_type; + bdaddr_t peer_addr; + u_int8_t own_address_type; + u_int16_t conn_interval_min; + u_int16_t conn_interval_max; + u_int16_t conn_latency; + u_int16_t supervision_timeout; + u_int16_t min_ce_length; + u_int16_t max_ce_length; +}__attribute__((packed)) ng_hci_le_create_connection_cp; +/* no return paramters*/ +#define NG_HCI_OCF_LE_CREATE_CONNECTION_CANCEL 0x000e +/*No command parameter*/ +typedef ng_hci_status_rp ng_hci_le_create_connection_cancel_rp; +#define NG_HCI_OCF_LE_READ_WHITE_LIST_SIZE 0x000f +/*No command parameter*/ +typedef struct { + u_int8_t status; + u_int8_t white_list_size; +} __attribute__ ((packed)) ng_hci_le_read_white_list_size_rp; + +#define NG_HCI_OCF_LE_CLEAR_WHITE_LIST 0x0010 +/*No command paramters*/ +typedef ng_hci_status_rp ng_hci_le_clear_white_list_rp; +#define NG_HCI_OCF_LE_ADD_DEVICE_TO_WHITE_LIST 0x0011 +typedef struct { + u_int8_t address_type; + bdaddr_t address; +} __attribute__ ((packed)) ng_hci_le_add_device_to_white_list_cp; +typedef ng_hci_status_rp ng_hci_le_add_device_to_white_list_rp; + +#define NG_HCI_OCF_LE_REMOVE_DEVICE_FROM_WHITE_LIST 0x0012 +typedef struct { + u_int8_t address_type; + bdaddr_t address; +} __attribute__ ((packed)) ng_hci_le_remove_device_from_white_list_cp; +typedef ng_hci_status_rp ng_hci_le_remove_device_from_white_list_rp; + +#define NG_HCI_OCF_LE_CONNECTION_UPDATE 0x0013 +typedef struct { + u_int16_t connection_handle; + u_int16_t conn_interval_min; + u_int16_t conn_interval_max; + u_int16_t conn_latency; + u_int16_t supervision_timeout; + u_int16_t minimum_ce_length; + u_int16_t maximum_ce_length; +}__attribute__ ((packed)) ng_hci_le_connection_update_cp; +/*no return parameter*/ + +#define NG_HCI_OCF_LE_SET_HOST_CHANNEL_CLASSIFICATION 0x0014 +typedef struct{ + u_int8_t le_channel_map[5]; +}__attribute__ ((packed)) ng_hci_le_set_host_channel_classification_cp; +typedef ng_hci_status_rp ng_hci_le_set_host_channel_classification_rp; + +#define NG_HCI_OCF_LE_READ_CHANNEL_MAP 0x0015 +typedef struct { + u_int16_t connection_handle; +}__attribute__ ((packed)) ng_hci_le_read_channel_map_cp; +typedef struct { + u_int8_t status; + u_int16_t connection_handle; + u_int8_t le_channel_map[5]; +} __attribute__ ((packed)) ng_hci_le_read_channel_map_rp; + +#define NG_HCI_OCF_LE_READ_REMOTE_USED_FEATURES 0x0016 +typedef struct { + u_int16_t connection_handle; +}__attribute__ ((packed)) ng_hci_le_read_remote_used_features_cp; +/*No return parameter*/ +#define NG_HCI_128BIT 16 +#define NG_HCI_OCF_LE_ENCRYPT 0x0017 +typedef struct { + u_int8_t key[NG_HCI_128BIT]; + u_int8_t plaintext_data[NG_HCI_128BIT]; +}__attribute__ ((packed)) ng_hci_le_encrypt_cp; +typedef struct { + u_int8_t status; + u_int8_t plaintext_data[NG_HCI_128BIT]; +}__attribute__ ((packed)) ng_hci_le_encrypt_rp; + +#define NG_HCI_OCF_LE_RAND 0x0018 +/*No command parameter*/ +typedef struct { + u_int8_t status; + u_int64_t random_number; +}__attribute__ ((packed)) ng_hci_le_rand_rp; + +#define NG_HCI_OCF_LE_START_ENCRYPTION 0x0019 +typedef struct { + u_int16_t connection_handle; + u_int64_t random_number; + u_int16_t encrypted_diversifier; + u_int8_t long_term_key[NG_HCI_128BIT]; +}__attribute__ ((packed)) ng_hci_le_start_encryption_cp; +/*No return parameter*/ +#define NG_HCI_OCF_LE_LONG_TERM_KEY_REQUEST_REPLY 0x001a +typedef struct { + u_int16_t connection_handle; + u_int8_t long_term_key[NG_HCI_128BIT]; +}__attribute__ ((packed)) ng_hci_le_long_term_key_request_reply_cp; +typedef struct { + u_int8_t status; + u_int16_t connection_handle; +}__attribute__ ((packed)) ng_hci_le_long_term_key_request_reply_rp; + +#define NG_HCI_OCF_LE_LONG_TERM_KEY_REQUEST_NEGATIVE_REPLY 0x001b +typedef struct{ + u_int16_t connection_handle; +}ng_hci_le_long_term_key_request_negative_reply_cp; +typedef struct { + u_int8_t status; + u_int16_t connection_handle; +}__attribute__ ((packed)) ng_hci_le_long_term_key_request_negative_reply_rp; + + +#define NG_HCI_OCF_LE_READ_SUPPORTED_STATUS 0x001c +/*No command parameter*/ +typedef struct { + u_int8_t status; + u_int64_t le_status; +}__attribute__ ((packed)) ng_hci_le_read_supported_status_rp; + +#define NG_HCI_OCF_LE_RECEIVER_TEST 0x001d +typedef struct{ + u_int8_t rx_frequency; +} __attribute__((packed)) ng_le_receiver_test_cp; +typedef ng_hci_status_rp ng_hci_le_receiver_test_rp; + +#define NG_HCI_OCF_LE_TRANSMITTER_TEST 0x001e +typedef struct{ + u_int8_t tx_frequency; + u_int8_t length_of_test_data; + u_int8_t packet_payload; +} __attribute__((packed)) ng_le_transmitter_test_cp; +typedef ng_hci_status_rp ng_hci_le_transmitter_test_rp; + +#define NG_HCI_OCF_LE_TEST_END 0x001f +/*No command paramter*/ +typedef struct { + u_int8_t status; + u_int16_t number_of_packets; +}__attribute__ ((packed)) ng_hci_le_test_end_rp; + +/************************************************************************** + ************************************************************************** ** Special HCI OpCode group field values ************************************************************************** **************************************************************************/ @@ -1654,6 +1924,54 @@ bdaddr_t bdaddr; /* destination address */ u_int8_t page_scan_rep_mode; /* page scan repetition mode */ } __attribute__ ((packed)) ng_hci_page_scan_rep_mode_change_ep; +#define NG_HCI_EVENT_LE 0x3e +typedef struct { + u_int8_t subevent_code; +}__attribute__ ((packed)) ng_hci_le_ep; + +#define NG_HCI_LEEV_CON_COMPL 0x01 + +typedef struct { + u_int8_t status; + u_int16_t handle; + u_int8_t role; + u_int8_t address_type; + bdaddr_t address; + u_int16_t interval; + u_int8_t latency; + u_int16_t supervision_timeout; + u_int8_t master_clock_accracy; + +} __attribute__ ((packed)) ng_hci_le_connection_complete_ep; + +#define NG_HCI_LEEV_ADVREP 0x02 +typedef struct { + u_int8_t num_reports; + +}__attribute__ ((packed)) ng_hci_le_advertising_report_ep; +#define NG_HCI_SCAN_RESPONSE_DATA_MAX 0x1f + +typedef struct { + u_int8_t event_type; + u_int8_t addr_type; + bdaddr_t bdaddr; + u_int8_t length_data; + u_int8_t data[NG_HCI_SCAN_RESPONSE_DATA_MAX]; +}__attribute__((packed)) ng_hci_le_advreport; + +#define NG_HCI_LEEV_CON_UPDATE_COMPL 0x03 +typedef struct { + u_int8_t status; + u_int16_t connection_handle; + u_int16_t conn_interval; + u_int16_t conn_latency; + u_int16_t supervision_timeout; +}__attribute__((packed)) ng_hci_connection_update_complete_ep; +#define NG_HCI_LEEV_READ_REMOTE_FEATURES_COMPL 0x04 +//TBD +#define NG_HCI_LEEV_LONG_TERM_KEY_REQUEST 0x05 +//TBD + #define NG_HCI_EVENT_BT_LOGO 0xfe Index: head/sys/netgraph/bluetooth/include/ng_l2cap.h =================================================================== --- head/sys/netgraph/bluetooth/include/ng_l2cap.h +++ head/sys/netgraph/bluetooth/include/ng_l2cap.h @@ -73,11 +73,18 @@ #define NG_L2CAP_NULL_CID 0x0000 /* DO NOT USE THIS CID */ #define NG_L2CAP_SIGNAL_CID 0x0001 /* signaling channel ID */ #define NG_L2CAP_CLT_CID 0x0002 /* connectionless channel ID */ - /* 0x0003 - 0x003f Reserved */ +#define NG_L2CAP_A2MP_CID 0x0003 +#define NG_L2CAP_ATT_CID 0x0004 +#define NG_L2CAP_LESIGNAL_CID 0x0005 +#define NG_L2CAP_SMP_CID 0x0006 + /* 0x0007 - 0x003f Reserved */ #define NG_L2CAP_FIRST_CID 0x0040 /* dynamically alloc. (start) */ #define NG_L2CAP_LAST_CID 0xffff /* dynamically alloc. (end) */ +#define NG_L2CAP_LELAST_CID 0x007f + /* L2CAP MTU */ +#define NG_L2CAP_MTU_LE_MINIMAM 23 #define NG_L2CAP_MTU_MINIMUM 48 #define NG_L2CAP_MTU_DEFAULT 672 #define NG_L2CAP_MTU_MAXIMUM 0xffff @@ -289,7 +296,6 @@ * NG_L2CAP_CONNLESS_MTU - 2 bytes connectionless MTU */ } __attribute__ ((packed)) ng_l2cap_info_rsp_cp; - typedef union { /* NG_L2CAP_CONNLESS_MTU */ struct { @@ -298,6 +304,20 @@ } ng_l2cap_info_rsp_data_t; typedef ng_l2cap_info_rsp_data_t * ng_l2cap_info_rsp_data_p; +#define NG_L2CAP_CMD_PARAM_UPDATE_REQUEST 0x12 + +typedef struct { + uint16_t interval_min; + uint16_t interval_max; + uint16_t slave_latency; + uint16_t timeout_mpl; +} __attribute__ ((packed)) ng_l2cap_param_update_req_cp; + +#define NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE 0x13 +#define NG_L2CAP_UPDATE_PARAM_ACCEPT 0 +#define NG_L2CAP_UPDATE_PARAM_REJECT 1 + +//typedef uint16_t update_response; /************************************************************************** ************************************************************************** ** Upper layer protocol interface. L2CA_xxx messages @@ -332,19 +352,25 @@ u_int32_t token; /* token to use in L2CAP_L2CA_WRITE */ u_int16_t length; /* length of the data */ u_int16_t lcid; /* local channel ID */ + uint16_t idtype; } __attribute__ ((packed)) ng_l2cap_l2ca_hdr_t; - +#define NG_L2CAP_L2CA_IDTYPE_BREDR 0 +#define NG_L2CAP_L2CA_IDTYPE_ATT 1 +#define NG_L2CAP_L2CA_IDTYPE_LE 2 /* L2CA_Connect */ #define NGM_L2CAP_L2CA_CON 0x80 /* Upper -> L2CAP */ typedef struct { u_int16_t psm; /* Protocol/Service Multiplexor */ bdaddr_t bdaddr; /* remote unit address */ + uint8_t linktype; + uint8_t idtype; } ng_l2cap_l2ca_con_ip; /* L2CAP -> Upper */ typedef struct { u_int16_t lcid; /* local channel ID */ + uint16_t idtype; /*ID type*/ u_int16_t result; /* 0x00 - success */ u_int16_t status; /* if result != 0x00 */ } ng_l2cap_l2ca_con_op; @@ -357,7 +383,7 @@ u_int16_t lcid; /* local channel ID */ u_int16_t psm; /* Procotol/Service Multiplexor */ u_int8_t ident; /* indentifier */ - u_int8_t unused; /* place holder */ + u_int8_t linktype; /* link type*/ } ng_l2cap_l2ca_con_ind_ip; /* No output parameters */ @@ -367,7 +393,7 @@ typedef struct { bdaddr_t bdaddr; /* remote unit address */ u_int8_t ident; /* "ident" from L2CAP_ConnectInd event */ - u_int8_t unused; /* place holder */ + u_int8_t linktype; /*link type */ u_int16_t lcid; /* local channel ID */ u_int16_t result; /* 0x00 - success */ u_int16_t status; /* if response != 0x00 */ @@ -435,6 +461,7 @@ /* Upper -> L2CAP */ typedef struct { u_int16_t lcid; /* local channel ID */ + u_int16_t idtype; } ng_l2cap_l2ca_discon_ip; /* L2CAP -> Upper */ @@ -457,6 +484,7 @@ int result; /* result (0x00 - success) */ u_int16_t length; /* amount of data written */ u_int16_t lcid; /* local channel ID */ + uint16_t idtype; } ng_l2cap_l2ca_write_op; /* L2CA_GroupCreate */ @@ -545,6 +573,8 @@ typedef struct { bdaddr_t bdaddr; /* remote unit address */ u_int16_t info_type; /* info type */ + uint8_t linktype; + uint8_t unused; } ng_l2cap_l2ca_get_info_ip; /* L2CAP -> Upper */ @@ -610,7 +640,9 @@ typedef u_int16_t ng_l2cap_node_debug_ep; #define NGM_L2CAP_NODE_HOOK_INFO 0x409 /* L2CAP -> Upper */ -/* bdaddr_t bdaddr; -- local (source BDADDR) */ +typedef struct { + bdaddr_t addr; +}ng_l2cap_node_hook_info_ep; #define NGM_L2CAP_NODE_GET_CON_LIST 0x40a /* L2CAP -> User */ typedef struct { Index: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.h =================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.h +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.h @@ -199,6 +199,25 @@ c->hdr.length = htole16(c->hdr.length); \ } while (0) +#define _ng_l2cap_cmd_urs(_m, _ident, _result) \ +do { \ + struct _cmd_urs{ \ + ng_l2cap_cmd_hdr_t hdr; \ + uint16_t result; \ + } __attribute__ ((packed)) *c = NULL; \ + \ + MGETHDR((_m), M_NOWAIT, MT_DATA); \ + \ + (_m)->m_pkthdr.len = (_m)->m_len = sizeof(*c); \ + \ + c = mtod((_m), struct _cmd_urs *); \ + c->hdr.code = NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE; \ + c->hdr.ident = (_ident); \ + c->hdr.length = sizeof(c->result); \ + \ + c->result = htole16((_result)); \ +} while (0) + /* Build configuration options */ #define _ng_l2cap_build_cfg_options(_m, _mtu, _flush_timo, _flow) \ do { \ Index: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.c =================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.c +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_cmds.c @@ -86,7 +86,6 @@ /* Process command */ switch (cmd->code) { - case NG_L2CAP_CMD_REJ: case NG_L2CAP_DISCON_RSP: case NG_L2CAP_ECHO_RSP: case NG_L2CAP_INFO_RSP: @@ -104,7 +103,16 @@ ng_l2cap_unlink_cmd(cmd); ng_l2cap_free_cmd(cmd); break; - + case NG_L2CAP_CMD_REJ: + (void) ng_l2cap_lp_send(con, + (con->linktype == NG_HCI_LINK_ACL)? + NG_L2CAP_SIGNAL_CID: + NG_L2CAP_LESIGNAL_CID + , m); + ng_l2cap_unlink_cmd(cmd); + ng_l2cap_free_cmd(cmd); + break; + case NG_L2CAP_CON_REQ: error = ng_l2cap_lp_send(con, NG_L2CAP_SIGNAL_CID, m); if (error != 0) { @@ -115,7 +123,6 @@ ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout()); break; - case NG_L2CAP_CON_RSP: error = ng_l2cap_lp_send(con, NG_L2CAP_SIGNAL_CID, m); ng_l2cap_unlink_cmd(cmd); @@ -208,9 +215,14 @@ ng_l2cap_unlink_cmd(cmd); ng_l2cap_free_cmd(cmd); } break; - + case NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE: + error = ng_l2cap_lp_send(con, NG_L2CAP_LESIGNAL_CID, m); + ng_l2cap_unlink_cmd(cmd); + ng_l2cap_free_cmd(cmd); + break; + case NG_L2CAP_CMD_PARAM_UPDATE_REQUEST: + /*TBD.*/ /* XXX FIXME add other commands */ - default: panic( "%s: %s - unknown command code=%d\n", @@ -256,6 +268,7 @@ case NG_L2CAP_DISCON_RSP: case NG_L2CAP_ECHO_RSP: case NG_L2CAP_INFO_RSP: + case NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE: break; case NG_L2CAP_CON_REQ: Index: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c =================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c @@ -57,7 +57,10 @@ ******************************************************************************/ static int ng_l2cap_process_signal_cmd (ng_l2cap_con_p); +static int ng_l2cap_process_lesignal_cmd (ng_l2cap_con_p); static int ng_l2cap_process_cmd_rej (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_cmd_urq (ng_l2cap_con_p, u_int8_t); +static int ng_l2cap_process_cmd_urs (ng_l2cap_con_p, u_int8_t); static int ng_l2cap_process_con_req (ng_l2cap_con_p, u_int8_t); static int ng_l2cap_process_con_rsp (ng_l2cap_con_p, u_int8_t); static int ng_l2cap_process_cfg_req (ng_l2cap_con_p, u_int8_t); @@ -74,6 +77,9 @@ (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t); static int send_l2cap_cfg_rsp (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *); +static int send_l2cap_param_urs + (ng_l2cap_con_p , u_int8_t , u_int16_t); + static int get_next_l2cap_opt (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p); @@ -124,7 +130,10 @@ m_adj(con->rx_pkt, sizeof(*hdr)); error = ng_l2cap_process_signal_cmd(con); break; - + case NG_L2CAP_LESIGNAL_CID: + m_adj(con->rx_pkt, sizeof(*hdr)); + error = ng_l2cap_process_lesignal_cmd(con); + break; case NG_L2CAP_CLT_CID: /* Connectionless packet */ error = ng_l2cap_l2ca_clt_receive(con); break; @@ -264,6 +273,105 @@ return (0); } /* ng_l2cap_process_signal_cmd */ +static int +ng_l2cap_process_lesignal_cmd(ng_l2cap_con_p con) +{ + ng_l2cap_p l2cap = con->l2cap; + ng_l2cap_cmd_hdr_t *hdr = NULL; + struct mbuf *m = NULL; + + while (con->rx_pkt != NULL) { + /* Verify packet length */ + if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { + NG_L2CAP_ERR( +"%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + con->rx_pkt->m_pkthdr.len); + NG_FREE_M(con->rx_pkt); + + return (EMSGSIZE); + } + + /* Get signaling command */ + NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); + if (con->rx_pkt == NULL) + return (ENOBUFS); + + hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); + hdr->length = le16toh(hdr->length); + m_adj(con->rx_pkt, sizeof(*hdr)); + + /* Verify command length */ + if (con->rx_pkt->m_pkthdr.len < hdr->length) { + NG_L2CAP_ERR( +"%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \ +"Invalid command length=%d, m_pkthdr.len=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + hdr->code, hdr->ident, hdr->length, + con->rx_pkt->m_pkthdr.len); + NG_FREE_M(con->rx_pkt); + + return (EMSGSIZE); + } + + /* Get the command, save the rest (if any) */ + if (con->rx_pkt->m_pkthdr.len > hdr->length) + m = m_split(con->rx_pkt, hdr->length, M_NOWAIT); + else + m = NULL; + + /* Process command */ + switch (hdr->code) { + case NG_L2CAP_CMD_REJ: + ng_l2cap_process_cmd_rej(con, hdr->ident); + break; + case NG_L2CAP_CMD_PARAM_UPDATE_REQUEST: + ng_l2cap_process_cmd_urq(con, hdr->ident); + break; + case NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE: + ng_l2cap_process_cmd_urs(con, hdr->ident); + break; + + + default: + NG_L2CAP_ERR( +"%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n", + __func__, NG_NODE_NAME(l2cap->node), + hdr->code, hdr->ident, hdr->length); + + /* + * Send L2CAP_CommandRej. Do not really care + * about the result + */ + + send_l2cap_reject(con, hdr->ident, + NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); + NG_FREE_M(con->rx_pkt); + break; + } + + con->rx_pkt = m; + } + + return (0); +} /* ng_l2cap_process_signal_cmd */ +/*Update Paramater Request*/ +static int ng_l2cap_process_cmd_urq(ng_l2cap_con_p con, uint8_t ident) +{ + /*We do not implement paramter negotiasion for now*/ + send_l2cap_param_urs(con, ident, NG_L2CAP_UPDATE_PARAM_ACCEPT); + NG_FREE_M(con->rx_pkt); + return 0; +} + +static int ng_l2cap_process_cmd_urs(ng_l2cap_con_p con, uint8_t ident) +{ + /* We only support master side yet .*/ + //send_l2cap_reject(con,ident ... ); + + NG_FREE_M(con->rx_pkt); + return 0; +} /* * Process L2CAP_CommandRej command @@ -352,7 +460,8 @@ ng_l2cap_chan_p ch = NULL; int error = 0; u_int16_t dcid, psm; - + int idtype; + /* Get command parameters */ NG_L2CAP_M_PULLUP(m, sizeof(*cp)); if (m == NULL) @@ -364,13 +473,20 @@ NG_FREE_M(m); con->rx_pkt = NULL; + if(dcid == NG_L2CAP_ATT_CID) + idtype = NG_L2CAP_L2CA_IDTYPE_ATT; + else if( con->linktype != NG_HCI_LINK_ACL) + idtype = NG_L2CAP_L2CA_IDTYPE_LE; + else + idtype = NG_L2CAP_L2CA_IDTYPE_BREDR; /* * Create new channel and send L2CA_ConnectInd notification * to the upper layer protocol. */ - ch = ng_l2cap_new_chan(l2cap, con, psm); + ch = ng_l2cap_new_chan(l2cap, con, psm, idtype); + if (ch == NULL) return (send_l2cap_con_rej(con, ident, 0, dcid, NG_L2CAP_NO_RESOURCES)); @@ -486,7 +602,8 @@ */ cmd->ch->dcid = dcid; - cmd->ch->state = NG_L2CAP_CONFIG; + cmd->ch->state = (cmd->ch->scid == NG_L2CAP_ATT_CID)? + NG_L2CAP_OPEN : NG_L2CAP_CONFIG; } else /* There was an error, so close the channel */ NG_L2CAP_INFO( @@ -541,7 +658,7 @@ m_adj(m, sizeof(*cp)); /* Check if we have this channel and it is in valid state */ - ch = ng_l2cap_chan_by_scid(l2cap, dcid); + ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR); if (ch == NULL) { NG_L2CAP_ERR( "%s: %s - unexpected L2CAP_ConfigReq command. " \ @@ -826,7 +943,7 @@ NG_FREE_M(con->rx_pkt); /* Check if we have this channel and it is in valid state */ - ch = ng_l2cap_chan_by_scid(l2cap, dcid); + ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR); if (ch == NULL) { NG_L2CAP_ERR( "%s: %s - unexpected L2CAP_DisconnectReq message. " \ @@ -1251,6 +1368,34 @@ return (0); } /* send_l2cap_cfg_rsp */ +static int +send_l2cap_param_urs(ng_l2cap_con_p con, u_int8_t ident, + u_int16_t result) +{ + ng_l2cap_cmd_p cmd = NULL; + + cmd = ng_l2cap_new_cmd(con, NULL, ident, + NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE, + 0); + if (cmd == NULL) { + + return (ENOMEM); + } + + _ng_l2cap_cmd_urs(cmd->aux, cmd->ident, result); + if (cmd->aux == NULL) { + ng_l2cap_free_cmd(cmd); + + return (ENOBUFS); + } + + /* Link command to the queue */ + ng_l2cap_link_cmd(con, cmd); + ng_l2cap_lp_deliver(con); + + return (0); +} /* send_l2cap_cfg_rsp */ + /* * Get next L2CAP configuration option * Index: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h =================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h @@ -34,7 +34,7 @@ #ifndef _NETGRAPH_L2CAP_LLPI_H_ #define _NETGRAPH_L2CAP_LLPI_H_ -int ng_l2cap_lp_con_req (ng_l2cap_p, bdaddr_p); +int ng_l2cap_lp_con_req (ng_l2cap_p, bdaddr_p, int); int ng_l2cap_lp_con_cfm (ng_l2cap_p, struct ng_mesg *); int ng_l2cap_lp_con_ind (ng_l2cap_p, struct ng_mesg *); int ng_l2cap_lp_discon_ind (ng_l2cap_p, struct ng_mesg *); Index: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c =================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c @@ -64,7 +64,7 @@ */ int -ng_l2cap_lp_con_req(ng_l2cap_p l2cap, bdaddr_p bdaddr) +ng_l2cap_lp_con_req(ng_l2cap_p l2cap, bdaddr_p bdaddr, int type) { struct ng_mesg *msg = NULL; ng_hci_lp_con_req_ep *ep = NULL; @@ -72,7 +72,7 @@ int error = 0; /* Verify that we DO NOT have connection to the remote unit */ - con = ng_l2cap_con_by_addr(l2cap, bdaddr); + con = ng_l2cap_con_by_addr(l2cap, bdaddr, type); if (con != NULL) { NG_L2CAP_ALERT( "%s: %s - unexpected LP_ConnectReq event. " \ @@ -93,7 +93,7 @@ } /* Create and intialize new connection descriptor */ - con = ng_l2cap_new_con(l2cap, bdaddr); + con = ng_l2cap_new_con(l2cap, bdaddr, type); if (con == NULL) return (ENOMEM); @@ -108,7 +108,7 @@ ep = (ng_hci_lp_con_req_ep *) (msg->data); bcopy(bdaddr, &ep->bdaddr, sizeof(ep->bdaddr)); - ep->link_type = NG_HCI_LINK_ACL; + ep->link_type = type; con->flags |= NG_L2CAP_CON_OUTGOING; con->state = NG_L2CAP_W4_LP_CON_CFM; @@ -152,9 +152,8 @@ } ep = (ng_hci_lp_con_cfm_ep *) (msg->data); - /* Check if we have requested/accepted this connection */ - con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr); + con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr, ep->link_type); if (con == NULL) { NG_L2CAP_ERR( "%s: %s - unexpected LP_ConnectCfm event. Connection does not exist\n", @@ -224,7 +223,7 @@ 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); + con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr, ep->link_type); if (con != NULL) { NG_L2CAP_ALERT( "%s: %s - unexpected LP_ConnectInd event. " \ @@ -245,7 +244,7 @@ } /* Create and intialize new connection descriptor */ - con = ng_l2cap_new_con(l2cap, &ep->bdaddr); + con = ng_l2cap_new_con(l2cap, &ep->bdaddr, ep->link_type); if (con == NULL) return (ENOMEM); @@ -773,6 +772,10 @@ con->tx_pkt = con->tx_pkt->m_nextpkt; m->m_nextpkt = NULL; + if(m->m_flags &M_PROTO2){ + ng_l2cap_lp_receive(con->l2cap, m); + continue; + } NG_L2CAP_INFO( "%s: %s - sending ACL packet, con_handle=%d, len=%d\n", __func__, NG_NODE_NAME(l2cap->node), con->con_handle, Index: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.h =================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.h +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.h @@ -40,10 +40,10 @@ * ACL Connections */ -ng_l2cap_con_p ng_l2cap_new_con (ng_l2cap_p, bdaddr_p); +ng_l2cap_con_p ng_l2cap_new_con (ng_l2cap_p, bdaddr_p, int); void ng_l2cap_con_ref (ng_l2cap_con_p); void ng_l2cap_con_unref (ng_l2cap_con_p); -ng_l2cap_con_p ng_l2cap_con_by_addr (ng_l2cap_p, bdaddr_p); +ng_l2cap_con_p ng_l2cap_con_by_addr (ng_l2cap_p, bdaddr_p, unsigned int); ng_l2cap_con_p ng_l2cap_con_by_handle (ng_l2cap_p, u_int16_t); void ng_l2cap_free_con (ng_l2cap_con_p); @@ -51,8 +51,10 @@ * L2CAP channels */ -ng_l2cap_chan_p ng_l2cap_new_chan (ng_l2cap_p, ng_l2cap_con_p, u_int16_t); -ng_l2cap_chan_p ng_l2cap_chan_by_scid (ng_l2cap_p, u_int16_t); +ng_l2cap_chan_p ng_l2cap_new_chan (ng_l2cap_p, ng_l2cap_con_p, u_int16_t, int); +ng_l2cap_chan_p ng_l2cap_chan_by_scid (ng_l2cap_p, u_int16_t, int); +ng_l2cap_chan_p ng_l2cap_chan_by_conhandle(ng_l2cap_p , uint16_t , u_int16_t); + void ng_l2cap_free_chan (ng_l2cap_chan_p); /* Index: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c =================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c @@ -49,7 +49,7 @@ #include #include -static u_int16_t ng_l2cap_get_cid (ng_l2cap_p); +static u_int16_t ng_l2cap_get_cid (ng_l2cap_p, int); /****************************************************************************** ****************************************************************************** @@ -67,6 +67,7 @@ ng_l2cap_p l2cap = NULL; struct ng_mesg *msg = NULL; int error = 0; + ng_l2cap_node_hook_info_ep *ep ; if (node == NULL || NG_NODE_NOT_VALID(node) || hook == NULL || NG_HOOK_NOT_VALID(hook)) @@ -78,9 +79,11 @@ return; NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_NODE_HOOK_INFO, - sizeof(bdaddr_t), M_NOWAIT); + sizeof(*ep), M_NOWAIT); + if (msg != NULL) { - bcopy(&l2cap->bdaddr, msg->data, sizeof(bdaddr_t)); + ep = (ng_l2cap_node_hook_info_ep *) &msg->data; + bcopy(&l2cap->bdaddr, &ep->addr, sizeof(bdaddr_t)); NG_SEND_MSG_HOOK(error, node, msg, hook, 0); } else error = ENOMEM; @@ -98,7 +101,7 @@ */ ng_l2cap_con_p -ng_l2cap_new_con(ng_l2cap_p l2cap, bdaddr_p bdaddr) +ng_l2cap_new_con(ng_l2cap_p l2cap, bdaddr_p bdaddr, int type) { static int fake_con_handle = 0x0f00; ng_l2cap_con_p con = NULL; @@ -131,6 +134,7 @@ fake_con_handle = 0x0f00; bcopy(bdaddr, &con->remote, sizeof(con->remote)); + con->linktype = type; ng_callout_init(&con->con_timo); con->ident = NG_L2CAP_FIRST_IDENT - 1; @@ -292,12 +296,13 @@ */ ng_l2cap_con_p -ng_l2cap_con_by_addr(ng_l2cap_p l2cap, bdaddr_p bdaddr) +ng_l2cap_con_by_addr(ng_l2cap_p l2cap, bdaddr_p bdaddr, unsigned int type) { ng_l2cap_con_p con = NULL; LIST_FOREACH(con, &l2cap->con_list, next) - if (bcmp(bdaddr, &con->remote, sizeof(con->remote)) == 0) + if ((bcmp(bdaddr, &con->remote, sizeof(con->remote)) == 0)&& + (con->linktype == type)) break; return (con); @@ -325,7 +330,7 @@ */ ng_l2cap_chan_p -ng_l2cap_new_chan(ng_l2cap_p l2cap, ng_l2cap_con_p con, u_int16_t psm) +ng_l2cap_new_chan(ng_l2cap_p l2cap, ng_l2cap_con_p con, u_int16_t psm, int idtype) { ng_l2cap_chan_p ch = NULL; @@ -333,8 +338,12 @@ M_NOWAIT|M_ZERO); if (ch == NULL) return (NULL); - - ch->scid = ng_l2cap_get_cid(l2cap); + if(idtype == NG_L2CAP_L2CA_IDTYPE_ATT){ + ch->scid = ch->dcid = NG_L2CAP_ATT_CID; + }else{ + ch->scid = ng_l2cap_get_cid(l2cap, + (con->linktype!= NG_HCI_LINK_ACL)); + } if (ch->scid != NG_L2CAP_NULL_CID) { /* Initialize channel */ @@ -364,19 +373,42 @@ return (ch); } /* ng_l2cap_new_chan */ -/* - * Get channel by source (local) channel ID - */ ng_l2cap_chan_p -ng_l2cap_chan_by_scid(ng_l2cap_p l2cap, u_int16_t scid) +ng_l2cap_chan_by_scid(ng_l2cap_p l2cap, u_int16_t scid, int idtype) { ng_l2cap_chan_p ch = NULL; - LIST_FOREACH(ch, &l2cap->chan_list, next) + if(idtype == NG_L2CAP_L2CA_IDTYPE_ATT){ + return NULL; + } + + LIST_FOREACH(ch, &l2cap->chan_list, next){ + if((idtype != NG_L2CAP_L2CA_IDTYPE_BREDR)&& + (ch->con->linktype == NG_HCI_LINK_ACL )) + continue; + if((idtype != NG_L2CAP_L2CA_IDTYPE_LE)&& + (ch->con->linktype != NG_HCI_LINK_ACL )) + continue; + if (ch->scid == scid) break; + } + return (ch); +} /* ng_l2cap_chan_by_scid */ +ng_l2cap_chan_p +ng_l2cap_chan_by_conhandle(ng_l2cap_p l2cap, uint16_t scid, + u_int16_t con_handle) +{ + ng_l2cap_chan_p ch = NULL; + + + LIST_FOREACH(ch, &l2cap->chan_list, next){ + if ((ch->scid == scid) && + (ch->con->con_handle == con_handle)) + break; + } return (ch); } /* ng_l2cap_chan_by_scid */ @@ -390,6 +422,7 @@ ng_l2cap_cmd_p f = NULL, n = NULL; f = TAILQ_FIRST(&ch->con->cmd_list); + while (f != NULL) { n = TAILQ_NEXT(f, next); @@ -591,21 +624,40 @@ */ static u_int16_t -ng_l2cap_get_cid(ng_l2cap_p l2cap) +ng_l2cap_get_cid(ng_l2cap_p l2cap,int isle) { - u_int16_t cid = l2cap->cid + 1; - + u_int16_t cid ; + u_int16_t endcid; + uint16_t mask; + int idtype; + if(isle){ + endcid = l2cap->lecid; + /*Assume Last CID is 2^n-1 */ + mask = NG_L2CAP_LELAST_CID; + idtype = NG_L2CAP_L2CA_IDTYPE_LE; + }else{ + endcid = l2cap->cid; + /*Assume Last CID is 2^n-1 */ + mask = NG_L2CAP_LAST_CID; + idtype = NG_L2CAP_L2CA_IDTYPE_BREDR; + } + cid = (endcid+1) & mask; + if (cid < NG_L2CAP_FIRST_CID) cid = NG_L2CAP_FIRST_CID; - while (cid != l2cap->cid) { - if (ng_l2cap_chan_by_scid(l2cap, cid) == NULL) { - l2cap->cid = cid; - + while (cid != endcid) { + if (ng_l2cap_chan_by_scid(l2cap, cid, idtype) == NULL) { + if(!isle){ + l2cap->cid = cid; + }else{ + l2cap->lecid = cid; + } return (cid); } cid ++; + cid &= mask; if (cid < NG_L2CAP_FIRST_CID) cid = NG_L2CAP_FIRST_CID; } Index: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c =================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c @@ -81,10 +81,10 @@ ip = (ng_l2cap_l2ca_con_ip *)(msg->data); /* Check if we have connection to the remote unit */ - con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); + con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr, ip->linktype); if (con == NULL) { /* Submit LP_ConnectReq to the lower layer */ - error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr); + error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr,ip->linktype); if (error != 0) { NG_L2CAP_ERR( "%s: %s - unable to send LP_ConnectReq message, error=%d\n", @@ -93,7 +93,7 @@ } /* This should not fail */ - con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); + con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr, ip->linktype); KASSERT((con != NULL), ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node))); } @@ -103,7 +103,7 @@ * not touch connection descriptor. */ - ch = ng_l2cap_new_chan(l2cap, con, ip->psm); + ch = ng_l2cap_new_chan(l2cap, con, ip->psm, ip->idtype); if (ch == NULL) { error = ENOMEM; goto out; @@ -126,7 +126,13 @@ } /* Create L2CAP command packet */ - _ng_l2cap_con_req(cmd->aux, cmd->ident, ch->psm, ch->scid); + if(ip->idtype == NG_L2CAP_L2CA_IDTYPE_ATT){ + _ng_l2cap_con_rsp(cmd->aux, cmd->ident, NG_L2CAP_ATT_CID, + NG_L2CAP_ATT_CID, 0, 0); + cmd->aux->m_flags |= M_PROTO2; + }else{ + _ng_l2cap_con_req(cmd->aux, cmd->ident, ch->psm, ch->scid); + } if (cmd->aux == NULL) { ng_l2cap_free_cmd(cmd); ng_l2cap_free_chan(ch); @@ -182,8 +188,16 @@ * What about PENDING? What the heck, for now always populate * LCID :) */ + if(ch->scid == NG_L2CAP_ATT_CID){ + op->idtype = NG_L2CAP_L2CA_IDTYPE_ATT; + op->lcid = ch->con->con_handle; + }else{ + op->idtype = (ch->con->linktype == NG_HCI_LINK_ACL)? + NG_L2CAP_L2CA_IDTYPE_BREDR : + NG_L2CAP_L2CA_IDTYPE_LE; + op->lcid = ch->scid; + } - op->lcid = ch->scid; op->result = result; op->status = status; @@ -220,7 +234,15 @@ 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(ip->lcid != NG_L2CAP_ATT_CID){ + ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid + ,(ip->linktype == NG_HCI_LINK_ACL)? + NG_L2CAP_L2CA_IDTYPE_BREDR: + NG_L2CAP_L2CA_IDTYPE_LE); + }else{ + // For now not support on ATT device. + ch = NULL; + } if (ch == NULL) { NG_L2CAP_ALERT( "%s: %s - unexpected L2CA_ConnectRsp request message. " \ @@ -259,7 +281,8 @@ /* Check result */ switch (ip->result) { case NG_L2CAP_SUCCESS: - ch->state = NG_L2CAP_CONFIG; + ch->state = (ch->scid == NG_L2CAP_ATT_CID)? + NG_L2CAP_OPEN : NG_L2CAP_CONFIG; ch->cfg_state = 0; break; @@ -410,7 +433,7 @@ ip = (ng_l2cap_l2ca_cfg_ip *)(msg->data); /* Check if we have this channel */ - ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); + ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid, NG_L2CAP_L2CA_IDTYPE_BREDR); if (ch == NULL) { NG_L2CAP_ERR( "%s: %s - unexpected L2CA_Config request message. " \ @@ -478,7 +501,8 @@ /* Adjust channel state for re-configuration */ if (ch->state == NG_L2CAP_OPEN) { - ch->state = NG_L2CAP_CONFIG; + ch->state = (ch->scid == NG_L2CAP_ATT_CID)? + NG_L2CAP_OPEN : NG_L2CAP_CONFIG; ch->cfg_state = 0; } @@ -580,7 +604,8 @@ ip = (ng_l2cap_l2ca_cfg_rsp_ip *)(msg->data); /* Check if we have this channel */ - ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); + ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid, + NG_L2CAP_L2CA_IDTYPE_BREDR); if (ch == NULL) { NG_L2CAP_ERR( "%s: %s - unexpected L2CA_ConfigRsp request message. " \ @@ -784,16 +809,24 @@ } /* Check channel ID */ - 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; - } + 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->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); + /* 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", @@ -865,8 +898,16 @@ op = (ng_l2cap_l2ca_write_op *)(msg->data); op->result = result; op->length = length; - op->lcid = ch->scid; - + if(ch->scid == NG_L2CAP_ATT_CID){ + op->idtype = NG_L2CAP_L2CA_IDTYPE_ATT; + op->lcid = ch->con->con_handle; + }else{ + op->idtype = (ch->con->linktype == NG_HCI_LINK_ACL)? + NG_L2CAP_L2CA_IDTYPE_BREDR : + NG_L2CAP_L2CA_IDTYPE_LE; + op->lcid = ch->scid; + + } NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); } @@ -885,6 +926,8 @@ 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) @@ -893,11 +936,26 @@ hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *); /* Check channel */ - ch = ng_l2cap_chan_by_scid(l2cap, hdr->dcid); + + 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\n", - __func__, NG_NODE_NAME(l2cap->node), hdr->dcid); +"%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; } @@ -938,6 +996,11 @@ 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; @@ -1091,8 +1154,26 @@ ip = (ng_l2cap_l2ca_discon_ip *)(msg->data); - /* Check if we have this channel */ - ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid); + + if(ip->idtype == NG_L2CAP_L2CA_IDTYPE_ATT){ + /* Don't send Disconnect request on L2CAP Layer*/ + ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_ATT_CID, + ip->lcid); + + if(ch != NULL){ + ng_l2cap_free_chan(ch); + }else{ + NG_L2CAP_ERR( +"%s: %s - unexpected L2CA_Disconnect request message. " \ +"Channel does not exist, conhandle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ip->lcid); + error = EINVAL; + } + goto out; + }else{ + /* Check if we have this channel */ + ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid, ip->idtype); + } if (ch == NULL) { NG_L2CAP_ERR( "%s: %s - unexpected L2CA_Disconnect request message. " \ @@ -1322,10 +1403,10 @@ } /* Check if we have connection to the unit */ - con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); + con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr, NG_HCI_LINK_ACL); if (con == NULL) { /* Submit LP_ConnectReq to the lower layer */ - error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr); + error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr, NG_HCI_LINK_ACL); if (error != 0) { NG_L2CAP_ERR( "%s: %s - unable to send LP_ConnectReq message, error=%d\n", @@ -1334,7 +1415,7 @@ } /* This should not fail */ - con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); + con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr, NG_HCI_LINK_ACL); KASSERT((con != NULL), ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node))); } @@ -1444,10 +1525,10 @@ ip = (ng_l2cap_l2ca_get_info_ip *)(msg->data); /* Check if we have connection to the unit */ - con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); + con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr,ip->linktype); if (con == NULL) { /* Submit LP_ConnectReq to the lower layer */ - error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr); + error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr,ip->linktype); if (error != 0) { NG_L2CAP_ERR( "%s: %s - unable to send LP_ConnectReq message, error=%d\n", @@ -1456,7 +1537,7 @@ } /* This should not fail */ - con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr); + con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr, ip->linktype); KASSERT((con != NULL), ("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node))); } Index: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h =================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h @@ -92,7 +92,9 @@ LIST_HEAD(, ng_l2cap_con) con_list; /* ACL connections */ - u_int16_t cid; /* last allocated CID */ + u_int16_t cid; /* last allocated CID */ + u_int16_t lecid; /* last allocated CID for LE */ + LIST_HEAD(, ng_l2cap_chan) chan_list; /* L2CAP channels */ } ng_l2cap_t; typedef ng_l2cap_t * ng_l2cap_p; @@ -116,6 +118,8 @@ struct callout con_timo; /* connection timeout */ u_int8_t ident; /* last allocated ident */ + uint8_t linktype; + TAILQ_HEAD(, ng_l2cap_cmd) cmd_list; /* pending L2CAP cmds */ struct mbuf *tx_pkt; /* xmitted L2CAP packet */ @@ -148,6 +152,7 @@ u_int16_t scid; /* source channel ID */ u_int16_t dcid; /* destination channel ID */ + uint16_t idtype; u_int16_t imtu; /* incoming channel MTU */ ng_l2cap_flow_t iflow; /* incoming flow control */ Index: head/sys/netgraph/bluetooth/socket/ng_btsocket_hci_raw.c =================================================================== --- head/sys/netgraph/bluetooth/socket/ng_btsocket_hci_raw.c +++ head/sys/netgraph/bluetooth/socket/ng_btsocket_hci_raw.c @@ -876,6 +876,9 @@ /* Commands - Testing */ f = ng_btsocket_hci_raw_sec_filter->commands[NG_HCI_OGF_TESTING - 1]; bit_set(f, NG_HCI_OCF_READ_LOOPBACK_MODE - 1); + /*Commands - LE*/ + f = ng_btsocket_hci_raw_sec_filter->commands[NG_HCI_OGF_LE -1]; + } /* ng_btsocket_hci_raw_init */ /* Index: head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c =================================================================== --- head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c +++ head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c @@ -184,7 +184,7 @@ static int ng_btsocket_l2cap_send_l2ca_con_req (ng_btsocket_l2cap_pcb_p); static int ng_btsocket_l2cap_send_l2ca_con_rsp_req - (u_int32_t, ng_btsocket_l2cap_rtentry_p, bdaddr_p, int, int, int); + (u_int32_t, ng_btsocket_l2cap_rtentry_p, bdaddr_p, int, int, int, int); static int ng_btsocket_l2cap_send_l2ca_cfg_req (ng_btsocket_l2cap_pcb_p); static int ng_btsocket_l2cap_send_l2ca_cfg_rsp @@ -209,15 +209,42 @@ static ng_btsocket_l2cap_pcb_p ng_btsocket_l2cap_pcb_by_addr(bdaddr_p, int); static ng_btsocket_l2cap_pcb_p ng_btsocket_l2cap_pcb_by_token(u_int32_t); -static ng_btsocket_l2cap_pcb_p ng_btsocket_l2cap_pcb_by_cid (bdaddr_p, int); +static ng_btsocket_l2cap_pcb_p ng_btsocket_l2cap_pcb_by_cid (bdaddr_p, int,int); static int ng_btsocket_l2cap_result2errno(int); +static int ng_btsock_l2cap_addrtype_to_linktype(int addrtype); +static int ng_btsock_l2cap_pcb_to_idtype(struct ng_btsocket_l2cap_pcb *); #define ng_btsocket_l2cap_wakeup_input_task() \ taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_queue_task) #define ng_btsocket_l2cap_wakeup_route_task() \ taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_rt_task) + +int ng_btsock_l2cap_pcb_to_idtype(struct ng_btsocket_l2cap_pcb *pcb) +{ + if(pcb->dsttype == BDADDR_BREDR){ + return NG_L2CAP_L2CA_IDTYPE_BREDR; + }else if(pcb->psm == 0){ + return NG_L2CAP_L2CA_IDTYPE_ATT; + }else{ + return NG_L2CAP_L2CA_IDTYPE_LE; + } +} + +int ng_btsock_l2cap_addrtype_to_linktype(int addrtype) +{ + switch(addrtype){ + case BDADDR_LE_PUBLIC: + return NG_HCI_LINK_LE_PUBLIC; + case BDADDR_LE_RANDOM: + return NG_HCI_LINK_LE_RANDOM; + default: + return NG_HCI_LINK_ACL; + } +} + + /***************************************************************************** ***************************************************************************** ** Netgraph node interface @@ -445,28 +472,35 @@ return (0); } - if (op->result == NG_L2CAP_SUCCESS) { - /* - * Channel is now open, so update local channel ID and - * start configuration process. Source and destination - * addresses as well as route must be already set. - */ - - pcb->cid = op->lcid; - - error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb); - if (error != 0) { - /* Send disconnect request with "zero" token */ - ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb); - - /* ... and close the socket */ - pcb->state = NG_BTSOCKET_L2CAP_CLOSED; - soisdisconnected(pcb->so); - } else { - pcb->cfg_state = NG_BTSOCKET_L2CAP_CFG_IN_SENT; - pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING; - - ng_btsocket_l2cap_timeout(pcb); + if (op->result == NG_L2CAP_SUCCESS){ + if(ng_btsock_l2cap_pcb_to_idtype(pcb) == + NG_L2CAP_L2CA_IDTYPE_ATT){ + pcb->state = NG_BTSOCKET_L2CAP_OPEN; + soisconnected(pcb->so); + pcb->cid = op->lcid; + }else{ + /* + * Channel is now open, so update local channel ID and + * start configuration process. Source and destination + * addresses as well as route must be already set. + */ + + pcb->cid = op->lcid; + + error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb); + if (error != 0) { + /* Send disconnect request with "zero" token */ + ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb); + + /* ... and close the socket */ + pcb->state = NG_BTSOCKET_L2CAP_CLOSED; + soisdisconnected(pcb->so); + } else { + pcb->cfg_state = NG_BTSOCKET_L2CAP_CFG_IN_SENT; + pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING; + + ng_btsocket_l2cap_timeout(pcb); + } } } else { /* @@ -643,7 +677,9 @@ respond: error = ng_btsocket_l2cap_send_l2ca_con_rsp_req(token, rt, - &ip->bdaddr, ip->ident, ip->lcid, result); + &ip->bdaddr, + ip->ident, ip->lcid, + result,ip->linktype); if (pcb1 != NULL) { if (error != 0) { pcb1->so->so_error = error; @@ -899,7 +935,8 @@ mtx_lock(&ng_btsocket_l2cap_sockets_mtx); /* Check for the open socket that has given channel ID */ - pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid); + pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid, + NG_L2CAP_L2CA_IDTYPE_BREDR); if (pcb == NULL) { mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); return (ENOENT); @@ -1040,7 +1077,8 @@ mtx_lock(&ng_btsocket_l2cap_sockets_mtx); /* Look for the socket with given channel ID */ - pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid); + pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid, + NG_L2CAP_L2CA_IDTYPE_BREDR); if (pcb == NULL) { mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); return (0); @@ -1176,7 +1214,8 @@ ip = (ng_l2cap_l2ca_con_ip *)(msg->data); bcopy(&pcb->dst, &ip->bdaddr, sizeof(ip->bdaddr)); ip->psm = pcb->psm; - + ip->linktype = ng_btsock_l2cap_addrtype_to_linktype(pcb->dsttype); + ip->idtype = ng_btsock_l2cap_pcb_to_idtype(pcb); NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0); return (error); @@ -1189,7 +1228,7 @@ static int ng_btsocket_l2cap_send_l2ca_con_rsp_req(u_int32_t token, ng_btsocket_l2cap_rtentry_p rt, bdaddr_p dst, int ident, - int lcid, int result) + int lcid, int result, int linktype) { struct ng_mesg *msg = NULL; ng_l2cap_l2ca_con_rsp_ip *ip = NULL; @@ -1209,6 +1248,7 @@ bcopy(dst, &ip->bdaddr, sizeof(ip->bdaddr)); ip->ident = ident; ip->lcid = lcid; + ip->linktype = linktype; ip->result = result; ip->status = 0; @@ -1314,6 +1354,7 @@ ip = (ng_l2cap_l2ca_discon_ip *)(msg->data); ip->lcid = pcb->cid; + ip->idtype = ng_btsock_l2cap_pcb_to_idtype(pcb); NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0); @@ -1337,6 +1378,7 @@ ng_l2cap_clt_hdr_t *clt_hdr = NULL; ng_btsocket_l2cap_pcb_t *pcb = NULL; ng_btsocket_l2cap_rtentry_t *rt = NULL; + uint16_t idtype; if (hook == NULL) { NG_BTSOCKET_L2CAP_ALERT( @@ -1351,6 +1393,10 @@ goto drop; } + m = m_pullup(m, sizeof(uint16_t)); + idtype = *mtod(m, uint16_t *); + m_adj(m, sizeof(uint16_t)); + /* Make sure we can access header */ if (m->m_pkthdr.len < sizeof(*hdr)) { NG_BTSOCKET_L2CAP_ERR( @@ -1394,12 +1440,13 @@ rt->src.b[2], rt->src.b[1], rt->src.b[0], hdr->dcid, hdr->length); - if (hdr->dcid >= NG_L2CAP_FIRST_CID) { + if ((hdr->dcid >= NG_L2CAP_FIRST_CID) || + (idtype == NG_L2CAP_L2CA_IDTYPE_ATT)){ mtx_lock(&ng_btsocket_l2cap_sockets_mtx); /* Normal packet: find connected socket */ - pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, hdr->dcid); + pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, hdr->dcid,idtype); if (pcb == NULL) { mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); goto drop; @@ -1557,11 +1604,12 @@ switch (msg->header.cmd) { case NGM_L2CAP_NODE_HOOK_INFO: { ng_btsocket_l2cap_rtentry_t *rt = NULL; - - if (hook == NULL || msg->header.arglen != sizeof(bdaddr_t)) + ng_l2cap_node_hook_info_ep *ep = + (ng_l2cap_node_hook_info_ep *)msg->data; + if (hook == NULL || msg->header.arglen != sizeof(*ep)) break; - if (bcmp(msg->data, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) + if (bcmp(&ep->addr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) break; mtx_lock(&ng_btsocket_l2cap_rt_mtx); @@ -1580,7 +1628,7 @@ NG_HOOK_SET_PRIVATE(hook, rt); } - bcopy(msg->data, &rt->src, sizeof(rt->src)); + bcopy(&ep->addr, &rt->src, sizeof(rt->src)); rt->hook = hook; mtx_unlock(&ng_btsocket_l2cap_rt_mtx); @@ -2035,7 +2083,9 @@ return (EINVAL); if (sa->l2cap_family != AF_BLUETOOTH) return (EAFNOSUPPORT); - if (sa->l2cap_len != sizeof(*sa)) + /*For the time being, Not support LE binding.*/ + if ((sa->l2cap_len != sizeof(*sa))&& + (sa->l2cap_len != sizeof(struct sockaddr_l2cap_compat))) return (EINVAL); psm = le16toh(sa->l2cap_psm); @@ -2080,7 +2130,9 @@ struct thread *td) { ng_btsocket_l2cap_pcb_t *pcb = so2l2cap_pcb(so); - struct sockaddr_l2cap *sa = (struct sockaddr_l2cap *) nam; + struct sockaddr_l2cap_compat *sal = (struct sockaddr_l2cap_compat *) nam; + struct sockaddr_l2cap *sa = (struct sockaddr_l2cap *)nam; + struct sockaddr_l2cap ba; ng_btsocket_l2cap_rtentry_t *rt = NULL; int have_src, error = 0; @@ -2097,14 +2149,27 @@ return (EINVAL); if (sa->l2cap_family != AF_BLUETOOTH) return (EAFNOSUPPORT); + if (sa->l2cap_len == sizeof(*sal)){ + bcopy(sal, &ba, sizeof(*sal)); + sa = &ba; + sa->l2cap_len = sizeof(*sa); + sa->l2cap_bdaddr_type = BDADDR_BREDR; + } if (sa->l2cap_len != sizeof(*sa)) return (EINVAL); - if (sa->l2cap_psm == 0 || - bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) + if ((sa->l2cap_psm && sa->l2cap_cid)) + return EINVAL; + if (bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) return (EDESTADDRREQ); + if((sa->l2cap_bdaddr_type == BDADDR_BREDR)&& + (sa->l2cap_psm == 0)) + return EDESTADDRREQ; + if((sa->l2cap_bdaddr_type != BDADDR_BREDR)&& + (sa->l2cap_cid != NG_L2CAP_ATT_CID)){ + return EINVAL; + } if (pcb->psm != 0 && pcb->psm != le16toh(sa->l2cap_psm)) return (EINVAL); - /* * Routing. Socket should be bound to some source address. The source * address can be ANY. Destination address must be set and it must not @@ -2119,7 +2184,9 @@ /* Send destination address and PSM */ bcopy(&sa->l2cap_bdaddr, &pcb->dst, sizeof(pcb->dst)); pcb->psm = le16toh(sa->l2cap_psm); - + pcb->dsttype = sa->l2cap_bdaddr_type; + pcb->cid = sa->l2cap_cid; + pcb->rt = NULL; have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)); @@ -2140,8 +2207,12 @@ if (rt != NULL) { pcb->rt = rt; - if (!have_src) + if (!have_src){ bcopy(&rt->src, &pcb->src, sizeof(pcb->src)); + pcb->srctype = + (sa->l2cap_bdaddr_type == BDADDR_BREDR)? + BDADDR_BREDR : BDADDR_LE_RANDOM; + } } else error = EHOSTUNREACH; @@ -2418,7 +2489,8 @@ sa.l2cap_psm = htole16(pcb->psm); sa.l2cap_len = sizeof(sa); sa.l2cap_family = AF_BLUETOOTH; - + sa.l2cap_cid = 0; + sa.l2cap_bdaddr_type = pcb->dsttype; *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); return ((*nam == NULL)? ENOMEM : 0); @@ -2536,7 +2608,7 @@ hdr->token = pcb->token; hdr->length = m->m_pkthdr.len - sizeof(*hdr); hdr->lcid = pcb->cid; - + hdr->idtype = ng_btsock_l2cap_pcb_to_idtype(pcb); NG_BTSOCKET_L2CAP_INFO( "%s: Sending packet: len=%d, length=%d, lcid=%d, token=%d, state=%d\n", __func__, m->m_pkthdr.len, hdr->length, hdr->lcid, @@ -2571,6 +2643,8 @@ sa.l2cap_psm = htole16(pcb->psm); sa.l2cap_len = sizeof(sa); sa.l2cap_family = AF_BLUETOOTH; + sa.l2cap_cid = 0; + sa.l2cap_bdaddr_type = pcb->srctype; *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); @@ -2638,16 +2712,19 @@ */ static ng_btsocket_l2cap_pcb_p -ng_btsocket_l2cap_pcb_by_cid(bdaddr_p src, int cid) +ng_btsocket_l2cap_pcb_by_cid(bdaddr_p src, int cid, int idtype) { ng_btsocket_l2cap_pcb_p p = NULL; mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED); - LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next) - if (p->cid == cid && bcmp(src, &p->src, sizeof(p->src)) == 0) + LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next){ + if (p->cid == cid && + bcmp(src, &p->src, sizeof(p->src)) == 0&& + ng_btsock_l2cap_pcb_to_idtype(p) == idtype) break; + } return (p); } /* ng_btsocket_l2cap_pcb_by_cid */ Index: head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap_raw.c =================================================================== --- head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap_raw.c +++ head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap_raw.c @@ -667,7 +667,8 @@ return (EINVAL); if (sa->l2cap_family != AF_BLUETOOTH) return (EAFNOSUPPORT); - if (sa->l2cap_len != sizeof(*sa)) + if((sa->l2cap_len != sizeof(*sa))&& + (sa->l2cap_len != sizeof(struct sockaddr_l2cap_compat))) return (EINVAL); if (bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, @@ -720,8 +721,10 @@ return (EINVAL); if (sa->l2cap_family != AF_BLUETOOTH) return (EAFNOSUPPORT); - if (sa->l2cap_len != sizeof(*sa)) + if((sa->l2cap_len != sizeof(*sa))&& + (sa->l2cap_len != sizeof(struct sockaddr_l2cap_compat))) return (EINVAL); + if (bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) return (EINVAL); @@ -1179,6 +1182,8 @@ sa.l2cap_psm = 0; sa.l2cap_len = sizeof(sa); sa.l2cap_family = AF_BLUETOOTH; + sa.l2cap_cid = 0; + sa.l2cap_bdaddr_type = BDADDR_BREDR; *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); @@ -1221,7 +1226,8 @@ sa.l2cap_psm = 0; sa.l2cap_len = sizeof(sa); sa.l2cap_family = AF_BLUETOOTH; - + sa.l2cap_cid = 0; + sa.l2cap_bdaddr_type = BDADDR_BREDR; *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); return ((*nam == NULL)? ENOMEM : 0);