[PATCH 07/31] usb: usbssp: Initialization - added usbssp_mem_init

[Pawel Laszczak]

Patch adds some stuff regarding initialization of
usbssp driver.

It introduces new file gadget-mem.c which contains
functions responsible for allocating and freeing object
used in driver. It will be gradually expended with
new functionality.

To limits the size of this patch not all functions are
implemented - some of them are empty end will be implemented
in next patch.

Signed-off-by: Pawel Laszczak <pawell@xxxxxxxxxxx>
---
 drivers/usb/usbssp/Makefile          |   2 +-
 drivers/usb/usbssp/gadget-ext-caps.h |  46 ++
 drivers/usb/usbssp/gadget-mem.c      | 622 +++++++++++++++++++++++++++
 drivers/usb/usbssp/gadget-ring.c     |   6 +
 drivers/usb/usbssp/gadget.c          |   3 +-
 drivers/usb/usbssp/gadget.h          |   5 +
 6 files changed, 681 insertions(+), 3 deletions(-)
 create mode 100644 drivers/usb/usbssp/gadget-mem.c

diff --git a/drivers/usb/usbssp/Makefile b/drivers/usb/usbssp/Makefile
index 0606f3c63cd0..4f5f12ab6420 100644
--- a/drivers/usb/usbssp/Makefile
+++ b/drivers/usb/usbssp/Makefile
@@ -4,7 +4,7 @@ CFLAGS_gadget-trace.o := -I$(src)
 
 obj-$(CONFIG_USB_USBSSP_GADGET) += usbssp.o
 usbssp-y 			:= usbssp-plat.o gadget-ring.o \
-				   gadget.o
+				   gadget.o gadget-mem.o \
 				    gadget-dbg.o
 
 ifneq ($(CONFIG_TRACING),)
diff --git a/drivers/usb/usbssp/gadget-ext-caps.h b/drivers/usb/usbssp/gadget-ext-caps.h
index 86c0ce331037..478cb0b6a098 100644
--- a/drivers/usb/usbssp/gadget-ext-caps.h
+++ b/drivers/usb/usbssp/gadget-ext-caps.h
@@ -54,3 +54,49 @@
 
 /* true: Controller Not Ready to accept doorbell or op reg writes after reset */
 #define USBSSP_STS_CNR			BIT(11)
+
+#include <linux/io.h>
+
+/**
+ * Find the offset of the extended capabilities with capability ID id.
+ *
+ * @base	PCI MMIO registers base address.
+ * @start	address at which to start looking, (0 or HCC_PARAMS to start at
+ *		beginning of list)
+ * @id		Extended capability ID to search for.
+ *
+ * Returns the offset of the next matching extended capability structure.
+ * Some capabilities can occur several times,
+ * e.g., the USBSSP_EXT_CAPS_PROTOCOL, and this provides a way to find them all.
+ */
+
+static inline int usbssp_find_next_ext_cap(void __iomem *base,
+					   u32 start, int id)
+{
+	u32 val;
+	u32 next;
+	u32 offset;
+
+	offset = start;
+	if (!start || start == USBSSP_HCC_PARAMS_OFFSET) {
+		val = readl(base + USBSSP_HCC_PARAMS_OFFSET);
+		if (val == ~0)
+			return 0;
+		offset = USBSSP_HCC_EXT_CAPS(val) << 2;
+		if (!offset)
+			return 0;
+	};
+
+	do {
+		val = readl(base + offset);
+		if (val == ~0)
+			return 0;
+		if (USBSSP_EXT_CAPS_ID(val) == id && offset != start)
+			return offset;
+
+		next = USBSSP_EXT_CAPS_NEXT(val);
+		offset += next << 2;
+	} while (next);
+
+	return 0;
+}
diff --git a/drivers/usb/usbssp/gadget-mem.c b/drivers/usb/usbssp/gadget-mem.c
new file mode 100644
index 000000000000..11bbf18dacba
--- /dev/null
+++ b/drivers/usb/usbssp/gadget-mem.c
@@ -0,0 +1,622 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * USBSSP device controller driver
+ *
+ * Copyright (C) 2018 Cadence.
+ *
+ * Author: Pawel Laszczak
+ *
+ * A lot of code based on Linux XHCI driver.
+ * Origin: Copyright (C) 2008 Intel Corp.
+ */
+
+#include <linux/usb.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+#include <linux/dmapool.h>
+#include <linux/dma-mapping.h>
+#include "gadget.h"
+#include "gadget-trace.h"
+
+/**
+ * Create a new ring with zero or more segments.
+ *
+ * Link each segment together into a ring.
+ * Set the end flag and the cycle toggle bit on the last segment.
+ * See section 4.9.1 and figures 15 and 16.
+ */
+static struct usbssp_ring *usbssp_ring_alloc(struct usbssp_udc *usbssp_data,
+					     unsigned int num_segs,
+					     unsigned int cycle_state,
+					     enum usbssp_ring_type type,
+					     unsigned int max_packet,
+					     gfp_t flags)
+{
+	/*TODO: implements function*/
+	return NULL;
+}
+
+struct usbssp_container_ctx *usbssp_alloc_container_ctx(
+					struct usbssp_udc *usbssp_data,
+					int type, gfp_t flags)
+{
+	struct usbssp_container_ctx *ctx;
+
+	if ((type != USBSSP_CTX_TYPE_DEVICE) && (type != USBSSP_CTX_TYPE_INPUT))
+		return NULL;
+
+	ctx = kzalloc(sizeof(*ctx), flags);
+	if (!ctx)
+		return NULL;
+
+	ctx->type = type;
+	ctx->size = HCC_64BYTE_CONTEXT(usbssp_data->hcc_params) ? 2048 : 1024;
+	if (type == USBSSP_CTX_TYPE_INPUT)
+		ctx->size += CTX_SIZE(usbssp_data->hcc_params);
+
+	ctx->bytes = dma_pool_zalloc(usbssp_data->device_pool,
+				flags, &ctx->dma);
+
+	if (!ctx->bytes) {
+		kfree(ctx);
+		return NULL;
+	}
+	return ctx;
+}
+
+struct usbssp_command *usbssp_alloc_command(struct usbssp_udc *usbssp_data,
+					    bool allocate_completion,
+					    gfp_t mem_flags)
+{
+	struct usbssp_command *command;
+
+	command = kzalloc(sizeof(*command), mem_flags);
+	if (!command)
+		return NULL;
+
+	if (allocate_completion) {
+		command->completion =
+			kzalloc(sizeof(struct completion), mem_flags);
+		if (!command->completion) {
+			kfree(command);
+			return NULL;
+		}
+		init_completion(command->completion);
+	}
+
+	command->status = 0;
+	INIT_LIST_HEAD(&command->cmd_list);
+
+	return command;
+}
+
+struct usbssp_command *usbssp_alloc_command_with_ctx(
+						struct usbssp_udc *usbssp_data,
+						bool allocate_completion,
+						gfp_t mem_flags)
+{
+	struct usbssp_command *command;
+
+	command = usbssp_alloc_command(usbssp_data,
+			allocate_completion, mem_flags);
+	if (!command)
+		return NULL;
+
+	command->in_ctx = usbssp_alloc_container_ctx(usbssp_data,
+				USBSSP_CTX_TYPE_INPUT, mem_flags);
+	if (!command->in_ctx) {
+		kfree(command->completion);
+		kfree(command);
+		return NULL;
+	}
+	return command;
+}
+
+void usbssp_request_free_priv(struct usbssp_request *priv_req)
+{
+	if (priv_req)
+		kfree(priv_req->td);
+}
+
+
+int usbssp_alloc_erst(struct usbssp_udc *usbssp_data,
+		      struct usbssp_ring *evt_ring,
+		      struct usbssp_erst *erst,
+		      gfp_t flags)
+{
+	size_t size;
+	unsigned int val;
+	struct usbssp_segment *seg;
+	struct usbssp_erst_entry *entry;
+
+	size = sizeof(struct usbssp_erst_entry) * evt_ring->num_segs;
+	erst->entries = dma_zalloc_coherent(usbssp_data->dev,
+			size, &erst->erst_dma_addr, flags);
+	if (!erst->entries)
+		return -ENOMEM;
+
+	erst->num_entries = evt_ring->num_segs;
+
+	seg = evt_ring->first_seg;
+	for (val = 0; val < evt_ring->num_segs; val++) {
+		entry = &erst->entries[val];
+		entry->seg_addr = cpu_to_le64(seg->dma);
+		entry->seg_size = cpu_to_le32(TRBS_PER_SEGMENT);
+		entry->rsvd = 0;
+		seg = seg->next;
+	}
+
+	return 0;
+}
+
+void usbssp_mem_cleanup(struct usbssp_udc *usbssp_data)
+{
+	/*TODO: implements functions*/
+}
+
+
+static void usbssp_set_event_deq(struct usbssp_udc *usbssp_data)
+{
+	u64 temp;
+	dma_addr_t deq;
+
+	deq = usbssp_trb_virt_to_dma(usbssp_data->event_ring->deq_seg,
+			usbssp_data->event_ring->dequeue);
+	if (deq == 0 && !in_interrupt())
+		dev_warn(usbssp_data->dev,
+			"WARN something wrong with SW event ring dequeue ptr.\n");
+
+	/* Update USBSSP event ring dequeue pointer */
+	temp = usbssp_read_64(usbssp_data, &usbssp_data->ir_set->erst_dequeue);
+	temp &= ERST_PTR_MASK;
+	/*
+	 * Don't clear the EHB bit (which is RW1C) because
+	 * there might be more events to service.
+	 */
+	temp &= ~ERST_EHB;
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+		"// Write event ring dequeue pointer, preserving EHB bit");
+	usbssp_write_64(usbssp_data, ((u64) deq & (u64) ~ERST_PTR_MASK) | temp,
+			&usbssp_data->ir_set->erst_dequeue);
+}
+
+static void usbssp_add_in_port(struct usbssp_udc *usbssp_data,
+			       unsigned int num_ports,
+			       __le32 __iomem *addr,
+			       int max_caps)
+{
+	u32 temp, port_offset, port_count;
+	int i;
+	u8 major_revision;
+	struct usbssp_ports *rport;
+
+	temp = readl(addr);
+	major_revision = USBSSP_EXT_PORT_MAJOR(temp);
+
+	if (major_revision == 0x03) {
+		rport = &usbssp_data->usb3_rhub;
+	} else if (major_revision <= 0x02) {
+		rport = &usbssp_data->usb2_rhub;
+	} else {
+		dev_warn(usbssp_data->dev, "Ignoring unknown port speed, "
+				"Ext Cap %p, revision = 0x%x\n",
+				addr, major_revision);
+		/* Ignoring port protocol we can't understand. */
+		return;
+	}
+
+	rport->maj_rev = USBSSP_EXT_PORT_MAJOR(temp);
+	rport->min_rev = USBSSP_EXT_PORT_MINOR(temp);
+
+	/* Port offset and count in the third dword.*/
+	temp = readl(addr + 2);
+	port_offset = USBSSP_EXT_PORT_OFF(temp);
+	port_count = USBSSP_EXT_PORT_COUNT(temp);
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+		"Ext Cap %p, port offset = %u, count = %u, revision = 0x%x",
+		addr, port_offset, port_count, major_revision);
+
+	if (port_count > 1) {
+		dev_warn(usbssp_data->dev,
+			"DC support only single port but it detect %d ports",
+			port_count);
+		port_count = 1;
+	}
+
+	/* Port count includes the current port offset */
+	if (port_offset == 0 || (port_offset + port_count - 1) > num_ports)
+		return;
+
+	rport->psi_count = USBSSP_EXT_PORT_PSIC(temp);
+	if (rport->psi_count) {
+		rport->psi = kcalloc(rport->psi_count, sizeof(*rport->psi),
+				GFP_KERNEL);
+		if (!rport->psi)
+			rport->psi_count = 0;
+
+		rport->psi_uid_count++;
+		for (i = 0; i < rport->psi_count; i++) {
+			rport->psi[i] = readl(addr + 4 + i);
+
+			/*
+			 *  count unique ID values, two consecutive entries can
+			 * have the same ID if link is assymetric
+			 */
+			if (i && (USBSSP_EXT_PORT_PSIV(rport->psi[i]) !=
+				  USBSSP_EXT_PORT_PSIV(rport->psi[i - 1])))
+				rport->psi_uid_count++;
+
+			dev_dbg(usbssp_data->dev,
+				"PSIV:%d PSIE:%d PLT:%d PFD:%d LP:%d PSIM:%d\n",
+				USBSSP_EXT_PORT_PSIV(rport->psi[i]),
+				USBSSP_EXT_PORT_PSIE(rport->psi[i]),
+				USBSSP_EXT_PORT_PLT(rport->psi[i]),
+				USBSSP_EXT_PORT_PFD(rport->psi[i]),
+				USBSSP_EXT_PORT_LP(rport->psi[i]),
+				USBSSP_EXT_PORT_PSIM(rport->psi[i]));
+		}
+	}
+
+	/* cache usb2 port capabilities */
+	if (major_revision < 0x03 && usbssp_data->num_ext_caps < max_caps)
+		usbssp_data->ext_caps[usbssp_data->num_ext_caps++] = temp;
+
+	if (major_revision != 0x03) {
+		usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+				"USBSSP: support USB2 software lpm");
+		usbssp_data->sw_lpm_support = 1;
+		if (temp & USBSSP_HLC) {
+			usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+					"USBSSP: support USB2 hardware lpm");
+			usbssp_data->hw_lpm_support = 1;
+		}
+	}
+
+	usbssp_data->port_array[port_offset-1] = major_revision;
+	if (major_revision == 0x03)
+		usbssp_data->num_usb3_ports++;
+	else
+		usbssp_data->num_usb2_ports++;
+}
+
+/*
+ * Scan the Extended Capabilities for the "Supported Protocol Capabilities" that
+ * specify what speeds each port is supposed to be.
+ */
+static int usbssp_setup_port_arrays(struct usbssp_udc *usbssp_data, gfp_t flags)
+{
+	void __iomem *base;
+	u32 offset;
+	u32 port3offset = 0;
+	u32 port2offset = 0;
+	unsigned int num_ports;
+	int i;
+	int cap_count = 0;
+	u32 cap_start;
+
+	num_ports = HCS_MAX_PORTS(usbssp_data->hcs_params1);
+
+	/*
+	 * USBSSP can support only two ports - one for USB2.0 and
+	 * second for USB3.0
+	 */
+	if (num_ports > MAX_USBSSP_PORTS) {
+		dev_err(usbssp_data->dev,
+			"USBSSP-Dev can't support more then %d ports\n",
+			MAX_USBSSP_PORTS);
+		return -EINVAL;
+	}
+
+	usbssp_data->port_array =
+		kzalloc(sizeof(*usbssp_data->port_array)*num_ports, flags);
+	if (!usbssp_data->port_array)
+		return -ENOMEM;
+
+	base = &usbssp_data->cap_regs->hc_capbase;
+
+	cap_start = usbssp_find_next_ext_cap(base, 0, USBSSP_EXT_CAPS_PROTOCOL);
+	if (!cap_start) {
+		dev_err(usbssp_data->dev,
+			"No Ext. Cap. registers, unable to set up ports\n");
+		return -ENODEV;
+	}
+
+	offset = cap_start;
+
+	/* count extended protocol capability entries for later caching */
+	while (offset) {
+		u32 temp;
+		u8 major_revision;
+
+		temp = readl(base + offset);
+		major_revision = USBSSP_EXT_PORT_MAJOR(temp);
+
+		if (major_revision == 0x03 && port3offset == 0)
+			port3offset = offset;
+		else if (major_revision <= 0x02 && port2offset == 0)
+			port2offset = offset;
+
+		cap_count++;
+
+		offset = usbssp_find_next_ext_cap(base, offset,
+				USBSSP_EXT_CAPS_PROTOCOL);
+	}
+
+	if (cap_count > MAX_USBSSP_PORTS) {
+		dev_err(usbssp_data->dev, "Too many  Ext. Cap. registers\n");
+		return -EINVAL;
+	}
+
+	if (!port3offset &&  !port2offset) {
+		dev_warn(usbssp_data->dev, "No ports on the USBSSP?\n");
+		return -ENODEV;
+	}
+
+	usbssp_data->ext_caps =
+		kzalloc(sizeof(*usbssp_data->ext_caps) * cap_count, flags);
+	if (!usbssp_data->ext_caps)
+		return -ENOMEM;
+
+	/** if exist add USB3 port*/
+	if (port3offset)
+		usbssp_add_in_port(usbssp_data, num_ports,
+				base + port3offset, cap_count);
+
+	/** add USB2 port*/
+	if (port2offset)
+		usbssp_add_in_port(usbssp_data, num_ports,
+				base + port2offset, cap_count);
+
+	if (usbssp_data->num_usb2_ports == 0 &&
+	    usbssp_data->num_usb3_ports == 0) {
+		dev_warn(usbssp_data->dev, "No ports on the USBSSP?\n");
+		return -ENODEV;
+	}
+
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"Found %u USB 2.0 ports and %u USB 3.0 ports.",
+			usbssp_data->num_usb2_ports,
+			usbssp_data->num_usb3_ports);
+
+	/*Only one port USB3.0 and USB2.0 can be supported by USBSSP_DEV*/
+	if (usbssp_data->num_usb3_ports > 1) {
+		dev_err(usbssp_data->dev, "Limiting USB 3.0 ports to 1\n");
+		return -EINVAL;
+	}
+
+	if (usbssp_data->num_usb2_ports > 1) {
+		dev_err(usbssp_data->dev, "Limiting USB 2.0 ports to 1\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Note we could have only USB 3.0 ports, or  USB 2.0 ports.
+	 */
+	if (usbssp_data->num_usb2_ports) {
+		for (i = 0; i < num_ports; i++) {
+
+			if (usbssp_data->port_array[i] == 0x03)
+				continue;
+			usbssp_data->usb2_ports =
+				&usbssp_data->op_regs->port_status_base +
+				NUM_PORT_REGS*i;
+
+			usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+					"USB 2.0 port at index %u, addr = %p",
+					i, usbssp_data->usb2_ports);
+		}
+	}
+
+	if (usbssp_data->num_usb3_ports) {
+
+		for (i = 0; i < num_ports; i++)
+			if (usbssp_data->port_array[i] == 0x03) {
+				usbssp_data->usb3_ports =
+					&usbssp_data->op_regs->port_status_base +
+					NUM_PORT_REGS*i;
+
+				usbssp_dbg_trace(usbssp_data,
+					trace_usbssp_dbg_init,
+					"USB 3.0 port at index %u, addr = %p",
+					i, usbssp_data->usb3_ports);
+			}
+	}
+
+	return 0;
+}
+
+int usbssp_mem_init(struct usbssp_udc *usbssp_data, gfp_t flags)
+{
+	dma_addr_t	dma;
+	struct device	*dev = usbssp_data->dev;
+	unsigned int	val, val2;
+	u64		val_64;
+	u32 page_size;
+	int i, ret;
+
+	INIT_LIST_HEAD(&usbssp_data->cmd_list);
+
+	/* init command timeout work */
+	INIT_DELAYED_WORK(&usbssp_data->cmd_timer,
+			usbssp_handle_command_timeout);
+	init_completion(&usbssp_data->cmd_ring_stop_completion);
+
+	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);
+	for (i = 0; i < 16; i++) {
+		if ((0x1 & page_size) != 0)
+			break;
+		page_size = page_size >> 1;
+	}
+	if (i < 16)
+		usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"Supported page size of %iK", (1 << (i+12)) / 1024);
+	else
+		dev_warn(usbssp_data->dev, "WARN: no supported page size\n");
+
+	/*
+	 *  Use 4K pages, since that's common and the minimum the
+	 * USBSSP supports
+	 */
+	usbssp_data->page_shift = 12;
+	usbssp_data->page_size = 1 << usbssp_data->page_shift;
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"USBSSP page size set to %iK",
+			usbssp_data->page_size / 1024);
+
+	/* In device mode this value should be equal 1*/
+	val = DEV_HCS_MAX_SLOTS(readl(&usbssp_data->cap_regs->hcs_params1));
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// USBSSP can handle at most %d device slots.", val);
+
+	/*device should have only 1 slot*/
+	if (val > DEV_MAX_SLOTS)
+		pr_err("Invalid number of supported slots");
+
+	val2 = readl(&usbssp_data->op_regs->config_reg);
+	val |= (val2 & ~DEV_HCS_SLOTS_MASK);
+
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// Setting Max device slots reg = 0x%x.", val);
+	writel(val, &usbssp_data->op_regs->config_reg);
+
+	/*
+	 * Doorbell array must be physically contiguous
+	 * and 64-byte (cache line) aligned.
+	 */
+	usbssp_data->dcbaa = dma_alloc_coherent(dev,
+			sizeof(*usbssp_data->dcbaa), &dma, GFP_KERNEL);
+	if (!usbssp_data->dcbaa)
+		goto fail;
+	memset(usbssp_data->dcbaa, 0, sizeof *(usbssp_data->dcbaa));
+	usbssp_data->dcbaa->dma = dma;
+
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// DCBA array address = 0x%llx (DMA), %p (virt)",
+			(unsigned long long)usbssp_data->dcbaa->dma,
+			usbssp_data->dcbaa);
+	usbssp_write_64(usbssp_data, dma, &usbssp_data->op_regs->dcbaa_ptr);
+
+	/*
+	 * Initialize the ring segment pool.  The ring must be a contiguous
+	 * structure comprised of TRBs.  The TRBs must be 16 byte aligned,
+	 * however, the command ring segment needs 64-byte aligned segments
+	 * and our use of dma addresses in the trb_address_map radix tree needs
+	 * TRB_SEGMENT_SIZE alignment, so we pick the greater alignment need.
+	 */
+	usbssp_data->segment_pool = dma_pool_create("USBSSP ring segments", dev,
+			TRB_SEGMENT_SIZE, TRB_SEGMENT_SIZE,
+			usbssp_data->page_size);
+
+	usbssp_data->device_pool =
+			dma_pool_create("USBSSP input/output contexts", dev,
+					2112, 64, usbssp_data->page_size);
+	if (!usbssp_data->segment_pool || !usbssp_data->device_pool)
+		goto fail;
+
+	/*
+	 * Linear stream context arrays don't have any boundary restrictions,
+	 * and only need to be 16-byte aligned.
+	 */
+	usbssp_data->small_streams_pool =
+			dma_pool_create("USBSSP 256 byte stream ctx arrays",
+					dev, SMALL_STREAM_ARRAY_SIZE, 16, 0);
+	usbssp_data->medium_streams_pool =
+			dma_pool_create("USBSSP 1KB stream ctx arrays",
+					dev, MEDIUM_STREAM_ARRAY_SIZE, 16, 0);
+
+	/*
+	 * Any stream context array bigger than MEDIUM_STREAM_ARRAY_SIZE
+	 * will be allocated with dma_alloc_coherent()
+	 */
+	if (!usbssp_data->small_streams_pool ||
+	    !usbssp_data->medium_streams_pool)
+		goto fail;
+
+	/* Set up the command ring to have one segments for now. */
+	usbssp_data->cmd_ring = usbssp_ring_alloc(usbssp_data, 1, 1,
+			TYPE_COMMAND, 0, flags);
+	if (!usbssp_data->cmd_ring)
+		goto fail;
+
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"Allocated command ring at %p", usbssp_data->cmd_ring);
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"First segment DMA is 0x%llx",
+			(unsigned long long)usbssp_data->cmd_ring->first_seg->dma);
+
+	/* Set the address in the Command Ring Control register */
+	val_64 = usbssp_read_64(usbssp_data, &usbssp_data->op_regs->cmd_ring);
+	val_64 = (val_64 & (u64) CMD_RING_RSVD_BITS) |
+		(usbssp_data->cmd_ring->first_seg->dma & (u64) ~CMD_RING_RSVD_BITS) |
+		usbssp_data->cmd_ring->cycle_state;
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// Setting command ring address to 0x%x", val_64);
+	usbssp_write_64(usbssp_data, val_64, &usbssp_data->op_regs->cmd_ring);
+
+	val = readl(&usbssp_data->cap_regs->db_off);
+	val &= DBOFF_MASK;
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// Doorbell array is located at offset 0x%x"
+			" from cap regs base addr", val);
+	usbssp_data->dba = (void __iomem *) usbssp_data->cap_regs + val;
+
+	/* Set ir_set to interrupt register set 0 */
+	usbssp_data->ir_set = &usbssp_data->run_regs->ir_set[0];
+
+	/*
+	 * Event ring setup: Allocate a normal ring, but also setup
+	 * the event ring segment table (ERST).
+	 */
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// Allocating event ring");
+	usbssp_data->event_ring = usbssp_ring_alloc(usbssp_data,
+			ERST_NUM_SEGS, 1, TYPE_EVENT, 0, flags);
+	if (!usbssp_data->event_ring)
+		goto fail;
+
+	ret = usbssp_alloc_erst(usbssp_data, usbssp_data->event_ring,
+			&usbssp_data->erst, flags);
+	if (ret)
+		goto fail;
+
+	/* set ERST count with the number of entries in the segment table */
+	val = readl(&usbssp_data->ir_set->erst_size);
+	val &= ERST_SIZE_MASK;
+	val |= ERST_NUM_SEGS;
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// Write ERST size = %i to ir_set 0 (some bits preserved)",
+			val);
+	writel(val, &usbssp_data->ir_set->erst_size);
+
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// Set ERST entries to point to event ring.");
+
+	/* set the segment table base address */
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// Set ERST base address for ir_set 0 = 0x%llx",
+			(unsigned long long)usbssp_data->erst.erst_dma_addr);
+	val_64 = usbssp_read_64(usbssp_data, &usbssp_data->ir_set->erst_base);
+	val_64 &= ERST_PTR_MASK;
+	val_64 |= (usbssp_data->erst.erst_dma_addr & (u64) ~ERST_PTR_MASK);
+	usbssp_write_64(usbssp_data, val_64, &usbssp_data->ir_set->erst_base);
+
+	/* Set the event ring dequeue address */
+	usbssp_set_event_deq(usbssp_data);
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"Wrote ERST address to ir_set 0.");
+
+	if (usbssp_setup_port_arrays(usbssp_data, flags))
+		goto fail;
+
+	return 0;
+
+fail:
+	dev_warn(usbssp_data->dev, "Couldn't initialize memory\n");
+	usbssp_halt(usbssp_data);
+	usbssp_reset(usbssp_data);
+	usbssp_mem_cleanup(usbssp_data);
+	return -ENOMEM;
+}
+
diff --git a/drivers/usb/usbssp/gadget-ring.c b/drivers/usb/usbssp/gadget-ring.c
index d1da59306d02..69cf478c222b 100644
--- a/drivers/usb/usbssp/gadget-ring.c
+++ b/drivers/usb/usbssp/gadget-ring.c
@@ -46,3 +46,9 @@ irqreturn_t usbssp_irq(int irq, void *priv)
 	spin_unlock_irqrestore(&usbssp_data->lock, flags);
 	return ret;
 }
+
+
+void usbssp_handle_command_timeout(struct work_struct *work)
+{
+	/*TODO: implements function*/
+}
diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index 195f5777cf8a..534fb75740f7 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -195,8 +195,7 @@ int usbssp_init(struct usbssp_udc *usbssp_data)
 	spin_lock_init(&usbssp_data->lock);
 	spin_lock_init(&usbssp_data->irq_thread_lock);
 
-	/*TODO: memory initialization*/
-	//retval = usbssp_mem_init(usbssp_data, GFP_KERNEL);
+	retval = usbssp_mem_init(usbssp_data, GFP_KERNEL);
 
 	return retval;
 }
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 5d8918f8da84..4a9a1ca2e9b5 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1682,12 +1682,16 @@ static inline void usbssp_write_64(struct usbssp_udc *usbssp_data,
 void usbssp_dbg_trace(struct usbssp_udc *usbssp_data,
 		void (*trace)(struct va_format *),
 		const char *fmt, ...);
+/* USBSSP memory management */
+void usbssp_mem_cleanup(struct usbssp_udc *usbssp_data);
+int usbssp_mem_init(struct usbssp_udc *usbssp_data, gfp_t flags);
 /* USBSSP Device controller glue */
 void usbssp_bottom_irq(struct work_struct *work);
 int usbssp_init(struct usbssp_udc *usbssp_data);
 void usbssp_stop(struct usbssp_udc *usbssp_data);
 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_suspend(struct usbssp_udc *usbssp_data, bool do_wakeup);
@@ -1698,6 +1702,7 @@ irqreturn_t usbssp_irq(int irq, void *priv);
 /* USBSSP ring, segment, TRB, and TD functions */
 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);
 
 /* USBSSP gadget interface*/
 int usbssp_gadget_init(struct usbssp_udc *usbssp_data);
-- 
2.17.1