[PATCH 18/31] usb: usbssp: added handling of Port Reset event.

[Pawel Laszczak]

Patch adds functionality used during resetting USB port.
During this process driver must finish all queued transfers,
and enter the controller to default state.

Signed-off-by: Pawel Laszczak <pawell@xxxxxxxxxxx>
---
 drivers/usb/usbssp/gadget-dbg.c |   9 +
 drivers/usb/usbssp/gadget-mem.c | 101 +++++++++++
 drivers/usb/usbssp/gadget.c     | 305 +++++++++++++++++++++++++++++++-
 drivers/usb/usbssp/gadget.h     |  13 ++
 4 files changed, 427 insertions(+), 1 deletion(-)

diff --git a/drivers/usb/usbssp/gadget-dbg.c b/drivers/usb/usbssp/gadget-dbg.c
index 277617d1a996..a425469d95d9 100644
--- a/drivers/usb/usbssp/gadget-dbg.c
+++ b/drivers/usb/usbssp/gadget-dbg.c
@@ -12,6 +12,15 @@
 
 #include "gadget.h"
 
+char *usbssp_get_slot_state(struct usbssp_udc *usbssp_data,
+			    struct usbssp_container_ctx *ctx)
+{
+	struct usbssp_slot_ctx *slot_ctx = usbssp_get_slot_ctx(usbssp_data, ctx);
+	int state = GET_SLOT_STATE(le32_to_cpu(slot_ctx->dev_state));
+
+	return usbssp_slot_state_string(state);
+}
+
 void usbssp_dbg_trace(struct usbssp_udc *usbssp_data,
 		      void (*trace)(struct va_format *),
 		      const char *fmt, ...)
diff --git a/drivers/usb/usbssp/gadget-mem.c b/drivers/usb/usbssp/gadget-mem.c
index 2c220582a5f8..858bee77b0dc 100644
--- a/drivers/usb/usbssp/gadget-mem.c
+++ b/drivers/usb/usbssp/gadget-mem.c
@@ -707,6 +707,107 @@ int usbssp_alloc_priv_device(struct usbssp_udc *usbssp_data, gfp_t flags)
 	return 0;
 }
 
+void usbssp_copy_ep0_dequeue_into_input_ctx(struct usbssp_udc *usbssp_data)
+{
+	struct usbssp_device *priv_dev;
+	struct usbssp_ep_ctx *ep0_ctx;
+	struct usbssp_ring *ep_ring;
+
+	priv_dev = &usbssp_data->devs;
+	ep0_ctx = usbssp_get_ep_ctx(usbssp_data, priv_dev->in_ctx, 0);
+	ep_ring = priv_dev->eps[0].ring;
+
+	/*
+	 * We don't keep track of the dequeue pointer very well after a
+	 * Set TR dequeue pointer, so we're setting the dequeue pointer of the
+	 * device to our enqueue pointer. This should only be called after a
+	 * configured device has reset, so all control transfers should have
+	 * been completed or cancelled before the reset.
+	 */
+	ep0_ctx->deq = cpu_to_le64(usbssp_trb_virt_to_dma(ep_ring->enq_seg,
+			ep_ring->enqueue) | ep_ring->cycle_state);
+}
+
+/* Setup an DC private device for a Set Address command */
+int usbssp_setup_addressable_priv_dev(struct usbssp_udc *usbssp_data)
+{
+	struct usbssp_device *dev_priv;
+	struct usbssp_ep_ctx *ep0_ctx;
+	struct usbssp_slot_ctx *slot_ctx;
+	u32 max_packets;
+
+	dev_priv = &usbssp_data->devs;
+	/* Slot ID 0 is reserved */
+	if (usbssp_data->slot_id == 0 || !dev_priv->gadget) {
+		dev_warn(usbssp_data->dev,
+			"Slot ID %d is not assigned to this device\n",
+			usbssp_data->slot_id);
+		return -EINVAL;
+	}
+
+	ep0_ctx = usbssp_get_ep_ctx(usbssp_data, dev_priv->in_ctx, 0);
+	slot_ctx = usbssp_get_slot_ctx(usbssp_data, dev_priv->in_ctx);
+
+	/* 3) Only the control endpoint is valid - one endpoint context */
+	slot_ctx->dev_info |= cpu_to_le32(LAST_CTX(1));
+
+	switch (dev_priv->gadget->speed) {
+	case USB_SPEED_SUPER_PLUS:
+		slot_ctx->dev_info |= cpu_to_le32(SLOT_SPEED_SSP);
+		max_packets = MAX_PACKET(512);
+		break;
+	case USB_SPEED_SUPER:
+		slot_ctx->dev_info |= cpu_to_le32(SLOT_SPEED_SS);
+		max_packets = MAX_PACKET(512);
+		break;
+	case USB_SPEED_HIGH:
+		slot_ctx->dev_info |= cpu_to_le32(SLOT_SPEED_HS);
+		max_packets = MAX_PACKET(64);
+		break;
+	case USB_SPEED_FULL:
+		slot_ctx->dev_info |= cpu_to_le32(SLOT_SPEED_FS);
+		max_packets = MAX_PACKET(64);
+		break;
+	case USB_SPEED_LOW:
+		slot_ctx->dev_info |= cpu_to_le32(SLOT_SPEED_LS);
+		max_packets = MAX_PACKET(8);
+		break;
+	case USB_SPEED_WIRELESS:
+		dev_dbg(usbssp_data->dev,
+			"USBSSP doesn't support wireless speeds\n");
+		return -EINVAL;
+	default:
+		/* Speed was not set , this shouldn't happen. */
+		return -EINVAL;
+	}
+
+	if (!usbssp_data->devs.port_num)
+		return -EINVAL;
+
+	slot_ctx->dev_info2 |=
+			cpu_to_le32(ROOT_DEV_PORT(usbssp_data->devs.port_num));
+	slot_ctx->dev_state |= (usbssp_data->device_address & DEV_ADDR_MASK);
+
+	ep0_ctx->tx_info = EP_AVG_TRB_LENGTH(0x8);
+
+
+	/* Step 4 - ring already allocated */
+	/* Step 5 */
+	ep0_ctx->ep_info2 = cpu_to_le32(EP_TYPE(CTRL_EP));
+
+	/* EP 0 can handle "burst" sizes of 1, so Max Burst Size field is 0 */
+	ep0_ctx->ep_info2 |= cpu_to_le32(MAX_BURST(0) | ERROR_COUNT(3) |
+			max_packets);
+
+	ep0_ctx->deq = cpu_to_le64(dev_priv->eps[0].ring->first_seg->dma |
+				dev_priv->eps[0].ring->cycle_state);
+
+	trace_usbssp_setup_addressable_priv_device(dev_priv);
+	/* Steps 7 and 8 were done in usbssp_alloc_priv_device() */
+
+	return 0;
+}
+
 struct usbssp_command *usbssp_alloc_command(struct usbssp_udc *usbssp_data,
 					    bool allocate_completion,
 					    gfp_t mem_flags)
diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index 4dac1b3cbb85..5102dd31c881 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -69,7 +69,27 @@ void usbssp_bottom_irq(struct work_struct *work)
 	}
 
 	if (usbssp_data->defered_event & EVENT_USB_RESET) {
-		/*TODO: implement handling of USB_RESET*/
+		__le32 __iomem *port_regs;
+		u32 temp;
+
+		dev_dbg(usbssp_data->dev, "Beginning USB reset device sequence\n");
+
+		/*Reset Device Command*/
+		usbssp_data->defered_event &= ~EVENT_USB_RESET;
+		usbssp_reset_device(usbssp_data);
+		usbssp_data->devs.eps[0].ep_state |= USBSSP_EP_ENABLED;
+		usbssp_data->defered_event &= ~EVENT_DEV_CONNECTED;
+
+		usbssp_enable_device(usbssp_data);
+		if ((usbssp_data->gadget.speed == USB_SPEED_SUPER) ||
+		    (usbssp_data->gadget.speed == USB_SPEED_SUPER_PLUS)) {
+			dev_dbg(usbssp_data->dev, "Set U1/U2 enable\n");
+			port_regs = usbssp_get_port_io_addr(usbssp_data);
+			temp = readl(port_regs+PORTPMSC);
+			temp &= ~(PORT_U1_TIMEOUT_MASK | PORT_U2_TIMEOUT_MASK);
+			temp |= PORT_U1_TIMEOUT(1) | PORT_U2_TIMEOUT(1);
+			writel(temp, port_regs+PORTPMSC);
+		}
 	}
 
 	/*handle setup packet*/
@@ -488,6 +508,108 @@ int usbssp_halt_endpoint(struct usbssp_udc *usbssp_data, struct usbssp_ep *dep,
 	return 0;
 }
 
+/*
+ * This submits a Reset Device Command, which will set the device state to 0,
+ * set the device address to 0, and disable all the endpoints except the default
+ * control endpoint. The USB core should come back and call
+ * usbssp_address_device(), and then re-set up the configuration.
+ *
+ * Wait for the Reset Device command to finish. Remove all structures
+ * associated with the endpoints that were disabled. Clear the input device
+ * structure? Reset the control endpoint 0 max packet size?
+ */
+int usbssp_reset_device(struct usbssp_udc *usbssp_data)
+{
+	struct usbssp_device *dev_priv;
+	struct usbssp_command *reset_device_cmd;
+	struct usbssp_slot_ctx *slot_ctx;
+	int slot_state;
+	int ret = 0;
+
+	ret = usbssp_check_args(usbssp_data, NULL, 0, false, __func__);
+	if (ret <= 0)
+		return ret;
+
+	dev_priv = &usbssp_data->devs;
+
+	/* If device is not setup, there is no point in resetting it */
+	slot_ctx = usbssp_get_slot_ctx(usbssp_data, dev_priv->out_ctx);
+	slot_state = GET_SLOT_STATE(le32_to_cpu(slot_ctx->dev_state));
+	pr_info("usbssp_reset_deviceslot_stated\n");
+	if (slot_state == SLOT_STATE_DISABLED ||
+	    slot_state == SLOT_STATE_ENABLED ||
+	    slot_state == SLOT_STATE_DEFAULT) {
+		dev_dbg(usbssp_data->dev,
+			"Slot in DISABLED/ENABLED state - reset not allowed\n");
+		return 0;
+	}
+
+	trace_usbssp_reset_device(slot_ctx);
+
+	dev_dbg(usbssp_data->dev, "Resetting device with slot ID %u\n",
+		usbssp_data->slot_id);
+
+	/* Allocate the command structure that holds the struct completion.*/
+	reset_device_cmd = usbssp_alloc_command(usbssp_data, true, GFP_ATOMIC);
+
+	if (!reset_device_cmd) {
+		dev_dbg(usbssp_data->dev,
+			"Couldn't allocate command structure.\n");
+		return -ENOMEM;
+	}
+
+	/* Attempt to submit the Reset Device command to the command ring */
+	ret = usbssp_queue_reset_device(usbssp_data, reset_device_cmd);
+	if (ret) {
+		dev_dbg(usbssp_data->dev,
+			"FIXME: allocate a command ring segment\n");
+		goto command_cleanup;
+	}
+	usbssp_ring_cmd_db(usbssp_data);
+
+	spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+			usbssp_data->irq_thread_flag);
+
+	/* Wait for the Reset Device command to finish */
+	wait_for_completion(reset_device_cmd->completion);
+	spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+			usbssp_data->irq_thread_flag);
+
+	/*
+	 * The Reset Device command can't fail, according to spec,
+	 * unless we tried to reset a slot ID that wasn't enabled,
+	 * or the device wasn't in the addressed or configured state.
+	 */
+	ret = reset_device_cmd->status;
+	switch (ret) {
+	case COMP_COMMAND_ABORTED:
+	case COMP_COMMAND_RING_STOPPED:
+		dev_warn(usbssp_data->dev,
+			"Timeout waiting for reset device command\n");
+		ret = -ETIME;
+		goto command_cleanup;
+	case COMP_SLOT_NOT_ENABLED_ERROR: /*completion code for bad slot ID */
+	case COMP_CONTEXT_STATE_ERROR: /* completion code for same thing */
+		dev_dbg(usbssp_data->dev, "Not freeing device rings.\n");
+		ret = 0;
+		goto command_cleanup;
+	case COMP_SUCCESS:
+		dev_dbg(usbssp_data->dev, "Successful reset device command.\n");
+		break;
+	default:
+		dev_warn(usbssp_data->dev, "Unknown completion code %u for "
+			"reset device command.\n", ret);
+		ret = -EINVAL;
+		goto command_cleanup;
+	}
+
+	ret = 0;
+
+command_cleanup:
+	usbssp_free_command(usbssp_data, reset_device_cmd);
+	return ret;
+}
+
 /*
  * At this point, the struct usb_device is about to go away, the device has
  * disconnected, and all traffic has been stopped and the endpoints have been
@@ -598,6 +720,187 @@ int usbssp_alloc_dev(struct usbssp_udc *usbssp_data)
 	return 0;
 }
 
+/*
+ * Issue an Address Device command
+ */
+static int usbssp_setup_device(struct usbssp_udc *usbssp_data,
+			       enum usbssp_setup_dev setup)
+{
+	const char *act = setup == SETUP_CONTEXT_ONLY ? "context" : "address";
+	struct usbssp_device *dev_priv;
+	int ret = 0;
+	struct usbssp_slot_ctx *slot_ctx;
+	struct usbssp_input_control_ctx *ctrl_ctx;
+	u64 temp_64;
+	struct usbssp_command *command = NULL;
+	int dev_state = 0;
+	int slot_id = usbssp_data->slot_id;
+
+	if (usbssp_data->usbssp_state) {/* dying, removing or halted */
+		ret = -ESHUTDOWN;
+		goto out;
+	}
+
+	if (!slot_id) {
+		usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_address,
+				"Bad Slot ID %d", slot_id);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	dev_priv = &usbssp_data->devs;
+
+	slot_ctx = usbssp_get_slot_ctx(usbssp_data, dev_priv->out_ctx);
+	trace_usbssp_setup_device_slot(slot_ctx);
+
+	dev_state = GET_SLOT_STATE(le32_to_cpu(slot_ctx->dev_state));
+
+	if (setup == SETUP_CONTEXT_ONLY) {
+		if (dev_state == SLOT_STATE_DEFAULT) {
+			dev_dbg(usbssp_data->dev,
+				"Slot already in default state\n");
+			goto out;
+		}
+	}
+
+	command = usbssp_alloc_command(usbssp_data, true, GFP_ATOMIC);
+	if (!command) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	command->in_ctx = dev_priv->in_ctx;
+
+	slot_ctx = usbssp_get_slot_ctx(usbssp_data, dev_priv->in_ctx);
+	ctrl_ctx = usbssp_get_input_control_ctx(dev_priv->in_ctx);
+
+	if (!ctrl_ctx) {
+		dev_warn(usbssp_data->dev,
+			"%s: Could not get input context, bad type.\n",
+			__func__);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/*
+	 * If this is the first Set Address (BSR=0) or driver trays
+	 * transition to Default (BSR=1) since device plug-in or
+	 * priv device reallocation after a resume with an USBSSP power loss,
+	 * then set up the slot context or update device address in slot
+	 * context.
+	 */
+	if (!slot_ctx->dev_info || dev_state == SLOT_STATE_DEFAULT)
+		usbssp_setup_addressable_priv_dev(usbssp_data);
+
+	if (dev_state == SLOT_STATE_DEFAULT)
+		usbssp_copy_ep0_dequeue_into_input_ctx(usbssp_data);
+
+	ctrl_ctx->add_flags = cpu_to_le32(SLOT_FLAG | EP0_FLAG);
+	ctrl_ctx->drop_flags = 0;
+
+	trace_usbssp_address_ctx(usbssp_data, dev_priv->in_ctx,
+				le32_to_cpu(slot_ctx->dev_info) >> 27);
+
+	ret = usbssp_queue_address_device(usbssp_data, command,
+					dev_priv->in_ctx->dma, setup);
+
+	if (ret) {
+		usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_address,
+				"Prabably command ring segment is full");
+		goto out;
+	}
+
+	usbssp_ring_cmd_db(usbssp_data);
+
+	spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+			usbssp_data->irq_thread_flag);
+	wait_for_completion(command->completion);
+	spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+			usbssp_data->irq_thread_flag);
+
+	switch (command->status) {
+	case COMP_COMMAND_ABORTED:
+	case COMP_COMMAND_RING_STOPPED:
+		dev_warn(usbssp_data->dev,
+			"Timeout while waiting for setup device command\n");
+		ret = -ETIME;
+		break;
+	case COMP_CONTEXT_STATE_ERROR:
+	case COMP_SLOT_NOT_ENABLED_ERROR:
+		dev_err(usbssp_data->dev,
+			"Setup ERROR: setup %s command for slot %d.\n",
+			act, slot_id);
+		ret = -EINVAL;
+		break;
+	case COMP_INCOMPATIBLE_DEVICE_ERROR:
+		dev_warn(usbssp_data->dev,
+			"ERROR: Incompatible device for setup %s command\n",
+			act);
+		ret = -ENODEV;
+		break;
+	case COMP_SUCCESS:
+		usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_address,
+				"Successful setup %s command", act);
+		break;
+	default:
+		dev_err(usbssp_data->dev,
+			"ERROR: unexpected setup %s command completion code 0x%x.\n",
+			 act, command->status);
+
+		trace_usbssp_address_ctx(usbssp_data, dev_priv->out_ctx, 1);
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret)
+		goto out;
+
+	temp_64 = usbssp_read_64(usbssp_data, &usbssp_data->op_regs->dcbaa_ptr);
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_address,
+			"Op regs DCBAA ptr = %#016llx", temp_64);
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_address,
+			"Slot ID %d dcbaa entry @%p = %#016llx",
+			slot_id, &usbssp_data->dcbaa->dev_context_ptrs[slot_id],
+			(unsigned long long)
+			le64_to_cpu(usbssp_data->dcbaa->dev_context_ptrs[slot_id]));
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_address,
+			"Output Context DMA address = %#08llx",
+			(unsigned long long)dev_priv->out_ctx->dma);
+
+	trace_usbssp_address_ctx(usbssp_data, dev_priv->in_ctx,
+				le32_to_cpu(slot_ctx->dev_info) >> 27);
+
+	slot_ctx = usbssp_get_slot_ctx(usbssp_data, dev_priv->out_ctx);
+	trace_usbssp_address_ctx(usbssp_data, dev_priv->out_ctx,
+				le32_to_cpu(slot_ctx->dev_info) >> 27);
+	/* Zero the input context control for later use */
+	ctrl_ctx->add_flags = 0;
+	ctrl_ctx->drop_flags = 0;
+
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_address,
+			"Internal device address = %d",
+			le32_to_cpu(slot_ctx->dev_state) & DEV_ADDR_MASK);
+
+	if (setup == SETUP_CONTEXT_ADDRESS)
+		usbssp_status_stage(usbssp_data);
+out:
+	if (command) {
+		kfree(command->completion);
+		kfree(command);
+	}
+	return ret;
+}
+
+int usbssp_address_device(struct usbssp_udc *usbssp_data)
+{
+	return usbssp_setup_device(usbssp_data, SETUP_CONTEXT_ADDRESS);
+}
+
+int usbssp_enable_device(struct usbssp_udc *usbssp_data)
+{
+	return usbssp_setup_device(usbssp_data, SETUP_CONTEXT_ONLY);
+}
+
 int usbssp_gen_setup(struct usbssp_udc *usbssp_data)
 {
 	int retval;
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 64168ea16dbf..ea110667e964 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1679,6 +1679,8 @@ static inline void usbssp_write_64(struct usbssp_udc *usbssp_data,
 }
 
 /* USBSSP memory management */
+char *usbssp_get_slot_state(struct usbssp_udc *usbssp_data,
+		struct usbssp_container_ctx *ctx);
 void usbssp_dbg_trace(struct usbssp_udc *usbssp_data,
 		void (*trace)(struct va_format *),
 		const char *fmt, ...);
@@ -1687,6 +1689,8 @@ void usbssp_mem_cleanup(struct usbssp_udc *usbssp_data);
 int usbssp_mem_init(struct usbssp_udc *usbssp_data, gfp_t flags);
 void usbssp_free_priv_device(struct usbssp_udc *usbssp_data);
 int usbssp_alloc_priv_device(struct usbssp_udc *usbssp_data, gfp_t flags);
+int usbssp_setup_addressable_priv_dev(struct usbssp_udc *usbssp_data);
+void usbssp_copy_ep0_dequeue_into_input_ctx(struct usbssp_udc *usbssp_data);
 unsigned int usbssp_get_endpoint_index(const struct usb_endpoint_descriptor *desc);
 unsigned int usbssp_last_valid_endpoint(u32 added_ctxs);
 int usbssp_ring_expansion(struct usbssp_udc *usbssp_data,
@@ -1724,6 +1728,8 @@ irqreturn_t usbssp_irq(int irq, void *priv);
 
 int usbssp_alloc_dev(struct usbssp_udc *usbssp_data);
 void usbssp_free_dev(struct usbssp_udc *usbssp_data);
+int usbssp_address_device(struct usbssp_udc *usbssp_data);
+int usbssp_enable_device(struct usbssp_udc *usbssp_data);
 /* USBSSP ring, segment, TRB, and TD functions */
 dma_addr_t usbssp_trb_virt_to_dma(struct usbssp_segment *seg,
 				union usbssp_trb *trb);
@@ -1738,6 +1744,10 @@ int usbssp_is_vendor_info_code(struct usbssp_udc *usbssp_data,
 void usbssp_ring_cmd_db(struct usbssp_udc *usbssp_data);
 int usbssp_queue_slot_control(struct usbssp_udc *usbssp_data,
 			struct usbssp_command *cmd, u32 trb_type);
+int usbssp_queue_address_device(struct usbssp_udc *usbssp_data,
+				struct usbssp_command *cmd,
+				dma_addr_t in_ctx_ptr,
+				enum usbssp_setup_dev setup);
 int usbssp_queue_stop_endpoint(struct usbssp_udc *usbssp_data,
 			struct usbssp_command *cmd,
 			unsigned int ep_index, int suspend);
@@ -1753,6 +1763,8 @@ void usbssp_cleanup_halted_endpoint(struct usbssp_udc *usbssp_data,
 int usbssp_queue_halt_endpoint(struct usbssp_udc *usbssp_data,
 			struct usbssp_command *cmd,
 			unsigned int ep_index);
+int usbssp_queue_reset_device(struct usbssp_udc *usbssp_data,
+			struct usbssp_command *cmd);
 void usbssp_handle_command_timeout(struct work_struct *work);
 
 void usbssp_cleanup_command_queue(struct usbssp_udc *usbssp_data);
@@ -1788,6 +1800,7 @@ int usbssp_cmd_stop_ep(struct usbssp_udc *usbssp_data, struct usb_gadget *g,
 		struct usbssp_ep *ep_priv);
 int usbssp_status_stage(struct usbssp_udc *usbssp_data);
 
+int usbssp_reset_device(struct usbssp_udc *usbssp_data);
 static inline char *usbssp_slot_state_string(u32 state)
 {
 	switch (state) {
-- 
2.17.1