1 files changed, 143 insertions, 33 deletions

diff --git a/drivers/net/ethernet/intel/igc/igc_tsn.c b/drivers/net/ethernet/intel/igc/igc_tsn.c
index 174103c4bea6..0fce22de2ab8 100644
--- a/drivers/net/ethernet/intel/igc/igc_tsn.c
+++ b/drivers/net/ethernet/intel/igc/igc_tsn.c
@@ -18,8 +18,38 @@ static bool is_any_launchtime(struct igc_adapter *adapter)
 	return false;
 }
 
+static bool is_cbs_enabled(struct igc_adapter *adapter)
+{
+	int i;
+
+	for (i = 0; i < adapter->num_tx_queues; i++) {
+		struct igc_ring *ring = adapter->tx_ring[i];
+
+		if (ring->cbs_enable)
+			return true;
+	}
+
+	return false;
+}
+
+static unsigned int igc_tsn_new_flags(struct igc_adapter *adapter)
+{
+	unsigned int new_flags = adapter->flags & ~IGC_FLAG_TSN_ANY_ENABLED;
+
+	if (adapter->base_time)
+		new_flags |= IGC_FLAG_TSN_QBV_ENABLED;
+
+	if (is_any_launchtime(adapter))
+		new_flags |= IGC_FLAG_TSN_QBV_ENABLED;
+
+	if (is_cbs_enabled(adapter))
+		new_flags |= IGC_FLAG_TSN_QAV_ENABLED;
+
+	return new_flags;
+}
+
 /* Returns the TSN specific registers to their default values after
- * TSN offloading is disabled.
+ * the adapter is reset.
  */
 static int igc_tsn_disable_offload(struct igc_adapter *adapter)
 {
@@ -27,11 +57,6 @@ static int igc_tsn_disable_offload(struct igc_adapter *adapter)
 	u32 tqavctrl;
 	int i;
 
-	if (!(adapter->flags & IGC_FLAG_TSN_QBV_ENABLED))
-		return 0;
-
-	adapter->cycle_time = 0;
-
 	wr32(IGC_TXPBS, I225_TXPBSIZE_DEFAULT);
 	wr32(IGC_DTXMXPKTSZ, IGC_DTXMXPKTSZ_DEFAULT);
 
@@ -41,18 +66,12 @@ static int igc_tsn_disable_offload(struct igc_adapter *adapter)
 	wr32(IGC_TQAVCTRL, tqavctrl);
 
 	for (i = 0; i < adapter->num_tx_queues; i++) {
-		struct igc_ring *ring = adapter->tx_ring[i];
-
-		ring->start_time = 0;
-		ring->end_time = 0;
-		ring->launchtime_enable = false;
-
 		wr32(IGC_TXQCTL(i), 0);
 		wr32(IGC_STQT(i), 0);
 		wr32(IGC_ENDQT(i), NSEC_PER_SEC);
 	}
 
-	wr32(IGC_QBVCYCLET_S, NSEC_PER_SEC);
+	wr32(IGC_QBVCYCLET_S, 0);
 	wr32(IGC_QBVCYCLET, NSEC_PER_SEC);
 
 	adapter->flags &= ~IGC_FLAG_TSN_QBV_ENABLED;
@@ -68,9 +87,6 @@ static int igc_tsn_enable_offload(struct igc_adapter *adapter)
 	ktime_t base_time, systim;
 	int i;
 
-	if (adapter->flags & IGC_FLAG_TSN_QBV_ENABLED)
-		return 0;
-
 	cycle = adapter->cycle_time;
 	base_time = adapter->base_time;
 
@@ -88,6 +104,8 @@ static int igc_tsn_enable_offload(struct igc_adapter *adapter)
 	for (i = 0; i < adapter->num_tx_queues; i++) {
 		struct igc_ring *ring = adapter->tx_ring[i];
 		u32 txqctl = 0;
+		u16 cbs_value;
+		u32 tqavcc;
 
 		wr32(IGC_STQT(i), ring->start_time);
 		wr32(IGC_ENDQT(i), ring->end_time);
@@ -105,6 +123,90 @@ static int igc_tsn_enable_offload(struct igc_adapter *adapter)
 		if (ring->launchtime_enable)
 			txqctl |= IGC_TXQCTL_QUEUE_MODE_LAUNCHT;
 
+		/* Skip configuring CBS for Q2 and Q3 */
+		if (i > 1)
+			goto skip_cbs;
+
+		if (ring->cbs_enable) {
+			if (i == 0)
+				txqctl |= IGC_TXQCTL_QAV_SEL_CBS0;
+			else
+				txqctl |= IGC_TXQCTL_QAV_SEL_CBS1;
+
+			/* According to i225 datasheet section 7.5.2.7, we
+			 * should set the 'idleSlope' field from TQAVCC
+			 * register following the equation:
+			 *
+			 * value = link-speed   0x7736 * BW * 0.2
+			 *         ---------- *  -----------------         (E1)
+			 *          100Mbps            2.5
+			 *
+			 * Note that 'link-speed' is in Mbps.
+			 *
+			 * 'BW' is the percentage bandwidth out of full
+			 * link speed which can be found with the
+			 * following equation. Note that idleSlope here
+			 * is the parameter from this function
+			 * which is in kbps.
+			 *
+			 *     BW =     idleSlope
+			 *          -----------------                      (E2)
+			 *          link-speed * 1000
+			 *
+			 * That said, we can come up with a generic
+			 * equation to calculate the value we should set
+			 * it TQAVCC register by replacing 'BW' in E1 by E2.
+			 * The resulting equation is:
+			 *
+			 * value = link-speed * 0x7736 * idleSlope * 0.2
+			 *         -------------------------------------   (E3)
+			 *             100 * 2.5 * link-speed * 1000
+			 *
+			 * 'link-speed' is present in both sides of the
+			 * fraction so it is canceled out. The final
+			 * equation is the following:
+			 *
+			 *     value = idleSlope * 61036
+			 *             -----------------                   (E4)
+			 *                  2500000
+			 *
+			 * NOTE: For i225, given the above, we can see
+			 *       that idleslope is represented in
+			 *       40.959433 kbps units by the value at
+			 *       the TQAVCC register (2.5Gbps / 61036),
+			 *       which reduces the granularity for
+			 *       idleslope increments.
+			 *
+			 * In i225 controller, the sendSlope and loCredit
+			 * parameters from CBS are not configurable
+			 * by software so we don't do any
+			 * 'controller configuration' in respect to
+			 * these parameters.
+			 */
+			cbs_value = DIV_ROUND_UP_ULL(ring->idleslope
+						     * 61036ULL, 2500000);
+
+			tqavcc = rd32(IGC_TQAVCC(i));
+			tqavcc &= ~IGC_TQAVCC_IDLESLOPE_MASK;
+			tqavcc |= cbs_value | IGC_TQAVCC_KEEP_CREDITS;
+			wr32(IGC_TQAVCC(i), tqavcc);
+
+			wr32(IGC_TQAVHC(i),
+			     0x80000000 + ring->hicredit * 0x7735);
+		} else {
+			/* Disable any CBS for the queue */
+			txqctl &= ~(IGC_TXQCTL_QAV_SEL_MASK);
+
+			/* Set idleSlope to zero. */
+			tqavcc = rd32(IGC_TQAVCC(i));
+			tqavcc &= ~(IGC_TQAVCC_IDLESLOPE_MASK |
+				    IGC_TQAVCC_KEEP_CREDITS);
+			wr32(IGC_TQAVCC(i), tqavcc);
+
+			/* Set hiCredit to zero. */
+			wr32(IGC_TQAVHC(i), 0);
+		}
+skip_cbs:
 		wr32(IGC_TXQCTL(i), txqctl);
 	}
 
@@ -125,33 +227,41 @@ static int igc_tsn_enable_offload(struct igc_adapter *adapter)
 	wr32(IGC_BASET_H, baset_h);
 	wr32(IGC_BASET_L, baset_l);
 
-	adapter->flags |= IGC_FLAG_TSN_QBV_ENABLED;
-
 	return 0;
 }
 
-int igc_tsn_offload_apply(struct igc_adapter *adapter)
+int igc_tsn_reset(struct igc_adapter *adapter)
 {
-	bool is_any_enabled = adapter->base_time || is_any_launchtime(adapter);
+	unsigned int new_flags;
+	int err = 0;
 
-	if (!(adapter->flags & IGC_FLAG_TSN_QBV_ENABLED) && !is_any_enabled)
-		return 0;
+	new_flags = igc_tsn_new_flags(adapter);
 
-	if (!is_any_enabled) {
-		int err = igc_tsn_disable_offload(adapter);
+	if (!(new_flags & IGC_FLAG_TSN_ANY_ENABLED))
+		return igc_tsn_disable_offload(adapter);
 
-		if (err < 0)
-			return err;
+	err = igc_tsn_enable_offload(adapter);
+	if (err < 0)
+		return err;
 
-		/* The BASET registers aren't cleared when writing
-		 * into them, force a reset if the interface is
-		 * running.
-		 */
-		if (netif_running(adapter->netdev))
-			schedule_work(&adapter->reset_task);
+	adapter->flags = new_flags;
 
+	return err;
+}
+
+int igc_tsn_offload_apply(struct igc_adapter *adapter)
+{
+	int err;
+
+	if (netif_running(adapter->netdev)) {
+		schedule_work(&adapter->reset_task);
 		return 0;
 	}
 
-	return igc_tsn_enable_offload(adapter);
+	err = igc_tsn_enable_offload(adapter);
+	if (err < 0)
+		return err;
+
+	adapter->flags = igc_tsn_new_flags(adapter);
+	return 0;
 }