2 files changed, 85 insertions, 0 deletions

diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
index e5fb8c8eed94..2a96ed68c7bb 100644
--- a/net/nfc/nci/core.c
+++ b/net/nfc/nci/core.c
@@ -507,6 +507,44 @@ int nci_nfcee_mode_set(struct nci_dev *ndev, u8 nfcee_id, u8 nfcee_mode)
 }
 EXPORT_SYMBOL(nci_nfcee_mode_set);
 
+static void nci_core_conn_create_req(struct nci_dev *ndev, unsigned long opt)
+{
+	struct nci_core_conn_create_cmd cmd;
+	struct core_conn_create_dest_spec_params *params =
+				(struct core_conn_create_dest_spec_params *)opt;
+
+	cmd.destination_type = NCI_DESTINATION_NFCEE;
+	cmd.number_destination_params = 1;
+	memcpy(&cmd.params.type, params,
+	       sizeof(struct core_conn_create_dest_spec_params));
+	nci_send_cmd(ndev, NCI_OP_CORE_CONN_CREATE_CMD,
+		     sizeof(struct nci_core_conn_create_cmd), &cmd);
+}
+
+int nci_core_conn_create(struct nci_dev *ndev,
+			 struct core_conn_create_dest_spec_params *params)
+{
+	ndev->cur_id = params->value.id;
+	return nci_request(ndev, nci_core_conn_create_req,
+			(unsigned long)params,
+			msecs_to_jiffies(NCI_CMD_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_core_conn_create);
+
+static void nci_core_conn_close_req(struct nci_dev *ndev, unsigned long opt)
+{
+	__u8 conn_id = opt;
+
+	nci_send_cmd(ndev, NCI_OP_CORE_CONN_CLOSE_CMD, 1, &conn_id);
+}
+
+int nci_core_conn_close(struct nci_dev *ndev, u8 conn_id)
+{
+	return nci_request(ndev, nci_core_conn_close_req, conn_id,
+				msecs_to_jiffies(NCI_CMD_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_core_conn_close);
+
 static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev)
 {
 	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
diff --git a/net/nfc/nci/rsp.c b/net/nfc/nci/rsp.c
index 0a3e98240dd6..31ccf7d05e82 100644
--- a/net/nfc/nci/rsp.c
+++ b/net/nfc/nci/rsp.c
@@ -222,6 +222,45 @@ static void nci_nfcee_mode_set_rsp_packet(struct nci_dev *ndev,
 	nci_req_complete(ndev, status);
 }
 
+static void nci_core_conn_create_rsp_packet(struct nci_dev *ndev,
+					    struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+	struct nci_conn_info *conn_info;
+	struct nci_core_conn_create_rsp *rsp;
+
+	pr_debug("status 0x%x\n", status);
+
+	if (status == NCI_STATUS_OK) {
+		rsp = (struct nci_core_conn_create_rsp *)skb->data;
+		list_for_each_entry(conn_info, &ndev->conn_info_list, list) {
+			if (conn_info->id == ndev->cur_id)
+				break;
+		}
+
+		if (!conn_info || conn_info->id != ndev->cur_id) {
+			status = NCI_STATUS_REJECTED;
+			goto exit;
+		}
+
+		conn_info->conn_id = rsp->conn_id;
+		conn_info->max_pkt_payload_len = rsp->max_ctrl_pkt_payload_len;
+		atomic_set(&conn_info->credits_cnt, rsp->credits);
+	}
+
+exit:
+	nci_req_complete(ndev, status);
+}
+
+static void nci_core_conn_close_rsp_packet(struct nci_dev *ndev,
+					   struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+	nci_req_complete(ndev, status);
+}
+
 void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
 {
 	__u16 rsp_opcode = nci_opcode(skb->data);
@@ -251,6 +290,14 @@ void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
 		nci_core_set_config_rsp_packet(ndev, skb);
 		break;
 
+	case NCI_OP_CORE_CONN_CREATE_RSP:
+		nci_core_conn_create_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_CORE_CONN_CLOSE_RSP:
+		nci_core_conn_close_rsp_packet(ndev, skb);
+		break;
+
 	case NCI_OP_RF_DISCOVER_MAP_RSP:
 		nci_rf_disc_map_rsp_packet(ndev, skb);
 		break;