Author: takawata Date: Tue Oct 27 03:42:26 2015 New Revision: 290038 URL: https://svnweb.freebsd.org/changeset/base/290038
Log: Bluetooth LE Security Management channel support. Add a socket option to block until underlying HCI connection encrypted. Differential Revision: https://reviews.freebsd.org/D3981 Modified: head/sys/netgraph/bluetooth/hci/ng_hci_evnt.c head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h head/sys/netgraph/bluetooth/include/ng_btsocket.h head/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h head/sys/netgraph/bluetooth/include/ng_hci.h head/sys/netgraph/bluetooth/include/ng_l2cap.h head/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h head/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c head/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h head/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c Modified: head/sys/netgraph/bluetooth/hci/ng_hci_evnt.c ============================================================================== --- head/sys/netgraph/bluetooth/hci/ng_hci_evnt.c Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/hci/ng_hci_evnt.c Tue Oct 27 03:42:26 2015 (r290038) @@ -929,7 +929,7 @@ encryption_change(ng_hci_unit_p unit, st "%s: %s - invalid connection handle=%d\n", __func__, NG_NODE_NAME(unit->node), h); error = ENOENT; - } else if (con->link_type != NG_HCI_LINK_ACL) { + } else if (con->link_type == NG_HCI_LINK_SCO) { NG_HCI_ALERT( "%s: %s - invalid link type=%d\n", __func__, NG_NODE_NAME(unit->node), @@ -940,6 +940,7 @@ encryption_change(ng_hci_unit_p unit, st con->encryption_mode = NG_HCI_ENCRYPTION_MODE_P2P; else con->encryption_mode = NG_HCI_ENCRYPTION_MODE_NONE; + ng_hci_lp_enc_change(con, ep->encryption_enable); } else NG_HCI_ERR( "%s: %s - failed to change encryption mode, status=%d\n", Modified: head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c ============================================================================== --- head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c Tue Oct 27 03:42:26 2015 (r290038) @@ -814,6 +814,37 @@ ng_hci_lp_con_cfm(ng_hci_unit_con_p con, return (0); } /* ng_hci_lp_con_cfm */ +int +ng_hci_lp_enc_change(ng_hci_unit_con_p con, int status) +{ + ng_hci_unit_p unit = con->unit; + struct ng_mesg *msg = NULL; + ng_hci_lp_enc_change_ep *ep = NULL; + int error; + + + 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_ENC_CHG, + sizeof(*ep), M_NOWAIT); + if (msg != NULL) { + ep = (ng_hci_lp_enc_change_ep *) msg->data; + ep->status = status; + ep->link_type = con->link_type; + ep->con_handle = con->con_handle; + + NG_SEND_MSG_HOOK(error, unit->node, msg, + unit->acl, 0); + } + } else + NG_HCI_INFO( +"%s: %s - ACL hook not valid, hook=%p\n", + __func__, NG_NODE_NAME(unit->node), unit->acl); + + } + return (0); +} /* ng_hci_lp_con_cfm */ + /* * Send LP_ConnectInd event to the upper layer protocol */ Modified: head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h ============================================================================== --- head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h Tue Oct 27 03:42:26 2015 (r290038) @@ -47,6 +47,7 @@ int ng_hci_lp_discon_ind int ng_hci_lp_qos_req (ng_hci_unit_p, item_p, hook_p); int ng_hci_lp_qos_cfm (ng_hci_unit_con_p, int); int ng_hci_lp_qos_ind (ng_hci_unit_con_p); +int ng_hci_lp_enc_change (ng_hci_unit_con_p, int); void ng_hci_process_con_timeout (node_p, hook_p, void *, int); Modified: head/sys/netgraph/bluetooth/include/ng_btsocket.h ============================================================================== --- head/sys/netgraph/bluetooth/include/ng_btsocket.h Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/include/ng_btsocket.h Tue Oct 27 03:42:26 2015 (r290038) @@ -255,7 +255,7 @@ struct sockaddr_l2cap { #define SO_L2CAP_IFLOW 3 /* get incoming flow spec. */ #define SO_L2CAP_OFLOW 4 /* get/set outgoing flow spec. */ #define SO_L2CAP_FLUSH 5 /* get/set flush timeout */ - +#define SO_L2CAP_ENCRYPTED 6 /* get/set whether wait for encryptin on connect */ /* * Raw L2CAP sockets ioctl's */ Modified: head/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h ============================================================================== --- head/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h Tue Oct 27 03:42:26 2015 (r290038) @@ -136,7 +136,7 @@ struct ng_btsocket_l2cap_pcb { u_int16_t psm; /* PSM */ u_int16_t cid; /* Local channel ID */ - + uint8_t idtype; u_int16_t flags; /* socket flags */ #define NG_BTSOCKET_L2CAP_CLIENT (1 << 0) /* socket is client */ #define NG_BTSOCKET_L2CAP_TIMO (1 << 1) /* timeout pending */ @@ -147,6 +147,7 @@ struct ng_btsocket_l2cap_pcb { #define NG_BTSOCKET_L2CAP_CONFIGURING 2 /* wait for config */ #define NG_BTSOCKET_L2CAP_OPEN 3 /* socket open */ #define NG_BTSOCKET_L2CAP_DISCONNECTING 4 /* wait for disconnect */ +#define NG_BTSOCKET_L2CAP_W4_ENC_CHANGE 5 u_int8_t cfg_state; /* config state */ #define NG_BTSOCKET_L2CAP_CFG_IN (1 << 0) /* incoming path done */ @@ -156,7 +157,7 @@ struct ng_btsocket_l2cap_pcb { #define NG_BTSOCKET_L2CAP_CFG_IN_SENT (1 << 2) /* L2CAP ConfigReq sent */ #define NG_BTSOCKET_L2CAP_CFG_OUT_SENT (1 << 3) /* ---/--- */ - + uint8_t encryption; u_int16_t imtu; /* Incoming MTU */ ng_l2cap_flow_t iflow; /* Input flow spec */ @@ -172,7 +173,8 @@ struct ng_btsocket_l2cap_pcb { ng_btsocket_l2cap_rtentry_p rt; /* routing info */ struct mtx pcb_mtx; /* pcb mutex */ - + uint16_t need_encrypt; /*encryption needed*/ + LIST_ENTRY(ng_btsocket_l2cap_pcb) next; /* link to next PCB */ }; typedef struct ng_btsocket_l2cap_pcb ng_btsocket_l2cap_pcb_t; Modified: head/sys/netgraph/bluetooth/include/ng_hci.h ============================================================================== --- head/sys/netgraph/bluetooth/include/ng_hci.h Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/include/ng_hci.h Tue Oct 27 03:42:26 2015 (r290038) @@ -469,7 +469,13 @@ typedef struct { typedef struct { u_int16_t con_handle; /* connection handle */ } ng_hci_lp_qos_ind_ep; - +/*Encryption Change event*/ +#define NGM_HCI_LP_ENC_CHG 10 /* HCI->Upper*/ +typedef struct { + uint16_t con_handle; + uint8_t status; + uint8_t link_type; +}ng_hci_lp_enc_change_ep; /************************************************************************** ************************************************************************** ** HCI node command/event parameters Modified: head/sys/netgraph/bluetooth/include/ng_l2cap.h ============================================================================== --- head/sys/netgraph/bluetooth/include/ng_l2cap.h Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/include/ng_l2cap.h Tue Oct 27 03:42:26 2015 (r290038) @@ -256,6 +256,7 @@ typedef union { u_int16_t mtu; /* NG_L2CAP_OPT_MTU */ u_int16_t flush_timo; /* NG_L2CAP_OPT_FLUSH_TIMO */ ng_l2cap_flow_t flow; /* NG_L2CAP_OPT_QOS */ + uint16_t encryption; } ng_l2cap_cfg_opt_val_t; typedef ng_l2cap_cfg_opt_val_t * ng_l2cap_cfg_opt_val_p; @@ -357,6 +358,7 @@ typedef struct { #define NG_L2CAP_L2CA_IDTYPE_BREDR 0 #define NG_L2CAP_L2CA_IDTYPE_ATT 1 #define NG_L2CAP_L2CA_IDTYPE_LE 2 +#define NG_L2CAP_L2CA_IDTYPE_SMP 3 /* L2CA_Connect */ #define NGM_L2CAP_L2CA_CON 0x80 /* Upper -> L2CAP */ @@ -373,6 +375,7 @@ typedef struct { uint16_t idtype; /*ID type*/ u_int16_t result; /* 0x00 - success */ u_int16_t status; /* if result != 0x00 */ + uint8_t encryption; } ng_l2cap_l2ca_con_op; /* L2CA_ConnectInd */ @@ -598,6 +601,12 @@ typedef struct { * u_int16_t result; /* 0x00 - success */ * } ng_l2cap_l2ca_enable_clt_op; #endif +#define NGM_L2CAP_L2CA_ENC_CHANGE 0x92 +typedef struct { + uint16_t lcid; + uint16_t result; + uint8_t idtype; +} ng_l2cap_l2ca_enc_chg_op; /************************************************************************** ************************************************************************** Modified: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c ============================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c Tue Oct 27 03:42:26 2015 (r290038) @@ -475,6 +475,8 @@ ng_l2cap_process_con_req(ng_l2cap_con_p con->rx_pkt = NULL; if(dcid == NG_L2CAP_ATT_CID) idtype = NG_L2CAP_L2CA_IDTYPE_ATT; + else if(dcid == NG_L2CAP_SMP_CID) + idtype = NG_L2CAP_L2CA_IDTYPE_SMP; else if( con->linktype != NG_HCI_LINK_ACL) idtype = NG_L2CAP_L2CA_IDTYPE_LE; else @@ -602,7 +604,9 @@ ng_l2cap_process_con_rsp(ng_l2cap_con_p */ cmd->ch->dcid = dcid; - cmd->ch->state = (cmd->ch->scid == NG_L2CAP_ATT_CID)? + cmd->ch->state = ((cmd->ch->scid == NG_L2CAP_ATT_CID)|| + (cmd->ch->scid == NG_L2CAP_SMP_CID)) + ? NG_L2CAP_OPEN : NG_L2CAP_CONFIG; } else /* There was an error, so close the channel */ Modified: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c ============================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c Tue Oct 27 03:42:26 2015 (r290038) @@ -481,6 +481,58 @@ out: return (error); } /* ng_l2cap_qos_ind */ +int +ng_l2cap_lp_enc_change(ng_l2cap_p l2cap, struct ng_mesg *msg) +{ + ng_hci_lp_enc_change_ep *ep = NULL; + ng_l2cap_con_p con = NULL; + int error = 0; + ng_l2cap_chan_p ch = NULL; + /* Check message */ + if (msg->header.arglen != sizeof(*ep)) { + NG_L2CAP_ALERT( +"%s: %s - invalid LP_ENCChange message size\n", + __func__, NG_NODE_NAME(l2cap->node)); + error = EMSGSIZE; + goto out; + } + + ep = (ng_hci_lp_enc_change_ep *) (msg->data); + + /* Check if we have this connection */ + con = ng_l2cap_con_by_handle(l2cap, ep->con_handle); + if (con == NULL) { + NG_L2CAP_ERR( +"%s: %s - unexpected LP_Enc Change Event. " \ +"Connection does not exist, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ep->con_handle); + error = ENOENT; + goto out; + } + + /* Verify connection state */ + if (con->state != NG_L2CAP_CON_OPEN) { + NG_L2CAP_ERR( +"%s: %s - unexpected ENC_CHANGE event. " \ +"Invalid connection state, state=%d, con_handle=%d\n", + __func__, NG_NODE_NAME(l2cap->node), con->state, + con->con_handle); + error = EINVAL; + goto out; + } + + con->encryption = ep->status; + + LIST_FOREACH(ch, &l2cap->chan_list, next){ + if((ch->con->con_handle == ep->con_handle) && + (ch->con->linktype == ep->link_type)) + ng_l2cap_l2ca_encryption_change(ch, ep->status); + } + +out: + return (error); +} /* ng_l2cap_enc_change */ + /* * Prepare L2CAP packet. Prepend packet with L2CAP packet header and then * segment it according to HCI MTU. Modified: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h ============================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h Tue Oct 27 03:42:26 2015 (r290038) @@ -41,6 +41,7 @@ int ng_l2cap_lp_discon_ind (ng int ng_l2cap_lp_qos_req (ng_l2cap_p, u_int16_t, ng_l2cap_flow_p); int ng_l2cap_lp_qos_cfm (ng_l2cap_p, struct ng_mesg *); int ng_l2cap_lp_qos_ind (ng_l2cap_p, struct ng_mesg *); +int ng_l2cap_lp_enc_change (ng_l2cap_p, struct ng_mesg *); int ng_l2cap_lp_send (ng_l2cap_con_p, u_int16_t,struct mbuf *); int ng_l2cap_lp_receive (ng_l2cap_p, struct mbuf *); void ng_l2cap_lp_deliver (ng_l2cap_con_p); Modified: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c ============================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c Tue Oct 27 03:42:26 2015 (r290038) @@ -350,7 +350,9 @@ ng_l2cap_lower_rcvmsg(node_p node, item_ case NGM_HCI_LP_QOS_IND: error = ng_l2cap_lp_qos_ind(l2cap, msg); break; - + case NGM_HCI_LP_ENC_CHG: + error = ng_l2cap_lp_enc_change(l2cap, msg); + break; default: error = EINVAL; break; Modified: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c ============================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c Tue Oct 27 03:42:26 2015 (r290038) @@ -114,7 +114,7 @@ ng_l2cap_new_con(ng_l2cap_p l2cap, bdadd con->l2cap = l2cap; con->state = NG_L2CAP_CON_CLOSED; - + con->encryption = 0; /* * XXX * @@ -340,6 +340,8 @@ ng_l2cap_new_chan(ng_l2cap_p l2cap, ng_l return (NULL); if(idtype == NG_L2CAP_L2CA_IDTYPE_ATT){ ch->scid = ch->dcid = NG_L2CAP_ATT_CID; + }else if(idtype == NG_L2CAP_L2CA_IDTYPE_SMP){ + ch->scid = ch->dcid = NG_L2CAP_SMP_CID; }else{ ch->scid = ng_l2cap_get_cid(l2cap, (con->linktype!= NG_HCI_LINK_ACL)); @@ -379,7 +381,8 @@ ng_l2cap_chan_by_scid(ng_l2cap_p l2cap, { ng_l2cap_chan_p ch = NULL; - if(idtype == NG_L2CAP_L2CA_IDTYPE_ATT){ + if((idtype == NG_L2CAP_L2CA_IDTYPE_ATT)|| + (idtype == NG_L2CAP_L2CA_IDTYPE_SMP)){ return NULL; } @@ -390,7 +393,6 @@ ng_l2cap_chan_by_scid(ng_l2cap_p l2cap, if((idtype != NG_L2CAP_L2CA_IDTYPE_LE)&& (ch->con->linktype != NG_HCI_LINK_ACL )) continue; - if (ch->scid == scid) break; } Modified: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c ============================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c Tue Oct 27 03:42:26 2015 (r290038) @@ -130,6 +130,10 @@ ng_l2cap_l2ca_con_req(ng_l2cap_p l2cap, _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 if(ip->idtype == NG_L2CAP_L2CA_IDTYPE_SMP){ + _ng_l2cap_con_rsp(cmd->aux, cmd->ident, NG_L2CAP_SMP_CID, + NG_L2CAP_SMP_CID, 0, 0); + cmd->aux->m_flags |= M_PROTO2; }else{ _ng_l2cap_con_req(cmd->aux, cmd->ident, ch->psm, ch->scid); } @@ -191,13 +195,16 @@ ng_l2cap_l2ca_con_rsp(ng_l2cap_chan_p ch if(ch->scid == NG_L2CAP_ATT_CID){ op->idtype = NG_L2CAP_L2CA_IDTYPE_ATT; op->lcid = ch->con->con_handle; + }else if(ch->scid == NG_L2CAP_SMP_CID){ + op->idtype = NG_L2CAP_L2CA_IDTYPE_SMP; + 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->encryption = ch->con->encryption; op->result = result; op->status = status; @@ -234,7 +241,8 @@ ng_l2cap_l2ca_con_rsp_req(ng_l2cap_p l2c ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data); /* Check if we have this channel */ - if(ip->lcid != NG_L2CAP_ATT_CID){ + if((ip->lcid != NG_L2CAP_ATT_CID)&& + (ip->lcid != NG_L2CAP_SMP_CID)){ ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid ,(ip->linktype == NG_HCI_LINK_ACL)? NG_L2CAP_L2CA_IDTYPE_BREDR: @@ -281,7 +289,8 @@ ng_l2cap_l2ca_con_rsp_req(ng_l2cap_p l2c /* Check result */ switch (ip->result) { case NG_L2CAP_SUCCESS: - ch->state = (ch->scid == NG_L2CAP_ATT_CID)? + ch->state = ((ch->scid == NG_L2CAP_ATT_CID)|| + (ch->scid == NG_L2CAP_SMP_CID))? NG_L2CAP_OPEN : NG_L2CAP_CONFIG; ch->cfg_state = 0; break; @@ -324,6 +333,53 @@ out: return (error); } /* ng_l2cap_l2ca_con_rsp_req */ +int ng_l2cap_l2ca_encryption_change(ng_l2cap_chan_p ch, uint16_t result) +{ + ng_l2cap_p l2cap = ch->con->l2cap; + struct ng_mesg *msg = NULL; + ng_l2cap_l2ca_enc_chg_op *op = NULL; + int error = 0; + + /* Check if upstream hook is connected and valid */ + if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) { + NG_L2CAP_ERR( +"%s: %s - unable to send L2CA_ConnectRsp response message. " \ +"Hook is not connected or valid, psm=%d\n", + __func__, NG_NODE_NAME(l2cap->node), ch->psm); + + return (ENOTCONN); + } + + /* Create and send L2CA_ConnectRsp response message */ + NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_ENC_CHANGE, + sizeof(*op), M_NOWAIT); + if (msg == NULL) + error = ENOMEM; + else { + msg->header.token = 0; + msg->header.flags |= NGF_RESP; + + op = (ng_l2cap_l2ca_enc_chg_op *)(msg->data); + op->result = result; + if(ch->scid ==NG_L2CAP_ATT_CID|| + ch->scid ==NG_L2CAP_SMP_CID){ + op->lcid = ch->con->con_handle; + op->idtype = (ch->scid==NG_L2CAP_ATT_CID)? + NG_L2CAP_L2CA_IDTYPE_ATT: + NG_L2CAP_L2CA_IDTYPE_SMP; + }else{ + op->idtype =(ch->con->linktype ==NG_HCI_LINK_ACL)? + NG_L2CAP_L2CA_IDTYPE_BREDR: + NG_L2CAP_L2CA_IDTYPE_LE; + } + + + NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); + } + + return (error); + +} /* * Send L2CAP_ConnectRsp response to the upper layer */ @@ -399,6 +455,7 @@ ng_l2cap_l2ca_con_ind(ng_l2cap_chan_p ch ip->psm = ch->psm; ip->ident = ch->ident; ip->linktype = ch->con->linktype; + NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->l2c, 0); } @@ -501,7 +558,8 @@ ng_l2cap_l2ca_cfg_req(ng_l2cap_p l2cap, /* Adjust channel state for re-configuration */ if (ch->state == NG_L2CAP_OPEN) { - ch->state = (ch->scid == NG_L2CAP_ATT_CID)? + ch->state = ((ch->scid == NG_L2CAP_ATT_CID)|| + (ch->scid == NG_L2CAP_SMP_CID))? NG_L2CAP_OPEN : NG_L2CAP_CONFIG; ch->cfg_state = 0; } @@ -812,7 +870,10 @@ ng_l2cap_l2ca_write_req(ng_l2cap_p l2cap if (l2ca_hdr->idtype == NG_L2CAP_L2CA_IDTYPE_ATT){ ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_ATT_CID, l2ca_hdr->lcid); - } else{ + } else if (l2ca_hdr->idtype == NG_L2CAP_L2CA_IDTYPE_SMP){ + ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_SMP_CID, + l2ca_hdr->lcid); + }else{ if (l2ca_hdr->lcid < NG_L2CAP_FIRST_CID) { NG_L2CAP_ERR( "%s: %s - invalid L2CA Data packet. Inavlid channel ID, cid=%d\n", @@ -901,6 +962,9 @@ ng_l2cap_l2ca_write_rsp(ng_l2cap_chan_p if(ch->scid == NG_L2CAP_ATT_CID){ op->idtype = NG_L2CAP_L2CA_IDTYPE_ATT; op->lcid = ch->con->con_handle; + }else if(ch->scid == NG_L2CAP_SMP_CID){ + op->idtype = NG_L2CAP_L2CA_IDTYPE_SMP; + op->lcid = ch->con->con_handle; }else{ op->idtype = (ch->con->linktype == NG_HCI_LINK_ACL)? NG_L2CAP_L2CA_IDTYPE_BREDR : @@ -928,7 +992,8 @@ ng_l2cap_l2ca_receive(ng_l2cap_con_p con int error = 0; int idtype; uint16_t *idp; - + int silent = 0; + NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); if (con->rx_pkt == NULL) return (ENOBUFS); @@ -945,6 +1010,17 @@ ng_l2cap_l2ca_receive(ng_l2cap_con_p con * Here,ATT channel is distinguished by * connection handle */ + hdr->dcid = con->con_handle; + silent = 1; + }else if(hdr->dcid == NG_L2CAP_SMP_CID){ + idtype = NG_L2CAP_L2CA_IDTYPE_SMP; + ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_SMP_CID, + con->con_handle); + /* + * Here,SMP channel is distinguished by + * connection handle + */ + silent = 1; hdr->dcid = con->con_handle; }else{ idtype = (con->linktype==NG_HCI_LINK_ACL)? @@ -953,7 +1029,8 @@ ng_l2cap_l2ca_receive(ng_l2cap_con_p con ch = ng_l2cap_chan_by_scid(l2cap, hdr->dcid, idtype); } if (ch == NULL) { - NG_L2CAP_ERR( + if(!silent) + NG_L2CAP_ERR( "%s: %s - unexpected L2CAP data packet. Channel does not exist, cid=%d, idtype=%d\n", __func__, NG_NODE_NAME(l2cap->node), hdr->dcid, idtype); error = ENOENT; @@ -1170,6 +1247,21 @@ ng_l2cap_l2ca_discon_req(ng_l2cap_p l2ca error = EINVAL; } goto out; + }else if(ip->idtype == NG_L2CAP_L2CA_IDTYPE_SMP){ + /* Don't send Disconnect request on L2CAP Layer*/ + ch = ng_l2cap_chan_by_conhandle(l2cap, NG_L2CAP_SMP_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); Modified: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h ============================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h Tue Oct 27 03:42:26 2015 (r290038) @@ -74,6 +74,6 @@ int ng_l2cap_l2ca_get_info_rsp (ng_l2cap struct mbuf *); int ng_l2cap_l2ca_enable_clt (ng_l2cap_p, struct ng_mesg *); - +int ng_l2cap_l2ca_encryption_change(ng_l2cap_chan_p , uint16_t ); #endif /* ndef _NETGRAPH_L2CAP_ULPI_H_ */ Modified: head/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h ============================================================================== --- head/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h Tue Oct 27 03:42:26 2015 (r290038) @@ -119,7 +119,8 @@ typedef struct ng_l2cap_con { u_int8_t ident; /* last allocated ident */ uint8_t linktype; - + uint8_t encryption; + TAILQ_HEAD(, ng_l2cap_cmd) cmd_list; /* pending L2CAP cmds */ struct mbuf *tx_pkt; /* xmitted L2CAP packet */ Modified: head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c ============================================================================== --- head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c Tue Oct 27 03:02:48 2015 (r290037) +++ head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c Tue Oct 27 03:42:26 2015 (r290038) @@ -213,7 +213,7 @@ static ng_btsocket_l2cap_pcb_p ng_bt 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) @@ -221,16 +221,6 @@ static int ng_btsock_l2cap_pcb_to_idtype 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) { @@ -473,11 +463,15 @@ ng_btsocket_l2cap_process_l2ca_con_req_r } 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; + if((pcb->idtype == NG_L2CAP_L2CA_IDTYPE_ATT)|| + (pcb->idtype == NG_L2CAP_L2CA_IDTYPE_SMP)){ + pcb->encryption = op->encryption; pcb->cid = op->lcid; + if(pcb->need_encrypt && !(pcb->encryption)){ + pcb->state = NG_BTSOCKET_L2CAP_W4_ENC_CHANGE; + }else{ + pcb->state = NG_BTSOCKET_L2CAP_OPEN; + soisconnected(pcb->so); + } }else{ /* * Channel is now open, so update local channel ID and @@ -486,7 +480,7 @@ ng_btsocket_l2cap_process_l2ca_con_req_r */ pcb->cid = op->lcid; - + pcb->encryption = op->encryption; error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb); if (error != 0) { /* Send disconnect request with "zero" token */ @@ -513,7 +507,6 @@ ng_btsocket_l2cap_process_l2ca_con_req_r pcb->state = NG_BTSOCKET_L2CAP_CLOSED; soisdisconnected(pcb->so); } - mtx_unlock(&pcb->pcb_mtx); mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); @@ -702,7 +695,40 @@ respond: return (error); } /* ng_btsocket_l2cap_process_l2ca_con_ind */ +/*Encryption Change*/ +static int ng_btsocket_l2cap_process_l2ca_enc_change(struct ng_mesg *msg, ng_btsocket_l2cap_rtentry_p rt) +{ + ng_l2cap_l2ca_enc_chg_op *op = NULL; + ng_btsocket_l2cap_pcb_t *pcb = NULL; + + + if (msg->header.arglen != sizeof(*op)) + return (EMSGSIZE); + + op = (ng_l2cap_l2ca_enc_chg_op *)(msg->data); + + pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, op->lcid, + op->idtype); + mtx_lock(&pcb->pcb_mtx); + pcb->encryption = op->result; + + if(pcb->need_encrypt){ + if(pcb->state != NG_BTSOCKET_L2CAP_W4_ENC_CHANGE){ + NG_BTSOCKET_L2CAP_WARN("%s: Invalid pcb status %d", + __func__, pcb->state); + }else if(pcb->encryption){ + pcb->state = NG_BTSOCKET_L2CAP_OPEN; + soisconnected(pcb->so); + }else{ + pcb->so->so_error = EPERM; + pcb->state = NG_BTSOCKET_L2CAP_CLOSED; + soisdisconnected(pcb->so); + } + } + mtx_unlock(&pcb->pcb_mtx); + return 0; +} /* * Process L2CA_Config response */ @@ -1215,7 +1241,7 @@ ng_btsocket_l2cap_send_l2ca_con_req(ng_b 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); + ip->idtype = pcb->idtype; NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0); return (error); @@ -1354,7 +1380,7 @@ ng_btsocket_l2cap_send_l2ca_discon_req(u ip = (ng_l2cap_l2ca_discon_ip *)(msg->data); ip->lcid = pcb->cid; - ip->idtype = ng_btsock_l2cap_pcb_to_idtype(pcb); + ip->idtype = pcb->idtype; NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0); @@ -1441,7 +1467,9 @@ ng_btsocket_l2cap_data_input(struct mbuf hdr->dcid, hdr->length); if ((hdr->dcid >= NG_L2CAP_FIRST_CID) || - (idtype == NG_L2CAP_L2CA_IDTYPE_ATT)){ + (idtype == NG_L2CAP_L2CA_IDTYPE_ATT)|| + (idtype == NG_L2CAP_L2CA_IDTYPE_SMP) + ){ mtx_lock(&ng_btsocket_l2cap_sockets_mtx); @@ -1707,7 +1735,10 @@ ng_btsocket_l2cap_l2ca_msg_input(struct case NGM_L2CAP_L2CA_WRITE: /* L2CA_Write response */ ng_btsocket_l2cap_process_l2ca_write_rsp(msg, rt); break; + case NGM_L2CAP_L2CA_ENC_CHANGE: + ng_btsocket_l2cap_process_l2ca_enc_change(msg, rt); + break; /* XXX FIXME add other L2CA messages */ default: @@ -1764,6 +1795,7 @@ ng_btsocket_l2cap_input(void *context, i case NGM_L2CAP_L2CA_DISCON: case NGM_L2CAP_L2CA_DISCON_IND: case NGM_L2CAP_L2CA_WRITE: + case NGM_L2CAP_L2CA_ENC_CHANGE: /* XXX FIXME add other L2CA messages */ ng_btsocket_l2cap_l2ca_msg_input(msg, hook); break; @@ -2135,7 +2167,7 @@ ng_btsocket_l2cap_connect(struct socket struct sockaddr_l2cap ba; ng_btsocket_l2cap_rtentry_t *rt = NULL; int have_src, error = 0; - + int idtype = NG_L2CAP_L2CA_IDTYPE_BREDR; /* Check socket */ if (pcb == NULL) return (EINVAL); @@ -2164,9 +2196,16 @@ ng_btsocket_l2cap_connect(struct socket 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(sa->l2cap_bdaddr_type != BDADDR_BREDR){ + if(sa->l2cap_cid == NG_L2CAP_ATT_CID){ + idtype = NG_L2CAP_L2CA_IDTYPE_ATT; + }else if (sa->l2cap_cid == NG_L2CAP_SMP_CID){ + idtype =NG_L2CAP_L2CA_IDTYPE_SMP; + }else{ + //if cid == 0 idtype = NG_L2CAP_L2CA_IDTYPE_LE; + // Not supported yet + return EINVAL; + } } if (pcb->psm != 0 && pcb->psm != le16toh(sa->l2cap_psm)) return (EINVAL); @@ -2185,8 +2224,8 @@ ng_btsocket_l2cap_connect(struct socket 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->cid = 0; + pcb->idtype = idtype; pcb->rt = NULL; have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)); @@ -2211,7 +2250,7 @@ ng_btsocket_l2cap_connect(struct socket bcopy(&rt->src, &pcb->src, sizeof(pcb->src)); pcb->srctype = (sa->l2cap_bdaddr_type == BDADDR_BREDR)? - BDADDR_BREDR : BDADDR_LE_RANDOM; + BDADDR_BREDR : BDADDR_LE_PUBLIC; } } else error = EHOSTUNREACH; @@ -2297,6 +2336,11 @@ ng_btsocket_l2cap_ctloutput(struct socke error = sooptcopyout(sopt, &pcb->flush_timo, sizeof(pcb->flush_timo)); break; + case SO_L2CAP_ENCRYPTED: /* get encrypt required */ + error = sooptcopyout(sopt, &pcb->need_encrypt, + sizeof(pcb->need_encrypt)); + break; + default: error = ENOPROTOOPT; @@ -2337,7 +2381,17 @@ ng_btsocket_l2cap_ctloutput(struct socke if (error == 0) pcb->flush_timo = v.flush_timo; break; - + case SO_L2CAP_ENCRYPTED: /*set connect encryption opt*/ + if((pcb->state != NG_BTSOCKET_L2CAP_OPEN) && + (pcb->state != NG_BTSOCKET_L2CAP_W4_ENC_CHANGE)){ + error = sooptcopyin(sopt, &v, sizeof(v), + sizeof(v.encryption)); + if(error == 0) + pcb->need_encrypt = (v.encryption)?1:0; + }else{ + error = EINVAL; + } + break; default: error = ENOPROTOOPT; break; @@ -2489,7 +2543,17 @@ ng_btsocket_l2cap_peeraddr(struct socket sa.l2cap_psm = htole16(pcb->psm); sa.l2cap_len = sizeof(sa); sa.l2cap_family = AF_BLUETOOTH; - sa.l2cap_cid = 0; + switch(pcb->idtype){ + case NG_L2CAP_L2CA_IDTYPE_ATT: + sa.l2cap_cid = NG_L2CAP_ATT_CID; + break; + case NG_L2CAP_L2CA_IDTYPE_SMP: + sa.l2cap_cid = NG_L2CAP_SMP_CID; + break; + default: + sa.l2cap_cid = 0; + break; + } sa.l2cap_bdaddr_type = pcb->dsttype; *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); @@ -2608,7 +2672,7 @@ ng_btsocket_l2cap_send2(ng_btsocket_l2ca 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); + hdr->idtype = pcb->idtype; 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, @@ -2721,7 +2785,7 @@ ng_btsocket_l2cap_pcb_by_cid(bdaddr_p sr 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) + p->idtype == idtype) break; } _______________________________________________ svn-src-head@freebsd.org mailing list https://lists.freebsd.org/mailman/listinfo/svn-src-head To unsubscribe, send any mail to "svn-src-head-unsubscr...@freebsd.org"
