diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index f091e107d9cb..f75ce10407dd 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -1094,6 +1094,47 @@ static void dwc3_prepare_one_trb(struct dwc3_ep *dep,
 			stream_id, short_not_ok, no_interrupt, is_last);
 }
 
+/**
+ * dwc3_prepare_last_sg - prepare TRBs for the last SG entry
+ * @dep: The endpoint that the request belongs to
+ * @req: The request to prepare
+ * @entry_length: The last SG entry size
+ * @node: Indicates whether this is not the first entry (for isoc only)
+ *
+ * Return the number of TRBs prepared.
+ */
+static int dwc3_prepare_last_sg(struct dwc3_ep *dep,
+		struct dwc3_request *req, unsigned int entry_length,
+		unsigned int node)
+{
+	unsigned int maxp = usb_endpoint_maxp(dep->endpoint.desc);
+	unsigned int rem = req->request.length % maxp;
+	unsigned int num_trbs = 1;
+
+	if ((req->request.length && req->request.zero && !rem &&
+			!usb_endpoint_xfer_isoc(dep->endpoint.desc)) ||
+			(!req->direction && rem))
+		num_trbs++;
+
+	if (dwc3_calc_trbs_left(dep) < num_trbs)
+		return 0;
+
+	req->needs_extra_trb = num_trbs > 1;
+
+	/* Prepare a normal TRB */
+	if (req->direction || req->request.length)
+		dwc3_prepare_one_trb(dep, req, entry_length,
+				req->needs_extra_trb, node, false);
+
+	/* Prepare extra TRBs for ZLP and MPS OUT transfer alignment */
+	if ((!req->direction && !req->request.length) || req->needs_extra_trb)
+		dwc3_prepare_one_trb(dep, req,
+				req->direction ? 0 : maxp - rem,
+				false, 1, true);
+
+	return num_trbs;
+}
+
 static int dwc3_prepare_one_trb_sg(struct dwc3_ep *dep,
 		struct dwc3_request *req)
 {
@@ -1101,8 +1142,6 @@ static int dwc3_prepare_one_trb_sg(struct dwc3_ep *dep,
 	struct scatterlist *s;
 	int		i;
 	unsigned int length = req->request.length;
-	unsigned int maxp = usb_endpoint_maxp(dep->endpoint.desc);
-	unsigned int rem = length % maxp;
 	unsigned int remaining = req->request.num_mapped_sgs
 		- req->num_queued_sgs;
 	unsigned int num_trbs = req->num_trbs;
@@ -1116,7 +1155,7 @@ static int dwc3_prepare_one_trb_sg(struct dwc3_ep *dep,
 
 	for_each_sg(sg, s, remaining, i) {
 		unsigned int trb_length;
-		unsigned int chain = true;
+		bool last_sg = false;
 
 		trb_length = min_t(unsigned int, length, sg_dma_len(s));
 
@@ -1130,45 +1169,16 @@ static int dwc3_prepare_one_trb_sg(struct dwc3_ep *dep,
 		 * mapped sg.
 		 */
 		if ((i == remaining - 1) || !length)
-			chain = false;
+			last_sg = true;
 
 		if (!dwc3_calc_trbs_left(dep))
 			break;
 
-		if (rem && usb_endpoint_dir_out(dep->endpoint.desc) && !chain) {
-			/* prepare normal TRB */
-			if (req->request.length) {
-				if (dwc3_calc_trbs_left(dep) < 2)
-					goto out;
-
-				req->needs_extra_trb = true;
-				dwc3_prepare_one_trb(dep, req, trb_length,
-					true, i, false);
-			}
-
-			/* Now prepare one extra TRB to align transfer size */
-			dwc3_prepare_one_trb(dep, req, maxp - rem,
-					false, 1, true);
-		} else if (req->request.zero && req->request.length &&
-			   !usb_endpoint_xfer_isoc(dep->endpoint.desc) &&
-			   !rem && !chain) {
-
-			if (dwc3_calc_trbs_left(dep) < 2)
+		if (last_sg) {
+			if (!dwc3_prepare_last_sg(dep, req, trb_length, i))
 				goto out;
-
-			req->needs_extra_trb = true;
-
-			/* Prepare normal TRB */
-			dwc3_prepare_one_trb(dep, req, trb_length,
-					true, i, false);
-
-			/* Prepare one extra TRB to handle ZLP */
-			dwc3_prepare_one_trb(dep, req,
-					req->direction ? 0 : maxp,
-					false, 1, true);
 		} else {
-			dwc3_prepare_one_trb(dep, req, trb_length,
-					chain, i, false);
+			dwc3_prepare_one_trb(dep, req, trb_length, 1, i, false);
 		}
 
 		/*
@@ -1178,7 +1188,7 @@ static int dwc3_prepare_one_trb_sg(struct dwc3_ep *dep,
 		 * we have free trbs we can continue queuing from where we
 		 * previously stopped
 		 */
-		if (chain)
+		if (!last_sg)
 			req->start_sg = sg_next(s);
 
 		req->num_queued_sgs++;
@@ -1224,50 +1234,7 @@ out:
 static int dwc3_prepare_one_trb_linear(struct dwc3_ep *dep,
 		struct dwc3_request *req)
 {
-	unsigned int length = req->request.length;
-	unsigned int maxp = usb_endpoint_maxp(dep->endpoint.desc);
-	unsigned int rem = length % maxp;
-	unsigned int num_trbs = req->num_trbs;
-
-	if (!dwc3_calc_trbs_left(dep))
-		goto out;
-
-	if ((!length || rem) && usb_endpoint_dir_out(dep->endpoint.desc)) {
-		/* prepare normal TRB */
-		if (req->request.length) {
-			if (dwc3_calc_trbs_left(dep) < 2)
-				goto out;
-
-			req->needs_extra_trb = true;
-			dwc3_prepare_one_trb(dep, req, length, true, 0, false);
-		}
-
-		/* Now prepare one extra TRB to align transfer size */
-		dwc3_prepare_one_trb(dep, req, maxp - rem, false, 1, true);
-	} else if (req->request.zero && req->request.length &&
-		   !usb_endpoint_xfer_isoc(dep->endpoint.desc) &&
-		   (IS_ALIGNED(req->request.length, maxp))) {
-
-		if (dwc3_calc_trbs_left(dep) < 2)
-			goto out;
-
-		req->needs_extra_trb = true;
-
-		/* prepare normal TRB */
-		dwc3_prepare_one_trb(dep, req, length, true, 0, false);
-
-		/* Prepare one extra TRB to handle ZLP */
-		dwc3_prepare_one_trb(dep, req, req->direction ? 0 : maxp,
-				false, 1, true);
-	} else {
-		if (!dwc3_calc_trbs_left(dep))
-			goto out;
-
-		dwc3_prepare_one_trb(dep, req, length, false, 0, false);
-	}
-
-out:
-	return req->num_trbs - num_trbs;
+	return dwc3_prepare_last_sg(dep, req, req->request.length, 0);
 }
 
 /*