diff --git a/drivers/usb/gadget/function/f_qdss.c b/drivers/usb/gadget/function/f_qdss.c
index 29263a84bbea..88db253aeef4 100644
--- a/drivers/usb/gadget/function/f_qdss.c
+++ b/drivers/usb/gadget/function/f_qdss.c
@@ -493,11 +493,7 @@ static void usb_qdss_disconnect_work(struct work_struct *work)
 				NULL,
 				NULL);
 
-		status = set_qdss_data_connection(
-				qdss->gadget,
-				qdss->port.data,
-				qdss->port.data->address,
-				0);
+		status = set_qdss_data_connection(qdss, 0);
 		if (status)
 			pr_err("qdss_disconnect error");
 	}
@@ -543,11 +539,7 @@ static void usb_qdss_connect_work(struct work_struct *work)
 	}
 
 	pr_debug("usb_qdss_connect_work\n");
-	status = set_qdss_data_connection(
-			qdss->gadget,
-			qdss->port.data,
-			qdss->port.data->address,
-			1);
+	status = set_qdss_data_connection(qdss, 1);
 	if (status) {
 		pr_err("set_qdss_data_connection error(%d)", status);
 		return;
@@ -868,14 +860,9 @@ void usb_qdss_close(struct usb_qdss_ch *ch)
 	if (status)
 		pr_err("%s: uninit_data error\n", __func__);
 
-	status = set_qdss_data_connection(
-				gadget,
-				qdss->port.data,
-				qdss->port.data->address,
-				0);
+	status = set_qdss_data_connection(qdss, 0);
 	if (status)
 		pr_err("%s:qdss_disconnect error\n", __func__);
-	usb_gadget_restart(gadget);
 }
 EXPORT_SYMBOL(usb_qdss_close);
 
diff --git a/drivers/usb/gadget/function/f_qdss.h b/drivers/usb/gadget/function/f_qdss.h
index e3fe8ae03775..cad0f4cc06f9 100644
--- a/drivers/usb/gadget/function/f_qdss.h
+++ b/drivers/usb/gadget/function/f_qdss.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
 
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -69,6 +69,5 @@ struct usb_qdss_opts {
 };
 
 int uninit_data(struct usb_ep *ep);
-int set_qdss_data_connection(struct usb_gadget *gadget,
-	struct usb_ep *data_ep, u8 data_addr, int enable);
+int set_qdss_data_connection(struct f_qdss *qdss, int enable);
 #endif
diff --git a/drivers/usb/gadget/function/u_qdss.c b/drivers/usb/gadget/function/u_qdss.c
index 42a9cda68659..0ef1e2ab34be 100644
--- a/drivers/usb/gadget/function/u_qdss.c
+++ b/drivers/usb/gadget/function/u_qdss.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -40,19 +40,25 @@ static int alloc_sps_req(struct usb_ep *data_ep)
 }
 
 static int init_data(struct usb_ep *ep);
-int set_qdss_data_connection(struct usb_gadget *gadget,
-	struct usb_ep *data_ep, u8 data_addr, int enable)
+int set_qdss_data_connection(struct f_qdss *qdss, int enable)
 {
 	enum usb_ctrl		usb_bam_type;
 	int			res = 0;
 	int			idx;
-	struct f_qdss *qdss = data_ep->driver_data;
-	struct usb_qdss_bam_connect_info bam_info = qdss->bam_info;
+	struct usb_qdss_bam_connect_info bam_info;
+	struct usb_gadget *gadget;
 
 	pr_debug("set_qdss_data_connection\n");
 
+	if (!qdss) {
+		pr_err("%s: qdss ptr is NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	gadget = qdss->gadget;
 	usb_bam_type = usb_bam_get_bam_type(gadget->name);
 
+	bam_info = qdss->bam_info;
 	/* There is only one qdss pipe, so the pipe number can be set to 0 */
 	idx = usb_bam_get_connection_idx(usb_bam_type, QDSS_P_BAM,
 		PEER_PERIPHERAL_TO_USB, USB_BAM_DEVICE, 0);
@@ -67,14 +73,16 @@ int set_qdss_data_connection(struct usb_gadget *gadget,
 			kzalloc(sizeof(struct sps_mem_buffer), GFP_KERNEL);
 		if (!bam_info.data_fifo) {
 			pr_err("qdss_data_connection: memory alloc failed\n");
+			usb_bam_free_fifos(usb_bam_type, idx);
 			return -ENOMEM;
 		}
 		get_bam2bam_connection_info(usb_bam_type, idx,
 				&bam_info.usb_bam_pipe_idx,
 				NULL, bam_info.data_fifo, NULL);
 
-		alloc_sps_req(data_ep);
-		msm_data_fifo_config(data_ep, bam_info.data_fifo->phys_base,
+		alloc_sps_req(qdss->port.data);
+		msm_data_fifo_config(qdss->port.data,
+					bam_info.data_fifo->phys_base,
 					bam_info.data_fifo->size,
 					bam_info.usb_bam_pipe_idx);
 		init_data(qdss->port.data);