From 5e743ed2df7d23170c7f98591983d80d7a8f2773 Mon Sep 17 00:00:00 2001
From: takawata <takawata@FreeBSD.org>
Date: Tue, 27 Oct 2015 03:42:26 +0000
Subject: [PATCH] Bluetooth LE Security Management channel support. Add a
 socket option to block until underlying HCI connection encrypted.

Differential Revision:	https://reviews.freebsd.org/D3981
---
 sys/netgraph/bluetooth/hci/ng_hci_evnt.c      |   3 +-
 sys/netgraph/bluetooth/hci/ng_hci_ulpi.c      |  31 +++++
 sys/netgraph/bluetooth/hci/ng_hci_ulpi.h      |   1 +
 sys/netgraph/bluetooth/include/ng_btsocket.h  |   2 +-
 .../bluetooth/include/ng_btsocket_l2cap.h     |   8 +-
 sys/netgraph/bluetooth/include/ng_hci.h       |   8 +-
 sys/netgraph/bluetooth/include/ng_l2cap.h     |   9 ++
 sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c  |   6 +-
 sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c  |  52 +++++++
 sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h  |   1 +
 sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c  |   4 +-
 sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c  |   8 +-
 sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c  | 106 ++++++++++++++-
 sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h  |   2 +-
 sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h   |   3 +-
 .../bluetooth/socket/ng_btsocket_l2cap.c      | 128 +++++++++++++-----
 16 files changed, 320 insertions(+), 52 deletions(-)

diff --git a/sys/netgraph/bluetooth/hci/ng_hci_evnt.c b/sys/netgraph/bluetooth/hci/ng_hci_evnt.c
index a31c5fbe4b9a..d56da1b0f158 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_evnt.c
+++ b/sys/netgraph/bluetooth/hci/ng_hci_evnt.c
@@ -929,7 +929,7 @@ encryption_change(ng_hci_unit_p unit, struct mbuf *event)
 "%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, 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",
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c b/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c
index 9934ea851769..0d35245f91dc 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c
+++ b/sys/netgraph/bluetooth/hci/ng_hci_ulpi.c
@@ -814,6 +814,37 @@ ng_hci_lp_con_cfm(ng_hci_unit_con_p con, int status)
 	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
  */
diff --git a/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h b/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h
index ced697e45e66..4bf9f431080b 100644
--- a/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h
+++ b/sys/netgraph/bluetooth/hci/ng_hci_ulpi.h
@@ -47,6 +47,7 @@ int  ng_hci_lp_discon_ind                (ng_hci_unit_con_p, int);
 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);
 
diff --git a/sys/netgraph/bluetooth/include/ng_btsocket.h b/sys/netgraph/bluetooth/include/ng_btsocket.h
index 483858e5a79d..4815e4373b3d 100644
--- a/sys/netgraph/bluetooth/include/ng_btsocket.h
+++ b/sys/netgraph/bluetooth/include/ng_btsocket.h
@@ -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
  */
diff --git a/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h b/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h
index 7d8a20074c92..80c7193dc00b 100644
--- a/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h
+++ b/sys/netgraph/bluetooth/include/ng_btsocket_l2cap.h
@@ -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;
diff --git a/sys/netgraph/bluetooth/include/ng_hci.h b/sys/netgraph/bluetooth/include/ng_hci.h
index 1688c4c35bbe..40d6a79598f8 100644
--- a/sys/netgraph/bluetooth/include/ng_hci.h
+++ b/sys/netgraph/bluetooth/include/ng_hci.h
@@ -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
diff --git a/sys/netgraph/bluetooth/include/ng_l2cap.h b/sys/netgraph/bluetooth/include/ng_l2cap.h
index 48b3b5504be4..8a72e3d8837e 100644
--- a/sys/netgraph/bluetooth/include/ng_l2cap.h
+++ b/sys/netgraph/bluetooth/include/ng_l2cap.h
@@ -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;
 
 /**************************************************************************
  **************************************************************************
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c
index 287ab6632569..7429f78f95c6 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c
@@ -475,6 +475,8 @@ ng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident)
 	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 con, u_int8_t ident)
 			 */
 
 			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 */
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c
index e9e7412a9ee2..926a67481a56 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c
@@ -481,6 +481,58 @@ ng_l2cap_lp_qos_ind(ng_l2cap_p l2cap, struct ng_mesg *msg)
 	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.
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h
index 67ea6c033d90..6bdb1d965e71 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.h
@@ -41,6 +41,7 @@ int  ng_l2cap_lp_discon_ind          (ng_l2cap_p, struct ng_mesg *);
 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);
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c
index 5ed61aa2858c..070d8e9a03b8 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_main.c
@@ -350,7 +350,9 @@ ng_l2cap_lower_rcvmsg(node_p node, item_p item, hook_p lasthook)
 		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;
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c
index 886a918f1746..889f6ab3a66d 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c
@@ -114,7 +114,7 @@ ng_l2cap_new_con(ng_l2cap_p l2cap, bdaddr_p bdaddr, int type)
 
 	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_l2cap_con_p con, u_int16_t psm, int idtyp
 		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, u_int16_t scid, int idtype)
 {
 	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, u_int16_t scid, int idtype)
 		if((idtype != NG_L2CAP_L2CA_IDTYPE_LE)&&
 		   (ch->con->linktype != NG_HCI_LINK_ACL ))
 			continue;
-
 		if (ch->scid == scid)
 			break;
 	}
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c
index 67ca43ba5d98..4b88bc854f5e 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.c
@@ -130,6 +130,10 @@ ng_l2cap_l2ca_con_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
 		_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, u_int32_t token, u_int16_t result,
 		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 l2cap, struct ng_mesg *msg)
 	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 l2cap, struct ng_mesg *msg)
 	/* 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 @@ ng_l2cap_l2ca_con_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
 	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, struct ng_mesg *msg)
 
 	/* 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, struct mbuf *m)
 	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 ch, u_int32_t token, u_int16_t result,
 		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;
@@ -1165,6 +1242,21 @@ ng_l2cap_l2ca_discon_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
 		}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 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;
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h
index 8a7679a1de44..2e65439a4061 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h
@@ -74,6 +74,6 @@ int ng_l2cap_l2ca_get_info_rsp (ng_l2cap_con_p, u_int32_t, u_int16_t,
 					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_ */
 
diff --git a/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h b/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h
index 55db4ba942a5..1157e2331786 100644
--- a/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h
+++ b/sys/netgraph/bluetooth/l2cap/ng_l2cap_var.h
@@ -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 */
diff --git a/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c b/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c
index 7b666b1b5275..051786f7601e 100644
--- a/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c
+++ b/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c
@@ -213,7 +213,7 @@ static ng_btsocket_l2cap_pcb_p     ng_btsocket_l2cap_pcb_by_cid (bdaddr_p, int,i
 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(struct ng_btsocket_l2cap_pcb *);
 	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_rsp(struct ng_mesg *msg,
 	}
 
 	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_rsp(struct ng_mesg *msg,
 			 */
 			
 			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_rsp(struct ng_mesg *msg,
 		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 @@ ng_btsocket_l2cap_process_l2ca_con_ind(struct ng_mesg *msg,
 
 	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_btsocket_l2cap_pcb_p pcb)
 	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_int32_t token,
 
 	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 *m, hook_p hook)
 		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 ng_mesg *msg, hook_p hook)
 	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, int pending)
 			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 *so, struct sockaddr *nam,
 	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 *so, struct sockaddr *nam,
 	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 *so, struct sockaddr *nam,
 	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 *so, struct sockaddr *nam,
 			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 socket *so, struct sockopt *sopt)
 			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 socket *so, struct sockopt *sopt)
 			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 *so, struct sockaddr **nam)
 	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_l2cap_pcb_p pcb)
 	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 src, int cid, int idtype)
 	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;
 
 	}