From 483ed3951238709fb18966d8ffa99dc7a8350a16 Mon Sep 17 00:00:00 2001
From: Takanori Watanabe <takawata@FreeBSD.org>
Date: Sat, 7 Nov 2015 12:15:02 +0000
Subject: [PATCH] Fix encryption error handling. Close l2cap connection on
 encryption error.

---
 sys/netgraph/bluetooth/hci/ng_hci_evnt.c          | 10 ++++++----
 sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c |  4 ++++
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/sys/netgraph/bluetooth/hci/ng_hci_evnt.c b/sys/netgraph/bluetooth/hci/ng_hci_evnt.c
index d56da1b0f158..1c1aeee3b6b2 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_evnt.c
+++ b/sys/netgraph/bluetooth/hci/ng_hci_evnt.c
@@ -913,17 +913,17 @@ encryption_change(ng_hci_unit_p unit, struct mbuf *event)
 	ng_hci_encryption_change_ep	*ep = NULL;
 	ng_hci_unit_con_p		 con = NULL;
 	int				 error = 0;
+	u_int16_t	h;
 
 	NG_HCI_M_PULLUP(event, sizeof(*ep));
 	if (event == NULL)
 		return (ENOBUFS);
 
 	ep = mtod(event, ng_hci_encryption_change_ep *);
+	h = NG_HCI_CON_HANDLE(le16toh(ep->con_handle));
+	con = ng_hci_con_by_handle(unit, h);
 
 	if (ep->status == 0) {
-		u_int16_t	h = NG_HCI_CON_HANDLE(le16toh(ep->con_handle));
-
-		con = ng_hci_con_by_handle(unit, h);
 		if (con == NULL) {
 			NG_HCI_ALERT(
 "%s: %s - invalid connection handle=%d\n",
@@ -940,12 +940,14 @@ encryption_change(ng_hci_unit_p unit, struct mbuf *event)
 			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",
 			__func__, NG_NODE_NAME(unit->node), ep->status);
 
+	/*Anyway, propagete encryption status to upper layer*/
+	ng_hci_lp_enc_change(con, con->encryption_mode);
+
 	NG_FREE_M(event);
 
 	return (error);
diff --git a/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c b/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c
index 051786f7601e..d044ad93db29 100644
--- a/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c
+++ b/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c
@@ -467,6 +467,7 @@ ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
 		   (pcb->idtype == NG_L2CAP_L2CA_IDTYPE_SMP)){
 			pcb->encryption = op->encryption;					pcb->cid = op->lcid;	
 			if(pcb->need_encrypt && !(pcb->encryption)){
+				ng_btsocket_l2cap_timeout(pcb);
 				pcb->state = NG_BTSOCKET_L2CAP_W4_ENC_CHANGE;
 			}else{
 				pcb->state = NG_BTSOCKET_L2CAP_OPEN;
@@ -713,6 +714,7 @@ static int ng_btsocket_l2cap_process_l2ca_enc_change(struct ng_mesg *msg, ng_bts
 	pcb->encryption = op->result;
 	
 	if(pcb->need_encrypt){
+		ng_btsocket_l2cap_untimeout(pcb);		
 		if(pcb->state != NG_BTSOCKET_L2CAP_W4_ENC_CHANGE){
 			NG_BTSOCKET_L2CAP_WARN("%s: Invalid pcb status %d",
 					       __func__, pcb->state);
@@ -721,6 +723,7 @@ static int ng_btsocket_l2cap_process_l2ca_enc_change(struct ng_mesg *msg, ng_bts
 			soisconnected(pcb->so);
 		}else{
 			pcb->so->so_error = EPERM;
+			ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
 			pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 			soisdisconnected(pcb->so);
 		}
@@ -2844,6 +2847,7 @@ ng_btsocket_l2cap_process_timeout(void *xpcb)
 	switch (pcb->state) {
 	case NG_BTSOCKET_L2CAP_CONNECTING:
 	case NG_BTSOCKET_L2CAP_CONFIGURING:
+	case NG_BTSOCKET_L2CAP_W4_ENC_CHANGE:		
 		/* Send disconnect request with "zero" token */
 		if (pcb->cid != 0)
 			ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);