diff --git a/drivers/usb/usbssp/gadget-if.c b/drivers/usb/usbssp/gadget-if.c
index dbff5a400676..376e03b7ef1f 100644
--- a/drivers/usb/usbssp/gadget-if.c
+++ b/drivers/usb/usbssp/gadget-if.c
@@ -7,11 +7,35 @@
  * Author: Pawel Laszczak
  *
  */
-
+#include <linux/pm_runtime.h>
 #include <linux/usb/gadget.h>
 #include <linux/usb/composite.h>
 #include "gadget.h"
 
+#define usbssp_g_lock(flag, save_flags) { \
+	if (in_interrupt()) {\
+		spin_lock_irqsave(&usbssp_data->lock, save_flags); \
+	} else { \
+		if (!irqs_disabled()) { \
+			spin_lock_irqsave(&usbssp_data->irq_thread_lock,\
+					usbssp_data->irq_thread_flag);\
+			flag = 1; \
+		} else \
+			spin_lock(&usbssp_data->irq_thread_lock); \
+	} }
+
+
+#define usbssp_g_unlock(flag, save_flags) { \
+	if (in_interrupt()) \
+		spin_unlock_irqrestore(&usbssp_data->lock, save_flags); \
+	else { \
+		if (flag) \
+			spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,\
+					usbssp_data->irq_thread_flag);\
+		else \
+			spin_unlock(&usbssp_data->irq_thread_lock); \
+	} }
+
 static int usbssp_gadget_ep_enable(struct usb_ep *ep,
 				   const struct usb_endpoint_descriptor *desc)
 {
@@ -40,24 +64,27 @@ int usbssp_gadget_ep_disable(struct usb_ep *ep)
 static struct usb_request *usbssp_gadget_ep_alloc_request(struct usb_ep *ep,
 							  gfp_t gfp_flags)
 {
+	struct usbssp_request *req_priv;
 	struct usbssp_ep *ep_priv = to_usbssp_ep(ep);
 
-	if (!ep_priv)
+	req_priv = kzalloc(sizeof(*req_priv), gfp_flags);
+	if (!req_priv)
 		return NULL;
 
-	/*TODO: implements this function*/
-	return NULL;
+	req_priv->epnum = ep_priv->number;
+	req_priv->dep = ep_priv;
+
+	trace_usbssp_alloc_request(&req_priv->request);
+	return &req_priv->request;
 }
 
 static void usbssp_gadget_ep_free_request(struct usb_ep *ep,
 					  struct usb_request *request)
 {
-	struct usbssp_ep *ep_priv = to_usbssp_ep(ep);
+	struct usbssp_request *req_priv = to_usbssp_request(request);
 
-	if (!ep_priv)
-		return;
-
-	/*TODO: implements this function*/
+	trace_usbssp_free_request(&req_priv->request);
+	kfree(req_priv);
 }
 
 static int usbssp_gadget_ep_queue(struct usb_ep *ep,
@@ -65,12 +92,32 @@ static int usbssp_gadget_ep_queue(struct usb_ep *ep,
 				  gfp_t gfp_flags)
 {
 	struct usbssp_ep *ep_priv = to_usbssp_ep(ep);
-	int ret = 0;
+	struct usbssp_request *req_priv = to_usbssp_request(request);
+	struct usbssp_udc *usbssp_data = ep_priv->usbssp_data;
+	unsigned long flags = 0;
+	int irq_disabled_locally = 0;
+	int ret;
+
+	if (!ep_priv->endpoint.desc) {
+		dev_err(usbssp_data->dev,
+			"%s: can't queue to disabled endpoint\n",
+			ep_priv->name);
+		return -ESHUTDOWN;
+	}
 
-	if (!ep_priv)
-		return -EINVAL;
+	if ((ep_priv->ep_state & USBSSP_EP_DISABLE_PENDING ||
+			!(ep_priv->ep_state & USBSSP_EP_ENABLED))) {
+		dev_err(usbssp_data->dev,
+			"%s: can't queue to disabled endpoint\n",
+			ep_priv->name);
+		ret = -ESHUTDOWN;
+		goto out;
+	}
 
-	/*TODO: implements this function*/
+	usbssp_g_lock(irq_disabled_locally, flags);
+	ret = usbssp_enqueue(ep_priv, req_priv);
+	usbssp_g_unlock(irq_disabled_locally, flags);
+out:
 	return ret;
 }
 
@@ -78,36 +125,53 @@ static int usbssp_gadget_ep_dequeue(struct usb_ep *ep,
 				    struct usb_request *request)
 {
 	struct usbssp_ep *ep_priv = to_usbssp_ep(ep);
-	int ret = 0;
+	struct usbssp_request *req_priv = to_usbssp_request(request);
+	struct usbssp_udc *usbssp_data = ep_priv->usbssp_data;
+	unsigned long flags = 0;
+	int ret;
+	int irq_disabled_locally = 0;
+
+	if (!ep_priv->endpoint.desc) {
+		dev_err(usbssp_data->dev,
+			"%s: can't queue to disabled endpoint\n",
+			ep_priv->name);
+		return -ESHUTDOWN;
+	}
 
-	if (!ep_priv)
-		return -EINVAL;
+	usbssp_g_lock(irq_disabled_locally, flags);
+	ret = usbssp_dequeue(ep_priv, req_priv);
+	usbssp_g_unlock(irq_disabled_locally, flags);
 
-	/*TODO: implements this function*/
 	return ret;
 }
 
 static int usbssp_gadget_ep_set_halt(struct usb_ep *ep, int value)
 {
 	struct usbssp_ep *ep_priv = to_usbssp_ep(ep);
-	int ret = 0;
-
-	if (!ep_priv)
-		return -EINVAL;
+	struct usbssp_udc *usbssp_data = ep_priv->usbssp_data;
+	int ret;
+	int irq_disabled_locally = 0;
+	unsigned long flags = 0;
 
-	/*TODO: implements this function*/
+	usbssp_g_lock(irq_disabled_locally, flags);
+	ret = usbssp_halt_endpoint(usbssp_data, ep_priv, value);
+	usbssp_g_unlock(irq_disabled_locally, flags);
 	return ret;
 }
 
 static int usbssp_gadget_ep_set_wedge(struct usb_ep *ep)
 {
 	struct usbssp_ep *ep_priv = to_usbssp_ep(ep);
-	int ret = 0;
+	struct usbssp_udc *usbssp_data = ep_priv->usbssp_data;
+	unsigned long flags = 0;
+	int ret;
+	int irq_disabled_locally = 0;
 
-	if (!ep_priv)
-		return -EINVAL;
+	usbssp_g_lock(irq_disabled_locally, flags);
+	ep_priv->ep_state |= USBSSP_EP_WEDGE;
+	ret = usbssp_halt_endpoint(usbssp_data, ep_priv, 1);
+	usbssp_g_unlock(irq_disabled_locally, flags);
 
-	/*TODO: implements this function*/
 	return ret;
 }
 
@@ -182,19 +246,39 @@ static int usbssp_gadget_start(struct usb_gadget *g,
 				usbssp_data->gadget.name,
 				usbssp_data->gadget_driver->driver.name);
 		ret = -EBUSY;
+		goto err1;
 	}
 
-	/*TODO: add implementation*/
+	usbssp_data->gadget_driver = driver;
+
+	if (pm_runtime_active(usbssp_data->dev)) {
+		usbssp_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(512);
+		usbssp_data->ep0state = USBSSP_EP0_UNCONNECTED;
+		ret = usbssp_run(usbssp_data);
+		if (ret < 0)
+			goto err1;
+	}
+	return 0;
+err1:
 	return ret;
 }
 
 static int usbssp_gadget_stop(struct usb_gadget *g)
 {
+	unsigned long flags = 0;
+	int irq_disabled_locally = 0;
 	struct usbssp_udc *usbssp_data = gadget_to_usbssp(g);
 
-	if (!usbssp_data)
-		return -EINVAL;
-	/*TODO: add implementation*/
+	usbssp_g_lock(irq_disabled_locally, flags);
+	if (pm_runtime_suspended(usbssp_data->dev))
+		goto out;
+
+	usbssp_free_dev(usbssp_data);
+	usbssp_stop(usbssp_data);
+out:
+	usbssp_data->gadget_driver = NULL;
+	usbssp_g_unlock(irq_disabled_locally, flags);
+
 	return 0;
 }
 
@@ -202,33 +286,51 @@ static int usbssp_gadget_get_frame(struct usb_gadget *g)
 {
 	struct usbssp_udc *usbssp_data = gadget_to_usbssp(g);
 
-	if (!usbssp_data)
-		return -EINVAL;
-
-	/*TODO: add implementation*/
-	return 0;
+	return usbssp_get_frame(usbssp_data);
 }
 
 static int usbssp_gadget_wakeup(struct usb_gadget *g)
 {
 	struct usbssp_udc *usbssp_data = gadget_to_usbssp(g);
+	unsigned long flags = 0;
+	int irq_disabled_locally = 0;
+	__le32 __iomem *port_regs;
+	u32 temp;
+
+	if (!usbssp_data->port_remote_wakeup)
+		return -EINVAL;
 
-	if (!usbssp_data)
+	if (!usbssp_data->port_suspended)
 		return -EINVAL;
 
-	/*TODO: add implementation*/
+	usbssp_g_lock(irq_disabled_locally, flags);
+
+	port_regs = usbssp_get_port_io_addr(usbssp_data);
+	temp = readl(port_regs+PORTPMSC);
+
+	if (!(temp & PORT_RWE))
+		return 0;
+
+	temp = readl(port_regs + PORTSC);
+
+	temp &= ~PORT_PLS_MASK;
+	writel(temp, port_regs + PORTPMSC);
+	usbssp_g_unlock(irq_disabled_locally, flags);
 	return 0;
 }
 
 static int usbssp_gadget_set_selfpowered(struct usb_gadget *g,
 					 int is_selfpowered)
 {
+	unsigned long flags = 0;
+	int irq_disabled_locally = 0;
 	struct usbssp_udc *usbssp_data = gadget_to_usbssp(g);
 
-	if (!usbssp_data)
-		return -EINVAL;
+	usbssp_g_lock(irq_disabled_locally, flags);
+
+	g->is_selfpowered = !!is_selfpowered;
+	usbssp_g_unlock(irq_disabled_locally, flags);
 
-	/*TODO: add implementation*/
 	return 0;
 }
 
diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index fe373a7b7198..0180ecfdaf9c 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -261,6 +261,22 @@ int usbssp_reset(struct usbssp_udc *usbssp_data)
 	return ret;
 }
 
+static inline int usbssp_try_enable_msi(struct usbssp_udc *usbssp_data)
+{
+	usbssp_data->msi_enabled = 1;
+	return 0;
+}
+
+static inline void usbssp_cleanup_msix(struct usbssp_udc *usbssp_data)
+{
+	usbssp_data->msi_enabled = 0;
+}
+
+static inline void usbssp_msix_sync_irqs(struct usbssp_udc *usbssp_data)
+{
+	/*TODO*/
+}
+
 /*
  * Initialize memory for gadget driver and USBSSP (one-time init).
  *
@@ -280,6 +296,88 @@ int usbssp_init(struct usbssp_udc *usbssp_data)
 	return retval;
 }
 
+/*-------------------------------------------------------------------------*/
+/*
+ * Start the USBSSP after it was halted.
+ *
+ * This function is called by the usbssp_gadget_start function when the
+ * gadget driver is started. Its opposite is usbssp_stop().
+ *
+ * usbssp_init() must be called once before this function can be called.
+ * Reset the USBSSP, enable device slot contexts, program DCBAAP, and
+ * set command ring pointer and event ring pointer.
+ */
+int usbssp_run(struct usbssp_udc *usbssp_data)
+{
+	u32 temp;
+	u64 temp_64;
+	int ret;
+	__le32 __iomem	*portsc;
+	u32 portsc_val = 0;
+	int i = 0;
+
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init, "Driver running");
+
+	ret = usbssp_try_enable_msi(usbssp_data);
+	if (ret)
+		return ret;
+
+	temp_64 = usbssp_read_64(usbssp_data,
+				 &usbssp_data->ir_set->erst_dequeue);
+	temp_64 &= ~ERST_PTR_MASK;
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"ERST deq = 64'h%0lx",
+			(unsigned long int) temp_64);
+
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+		"// Set the interrupt modulation register");
+	temp = readl(&usbssp_data->ir_set->irq_control);
+	temp &= ~ER_IRQ_INTERVAL_MASK;
+	temp |= (usbssp_data->imod_interval / 250) & ER_IRQ_INTERVAL_MASK;
+	writel(temp, &usbssp_data->ir_set->irq_control);
+
+	/*enable USB2 port*/
+	for (i = 0; i < usbssp_data->num_usb2_ports; i++) {
+		portsc = usbssp_data->usb2_ports + PORTSC;
+		portsc_val = readl(portsc) & ~PORT_PLS_MASK;
+		portsc_val = portsc_val | (5 << 5) | PORT_LINK_STROBE;
+		writel(portsc_val, portsc);
+	}
+
+	/*enable USB3.0 port*/
+	for (i = 0; i < usbssp_data->num_usb3_ports; i++) {
+		portsc = usbssp_data->usb3_ports + PORTSC;
+		portsc_val = readl(portsc) & ~PORT_PLS_MASK;
+		portsc_val = portsc_val | (5 << 5) | PORT_LINK_STROBE;
+		writel(portsc_val, portsc);
+	}
+
+	if (usbssp_start(usbssp_data)) {
+		usbssp_halt(usbssp_data);
+		return -ENODEV;
+	}
+
+	/* Set the USBSSP state before we enable the irqs */
+	temp = readl(&usbssp_data->op_regs->command);
+	temp |= (CMD_EIE);
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// Enable interrupts, cmd = 0x%x.", temp);
+	writel(temp, &usbssp_data->op_regs->command);
+
+	temp = readl(&usbssp_data->ir_set->irq_pending);
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+		"// Enabling event ring interrupter %p by writing 0x%x to irq_pending",
+		usbssp_data->ir_set, (unsigned int) ER_IRQ_ENABLE(temp));
+	writel(ER_IRQ_ENABLE(temp), &usbssp_data->ir_set->irq_pending);
+
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"Finished usbssp_run for USBSSP controller");
+
+	usbssp_data->cmd_ring_state = CMD_RING_STATE_RUNNING;
+
+	return 0;
+}
+
 /*
  * Stop USBSSP controller.
  *
@@ -1098,6 +1196,12 @@ int usbssp_enable_device(struct usbssp_udc *usbssp_data)
 	return usbssp_setup_device(usbssp_data, SETUP_CONTEXT_ONLY);
 }
 
+
+int usbssp_get_frame(struct usbssp_udc *usbssp_data)
+{
+	return readl(&usbssp_data->run_regs->microframe_index) >> 3;
+}
+
 int usbssp_gen_setup(struct usbssp_udc *usbssp_data)
 {
 	int retval;
