[PATCH 09/31] usb: usbssp: add implementation of usbssp_mem_cleanup

[Pawel Laszczak]

Patch add implementation of usbssp_mem_cleanup and
all other functions used during cleaning driver during
unloading module.

Signed-off-by: Pawel Laszczak <pawell@xxxxxxxxxxx>
---
 drivers/usb/usbssp/gadget-mem.c  | 182 ++++++++++++++++++++++++++++++-
 drivers/usb/usbssp/gadget-ring.c |  21 ++++
 drivers/usb/usbssp/gadget.c      |   3 +-
 drivers/usb/usbssp/gadget.h      |  10 ++
 4 files changed, 213 insertions(+), 3 deletions(-)

diff --git a/drivers/usb/usbssp/gadget-mem.c b/drivers/usb/usbssp/gadget-mem.c
index 06a59febbff2..ecb6e1bbd212 100644
--- a/drivers/usb/usbssp/gadget-mem.c
+++ b/drivers/usb/usbssp/gadget-mem.c
@@ -495,6 +495,105 @@ struct usbssp_container_ctx *usbssp_alloc_container_ctx(
 	return ctx;
 }
 
+void usbssp_free_container_ctx(struct usbssp_udc *usbssp_data,
+			       struct usbssp_container_ctx *ctx)
+{
+	if (!ctx)
+		return;
+	dma_pool_free(usbssp_data->device_pool, ctx->bytes, ctx->dma);
+	kfree(ctx);
+}
+
+/***************** Streams structures manipulation *************************/
+static void usbssp_free_stream_ctx(struct usbssp_udc *usbssp_data,
+				   unsigned int num_stream_ctxs,
+				   struct usbssp_stream_ctx *stream_ctx,
+				   dma_addr_t dma)
+{
+	struct device *dev = usbssp_data->dev;
+	size_t size = sizeof(struct usbssp_stream_ctx) * num_stream_ctxs;
+
+	if (size > MEDIUM_STREAM_ARRAY_SIZE)
+		dma_free_coherent(dev, size, stream_ctx, dma);
+	else if (size <= SMALL_STREAM_ARRAY_SIZE)
+		return dma_pool_free(usbssp_data->small_streams_pool,
+				stream_ctx, dma);
+	else
+		return dma_pool_free(usbssp_data->medium_streams_pool,
+				stream_ctx, dma);
+}
+
+/**
+ * Frees all stream contexts associated with the endpoint,
+ *
+ * Caller should fix the endpoint context streams fields.
+ */
+void usbssp_free_stream_info(struct usbssp_udc *usbssp_data,
+			     struct usbssp_stream_info *stream_info)
+{
+	int cur_stream;
+	struct usbssp_ring *cur_ring;
+
+	if (!stream_info)
+		return;
+
+	for (cur_stream = 1; cur_stream < stream_info->num_streams;
+		cur_stream++) {
+		cur_ring = stream_info->stream_rings[cur_stream];
+		if (cur_ring) {
+			usbssp_ring_free(usbssp_data, cur_ring);
+			stream_info->stream_rings[cur_stream] = NULL;
+		}
+	}
+
+	usbssp_free_command(usbssp_data, stream_info->free_streams_command);
+	usbssp_data->cmd_ring_reserved_trbs--;
+	if (stream_info->stream_ctx_array)
+		usbssp_free_stream_ctx(usbssp_data,
+				stream_info->num_stream_ctxs,
+				stream_info->stream_ctx_array,
+				stream_info->ctx_array_dma);
+
+	kfree(stream_info->stream_rings);
+	kfree(stream_info);
+}
+
+/***************** Device context manipulation *************************/
+
+/* All the usbssp_tds in the ring's TD list should be freed at this point.*/
+void usbssp_free_priv_device(struct usbssp_udc *usbssp_data)
+{
+	struct usbssp_device *dev;
+	int i;
+
+	/* if slot_id = 0 then no device slot is used */
+	if (usbssp_data->slot_id == 0)
+		return;
+
+	dev = &usbssp_data->devs;
+	trace_usbssp_free_priv_device(dev);
+
+	usbssp_data->dcbaa->dev_context_ptrs[usbssp_data->slot_id] = 0;
+	if (!dev)
+		return;
+
+	for (i = 0; i < 31; ++i) {
+		if (dev->eps[i].ring)
+			usbssp_ring_free(usbssp_data, dev->eps[i].ring);
+
+		if (dev->eps[i].stream_info)
+			usbssp_free_stream_info(usbssp_data,
+					dev->eps[i].stream_info);
+	}
+
+	if (dev->in_ctx)
+		usbssp_free_container_ctx(usbssp_data, dev->in_ctx);
+	if (dev->out_ctx)
+		usbssp_free_container_ctx(usbssp_data, dev->out_ctx);
+
+	usbssp_data->slot_id = 0;
+}
+
 struct usbssp_command *usbssp_alloc_command(struct usbssp_udc *usbssp_data,
 					    bool allocate_completion,
 					    gfp_t mem_flags)
@@ -549,6 +648,13 @@ void usbssp_request_free_priv(struct usbssp_request *priv_req)
 		kfree(priv_req->td);
 }
 
+void usbssp_free_command(struct usbssp_udc *usbssp_data,
+			 struct usbssp_command *command)
+{
+	usbssp_free_container_ctx(usbssp_data, command->in_ctx);
+	kfree(command->completion);
+	kfree(command);
+}
 
 int usbssp_alloc_erst(struct usbssp_udc *usbssp_data,
 		      struct usbssp_ring *evt_ring,
@@ -580,9 +686,83 @@ int usbssp_alloc_erst(struct usbssp_udc *usbssp_data,
 	return 0;
 }
 
+void usbssp_free_erst(struct usbssp_udc *usbssp_data, struct usbssp_erst *erst)
+{
+	size_t size;
+	struct device *dev = usbssp_data->dev;
+
+	size = sizeof(struct usbssp_erst_entry) * (erst->num_entries);
+	if (erst->entries)
+		dma_free_coherent(dev, size, erst->entries,
+				erst->erst_dma_addr);
+	erst->entries = NULL;
+}
+
 void usbssp_mem_cleanup(struct usbssp_udc *usbssp_data)
 {
-	/*TODO: implements functions*/
+	struct device *dev = usbssp_data->dev;
+	int num_ports;
+
+	cancel_delayed_work_sync(&usbssp_data->cmd_timer);
+	cancel_work_sync(&usbssp_data->bottom_irq);
+
+	/* Free the Event Ring Segment Table and the actual Event Ring */
+	usbssp_free_erst(usbssp_data, &usbssp_data->erst);
+
+	if (usbssp_data->event_ring)
+		usbssp_ring_free(usbssp_data, usbssp_data->event_ring);
+	usbssp_data->event_ring = NULL;
+	usbssp_dbg_trace(usbssp_data,
+			trace_usbssp_dbg_init, "Freed event ring");
+
+	if (usbssp_data->cmd_ring)
+		usbssp_ring_free(usbssp_data, usbssp_data->cmd_ring);
+	usbssp_data->cmd_ring = NULL;
+	usbssp_dbg_trace(usbssp_data,
+			trace_usbssp_dbg_init, "Freed command ring");
+	usbssp_cleanup_command_queue(usbssp_data);
+
+	num_ports = HCS_MAX_PORTS(usbssp_data->hcs_params1);
+
+	usbssp_free_priv_device(usbssp_data);
+
+	dma_pool_destroy(usbssp_data->segment_pool);
+	usbssp_data->segment_pool = NULL;
+	usbssp_dbg_trace(usbssp_data,
+			trace_usbssp_dbg_init, "Freed segment pool");
+	dma_pool_destroy(usbssp_data->device_pool);
+	usbssp_data->device_pool = NULL;
+	usbssp_dbg_trace(usbssp_data,
+			trace_usbssp_dbg_init, "Freed device context pool");
+	dma_pool_destroy(usbssp_data->small_streams_pool);
+	usbssp_data->small_streams_pool = NULL;
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"Freed small stream array pool");
+
+	dma_pool_destroy(usbssp_data->medium_streams_pool);
+	usbssp_data->medium_streams_pool = NULL;
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"Freed medium stream array pool");
+
+	if (usbssp_data->dcbaa)
+		dma_free_coherent(dev, sizeof(*usbssp_data->dcbaa),
+				usbssp_data->dcbaa, usbssp_data->dcbaa->dma);
+
+	usbssp_data->dcbaa = NULL;
+
+	usbssp_data->cmd_ring_reserved_trbs = 0;
+	usbssp_data->num_usb2_ports = 0;
+	usbssp_data->num_usb3_ports = 0;
+	usbssp_data->num_active_eps = 0;
+	kfree(usbssp_data->port_array);
+	kfree(usbssp_data->ext_caps);
+	usbssp_data->usb2_ports = NULL;
+	usbssp_data->usb3_ports = NULL;
+	usbssp_data->port_array = NULL;
+	usbssp_data->ext_caps = NULL;
+
+	usbssp_data->page_size = 0;
+	usbssp_data->page_shift = 0;
 }
 
 
diff --git a/drivers/usb/usbssp/gadget-ring.c b/drivers/usb/usbssp/gadget-ring.c
index 69cf478c222b..7c4b6b7b7b0a 100644
--- a/drivers/usb/usbssp/gadget-ring.c
+++ b/drivers/usb/usbssp/gadget-ring.c
@@ -52,3 +52,24 @@ void usbssp_handle_command_timeout(struct work_struct *work)
 {
 	/*TODO: implements function*/
 }
+
+static void usbssp_complete_del_and_free_cmd(struct usbssp_command *cmd,
+					     u32 status)
+{
+	list_del(&cmd->cmd_list);
+
+	if (cmd->completion) {
+		cmd->status = status;
+		complete(cmd->completion);
+	} else {
+		kfree(cmd);
+	}
+}
+
+void usbssp_cleanup_command_queue(struct usbssp_udc *usbssp_data)
+{
+	struct usbssp_command *cur_cmd, *tmp_cmd;
+
+	list_for_each_entry_safe(cur_cmd, tmp_cmd, &usbssp_data->cmd_list, cmd_list)
+		usbssp_complete_del_and_free_cmd(cur_cmd, COMP_COMMAND_ABORTED);
+}
diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index 534fb75740f7..915983bc400f 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -363,8 +363,7 @@ int usbssp_gadget_init(struct usbssp_udc *usbssp_data)
 	/*TODO add implementation of usbssp_reset function*/
 	//usbssp_reset(usbssp_data);
 	usbssp_reset(usbssp_data);
-	/*TODO add implementation of freeing memory*/
-	//usbssp_mem_cleanup(usbssp_data);
+	usbssp_mem_cleanup(usbssp_data);
 err3:
 	return ret;
 }
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 4a9a1ca2e9b5..9dba86a0274a 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1685,6 +1685,15 @@ 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_command(struct usbssp_udc *usbssp_data,
+			struct usbssp_command *command);
+
+struct usbssp_container_ctx *usbssp_alloc_container_ctx(
+			struct usbssp_udc *usbssp_data,
+			int type, gfp_t flags);
+void usbssp_free_container_ctx(struct usbssp_udc *usbssp_data,
+			struct usbssp_container_ctx *ctx);
 /* USBSSP Device controller glue */
 void usbssp_bottom_irq(struct work_struct *work);
 int usbssp_init(struct usbssp_udc *usbssp_data);
@@ -1704,6 +1713,7 @@ dma_addr_t usbssp_trb_virt_to_dma(struct usbssp_segment *seg,
 				union usbssp_trb *trb);
 void usbssp_handle_command_timeout(struct work_struct *work);
 
+void usbssp_cleanup_command_queue(struct usbssp_udc *usbssp_data);
 /* USBSSP gadget interface*/
 int usbssp_gadget_init(struct usbssp_udc *usbssp_data);
 int usbssp_gadget_exit(struct usbssp_udc *usbssp_data);
-- 
2.17.1