[PATCH 25/31] usb: usbssp: added implementation of usbssp interface.

[Pawel Laszczak]

Patch add implementation of most function from usb_ep_ops and
usbssp_gadget_ops objects. The implementation of
usbssp_gadget_ep_enable and usbssp_gadget_ep_disable functions
will be added as separate patch.

Patch also adds usbssp_g_lock and usbssp_g_unlock macros.
They are responsible for proper handling of semaphores.
Some functions belonging to usb_ep_ops and usbssp_gadget_ops
can be invoked from some different context.
In usbssp driver we have Hard Irq interrupt context and deferred interrupt
context (thread context). Additionally, driver in thread context can
calls commands on which ends it must wait and must enable
interrupts to detect completion.
Therefor when driver is waiting for command completion, the new event can
be reported and driver starts handling it in Hard Irq context.
Therefor driver use two separate SpinLock objects. The first usbssp->lock
is used in Hard Irq context and second usbssp->irq_thread_lock is used
in kernel thread context.

Signed-off-by: Pawel Laszczak <pawell@xxxxxxxxxxx>
---
 drivers/usb/usbssp/gadget-if.c | 182 +++++++++++++++++++++++++--------
 drivers/usb/usbssp/gadget.c    | 104 +++++++++++++++++++
 2 files changed, 246 insertions(+), 40 deletions(-)

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;
-- 
2.17.1