net/sched: taprio: refactor one skb dequeue from TXQ to separate function

Future changes will refactor the TXQ selection procedure, and a lot of
stuff will become messy, the indentation of the bulk of the dequeue
procedure would increase, etc.

Break out the bulk of the function into a new one, which knows the TXQ
(child qdisc) we should perform a dequeue from.
Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
Reviewed-by: Kurt Kanzenbach <kurt@linutronix.de>
Signed-off-by: David S. Miller <davem@davemloft.net>

net/sched/sch_taprio.c

@@ -512,88 +512,93 @@ static void taprio_set_budget(struct taprio_sched *q, struct sched_entry *entry)
 			     atomic64_read(&q->picos_per_byte)));
 }
 
-/* Will not be called in the full offload case, since the TX queues are
- * attached to the Qdisc created using qdisc_create_dflt()
- */
-static struct sk_buff *taprio_dequeue(struct Qdisc *sch)
+static struct sk_buff *taprio_dequeue_from_txq(struct Qdisc *sch, int txq,
+					       struct sched_entry *entry,
+					       u32 gate_mask)
 {
 	struct taprio_sched *q = qdisc_priv(sch);
 	struct net_device *dev = qdisc_dev(sch);
-	struct sk_buff *skb = NULL;
-	struct sched_entry *entry;
-	u32 gate_mask;
-	int i;
-
-	rcu_read_lock();
-	entry = rcu_dereference(q->current_entry);
-	/* if there's no entry, it means that the schedule didn't
-	 * start yet, so force all gates to be open, this is in
-	 * accordance to IEEE 802.1Qbv-2015 Section 8.6.9.4.5
-	 * "AdminGateStates"
-	 */
-	gate_mask = entry ? entry->gate_mask : TAPRIO_ALL_GATES_OPEN;
-
-	if (!gate_mask)
-		goto done;
-
-	for (i = 0; i < dev->num_tx_queues; i++) {
-		struct Qdisc *child = q->qdiscs[i];
+	struct Qdisc *child = q->qdiscs[txq];
+	struct sk_buff *skb;
 	ktime_t guard;
 	int prio;
 	int len;
 	u8 tc;
 
 	if (unlikely(!child))
-			continue;
+		return NULL;
 
 	if (TXTIME_ASSIST_IS_ENABLED(q->flags)) {
 		skb = child->ops->dequeue(child);
 		if (!skb)
-				continue;
+			return NULL;
 		goto skb_found;
 	}
 
 	skb = child->ops->peek(child);
 	if (!skb)
-			continue;
+		return NULL;
 
 	prio = skb->priority;
 	tc = netdev_get_prio_tc_map(dev, prio);
 
-		if (!(gate_mask & BIT(tc))) {
-			skb = NULL;
-			continue;
-		}
+	if (!(gate_mask & BIT(tc)))
+		return NULL;
 
 	len = qdisc_pkt_len(skb);
-		guard = ktime_add_ns(taprio_get_time(q),
-				     length_to_duration(q, len));
+	guard = ktime_add_ns(taprio_get_time(q), length_to_duration(q, len));
 
 	/* In the case that there's no gate entry, there's no
 	 * guard band ...
 	 */
 	if (gate_mask != TAPRIO_ALL_GATES_OPEN &&
-		    ktime_after(guard, entry->close_time)) {
-			skb = NULL;
-			continue;
-		}
+	    ktime_after(guard, entry->close_time))
+		return NULL;
 
 	/* ... and no budget. */
 	if (gate_mask != TAPRIO_ALL_GATES_OPEN &&
-		    atomic_sub_return(len, &entry->budget) < 0) {
-			skb = NULL;
-			continue;
-		}
+	    atomic_sub_return(len, &entry->budget) < 0)
+		return NULL;
 
 	skb = child->ops->dequeue(child);
 	if (unlikely(!skb))
-			continue;
+		return NULL;
 
 skb_found:
 	qdisc_bstats_update(sch, skb);
 	qdisc_qstats_backlog_dec(sch, skb);
 	sch->q.qlen--;
 
+	return skb;
+}
+
+/* Will not be called in the full offload case, since the TX queues are
+ * attached to the Qdisc created using qdisc_create_dflt()
+ */
+static struct sk_buff *taprio_dequeue(struct Qdisc *sch)
+{
+	struct taprio_sched *q = qdisc_priv(sch);
+	struct net_device *dev = qdisc_dev(sch);
+	struct sk_buff *skb = NULL;
+	struct sched_entry *entry;
+	u32 gate_mask;
+	int i;
+
+	rcu_read_lock();
+	entry = rcu_dereference(q->current_entry);
+	/* if there's no entry, it means that the schedule didn't
+	 * start yet, so force all gates to be open, this is in
+	 * accordance to IEEE 802.1Qbv-2015 Section 8.6.9.4.5
+	 * "AdminGateStates"
+	 */
+	gate_mask = entry ? entry->gate_mask : TAPRIO_ALL_GATES_OPEN;
+
+	if (!gate_mask)
+		goto done;
+
+	for (i = 0; i < dev->num_tx_queues; i++) {
+		skb = taprio_dequeue_from_txq(sch, i, entry, gate_mask);
+		if (skb)
 			goto done;
 	}