diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index a055525..cde64a3 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -229,30 +229,22 @@ void dwc3_gadget_giveback(struct dwc3_ep *dep, struct dwc3_request *req,
  */
 int dwc3_send_gadget_generic_command(struct dwc3 *dwc, unsigned cmd, u32 param)
 {
-	u32		timeout = 500;
-	int		status = 0;
 	int		ret = 0;
 	u32		reg;
 
 	dwc3_writel(dwc->regs, DWC3_DGCMDPAR, param);
 	dwc3_writel(dwc->regs, DWC3_DGCMD, cmd | DWC3_DGCMD_CMDACT);
 
-	do {
-		reg = dwc3_readl(dwc->regs, DWC3_DGCMD);
-		if (!(reg & DWC3_DGCMD_CMDACT)) {
-			status = DWC3_DGCMD_STATUS(reg);
-			if (status)
-				ret = -EINVAL;
-			break;
-		}
-	} while (--timeout);
+	ret = dwc3_read_poll_timeout_atomic(dwc->regs, DWC3_DGCMD,
+					    &reg, DWC3_DGCMD_CMDACT,
+					    0, 100);
+	if (!ret)
+		ret = DWC3_DGCMD_STATUS(reg);
 
-	if (!timeout) {
-		ret = -ETIMEDOUT;
-		status = -ETIMEDOUT;
-	}
+	trace_dwc3_gadget_generic_cmd(cmd, param, ret);
 
-	trace_dwc3_gadget_generic_cmd(cmd, param, status);
+	if (ret > 0)
+		ret = -EINVAL;
 
 	return ret;
 }
@@ -273,7 +265,6 @@ int dwc3_send_gadget_ep_cmd(struct dwc3_ep *dep, unsigned cmd,
 {
 	const struct usb_endpoint_descriptor *desc = dep->endpoint.desc;
 	struct dwc3		*dwc = dep->dwc;
-	u32			timeout = 1000;
 	u32			saved_config = 0;
 	u32			reg;
 
@@ -346,44 +337,36 @@ int dwc3_send_gadget_ep_cmd(struct dwc3_ep *dep, unsigned cmd,
 		cmd |= DWC3_DEPCMD_CMDACT;
 
 	dwc3_writel(dep->regs, DWC3_DEPCMD, cmd);
-	do {
-		reg = dwc3_readl(dep->regs, DWC3_DEPCMD);
-		if (!(reg & DWC3_DEPCMD_CMDACT)) {
-			cmd_status = DWC3_DEPCMD_STATUS(reg);
-
-			switch (cmd_status) {
-			case 0:
-				ret = 0;
-				break;
-			case DEPEVT_TRANSFER_NO_RESOURCE:
-				dev_WARN(dwc->dev, "No resource for %s\n",
-					 dep->name);
-				ret = -EINVAL;
-				break;
-			case DEPEVT_TRANSFER_BUS_EXPIRY:
-				/*
-				 * SW issues START TRANSFER command to
-				 * isochronous ep with future frame interval. If
-				 * future interval time has already passed when
-				 * core receives the command, it will respond
-				 * with an error status of 'Bus Expiry'.
-				 *
-				 * Instead of always returning -EINVAL, let's
-				 * give a hint to the gadget driver that this is
-				 * the case by returning -EAGAIN.
-				 */
-				ret = -EAGAIN;
-				break;
-			default:
-				dev_WARN(dwc->dev, "UNKNOWN cmd status\n");
-			}
-
+	ret = dwc3_read_poll_timeout_atomic(dep->regs, DWC3_DEPCMD, &reg,
+					    DWC3_DEPCMD_CMDACT, 0, 500);
+	if (!ret) {
+		cmd_status = DWC3_DEPCMD_STATUS(reg);
+
+		switch (cmd_status) {
+		case 0:
+			ret = 0;
 			break;
+		case DEPEVT_TRANSFER_NO_RESOURCE:
+			ret = -EINVAL;
+			break;
+		case DEPEVT_TRANSFER_BUS_EXPIRY:
+			/*
+			 * SW issues START TRANSFER command to
+			 * isochronous ep with future frame interval. If
+			 * future interval time has already passed when
+			 * core receives the command, it will respond
+			 * with an error status of 'Bus Expiry'.
+			 *
+			 * Instead of always returning -EINVAL, let's
+			 * give a hint to the gadget driver that this is
+			 * the case by returning -EAGAIN.
+			 */
+			ret = -EAGAIN;
+			break;
+		default:
+			dev_WARN(dwc->dev, "UNKNOWN cmd status\n");
 		}
-	} while (--timeout);
-
-	if (timeout == 0) {
-		ret = -ETIMEDOUT;
+	} else {
 		cmd_status = -ETIMEDOUT;
 	}
 
@@ -1877,7 +1860,6 @@ static int dwc3_gadget_set_selfpowered(struct usb_gadget *g,
 static int dwc3_gadget_run_stop(struct dwc3 *dwc, int is_on, int suspend)
 {
 	u32			reg;
-	u32			timeout = 500;
 
 	if (pm_runtime_suspended(dwc->dev))
 		return 0;
@@ -1908,15 +1890,10 @@ static int dwc3_gadget_run_stop(struct dwc3 *dwc, int is_on, int suspend)
 
 	dwc3_gadget_dctl_write_safe(dwc, reg);
 
-	do {
-		reg = dwc3_readl(dwc->regs, DWC3_DSTS);
-		reg &= DWC3_DSTS_DEVCTRLHLT;
-	} while (--timeout && !(!is_on ^ !reg));
-
-	if (!timeout)
-		return -ETIMEDOUT;
-
-	return 0;
+	return dwc3_read_poll_timeout_atomic(dwc->regs, DWC3_DSTS,
+					     &reg, DWC3_DSTS_DEVCTRLHLT,
+					     is_on ? 0 : DWC3_DSTS_DEVCTRLHLT,
+					     100);
 }
 
 static int dwc3_gadget_pullup(struct usb_gadget *g, int is_on)
diff --git a/drivers/usb/dwc3/io.h b/drivers/usb/dwc3/io.h
index 9bbe5d4..7b7d21f 100644
--- a/drivers/usb/dwc3/io.h
+++ b/drivers/usb/dwc3/io.h
@@ -12,6 +12,7 @@
 #define __DRIVERS_USB_DWC3_IO_H
 
 #include <linux/io.h>
+#include <linux/iopoll.h>
 #include "trace.h"
 #include "debug.h"
 #include "core.h"
@@ -54,4 +55,25 @@ static inline void dwc3_writel(void __iomem *base, u32 offset, u32 value)
 	trace_dwc3_writel(base - DWC3_GLOBALS_REGS_START, offset, value);
 }
 
+static inline int dwc3_read_poll_timeout_atomic(void __iomem *base, u32 offset,
+						u32 *reg_addr, u32 mask,
+						u32 val, u32 timeout)
+{
+	u32 reg;
+	int ret;
+
+	/* Tracing the initial value */
+	reg = readl(base + offset - DWC3_GLOBALS_REGS_START);
+	trace_dwc3_readl(base - DWC3_GLOBALS_REGS_START, offset, reg);
+
+	ret = readl_poll_timeout_atomic(base + offset - DWC3_GLOBALS_REGS_START,
+					reg, (reg & mask) == val, 0, timeout);
+
+	*reg_addr = reg;
+	/* Tracing the finial value */
+	trace_dwc3_readl(base - DWC3_GLOBALS_REGS_START, offset, reg);
+
+	return ret;
+}
+
 #endif /* __DRIVERS_USB_DWC3_IO_H */
