diff --git a/drivers/usb/usbssp/Makefile b/drivers/usb/usbssp/Makefile
index d85f15afb51c..0606f3c63cd0 100644
--- a/drivers/usb/usbssp/Makefile
+++ b/drivers/usb/usbssp/Makefile
@@ -5,6 +5,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-dbg.o
 
 ifneq ($(CONFIG_TRACING),)
 	usbssp-y		+= gadget-trace.o
diff --git a/drivers/usb/usbssp/gadget-dbg.c b/drivers/usb/usbssp/gadget-dbg.c
new file mode 100644
index 000000000000..277617d1a996
--- /dev/null
+++ b/drivers/usb/usbssp/gadget-dbg.c
@@ -0,0 +1,30 @@
+// 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 "gadget.h"
+
+void usbssp_dbg_trace(struct usbssp_udc *usbssp_data,
+		      void (*trace)(struct va_format *),
+		      const char *fmt, ...)
+{
+	struct va_format vaf;
+	va_list args;
+
+	va_start(args, fmt);
+	vaf.fmt = fmt;
+	vaf.va = &args;
+	dev_dbg(usbssp_data->dev, "%pV\n", &vaf);
+	trace(&vaf);
+	va_end(args);
+}
+EXPORT_SYMBOL_GPL(usbssp_dbg_trace);
+
diff --git a/drivers/usb/usbssp/gadget-ext-caps.h b/drivers/usb/usbssp/gadget-ext-caps.h
new file mode 100644
index 000000000000..2bf327046376
--- /dev/null
+++ b/drivers/usb/usbssp/gadget-ext-caps.h
@@ -0,0 +1,53 @@
+// 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
+ */
+
+/* Up to 16 ms to halt an DC */
+#define USBSSP_MAX_HALT_USEC		(16*1000)
+
+/* DC not running - set to 1 when run/stop bit is cleared. */
+#define USBSSP_STS_HALT			BIT(0)
+
+/* HCCPARAMS offset from PCI base address */
+#define USBSSP_HCC_PARAMS_OFFSET	0x10
+/* HCCPARAMS contains the first extended capability pointer */
+#define USBSSP_HCC_EXT_CAPS(p)		(((p)>>16)&0xffff)
+
+/* Command and Status registers offset from the Operational Registers address */
+#define USBSSP_CMD_OFFSET		0x00
+#define USBSSP_STS_OFFSET		0x04
+
+/* Capability Register */
+/* bits 7:0 - how long is the Capabilities register */
+#define USBSSP_HC_LENGTH(p)		(((p)>>00)&0x00ff)
+
+/* Extended capability register fields */
+#define USBSSP_EXT_CAPS_ID(p)		(((p)>>0)&0xff)
+#define USBSSP_EXT_CAPS_NEXT(p)		(((p)>>8)&0xff)
+#define	v_EXT_CAPS_VAL(p)		((p)>>16)
+/* Extended capability IDs - ID 0 reserved */
+#define USBSSP_EXT_CAPS_PROTOCOL	2
+
+/* USB 2.0 hardware LMP capability*/
+#define USBSSP_HLC			BIT(19)
+#define USBSSP_BLC			BIT(20)
+
+/* command register values to disable interrupts and halt the DC */
+/* start/stop DC execution - do not write unless DC is halted*/
+#define USBSSP_CMD_RUN			BIT(0)
+/* Event Interrupt Enable - get irq when EINT bit is set in USBSTS register */
+#define USBSSP_CMD_EIE			BIT(2)
+/* Host System Error Interrupt Enable - get irq when HSEIE bit set in USBSTS */
+#define USBSSP_CMD_HSEIE		BIT(3)
+/* Enable Wrap Event - '1' means DC generates an event when MFINDEX wraps. */
+#define USBSSP_CMD_EWE			BIT(10)
+
+#define USBSSP_IRQS	(USBSSP_CMD_EIE | USBSSP_CMD_HSEIE | USBSSP_CMD_EWE)
diff --git a/drivers/usb/usbssp/gadget-if.c b/drivers/usb/usbssp/gadget-if.c
new file mode 100644
index 000000000000..d53e0fb65299
--- /dev/null
+++ b/drivers/usb/usbssp/gadget-if.c
@@ -0,0 +1,24 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * USBSSP device controller driver
+ *
+ * Copyright (C) 2018 Cadence.
+ *
+ * Author: Pawel Laszczak
+ *
+ */
+
+#include <linux/usb/gadget.h>
+#include <linux/usb/composite.h>
+#include "gadget.h"
+
+int usbssp_gadget_init_endpoint(struct usbssp_udc *usbssp_data)
+{
+	/*TODO: it has to be implemented*/
+	return 0;
+}
+
+void usbssp_gadget_free_endpoint(struct usbssp_udc *usbssp_data)
+{
+	/*TODO: it has to be implemented*/
+}
diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index 2f60d7dd1fe4..338ec2ec18b1 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -23,6 +23,107 @@
 #include "gadget-trace.h"
 #include "gadget.h"
 
+
+/*
+ * usbssp_handshake - spin reading dc until handshake completes or fails
+ * @ptr: address of dc register to be read
+ * @mask: bits to look at in result of read
+ * @done: value of those bits when handshake succeeds
+ * @usec: timeout in microseconds
+ *
+ * Returns negative errno, or zero on success
+ *
+ * Success happens when the "mask" bits have the specified value (hardware
+ * handshake done). There are two failure modes: "usec" have passed (major
+ * hardware flakeout), or the register reads as all-ones (hardware removed).
+ */
+int usbssp_handshake(void __iomem *ptr, u32 mask, u32 done, int usec)
+{
+	u32	result;
+
+	do {
+		result = readl(ptr);
+		if (result == ~(u32)0)	/* card removed */
+			return -ENODEV;
+		result &= mask;
+		if (result == done)
+			return 0;
+		udelay(1);
+		usec--;
+	} while (usec > 0);
+	return -ETIMEDOUT;
+}
+
+/*
+ * Disable interrupts and begin the DC halting process.
+ */
+void usbssp_quiesce(struct usbssp_udc *usbssp_data)
+{
+	u32 halted;
+	u32 cmd;
+	u32 mask;
+
+	mask = ~(u32)(USBSSP_IRQS);
+
+	halted = readl(&usbssp_data->op_regs->status) & STS_HALT;
+	if (!halted)
+		mask &= ~CMD_RUN;
+
+	cmd = readl(&usbssp_data->op_regs->command);
+	cmd &= mask;
+	writel(cmd, &usbssp_data->op_regs->command);
+}
+
+/*
+ * Force DC into halt state.
+ *
+ * Disable any IRQs and clear the run/stop bit.
+ * USBSSP will complete any current and actively pipelined transactions, and
+ * should halt within 16 ms of the run/stop bit being cleared.
+ * Read DC Halted bit in the status register to see when the DC is finished.
+ */
+int usbssp_halt(struct usbssp_udc *usbssp_data)
+{
+	int ret;
+
+	usbssp_dbg_trace(usbssp_data, trace_usbssp_dbg_init,
+			"// Halt the USBSSP");
+	usbssp_quiesce(usbssp_data);
+
+	ret = usbssp_handshake(&usbssp_data->op_regs->status,
+		STS_HALT, STS_HALT, USBSSP_MAX_HALT_USEC);
+
+	if (!ret) {
+		dev_warn(usbssp_data->dev, "Device halt failed, %d\n", ret);
+		return ret;
+	}
+
+	usbssp_data->usbssp_state |= USBSSP_STATE_HALTED;
+	usbssp_data->cmd_ring_state = CMD_RING_STATE_STOPPED;
+	return ret;
+}
+
+
+/*
+ * Initialize memory for gadget driver and USBSSP (one-time init).
+ *
+ * Program the PAGESIZE register, initialize the device context array, create
+ * device contexts, set up a command ring segment (or two?), create event
+ * ring (one for now).
+ */
+int usbssp_init(struct usbssp_udc *usbssp_data)
+{
+	int retval = 0;
+
+	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);
+
+	return retval;
+}
+
 #ifdef CONFIG_PM
 /*
  * Stop DC (not bus-specific)
@@ -50,9 +151,146 @@ int usbssp_resume(struct usbssp_udc *usbssp_data, bool hibernated)
 
 #endif	/* CONFIG_PM */
 
+int usbssp_gen_setup(struct usbssp_udc *usbssp_data)
+{
+	int retval;
+
+	mutex_init(&usbssp_data->mutex);
+
+	usbssp_data->cap_regs = usbssp_data->regs;
+	usbssp_data->op_regs = usbssp_data->regs +
+		HC_LENGTH(readl(&usbssp_data->cap_regs->hc_capbase));
+
+	usbssp_data->run_regs = usbssp_data->regs +
+		(readl(&usbssp_data->cap_regs->run_regs_off) & RTSOFF_MASK);
+	/* Cache read-only capability registers */
+	usbssp_data->hcs_params1 = readl(&usbssp_data->cap_regs->hcs_params1);
+	usbssp_data->hcs_params2 = readl(&usbssp_data->cap_regs->hcs_params2);
+	usbssp_data->hcs_params3 = readl(&usbssp_data->cap_regs->hcs_params3);
+	usbssp_data->hcc_params = readl(&usbssp_data->cap_regs->hc_capbase);
+	usbssp_data->hci_version = HC_VERSION(usbssp_data->hcc_params);
+	usbssp_data->hcc_params = readl(&usbssp_data->cap_regs->hcc_params);
+	usbssp_data->hcc_params2 = readl(&usbssp_data->cap_regs->hcc_params2);
+
+	/* Make sure the Device Controller is halted. */
+	retval = usbssp_halt(usbssp_data);
+	if (retval)
+		return retval;
+
+	dev_dbg(usbssp_data->dev, "Resetting Device Controller\n");
+	/* Reset the internal DC memory state and registers. */
+	/*TODO: add implementation of usbssp_reset function*/
+	//retval = usbssp_reset(usbssp_data);
+	if (retval)
+		return retval;
+	dev_dbg(usbssp_data->dev, "Reset complete\n");
+
+	/* Set dma_mask and coherent_dma_mask to 64-bits,
+	 * if USBSSP supports 64-bit addressing
+	 */
+	if (HCC_64BIT_ADDR(usbssp_data->hcc_params) &&
+	    !dma_set_mask(usbssp_data->dev, DMA_BIT_MASK(64))) {
+		dev_dbg(usbssp_data->dev, "Enabling 64-bit DMA addresses.\n");
+		dma_set_coherent_mask(usbssp_data->dev, DMA_BIT_MASK(64));
+	} else {
+		/*
+		 * This is to avoid error in cases where a 32-bit USB
+		 * controller is used on a 64-bit capable system.
+		 */
+		retval = dma_set_mask(usbssp_data->dev, DMA_BIT_MASK(32));
+		if (retval)
+			return retval;
+		dev_dbg(usbssp_data->dev, "Enabling 32-bit DMA addresses.\n");
+		dma_set_coherent_mask(usbssp_data->dev, DMA_BIT_MASK(32));
+	}
+
+	/* Initialize USBSSP controller data structures. */
+	retval = usbssp_init(usbssp_data);
+	if (retval)
+		return retval;
+
+	dev_info(usbssp_data->dev, "USBSSP params 0x%08x USBSSP version 0x%x\n",
+		usbssp_data->hcc_params, usbssp_data->hci_version);
+
+	return 0;
+}
+
+/*
+ * gadget-if.c file is part of gadget.c file and implements interface
+ * for gadget driver
+ */
+#include "gadget-if.c"
+
 int usbssp_gadget_init(struct usbssp_udc *usbssp_data)
 {
 	int ret;
+
+	/*
+	 * Check the compiler generated sizes of structures that must be laid
+	 * out in specific ways for hardware access.
+	 */
+	BUILD_BUG_ON(sizeof(struct usbssp_doorbell_array) != 2*32/8);
+	BUILD_BUG_ON(sizeof(struct usbssp_slot_ctx) != 8*32/8);
+	BUILD_BUG_ON(sizeof(struct usbssp_ep_ctx) != 8*32/8);
+	/* usbssp_device has eight fields, and also
+	 * embeds one usbssp_slot_ctx and 31 usbssp_ep_ctx
+	 */
+	BUILD_BUG_ON(sizeof(struct usbssp_stream_ctx) != 4*32/8);
+	BUILD_BUG_ON(sizeof(union usbssp_trb) != 4*32/8);
+	BUILD_BUG_ON(sizeof(struct usbssp_erst_entry) != 4*32/8);
+	BUILD_BUG_ON(sizeof(struct usbssp_cap_regs) != 8*32/8);
+	BUILD_BUG_ON(sizeof(struct usbssp_intr_reg) != 8*32/8);
+	/* usbssp_run_regs has eight fields and embeds 128 usbssp_intr_regs */
+	BUILD_BUG_ON(sizeof(struct usbssp_run_regs) != (8+8*128)*32/8);
+
+	/* fill gadget fields */
+	/*TODO: implements usbssp_gadget_ops object*/
+	//usbssp_data->gadget.ops = &usbssp_gadget_ops;
+	usbssp_data->gadget.name = "usbssp-gadget";
+	usbssp_data->gadget.max_speed = USB_SPEED_SUPER_PLUS;
+	usbssp_data->gadget.speed = USB_SPEED_UNKNOWN;
+	usbssp_data->gadget.sg_supported = true;
+	usbssp_data->gadget.lpm_capable = 1;
+
+	usbssp_data->setup_buf = kzalloc(USBSSP_EP0_SETUP_SIZE, GFP_KERNEL);
+	if (!usbssp_data->setup_buf)
+		return -ENOMEM;
+
+	/*USBSSP support not aligned buffer but this option
+	 * improve performance of this controller.
+	 */
+	usbssp_data->gadget.quirk_ep_out_aligned_size = true;
+	ret = usbssp_gen_setup(usbssp_data);
+	if (ret < 0) {
+		dev_err(usbssp_data->dev,
+				"Generic initialization failed with error code%d\n",
+				ret);
+		goto err3;
+	}
+
+	ret = usbssp_gadget_init_endpoint(usbssp_data);
+	if (ret < 0) {
+		dev_err(usbssp_data->dev, "failed to initialize endpoints\n");
+		goto err1;
+	}
+
+	ret = usb_add_gadget_udc(usbssp_data->dev, &usbssp_data->gadget);
+
+	if (ret) {
+		dev_err(usbssp_data->dev, "failed to register udc\n");
+		goto err2;
+	}
+
+	return ret;
+err2:
+	usbssp_gadget_free_endpoint(usbssp_data);
+err1:
+	usbssp_halt(usbssp_data);
+	/*TODO add implementation of usbssp_reset function*/
+	//usbssp_reset(usbssp_data);
+	/*TODO add implementation of freeing memory*/
+	//usbssp_mem_cleanup(usbssp_data);
+err3:
 	return ret;
 }
 
@@ -60,5 +298,9 @@ int usbssp_gadget_exit(struct usbssp_udc *usbssp_data)
 {
 	int ret = 0;
 
+	usb_del_gadget_udc(&usbssp_data->gadget);
+	usbssp_gadget_free_endpoint(usbssp_data);
+	/*TODO: add usbssp_stop implementation*/
+	//usbssp_stop(usbssp_data);
 	return ret;
 }
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 55e20795d900..5d8918f8da84 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -12,8 +12,10 @@
 #ifndef __LINUX_USBSSP_GADGET_H
 #define __LINUX_USBSSP_GADGET_H
 
+#include <linux/irq.h>
 #include <linux/io-64-nonatomic-lo-hi.h>
 #include <linux/usb/gadget.h>
+#include "gadget-ext-caps.h"
 
 /* Max number slots - only 1 is allowed */
 #define DEV_MAX_SLOTS		1
@@ -1676,7 +1678,18 @@ static inline void usbssp_write_64(struct usbssp_udc *usbssp_data,
 	lo_hi_writeq(val, regs);
 }
 
+/* USBSSP memory management */
+void usbssp_dbg_trace(struct usbssp_udc *usbssp_data,
+		void (*trace)(struct va_format *),
+		const char *fmt, ...);
 /* 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);
+extern int usbssp_reset(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);
 
@@ -1689,6 +1702,8 @@ dma_addr_t usbssp_trb_virt_to_dma(struct usbssp_segment *seg,
 /* USBSSP gadget interface*/
 int usbssp_gadget_init(struct usbssp_udc *usbssp_data);
 int usbssp_gadget_exit(struct usbssp_udc *usbssp_data);
+void usbssp_gadget_free_endpoint(struct usbssp_udc *usbssp_data);
+int usbssp_gadget_init_endpoint(struct usbssp_udc *usbssp_data);
 
 static inline char *usbssp_slot_state_string(u32 state)
 {
