[PATCH 16/31] usb: usbssp: added connect/disconnect procedures.

From: Pawel Laszczak
Date: Thu Jul 19 2018 - 14:02:39 EST

Patch adds functionality responsible for handling CONNECT/DISCONNECT
event. This event will be reported after attached/detached USB
device to/from USB port.

To complete this procedure usbssp_halt_endpoint function must to be
implemented. This will be added in next patch.

Signed-off-by: Pawel Laszczak <pawell@xxxxxxxxxxx>
---
drivers/usb/usbssp/gadget-if.c | 16 +++
drivers/usb/usbssp/gadget-mem.c | 73 +++++++++++++
drivers/usb/usbssp/gadget-port.c | 92 ++++++++++++++++
drivers/usb/usbssp/gadget-ring.c | 12 +++
drivers/usb/usbssp/gadget.c | 178 ++++++++++++++++++++++++++++++-
drivers/usb/usbssp/gadget.h | 17 +++
6 files changed, 386 insertions(+), 2 deletions(-)

diff --git a/drivers/usb/usbssp/gadget-if.c b/drivers/usb/usbssp/gadget-if.c
index 6f42ad33979d..ef466aa47f56 100644
--- a/drivers/usb/usbssp/gadget-if.c
+++ b/drivers/usb/usbssp/gadget-if.c
@@ -288,6 +288,16 @@ void usbssp_gadget_free_endpoint(struct usbssp_udc *usbssp_data)
}
}

+static void usbssp_disconnect_gadget(struct usbssp_udc *usbssp_data)
+{
+ if (usbssp_data->gadget_driver &&
+ usbssp_data->gadget_driver->disconnect) {
+ spin_unlock(&usbssp_data->irq_thread_lock);
+ usbssp_data->gadget_driver->disconnect(&usbssp_data->gadget);
+ spin_lock(&usbssp_data->irq_thread_lock);
+ }
+}
+
void usbssp_suspend_gadget(struct usbssp_udc *usbssp_data)
{
if (usbssp_data->gadget_driver && usbssp_data->gadget_driver->suspend) {
@@ -318,6 +328,12 @@ static void usbssp_reset_gadget(struct usbssp_udc *usbssp_data)
spin_lock(&usbssp_data->lock);
}
}
+
+void usbssp_gadget_disconnect_interrupt(struct usbssp_udc *usbssp_data)
+{
+ usbssp_disconnect_gadget(usbssp_data);
+}
+
void usbssp_gadget_reset_interrupt(struct usbssp_udc *usbssp_data)
{
usbssp_reset_gadget(usbssp_data);
diff --git a/drivers/usb/usbssp/gadget-mem.c b/drivers/usb/usbssp/gadget-mem.c
index 5708a0090ead..2c220582a5f8 100644
--- a/drivers/usb/usbssp/gadget-mem.c
+++ b/drivers/usb/usbssp/gadget-mem.c
@@ -643,6 +643,70 @@ void usbssp_free_priv_device(struct usbssp_udc *usbssp_data)
usbssp_data->slot_id = 0;
}

+int usbssp_alloc_priv_device(struct usbssp_udc *usbssp_data, gfp_t flags)
+{
+ struct usbssp_device *priv_dev;
+
+ /* Slot ID 0 is reserved */
+ if (usbssp_data->slot_id == 0) {
+ dev_warn(usbssp_data->dev, "Bad Slot ID %d\n",
+ usbssp_data->slot_id);
+ return 0;
+ }
+
+ priv_dev = &usbssp_data->devs;
+
+ /* Allocate the (output) device context that will be
+ * used in the USBSSP.
+ */
+ priv_dev->out_ctx = usbssp_alloc_container_ctx(usbssp_data,
+ USBSSP_CTX_TYPE_DEVICE, flags);
+
+ if (!priv_dev->out_ctx)
+ goto fail;
+
+ dev_dbg(usbssp_data->dev, "Slot %d output ctx = 0x%llx (dma)\n",
+ usbssp_data->slot_id,
+ (unsigned long long)priv_dev->out_ctx->dma);
+
+ /* Allocate the (input) device context for address device command */
+ priv_dev->in_ctx = usbssp_alloc_container_ctx(usbssp_data,
+ USBSSP_CTX_TYPE_INPUT, flags);
+
+ if (!priv_dev->in_ctx)
+ goto fail;
+
+ dev_dbg(usbssp_data->dev, "Slot %d input ctx = 0x%llx (dma)\n",
+ usbssp_data->slot_id,
+ (unsigned long long)priv_dev->in_ctx->dma);
+
+ /* Allocate endpoint 0 ring */
+ priv_dev->eps[0].ring = usbssp_ring_alloc(usbssp_data, 2, 1,
+ TYPE_CTRL, 0, flags);
+ if (!priv_dev->eps[0].ring)
+ goto fail;
+
+ priv_dev->gadget = &usbssp_data->gadget;
+
+ /* Point to output device context in dcbaa. */
+ usbssp_data->dcbaa->dev_context_ptrs[usbssp_data->slot_id] =
+ cpu_to_le64(priv_dev->out_ctx->dma);
+ dev_dbg(usbssp_data->dev, "Set slot id %d dcbaa entry %p to 0x%llx\n",
+ usbssp_data->slot_id,
+ &usbssp_data->dcbaa->dev_context_ptrs[usbssp_data->slot_id],
+ le64_to_cpu(usbssp_data->dcbaa->dev_context_ptrs[usbssp_data->slot_id]));
+
+ trace_usbssp_alloc_priv_device(priv_dev);
+ return 1;
+fail:
+ if (priv_dev->in_ctx)
+ usbssp_free_container_ctx(usbssp_data, priv_dev->in_ctx);
+ if (priv_dev->out_ctx)
+ usbssp_free_container_ctx(usbssp_data, priv_dev->out_ctx);
+
+ return 0;
+}
+
struct usbssp_command *usbssp_alloc_command(struct usbssp_udc *usbssp_data,
bool allocate_completion,
gfp_t mem_flags)
@@ -754,6 +818,7 @@ void usbssp_mem_cleanup(struct usbssp_udc *usbssp_data)

cancel_delayed_work_sync(&usbssp_data->cmd_timer);
cancel_work_sync(&usbssp_data->bottom_irq);
+ destroy_workqueue(usbssp_data->bottom_irq_wq);

/* Free the Event Ring Segment Table and the actual Event Ring */
usbssp_free_erst(usbssp_data, &usbssp_data->erst);
@@ -1265,6 +1330,14 @@ int usbssp_mem_init(struct usbssp_udc *usbssp_data, gfp_t flags)
usbssp_handle_command_timeout);
init_completion(&usbssp_data->cmd_ring_stop_completion);

+ usbssp_data->bottom_irq_wq =
+ create_singlethread_workqueue(dev_name(usbssp_data->dev));
+
+ if (!usbssp_data->bottom_irq_wq)
+ goto fail;
+
+ INIT_WORK(&usbssp_data->bottom_irq, usbssp_bottom_irq);
+
page_size = readl(&usbssp_data->op_regs->page_size);
usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
"Supported page size register = 0x%x", page_size);
diff --git a/drivers/usb/usbssp/gadget-port.c b/drivers/usb/usbssp/gadget-port.c
index 09ea5b574ae0..2c4d28070cab 100644
--- a/drivers/usb/usbssp/gadget-port.c
+++ b/drivers/usb/usbssp/gadget-port.c
@@ -65,6 +65,98 @@ u32 usbssp_port_state_to_neutral(u32 state)
return (state & USBSSP_PORT_RO) | (state & USBSSP_PORT_RWS);
}

+/*
+ * Stop device
+ * It issues stop endpoint command for EP 0 to 30. And wait the last command
+ * to complete.
+ */
+int usbssp_stop_device(struct usbssp_udc *usbssp_data, int suspend)
+{
+ struct usbssp_device *priv_dev;
+ struct usbssp_ep_ctx *ep_ctx;
+ int ret = 0;
+ int i;
+
+ ret = 0;
+ priv_dev = &usbssp_data->devs;
+
+ trace_usbssp_stop_device(priv_dev);
+
+ if (usbssp_data->gadget.state < USB_STATE_ADDRESS) {
+ dev_dbg(usbssp_data->dev,
+ "Device is not yet in USB_STATE_ADDRESS state\n");
+ goto stop_ep0;
+ }
+
+ for (i = LAST_EP_INDEX; i > 0; i--) {
+ if (priv_dev->eps[i].ring && priv_dev->eps[i].ring->dequeue) {
+ struct usbssp_command *command;
+
+ if (priv_dev->eps[i].ep_state & EP_HALTED) {
+ dev_dbg(usbssp_data->dev,
+ "ep_index %d is in halted state "
+ "- ep state: %x\n",
+ i, priv_dev->eps[i].ep_state);
+ usbssp_halt_endpoint(usbssp_data,
+ &priv_dev->eps[i], 0);
+ }
+
+ ep_ctx = usbssp_get_ep_ctx(usbssp_data,
+ priv_dev->out_ctx, i);
+
+ if (GET_EP_CTX_STATE(ep_ctx) != EP_STATE_RUNNING) {
+ dev_dbg(usbssp_data->dev,
+ "ep_index %d is already stopped.\n", i);
+ continue;
+ }
+
+ if (priv_dev->eps[i].ep_state & EP_STOP_CMD_PENDING) {
+ dev_dbg(usbssp_data->dev,
+ "Stop endpoint command is pending "
+ "for ep_index %d.\n", i);
+ continue;
+ }
+
+ /*
+ * Device was disconnected so endpoint should be disabled
+ * and transfer ring stopped.
+ */
+ priv_dev->eps[i].ep_state |= EP_STOP_CMD_PENDING |
+ USBSSP_EP_DISABLE_PENDING;
+
+ command = usbssp_alloc_command(usbssp_data, false,
+ GFP_ATOMIC);
+ if (!command)
+ return -ENOMEM;
+
+ ret = usbssp_queue_stop_endpoint(usbssp_data,
+ command, i, suspend);
+ if (ret) {
+ usbssp_free_command(usbssp_data, command);
+ return ret;
+ }
+ }
+ }
+
+stop_ep0:
+ if (priv_dev->eps[0].ep_state & EP_HALTED) {
+ dev_dbg(usbssp_data->dev,
+ "ep_index 0 is in halted state - ep state: %x\n",
+ priv_dev->eps[i].ep_state);
+ ret = usbssp_halt_endpoint(usbssp_data, &priv_dev->eps[0], 0);
+ } else {
+ /*
+ * Device was disconnected so endpoint should be disabled
+ * and transfer ring stopped.
+ */
+ priv_dev->eps[0].ep_state &= ~USBSSP_EP_ENABLED;
+ ret = usbssp_cmd_stop_ep(usbssp_data, &usbssp_data->gadget,
+ &priv_dev->eps[0]);
+ }
+
+ return ret;
+}
+
__le32 __iomem *usbssp_get_port_io_addr(struct usbssp_udc *usbssp_data)
{
if (usbssp_data->port_major_revision == 0x03)
diff --git a/drivers/usb/usbssp/gadget-ring.c b/drivers/usb/usbssp/gadget-ring.c
index eab7676fa744..f3ee1c4d82dc 100644
--- a/drivers/usb/usbssp/gadget-ring.c
+++ b/drivers/usb/usbssp/gadget-ring.c
@@ -218,6 +218,18 @@ static inline int room_on_ring(struct usbssp_udc *usbssp_data,
return 1;
}

+/* Ring the device controller doorbell after placing a command on the ring */
+void usbssp_ring_cmd_db(struct usbssp_udc *usbssp_data)
+{
+ if (!(usbssp_data->cmd_ring_state & CMD_RING_STATE_RUNNING))
+ return;
+
+ dev_dbg(usbssp_data->dev, "// Ding dong command ring!\n");
+ writel(DB_VALUE_CMD, &usbssp_data->dba->doorbell[0]);
+ /* Flush PCI posted writes */
+ readl(&usbssp_data->dba->doorbell[0]);
+}
+
static bool usbssp_mod_cmd_timer(struct usbssp_udc *usbssp_data,
unsigned long delay)
{
diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index 6b3dc973c0d9..32d095b32e9f 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -23,6 +23,64 @@
#include "gadget-trace.h"
#include "gadget.h"

+void usbssp_bottom_irq(struct work_struct *work)
+{
+ struct usbssp_udc *usbssp_data = container_of(work, struct usbssp_udc,
+ bottom_irq);
+
+ if (usbssp_data->usbssp_state & USBSSP_STATE_DYING) {
+ dev_err(usbssp_data->dev, "Device controller dying\n");
+ return;
+ }
+
+ mutex_lock(&usbssp_data->mutex);
+ spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+ usbssp_data->irq_thread_flag);
+
+ if (usbssp_data->defered_event & EVENT_DEV_DISCONECTED) {
+ dev_dbg(usbssp_data->dev, "Disconnecting device sequence\n");
+ usbssp_data->defered_event &= ~EVENT_DEV_DISCONECTED;
+ usbssp_data->usbssp_state |= USBSSP_STATE_DISCONNECT_PENDING;
+ usbssp_stop_device(usbssp_data, 0);
+
+ usbssp_gadget_disconnect_interrupt(usbssp_data);
+ usbssp_data->gadget.speed = USB_SPEED_UNKNOWN;
+ usb_gadget_set_state(&usbssp_data->gadget, USB_STATE_NOTATTACHED);
+
+ dev_dbg(usbssp_data->dev, "Wait for disconnect\n");
+
+ spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+ usbssp_data->irq_thread_flag);
+ /*fixme: should be replaced by wait_for_completion*/
+ msleep(200);
+ spin_lock_irqsave(&usbssp_data->irq_thread_lock,
+ usbssp_data->irq_thread_flag);
+ }
+
+ if (usbssp_data->defered_event & EVENT_DEV_CONNECTED) {
+ dev_dbg(usbssp_data->dev, "Connecting device sequence\n");
+ if (usbssp_data->usbssp_state & USBSSP_STATE_DISCONNECT_PENDING) {
+ usbssp_free_dev(usbssp_data);
+ usbssp_data->usbssp_state &= ~USBSSP_STATE_DISCONNECT_PENDING;
+ }
+
+ usbssp_data->defered_event &= ~EVENT_DEV_CONNECTED;
+ usbssp_alloc_dev(usbssp_data);
+ }
+
+ if (usbssp_data->defered_event & EVENT_USB_RESET) {
+ /*TODO: implement handling of USB_RESET*/
+ }
+
+ /*handle setup packet*/
+ if (usbssp_data->defered_event & EVENT_SETUP_PACKET) {
+ /*TODO: implement handling of SETUP packet*/
+ }
+
+ spin_unlock_irqrestore(&usbssp_data->irq_thread_lock,
+ usbssp_data->irq_thread_flag);
+ mutex_unlock(&usbssp_data->mutex);
+}

/*
* usbssp_handshake - spin reading dc until handshake completes or fails
@@ -273,6 +331,123 @@ unsigned int usbssp_last_valid_endpoint(u32 added_ctxs)
return fls(added_ctxs) - 1;
}

+int usbssp_halt_endpoint(struct usbssp_udc *usbssp_data, struct usbssp_ep *dep,
+ int value)
+{
+ /*TODO: implement this function*/
+ return 0;
+}
+
+/*
+ * 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
+ * disabled. Free any DC data structures associated with that device.
+ */
+void usbssp_free_dev(struct usbssp_udc *usbssp_data)
+{
+ struct usbssp_device *priv_dev;
+ int i, ret;
+ struct usbssp_slot_ctx *slot_ctx;
+
+ priv_dev = &usbssp_data->devs;
+ slot_ctx = usbssp_get_slot_ctx(usbssp_data, priv_dev->out_ctx);
+ trace_usbssp_free_dev(slot_ctx);
+
+ for (i = 0; i < 31; ++i)
+ priv_dev->eps[i].ep_state &= ~EP_STOP_CMD_PENDING;
+
+ ret = usbssp_disable_slot(usbssp_data);
+ if (ret)
+ usbssp_free_priv_device(usbssp_data);
+}
+
+int usbssp_disable_slot(struct usbssp_udc *usbssp_data)
+{
+ struct usbssp_command *command;
+ u32 state;
+ int ret = 0;
+
+ command = usbssp_alloc_command(usbssp_data, false, GFP_ATOMIC);
+ if (!command)
+ return -ENOMEM;
+
+ /* Don't disable the slot if the device controller is dead. */
+ state = readl(&usbssp_data->op_regs->status);
+ if (state == 0xffffffff ||
+ (usbssp_data->usbssp_state & USBSSP_STATE_DYING) ||
+ (usbssp_data->usbssp_state & USBSSP_STATE_HALTED)) {
+ kfree(command);
+ return -ENODEV;
+ }
+
+ ret = usbssp_queue_slot_control(usbssp_data, command, TRB_DISABLE_SLOT);
+ if (ret) {
+ kfree(command);
+ return ret;
+ }
+ usbssp_ring_cmd_db(usbssp_data);
+ return ret;
+}
+
+/*
+ * Returns 0 if the DC n out of device slots, the Enable Slot command
+ * timed out, or allocating memory failed. Returns 1 on success.
+ */
+int usbssp_alloc_dev(struct usbssp_udc *usbssp_data)
+{
+ int ret, slot_id;
+ struct usbssp_command *command;
+ struct usbssp_slot_ctx *slot_ctx;
+
+ command = usbssp_alloc_command(usbssp_data, true, GFP_ATOMIC);
+
+ if (!command)
+ return -ENOMEM;
+
+ ret = usbssp_queue_slot_control(usbssp_data, command, TRB_ENABLE_SLOT);
+
+ if (ret) {
+ usbssp_free_command(usbssp_data, command);
+ return ret;
+ }
+
+ 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);
+
+ slot_id = usbssp_data->slot_id;
+
+ if (!slot_id || command->status != COMP_SUCCESS) {
+ dev_err(usbssp_data->dev,
+ "Error while assigning device slot ID\n");
+ usbssp_free_command(usbssp_data, command);
+ return 0;
+ }
+
+ usbssp_free_command(usbssp_data, command);
+
+ if (!usbssp_alloc_priv_device(usbssp_data, GFP_ATOMIC)) {
+ dev_warn(usbssp_data->dev,
+ "Could not allocate usbssp_device data structures\n");
+ goto disable_slot;
+ }
+
+ slot_ctx = usbssp_get_slot_ctx(usbssp_data, usbssp_data->devs.out_ctx);
+ trace_usbssp_alloc_dev(slot_ctx);
+
+ return 1;
+
+disable_slot:
+ ret = usbssp_disable_slot(usbssp_data);
+ if (ret)
+ usbssp_free_priv_device(usbssp_data);
+
+ return 0;
+}
+
int usbssp_gen_setup(struct usbssp_udc *usbssp_data)
{
int retval;
@@ -420,7 +595,6 @@ int usbssp_gadget_exit(struct usbssp_udc *usbssp_data)

usb_del_gadget_udc(&usbssp_data->gadget);
usbssp_gadget_free_endpoint(usbssp_data);
- /*TODO: add usbssp_stop implementation*/
- //usbssp_stop(usbssp_data);
+ usbssp_stop(usbssp_data);
return ret;
}
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 145371eee47d..1827781125bd 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1685,6 +1685,8 @@ void usbssp_dbg_trace(struct usbssp_udc *usbssp_data,
/* USBSSP memory management */
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);
unsigned int usbssp_last_valid_endpoint(u32 added_ctxs);
int usbssp_ring_expansion(struct usbssp_udc *usbssp_data,
struct usbssp_ring *ring,
@@ -1712,12 +1714,15 @@ int usbssp_handshake(void __iomem *ptr, u32 mask, u32 done, int usec);
void usbssp_quiesce(struct usbssp_udc *usbssp_data);
int usbssp_halt(struct usbssp_udc *usbssp_data);
extern int usbssp_reset(struct usbssp_udc *usbssp_data);
+int usbssp_disable_slot(struct usbssp_udc *usbssp_data);

int usbssp_suspend(struct usbssp_udc *usbssp_data, bool do_wakeup);
int usbssp_resume(struct usbssp_udc *usbssp_data, bool hibernated);

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);
/* USBSSP ring, segment, TRB, and TD functions */
dma_addr_t usbssp_trb_virt_to_dma(struct usbssp_segment *seg,
union usbssp_trb *trb);
@@ -1726,6 +1731,12 @@ struct usbssp_segment *usbssp_trb_in_td(struct usbssp_udc *usbssp_data,
union usbssp_trb *start_trb,
union usbssp_trb *end_trb,
dma_addr_t suspect_dma, bool debug);
+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_stop_endpoint(struct usbssp_udc *usbssp_data,
+ struct usbssp_command *cmd,
+ unsigned int ep_index, int suspend);
void usbssp_handle_command_timeout(struct work_struct *work);

void usbssp_cleanup_command_queue(struct usbssp_udc *usbssp_data);
@@ -1753,6 +1764,12 @@ void usbssp_gadget_free_endpoint(struct usbssp_udc *usbssp_data);
int usbssp_gadget_init_endpoint(struct usbssp_udc *usbssp_data);
unsigned int usbssp_port_speed(unsigned int port_status);
void usbssp_gadget_reset_interrupt(struct usbssp_udc *usbssp_data);
+void usbssp_gadget_disconnect_interrupt(struct usbssp_udc *usbssp_data);
+int usbssp_stop_device(struct usbssp_udc *usbssp_data, int suspend);
+int usbssp_halt_endpoint(struct usbssp_udc *usbssp_data,
+ struct usbssp_ep *dep, int value);
+int usbssp_cmd_stop_ep(struct usbssp_udc *usbssp_data, struct usb_gadget *g,
+ struct usbssp_ep *ep_priv);

static inline char *usbssp_slot_state_string(u32 state)
{
--
2.17.1