Changeset View
Changeset View
Standalone View
Standalone View
head/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c
Show First 20 Lines • Show All 178 Lines • ▼ Show 20 Lines | |||||
/* | /* | ||||
* Send L2CA_xxx messages to the lower layer | * Send L2CA_xxx messages to the lower layer | ||||
*/ | */ | ||||
static int ng_btsocket_l2cap_send_l2ca_con_req | static int ng_btsocket_l2cap_send_l2ca_con_req | ||||
(ng_btsocket_l2cap_pcb_p); | (ng_btsocket_l2cap_pcb_p); | ||||
static int ng_btsocket_l2cap_send_l2ca_con_rsp_req | 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 | static int ng_btsocket_l2cap_send_l2ca_cfg_req | ||||
(ng_btsocket_l2cap_pcb_p); | (ng_btsocket_l2cap_pcb_p); | ||||
static int ng_btsocket_l2cap_send_l2ca_cfg_rsp | static int ng_btsocket_l2cap_send_l2ca_cfg_rsp | ||||
(ng_btsocket_l2cap_pcb_p); | (ng_btsocket_l2cap_pcb_p); | ||||
static int ng_btsocket_l2cap_send_l2ca_discon_req | static int ng_btsocket_l2cap_send_l2ca_discon_req | ||||
(u_int32_t, ng_btsocket_l2cap_pcb_p); | (u_int32_t, ng_btsocket_l2cap_pcb_p); | ||||
static int ng_btsocket_l2cap_send2 | static int ng_btsocket_l2cap_send2 | ||||
(ng_btsocket_l2cap_pcb_p); | (ng_btsocket_l2cap_pcb_p); | ||||
/* | /* | ||||
* Timeout processing routines | * Timeout processing routines | ||||
*/ | */ | ||||
static void ng_btsocket_l2cap_timeout (ng_btsocket_l2cap_pcb_p); | static void ng_btsocket_l2cap_timeout (ng_btsocket_l2cap_pcb_p); | ||||
static void ng_btsocket_l2cap_untimeout (ng_btsocket_l2cap_pcb_p); | static void ng_btsocket_l2cap_untimeout (ng_btsocket_l2cap_pcb_p); | ||||
static void ng_btsocket_l2cap_process_timeout (void *); | static void ng_btsocket_l2cap_process_timeout (void *); | ||||
/* | /* | ||||
* Other stuff | * Other stuff | ||||
*/ | */ | ||||
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_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_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_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() \ | #define ng_btsocket_l2cap_wakeup_input_task() \ | ||||
taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_queue_task) | taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_queue_task) | ||||
#define ng_btsocket_l2cap_wakeup_route_task() \ | #define ng_btsocket_l2cap_wakeup_route_task() \ | ||||
taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_rt_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 | ** Netgraph node interface | ||||
***************************************************************************** | ***************************************************************************** | ||||
*****************************************************************************/ | *****************************************************************************/ | ||||
/* | /* | ||||
* Netgraph node constructor. Do not allow to create node of this type. | * Netgraph node constructor. Do not allow to create node of this type. | ||||
▲ Show 20 Lines • Show All 211 Lines • ▼ Show 20 Lines | "state=%d\n", __func__, msg->header.token, | ||||
if (op->result == NG_L2CAP_PENDING) { | if (op->result == NG_L2CAP_PENDING) { | ||||
ng_btsocket_l2cap_timeout(pcb); | ng_btsocket_l2cap_timeout(pcb); | ||||
mtx_unlock(&pcb->pcb_mtx); | mtx_unlock(&pcb->pcb_mtx); | ||||
mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); | mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); | ||||
return (0); | return (0); | ||||
} | } | ||||
if (op->result == NG_L2CAP_SUCCESS) { | 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 | * Channel is now open, so update local channel ID and | ||||
* start configuration process. Source and destination | * start configuration process. Source and destination | ||||
* addresses as well as route must be already set. | * addresses as well as route must be already set. | ||||
*/ | */ | ||||
pcb->cid = op->lcid; | pcb->cid = op->lcid; | ||||
error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb); | error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb); | ||||
if (error != 0) { | if (error != 0) { | ||||
/* Send disconnect request with "zero" token */ | /* Send disconnect request with "zero" token */ | ||||
ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb); | ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb); | ||||
/* ... and close the socket */ | /* ... and close the socket */ | ||||
pcb->state = NG_BTSOCKET_L2CAP_CLOSED; | pcb->state = NG_BTSOCKET_L2CAP_CLOSED; | ||||
soisdisconnected(pcb->so); | soisdisconnected(pcb->so); | ||||
} else { | } else { | ||||
pcb->cfg_state = NG_BTSOCKET_L2CAP_CFG_IN_SENT; | pcb->cfg_state = NG_BTSOCKET_L2CAP_CFG_IN_SENT; | ||||
pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING; | pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING; | ||||
ng_btsocket_l2cap_timeout(pcb); | ng_btsocket_l2cap_timeout(pcb); | ||||
} | } | ||||
} | |||||
} else { | } else { | ||||
/* | /* | ||||
* We have failed to open connection, so convert result | * We have failed to open connection, so convert result | ||||
* code to "errno" code and disconnect the socket. Channel | * code to "errno" code and disconnect the socket. Channel | ||||
* already has been closed. | * already has been closed. | ||||
*/ | */ | ||||
pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result); | pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result); | ||||
▲ Show 20 Lines • Show All 159 Lines • ▼ Show 20 Lines | ("%s: pcb1 == NULL\n", __func__)); | ||||
token = pcb1->token; | token = pcb1->token; | ||||
} else | } else | ||||
/* Nobody listens on requested BDADDR/PSM */ | /* Nobody listens on requested BDADDR/PSM */ | ||||
result = NG_L2CAP_PSM_NOT_SUPPORTED; | result = NG_L2CAP_PSM_NOT_SUPPORTED; | ||||
respond: | respond: | ||||
error = ng_btsocket_l2cap_send_l2ca_con_rsp_req(token, rt, | 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 (pcb1 != NULL) { | ||||
if (error != 0) { | if (error != 0) { | ||||
pcb1->so->so_error = error; | pcb1->so->so_error = error; | ||||
pcb1->state = NG_BTSOCKET_L2CAP_CLOSED; | pcb1->state = NG_BTSOCKET_L2CAP_CLOSED; | ||||
soisdisconnected(pcb1->so); | soisdisconnected(pcb1->so); | ||||
} else { | } else { | ||||
pcb1->state = NG_BTSOCKET_L2CAP_CONNECTING; | pcb1->state = NG_BTSOCKET_L2CAP_CONNECTING; | ||||
soisconnecting(pcb1->so); | soisconnecting(pcb1->so); | ||||
▲ Show 20 Lines • Show All 239 Lines • ▼ Show 20 Lines | ng_btsocket_l2cap_process_l2ca_cfg_ind(struct ng_mesg *msg, | ||||
if (msg->header.arglen != sizeof(*ip)) | if (msg->header.arglen != sizeof(*ip)) | ||||
return (EMSGSIZE); | return (EMSGSIZE); | ||||
ip = (ng_l2cap_l2ca_cfg_ind_ip *)(msg->data); | ip = (ng_l2cap_l2ca_cfg_ind_ip *)(msg->data); | ||||
mtx_lock(&ng_btsocket_l2cap_sockets_mtx); | mtx_lock(&ng_btsocket_l2cap_sockets_mtx); | ||||
/* Check for the open socket that has given channel ID */ | /* 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) { | if (pcb == NULL) { | ||||
mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); | mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); | ||||
return (ENOENT); | return (ENOENT); | ||||
} | } | ||||
mtx_lock(&pcb->pcb_mtx); | mtx_lock(&pcb->pcb_mtx); | ||||
NG_BTSOCKET_L2CAP_INFO( | NG_BTSOCKET_L2CAP_INFO( | ||||
▲ Show 20 Lines • Show All 124 Lines • ▼ Show 20 Lines | ng_btsocket_l2cap_process_l2ca_discon_ind(struct ng_mesg *msg, | ||||
if (msg->header.arglen != sizeof(*ip)) | if (msg->header.arglen != sizeof(*ip)) | ||||
return (EMSGSIZE); | return (EMSGSIZE); | ||||
ip = (ng_l2cap_l2ca_discon_ind_ip *)(msg->data); | ip = (ng_l2cap_l2ca_discon_ind_ip *)(msg->data); | ||||
mtx_lock(&ng_btsocket_l2cap_sockets_mtx); | mtx_lock(&ng_btsocket_l2cap_sockets_mtx); | ||||
/* Look for the socket with given channel ID */ | /* 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) { | if (pcb == NULL) { | ||||
mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); | mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); | ||||
return (0); | return (0); | ||||
} | } | ||||
/* | /* | ||||
* Channel has already been destroyed, so disconnect the socket | * Channel has already been destroyed, so disconnect the socket | ||||
* and be done with it. If there was any pending request we can | * and be done with it. If there was any pending request we can | ||||
▲ Show 20 Lines • Show All 119 Lines • ▼ Show 20 Lines | ng_btsocket_l2cap_send_l2ca_con_req(ng_btsocket_l2cap_pcb_p pcb) | ||||
if (msg == NULL) | if (msg == NULL) | ||||
return (ENOMEM); | return (ENOMEM); | ||||
msg->header.token = pcb->token; | msg->header.token = pcb->token; | ||||
ip = (ng_l2cap_l2ca_con_ip *)(msg->data); | ip = (ng_l2cap_l2ca_con_ip *)(msg->data); | ||||
bcopy(&pcb->dst, &ip->bdaddr, sizeof(ip->bdaddr)); | bcopy(&pcb->dst, &ip->bdaddr, sizeof(ip->bdaddr)); | ||||
ip->psm = pcb->psm; | 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); | NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0); | ||||
return (error); | return (error); | ||||
} /* ng_btsocket_l2cap_send_l2ca_con_req */ | } /* ng_btsocket_l2cap_send_l2ca_con_req */ | ||||
/* | /* | ||||
* Send L2CA_Connect response | * Send L2CA_Connect response | ||||
*/ | */ | ||||
static int | static int | ||||
ng_btsocket_l2cap_send_l2ca_con_rsp_req(u_int32_t token, | ng_btsocket_l2cap_send_l2ca_con_rsp_req(u_int32_t token, | ||||
ng_btsocket_l2cap_rtentry_p rt, bdaddr_p dst, int ident, | 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; | struct ng_mesg *msg = NULL; | ||||
ng_l2cap_l2ca_con_rsp_ip *ip = NULL; | ng_l2cap_l2ca_con_rsp_ip *ip = NULL; | ||||
int error = 0; | int error = 0; | ||||
if (rt == NULL || rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook)) | if (rt == NULL || rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook)) | ||||
return (ENETDOWN); | return (ENETDOWN); | ||||
NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_RSP, | NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_RSP, | ||||
sizeof(*ip), M_NOWAIT); | sizeof(*ip), M_NOWAIT); | ||||
if (msg == NULL) | if (msg == NULL) | ||||
return (ENOMEM); | return (ENOMEM); | ||||
msg->header.token = token; | msg->header.token = token; | ||||
ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data); | ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data); | ||||
bcopy(dst, &ip->bdaddr, sizeof(ip->bdaddr)); | bcopy(dst, &ip->bdaddr, sizeof(ip->bdaddr)); | ||||
ip->ident = ident; | ip->ident = ident; | ||||
ip->lcid = lcid; | ip->lcid = lcid; | ||||
ip->linktype = linktype; | |||||
ip->result = result; | ip->result = result; | ||||
ip->status = 0; | ip->status = 0; | ||||
NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg, rt->hook, 0); | NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg, rt->hook, 0); | ||||
return (error); | return (error); | ||||
} /* ng_btsocket_l2cap_send_l2ca_con_rsp_req */ | } /* ng_btsocket_l2cap_send_l2ca_con_rsp_req */ | ||||
▲ Show 20 Lines • Show All 89 Lines • ▼ Show 20 Lines | NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON, | ||||
sizeof(*ip), M_NOWAIT); | sizeof(*ip), M_NOWAIT); | ||||
if (msg == NULL) | if (msg == NULL) | ||||
return (ENOMEM); | return (ENOMEM); | ||||
msg->header.token = token; | msg->header.token = token; | ||||
ip = (ng_l2cap_l2ca_discon_ip *)(msg->data); | ip = (ng_l2cap_l2ca_discon_ip *)(msg->data); | ||||
ip->lcid = pcb->cid; | 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); | NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0); | ||||
return (error); | return (error); | ||||
} /* ng_btsocket_l2cap_send_l2ca_discon_req */ | } /* ng_btsocket_l2cap_send_l2ca_discon_req */ | ||||
/***************************************************************************** | /***************************************************************************** | ||||
***************************************************************************** | ***************************************************************************** | ||||
** Socket interface | ** Socket interface | ||||
***************************************************************************** | ***************************************************************************** | ||||
*****************************************************************************/ | *****************************************************************************/ | ||||
/* | /* | ||||
* L2CAP sockets data input routine | * L2CAP sockets data input routine | ||||
*/ | */ | ||||
static void | static void | ||||
ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook) | ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook) | ||||
{ | { | ||||
ng_l2cap_hdr_t *hdr = NULL; | ng_l2cap_hdr_t *hdr = NULL; | ||||
ng_l2cap_clt_hdr_t *clt_hdr = NULL; | ng_l2cap_clt_hdr_t *clt_hdr = NULL; | ||||
ng_btsocket_l2cap_pcb_t *pcb = NULL; | ng_btsocket_l2cap_pcb_t *pcb = NULL; | ||||
ng_btsocket_l2cap_rtentry_t *rt = NULL; | ng_btsocket_l2cap_rtentry_t *rt = NULL; | ||||
uint16_t idtype; | |||||
if (hook == NULL) { | if (hook == NULL) { | ||||
NG_BTSOCKET_L2CAP_ALERT( | NG_BTSOCKET_L2CAP_ALERT( | ||||
"%s: Invalid source hook for L2CAP data packet\n", __func__); | "%s: Invalid source hook for L2CAP data packet\n", __func__); | ||||
goto drop; | goto drop; | ||||
} | } | ||||
rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook); | rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook); | ||||
if (rt == NULL) { | if (rt == NULL) { | ||||
NG_BTSOCKET_L2CAP_ALERT( | NG_BTSOCKET_L2CAP_ALERT( | ||||
"%s: Could not find out source bdaddr for L2CAP data packet\n", __func__); | "%s: Could not find out source bdaddr for L2CAP data packet\n", __func__); | ||||
goto drop; | 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 */ | /* Make sure we can access header */ | ||||
if (m->m_pkthdr.len < sizeof(*hdr)) { | if (m->m_pkthdr.len < sizeof(*hdr)) { | ||||
NG_BTSOCKET_L2CAP_ERR( | NG_BTSOCKET_L2CAP_ERR( | ||||
"%s: L2CAP data packet too small, len=%d\n", __func__, m->m_pkthdr.len); | "%s: L2CAP data packet too small, len=%d\n", __func__, m->m_pkthdr.len); | ||||
goto drop; | goto drop; | ||||
} | } | ||||
if (m->m_len < sizeof(*hdr)) { | if (m->m_len < sizeof(*hdr)) { | ||||
Show All 27 Lines | "%s: Bad L2CAP data packet length, len=%d, length=%d\n", | ||||
NG_BTSOCKET_L2CAP_INFO( | NG_BTSOCKET_L2CAP_INFO( | ||||
"%s: Received L2CAP data packet: src bdaddr=%x:%x:%x:%x:%x:%x, " \ | "%s: Received L2CAP data packet: src bdaddr=%x:%x:%x:%x:%x:%x, " \ | ||||
"dcid=%d, length=%d\n", | "dcid=%d, length=%d\n", | ||||
__func__, | __func__, | ||||
rt->src.b[5], rt->src.b[4], rt->src.b[3], | rt->src.b[5], rt->src.b[4], rt->src.b[3], | ||||
rt->src.b[2], rt->src.b[1], rt->src.b[0], | rt->src.b[2], rt->src.b[1], rt->src.b[0], | ||||
hdr->dcid, hdr->length); | 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); | mtx_lock(&ng_btsocket_l2cap_sockets_mtx); | ||||
/* Normal packet: find connected socket */ | /* 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) { | if (pcb == NULL) { | ||||
mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); | mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); | ||||
goto drop; | goto drop; | ||||
} | } | ||||
mtx_lock(&pcb->pcb_mtx); | mtx_lock(&pcb->pcb_mtx); | ||||
if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) { | if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) { | ||||
▲ Show 20 Lines • Show All 141 Lines • ▼ Show 20 Lines | |||||
*/ | */ | ||||
static void | static void | ||||
ng_btsocket_l2cap_default_msg_input(struct ng_mesg *msg, hook_p hook) | ng_btsocket_l2cap_default_msg_input(struct ng_mesg *msg, hook_p hook) | ||||
{ | { | ||||
switch (msg->header.cmd) { | switch (msg->header.cmd) { | ||||
case NGM_L2CAP_NODE_HOOK_INFO: { | case NGM_L2CAP_NODE_HOOK_INFO: { | ||||
ng_btsocket_l2cap_rtentry_t *rt = NULL; | ng_btsocket_l2cap_rtentry_t *rt = NULL; | ||||
ng_l2cap_node_hook_info_ep *ep = | |||||
if (hook == NULL || msg->header.arglen != sizeof(bdaddr_t)) | (ng_l2cap_node_hook_info_ep *)msg->data; | ||||
if (hook == NULL || msg->header.arglen != sizeof(*ep)) | |||||
break; | 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; | break; | ||||
mtx_lock(&ng_btsocket_l2cap_rt_mtx); | mtx_lock(&ng_btsocket_l2cap_rt_mtx); | ||||
rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook); | rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook); | ||||
if (rt == NULL) { | if (rt == NULL) { | ||||
rt = malloc(sizeof(*rt), | rt = malloc(sizeof(*rt), | ||||
M_NETGRAPH_BTSOCKET_L2CAP, M_NOWAIT|M_ZERO); | M_NETGRAPH_BTSOCKET_L2CAP, M_NOWAIT|M_ZERO); | ||||
if (rt == NULL) { | if (rt == NULL) { | ||||
mtx_unlock(&ng_btsocket_l2cap_rt_mtx); | mtx_unlock(&ng_btsocket_l2cap_rt_mtx); | ||||
break; | break; | ||||
} | } | ||||
LIST_INSERT_HEAD(&ng_btsocket_l2cap_rt, rt, next); | LIST_INSERT_HEAD(&ng_btsocket_l2cap_rt, rt, next); | ||||
NG_HOOK_SET_PRIVATE(hook, rt); | 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; | rt->hook = hook; | ||||
mtx_unlock(&ng_btsocket_l2cap_rt_mtx); | mtx_unlock(&ng_btsocket_l2cap_rt_mtx); | ||||
NG_BTSOCKET_L2CAP_INFO( | NG_BTSOCKET_L2CAP_INFO( | ||||
"%s: Updating hook \"%s\", src bdaddr=%x:%x:%x:%x:%x:%x\n", | "%s: Updating hook \"%s\", src bdaddr=%x:%x:%x:%x:%x:%x\n", | ||||
__func__, NG_HOOK_NAME(hook), | __func__, NG_HOOK_NAME(hook), | ||||
rt->src.b[5], rt->src.b[4], rt->src.b[3], | rt->src.b[5], rt->src.b[4], rt->src.b[3], | ||||
▲ Show 20 Lines • Show All 438 Lines • ▼ Show 20 Lines | ng_btsocket_l2cap_bind(struct socket *so, struct sockaddr *nam, | ||||
if (ng_btsocket_l2cap_node == NULL) | if (ng_btsocket_l2cap_node == NULL) | ||||
return (EINVAL); | return (EINVAL); | ||||
/* Verify address */ | /* Verify address */ | ||||
if (sa == NULL) | if (sa == NULL) | ||||
return (EINVAL); | return (EINVAL); | ||||
if (sa->l2cap_family != AF_BLUETOOTH) | if (sa->l2cap_family != AF_BLUETOOTH) | ||||
return (EAFNOSUPPORT); | 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); | return (EINVAL); | ||||
psm = le16toh(sa->l2cap_psm); | psm = le16toh(sa->l2cap_psm); | ||||
/* | /* | ||||
* Check if other socket has this address already (look for exact | * Check if other socket has this address already (look for exact | ||||
* match PSM and bdaddr) and assign socket address if it's available. | * match PSM and bdaddr) and assign socket address if it's available. | ||||
* | * | ||||
Show All 28 Lines | |||||
* Connect socket | * Connect socket | ||||
*/ | */ | ||||
int | int | ||||
ng_btsocket_l2cap_connect(struct socket *so, struct sockaddr *nam, | ng_btsocket_l2cap_connect(struct socket *so, struct sockaddr *nam, | ||||
struct thread *td) | struct thread *td) | ||||
{ | { | ||||
ng_btsocket_l2cap_pcb_t *pcb = so2l2cap_pcb(so); | ng_btsocket_l2cap_pcb_t *pcb = so2l2cap_pcb(so); | ||||
struct sockaddr_l2cap_compat *sal = (struct sockaddr_l2cap_compat *) nam; | |||||
struct sockaddr_l2cap *sa = (struct sockaddr_l2cap *) nam; | struct sockaddr_l2cap *sa = (struct sockaddr_l2cap *)nam; | ||||
struct sockaddr_l2cap ba; | |||||
ng_btsocket_l2cap_rtentry_t *rt = NULL; | ng_btsocket_l2cap_rtentry_t *rt = NULL; | ||||
int have_src, error = 0; | int have_src, error = 0; | ||||
/* Check socket */ | /* Check socket */ | ||||
if (pcb == NULL) | if (pcb == NULL) | ||||
return (EINVAL); | return (EINVAL); | ||||
if (ng_btsocket_l2cap_node == NULL) | if (ng_btsocket_l2cap_node == NULL) | ||||
return (EINVAL); | return (EINVAL); | ||||
if (pcb->state == NG_BTSOCKET_L2CAP_CONNECTING) | if (pcb->state == NG_BTSOCKET_L2CAP_CONNECTING) | ||||
return (EINPROGRESS); | return (EINPROGRESS); | ||||
/* Verify address */ | /* Verify address */ | ||||
if (sa == NULL) | if (sa == NULL) | ||||
return (EINVAL); | return (EINVAL); | ||||
if (sa->l2cap_family != AF_BLUETOOTH) | if (sa->l2cap_family != AF_BLUETOOTH) | ||||
return (EAFNOSUPPORT); | 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)) | if (sa->l2cap_len != sizeof(*sa)) | ||||
return (EINVAL); | return (EINVAL); | ||||
if (sa->l2cap_psm == 0 || | if ((sa->l2cap_psm && sa->l2cap_cid)) | ||||
bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) | return EINVAL; | ||||
if (bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) | |||||
return (EDESTADDRREQ); | 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)) | if (pcb->psm != 0 && pcb->psm != le16toh(sa->l2cap_psm)) | ||||
return (EINVAL); | return (EINVAL); | ||||
/* | /* | ||||
* Routing. Socket should be bound to some source address. The source | * Routing. Socket should be bound to some source address. The source | ||||
* address can be ANY. Destination address must be set and it must not | * address can be ANY. Destination address must be set and it must not | ||||
* be ANY. If source address is ANY then find first rtentry that has | * be ANY. If source address is ANY then find first rtentry that has | ||||
* src != dst. | * src != dst. | ||||
*/ | */ | ||||
mtx_lock(&ng_btsocket_l2cap_rt_mtx); | mtx_lock(&ng_btsocket_l2cap_rt_mtx); | ||||
mtx_lock(&ng_btsocket_l2cap_sockets_mtx); | mtx_lock(&ng_btsocket_l2cap_sockets_mtx); | ||||
mtx_lock(&pcb->pcb_mtx); | mtx_lock(&pcb->pcb_mtx); | ||||
/* Send destination address and PSM */ | /* Send destination address and PSM */ | ||||
bcopy(&sa->l2cap_bdaddr, &pcb->dst, sizeof(pcb->dst)); | bcopy(&sa->l2cap_bdaddr, &pcb->dst, sizeof(pcb->dst)); | ||||
pcb->psm = le16toh(sa->l2cap_psm); | pcb->psm = le16toh(sa->l2cap_psm); | ||||
pcb->dsttype = sa->l2cap_bdaddr_type; | |||||
pcb->cid = sa->l2cap_cid; | |||||
pcb->rt = NULL; | pcb->rt = NULL; | ||||
have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)); | have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)); | ||||
LIST_FOREACH(rt, &ng_btsocket_l2cap_rt, next) { | LIST_FOREACH(rt, &ng_btsocket_l2cap_rt, next) { | ||||
if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook)) | if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook)) | ||||
continue; | continue; | ||||
/* Match src and dst */ | /* Match src and dst */ | ||||
if (have_src) { | if (have_src) { | ||||
if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0) | if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0) | ||||
break; | break; | ||||
} else { | } else { | ||||
if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0) | if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0) | ||||
break; | break; | ||||
} | } | ||||
} | } | ||||
if (rt != NULL) { | if (rt != NULL) { | ||||
pcb->rt = rt; | pcb->rt = rt; | ||||
if (!have_src) | if (!have_src){ | ||||
bcopy(&rt->src, &pcb->src, sizeof(pcb->src)); | bcopy(&rt->src, &pcb->src, sizeof(pcb->src)); | ||||
pcb->srctype = | |||||
(sa->l2cap_bdaddr_type == BDADDR_BREDR)? | |||||
BDADDR_BREDR : BDADDR_LE_RANDOM; | |||||
} | |||||
} else | } else | ||||
error = EHOSTUNREACH; | error = EHOSTUNREACH; | ||||
/* | /* | ||||
* Send L2CA_Connect request | * Send L2CA_Connect request | ||||
*/ | */ | ||||
if (error == 0) { | if (error == 0) { | ||||
▲ Show 20 Lines • Show All 260 Lines • ▼ Show 20 Lines | if (pcb == NULL) | ||||
return (EINVAL); | return (EINVAL); | ||||
if (ng_btsocket_l2cap_node == NULL) | if (ng_btsocket_l2cap_node == NULL) | ||||
return (EINVAL); | return (EINVAL); | ||||
bcopy(&pcb->dst, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr)); | bcopy(&pcb->dst, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr)); | ||||
sa.l2cap_psm = htole16(pcb->psm); | sa.l2cap_psm = htole16(pcb->psm); | ||||
sa.l2cap_len = sizeof(sa); | sa.l2cap_len = sizeof(sa); | ||||
sa.l2cap_family = AF_BLUETOOTH; | sa.l2cap_family = AF_BLUETOOTH; | ||||
sa.l2cap_cid = 0; | |||||
sa.l2cap_bdaddr_type = pcb->dsttype; | |||||
*nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); | *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); | ||||
return ((*nam == NULL)? ENOMEM : 0); | return ((*nam == NULL)? ENOMEM : 0); | ||||
} /* ng_btsocket_l2cap_peeraddr */ | } /* ng_btsocket_l2cap_peeraddr */ | ||||
/* | /* | ||||
* Send data to socket | * Send data to socket | ||||
*/ | */ | ||||
▲ Show 20 Lines • Show All 101 Lines • ▼ Show 20 Lines | "%s: Failed to create L2CA packet header\n", __func__); | ||||
return (ENOBUFS); | return (ENOBUFS); | ||||
} | } | ||||
hdr = mtod(m, ng_l2cap_l2ca_hdr_t *); | hdr = mtod(m, ng_l2cap_l2ca_hdr_t *); | ||||
hdr->token = pcb->token; | hdr->token = pcb->token; | ||||
hdr->length = m->m_pkthdr.len - sizeof(*hdr); | hdr->length = m->m_pkthdr.len - sizeof(*hdr); | ||||
hdr->lcid = pcb->cid; | hdr->lcid = pcb->cid; | ||||
hdr->idtype = ng_btsock_l2cap_pcb_to_idtype(pcb); | |||||
NG_BTSOCKET_L2CAP_INFO( | NG_BTSOCKET_L2CAP_INFO( | ||||
"%s: Sending packet: len=%d, length=%d, lcid=%d, token=%d, state=%d\n", | "%s: Sending packet: len=%d, length=%d, lcid=%d, token=%d, state=%d\n", | ||||
__func__, m->m_pkthdr.len, hdr->length, hdr->lcid, | __func__, m->m_pkthdr.len, hdr->length, hdr->lcid, | ||||
hdr->token, pcb->state); | hdr->token, pcb->state); | ||||
/* | /* | ||||
* If we got here than we have successfuly creates new L2CAP | * If we got here than we have successfuly creates new L2CAP | ||||
* data packet and now we can send it to the L2CAP layer | * data packet and now we can send it to the L2CAP layer | ||||
Show All 18 Lines | if (pcb == NULL) | ||||
return (EINVAL); | return (EINVAL); | ||||
if (ng_btsocket_l2cap_node == NULL) | if (ng_btsocket_l2cap_node == NULL) | ||||
return (EINVAL); | return (EINVAL); | ||||
bcopy(&pcb->src, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr)); | bcopy(&pcb->src, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr)); | ||||
sa.l2cap_psm = htole16(pcb->psm); | sa.l2cap_psm = htole16(pcb->psm); | ||||
sa.l2cap_len = sizeof(sa); | sa.l2cap_len = sizeof(sa); | ||||
sa.l2cap_family = AF_BLUETOOTH; | sa.l2cap_family = AF_BLUETOOTH; | ||||
sa.l2cap_cid = 0; | |||||
sa.l2cap_bdaddr_type = pcb->srctype; | |||||
*nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); | *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); | ||||
return ((*nam == NULL)? ENOMEM : 0); | return ((*nam == NULL)? ENOMEM : 0); | ||||
} /* ng_btsocket_l2cap_sockaddr */ | } /* ng_btsocket_l2cap_sockaddr */ | ||||
/***************************************************************************** | /***************************************************************************** | ||||
***************************************************************************** | ***************************************************************************** | ||||
▲ Show 20 Lines • Show All 51 Lines • ▼ Show 20 Lines | |||||
} /* ng_btsocket_l2cap_pcb_by_token */ | } /* ng_btsocket_l2cap_pcb_by_token */ | ||||
/* | /* | ||||
* Look for the socket that assigned to given source address and channel ID. | * Look for the socket that assigned to given source address and channel ID. | ||||
* Caller must hold ng_btsocket_l2cap_sockets_mtx | * Caller must hold ng_btsocket_l2cap_sockets_mtx | ||||
*/ | */ | ||||
static ng_btsocket_l2cap_pcb_p | 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; | ng_btsocket_l2cap_pcb_p p = NULL; | ||||
mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED); | mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED); | ||||
LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next) | LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next){ | ||||
if (p->cid == cid && bcmp(src, &p->src, sizeof(p->src)) == 0) | if (p->cid == cid && | ||||
bcmp(src, &p->src, sizeof(p->src)) == 0&& | |||||
ng_btsock_l2cap_pcb_to_idtype(p) == idtype) | |||||
break; | break; | ||||
} | |||||
return (p); | return (p); | ||||
} /* ng_btsocket_l2cap_pcb_by_cid */ | } /* ng_btsocket_l2cap_pcb_by_cid */ | ||||
/* | /* | ||||
* Set timeout on socket | * Set timeout on socket | ||||
*/ | */ | ||||
static void | static void | ||||
▲ Show 20 Lines • Show All 168 Lines • Show Last 20 Lines |