dpaa: add PFC support

Multiply the number of work queues by the number of 802.1p PFC
priorities.
With this patch, egress traffic can be classiffied on different work
queues depending on the skb priority.
Each work queue can be paused by a PFC priority.
When the private interfaces' buffer pool depletes, PFC frames are
issued with all configured priorities set in the Class-Enable Vector.

Signed-off-by: Cristian Bercaru <cristian.bercaru@freescale.com>
Change-Id: Ibaf928e547c662dacf08063bd94953b9cf6138e0
Reviewed-on: http://git.am.freescale.net:8181/10498
Reviewed-by: Madalin-Cristian Bucur <madalin.bucur@freescale.com>
Tested-by: Madalin-Cristian Bucur <madalin.bucur@freescale.com>

7 files changed, 84 insertions, 28 deletions

diff --git a/drivers/net/ethernet/freescale/dpa/dpaa_eth.c b/drivers/net/ethernet/freescale/dpa/dpaa_eth.c
index 9a9f0c8..862357a 100644
--- a/drivers/net/ethernet/freescale/dpa/dpaa_eth.c
+++ b/drivers/net/ethernet/freescale/dpa/dpaa_eth.c
@@ -989,6 +989,17 @@ dpaa_eth_priv_probe(struct platform_device *_of_dev)
 	dpaa_eth_init_ports(mac_dev, dpa_bp, count, &port_fqs,
 			buf_layout, dev);
 
+#ifdef CONFIG_FMAN_PFC
+	for (i = 0; i < CONFIG_FMAN_PFC_COS_COUNT; i++) {
+		err = fm_port_set_pfc_priorities_mapping_to_qman_wq(
+				mac_dev->port_dev[TX], i, i);
+		if (unlikely(err != 0)) {
+			dev_err(dev, "Error maping PFC %u to WQ %u\n", i, i);
+			goto pfc_mapping_failed;
+		}
+	}
+#endif
+
 	priv->percpu_priv = alloc_percpu(*priv->percpu_priv);
 
 	if (priv->percpu_priv == NULL) {
@@ -1026,6 +1037,9 @@ netdev_init_failed:
 napi_add_failed:
 	dpa_private_napi_del(net_dev);
 	free_percpu(priv->percpu_priv);
+#ifdef CONFIG_FMAN_PFC
+pfc_mapping_failed:
+#endif
 alloc_percpu_failed:
 	dpa_fq_free(dev, &priv->dpa_fq_list);
 fq_alloc_failed:
@@ -1080,6 +1094,7 @@ static int __init __cold dpa_load(void)
 	/* initialise dpaa_eth mirror values */
 	dpa_rx_extra_headroom = fm_get_rx_extra_headroom();
 	dpa_max_frm = fm_get_max_frm();
+	dpa_num_cpus = num_possible_cpus();
 
 	_errno = platform_driver_register(&dpa_driver);
 	if (unlikely(_errno < 0)) {
diff --git a/drivers/net/ethernet/freescale/dpa/dpaa_eth.h b/drivers/net/ethernet/freescale/dpa/dpaa_eth.h
index a781954..8511826 100644
--- a/drivers/net/ethernet/freescale/dpa/dpaa_eth.h
+++ b/drivers/net/ethernet/freescale/dpa/dpaa_eth.h
@@ -43,6 +43,7 @@
 
 extern int dpa_rx_extra_headroom;
 extern int dpa_max_frm;
+extern int dpa_num_cpus;
 
 #define dpa_get_rx_extra_headroom() dpa_rx_extra_headroom
 #define dpa_get_max_frm() dpa_max_frm
@@ -185,7 +186,12 @@ struct dpa_buffer_layout_s {
 	((parse_result_ptr)->l4r & FM_L4_PARSE_RESULT_TCP)
 
 /* number of Tx queues to FMan */
+#ifdef CONFIG_FMAN_PFC
+#define DPAA_ETH_TX_QUEUES	(NR_CPUS * CONFIG_FMAN_PFC_COS_COUNT)
+#else
 #define DPAA_ETH_TX_QUEUES	NR_CPUS
+#endif
+
 #define DPAA_ETH_RX_QUEUES	128
 
 #ifdef CONFIG_PM
@@ -587,8 +593,17 @@ static inline void _dpa_assign_wq(struct dpa_fq *fq)
 
 #ifdef CONFIG_FSL_DPAA_ETH_USE_NDO_SELECT_QUEUE
 /* Use in lieu of skb_get_queue_mapping() */
+#ifdef CONFIG_FMAN_PFC
+#define dpa_get_queue_mapping(skb) \
+	(((skb)->priority < CONFIG_FMAN_PFC_COS_COUNT) ? \
+		((skb)->priority * dpa_num_cpus + smp_processor_id()) : \
+		((CONFIG_FMAN_PFC_COS_COUNT - 1) * \
+			dpa_num_cpus + smp_processor_id()));
+
+#else
 #define dpa_get_queue_mapping(skb) \
 	smp_processor_id()
+#endif
 #else
 /* Use the queue selected by XPS */
 #define dpa_get_queue_mapping(skb) \
diff --git a/drivers/net/ethernet/freescale/dpa/dpaa_eth_common.c b/drivers/net/ethernet/freescale/dpa/dpaa_eth_common.c
index 0a5e1c7..5a899b7 100644
--- a/drivers/net/ethernet/freescale/dpa/dpaa_eth_common.c
+++ b/drivers/net/ethernet/freescale/dpa/dpaa_eth_common.c
@@ -63,6 +63,7 @@ static struct dpa_bp *dpa_bp_array[64];
 
 int dpa_max_frm;
 int dpa_rx_extra_headroom;
+int dpa_num_cpus = NR_CPUS;
 
 static const struct fqid_cell tx_confirm_fqids[] = {
 	{0, DPAA_ETH_TX_QUEUES}
@@ -691,7 +692,14 @@ dpa_bp_alloc(struct dpa_bp *dpa_bp)
 		return -EINVAL;
 	}
 
-	bp_params.flags = 0;
+	memset(&bp_params, 0, sizeof(struct bman_pool_params));
+#ifdef CONFIG_FMAN_PFC
+	bp_params.flags = BMAN_POOL_FLAG_THRESH;
+	bp_params.thresholds[0] = bp_params.thresholds[2] =
+			CONFIG_FSL_DPAA_ETH_REFILL_THRESHOLD;
+	bp_params.thresholds[1] = bp_params.thresholds[3] =
+			CONFIG_FSL_DPAA_ETH_MAX_BUF_COUNT;
+#endif
 
 	/* If the pool is already specified, we only create one per bpid */
 	if (dpa_bpid2pool_use(dpa_bp->bpid))
@@ -833,7 +841,7 @@ bool dpa_bpid2pool_use(int bpid)
 #ifdef CONFIG_FSL_DPAA_ETH_USE_NDO_SELECT_QUEUE
 u16 dpa_select_queue(struct net_device *net_dev, struct sk_buff *skb)
 {
-	return smp_processor_id();
+	return dpa_get_queue_mapping(skb);
 }
 #endif
 
@@ -852,10 +860,19 @@ struct dpa_fq *dpa_fq_alloc(struct device *dev,
 	for (i = 0; i < fqids->count; i++) {
 		dpa_fq[i].fq_type = fq_type;
 		dpa_fq[i].fqid = fqids->start ? fqids->start + i : 0;
-		_dpa_assign_wq(dpa_fq + i);
 		list_add_tail(&dpa_fq[i].list, list);
 	}
 
+#ifdef CONFIG_FMAN_PFC
+	if (fq_type == FQ_TYPE_TX ||
+			fq_type == FQ_TYPE_TX_RECYCLE)
+		for (i = 0; i < fqids->count; i++)
+			dpa_fq[i].wq = i / dpa_num_cpus;
+	else
+#endif
+		for (i = 0; i < fqids->count; i++)
+			_dpa_assign_wq(dpa_fq + i);
+
 	return dpa_fq;
 }
 
diff --git a/drivers/net/ethernet/freescale/dpa/dpaa_ethtool.c b/drivers/net/ethernet/freescale/dpa/dpaa_ethtool.c
index 839c223..d36930e 100644
--- a/drivers/net/ethernet/freescale/dpa/dpaa_ethtool.c
+++ b/drivers/net/ethernet/freescale/dpa/dpaa_ethtool.c
@@ -199,7 +199,6 @@ static int __cold dpa_set_pauseparam(struct net_device *net_dev,
 	struct dpa_priv_s	*priv;
 	struct mac_device       *mac_dev;
 	struct phy_device       *phy_dev;
-	struct fm_mac_dev       *fm_mac_dev;
 	int _errno;
 	u32 newadv, oldadv;
 
@@ -261,16 +260,13 @@ static int __cold dpa_set_pauseparam(struct net_device *net_dev,
 	/* If PAUSE frame autonegotiation is disabled,
 	 * ethtool rx/tx settings are enforced.
 	 */
-	fm_mac_dev = mac_dev->get_mac_handle(mac_dev);
-	_errno = set_mac_rx_pause(mac_dev, fm_mac_dev,
-			!!epause->rx_pause);
+	_errno = set_mac_rx_pause(mac_dev, !!epause->rx_pause);
 	if (unlikely(_errno < 0)) {
 		netdev_err(net_dev, "set_mac_rx_pause() = %d\n", _errno);
 		return _errno;
 	}
 
-	_errno = set_mac_tx_pause(mac_dev, fm_mac_dev,
-			!!epause->tx_pause);
+	_errno = set_mac_tx_pause(mac_dev, !!epause->tx_pause);
 	if (unlikely(_errno < 0)) {
 		netdev_err(net_dev, "set_mac_tx_pause() = %d\n", _errno);
 		return _errno;
diff --git a/drivers/net/ethernet/freescale/dpa/mac-api.c b/drivers/net/ethernet/freescale/dpa/mac-api.c
index 5833a5e..a8b4704 100644
--- a/drivers/net/ethernet/freescale/dpa/mac-api.c
+++ b/drivers/net/ethernet/freescale/dpa/mac-api.c
@@ -332,13 +332,14 @@ static int __cold set_multi(struct net_device *net_dev,
 /* Avoid redundant calls to FMD, if the MAC driver already contains the desired
  * settings. Otherwise, the new MAC settings should be reflected in FMan.
  */
-int set_mac_rx_pause(struct mac_device *mac_dev,
-		struct fm_mac_dev *fm_mac_dev, bool en)
+int set_mac_rx_pause(struct mac_device *mac_dev, bool en)
 {
+	struct fm_mac_dev *fm_mac_dev;
 	int _errno = 0;
 
 	if (unlikely(en != mac_dev->rx_pause)) {
-		_errno = mac_dev->set_rx_pause(fm_mac_dev, en);
+		fm_mac_dev = mac_dev->get_mac_handle(mac_dev);
+		_errno = fm_mac_set_rx_pause_frames(fm_mac_dev, en);
 		if (likely(_errno == 0))
 			mac_dev->rx_pause = en;
 	}
@@ -346,13 +347,14 @@ int set_mac_rx_pause(struct mac_device *mac_dev,
 	return _errno;
 }
 
-int set_mac_tx_pause(struct mac_device *mac_dev,
-		struct fm_mac_dev *fm_mac_dev, bool en)
+int set_mac_tx_pause(struct mac_device *mac_dev, bool en)
 {
 	int _errno = 0;
+	struct fm_mac_dev *fm_mac_dev;
 
 	if (unlikely(en != mac_dev->tx_pause)) {
-		_errno = mac_dev->set_tx_pause(fm_mac_dev, en);
+		fm_mac_dev = mac_dev->get_mac_handle(mac_dev);
+		_errno = fm_mac_set_tx_pause_frames(fm_mac_dev, en);
 		if (likely(_errno == 0))
 			mac_dev->tx_pause = en;
 	}
@@ -427,10 +429,10 @@ static void adjust_link(struct net_device *net_dev)
 
 	get_pause_cfg(mac_dev, &rx_pause, &tx_pause);
 
-	_errno = set_mac_rx_pause(mac_dev, fm_mac_dev, rx_pause);
+	_errno = set_mac_rx_pause(mac_dev, rx_pause);
 	if (unlikely(_errno < 0))
 		netdev_err(net_dev, "set_rx_pause() = %d\n", _errno);
-	_errno = set_mac_tx_pause(mac_dev, fm_mac_dev, tx_pause);
+	_errno = set_mac_tx_pause(mac_dev, tx_pause);
 	if (unlikely(_errno < 0))
 		netdev_err(net_dev, "set_tx_pause() = %d\n", _errno);
 }
diff --git a/drivers/net/ethernet/freescale/dpa/mac.c b/drivers/net/ethernet/freescale/dpa/mac.c
index aaeeb5e..20994d4 100644
--- a/drivers/net/ethernet/freescale/dpa/mac.c
+++ b/drivers/net/ethernet/freescale/dpa/mac.c
@@ -173,13 +173,6 @@ static int __cold mac_probe(struct platform_device *_of_dev)
 
 	INIT_LIST_HEAD(&mac_dev->mc_addr_list);
 
-	/* pause frame autonegotiation enabled*/
-	mac_dev->autoneg_pause = true;
-	/* does not ignore PAUSE frames */
-	mac_dev->rx_pause = true;
-	/* transmits PAUSE frames when congested */
-	mac_dev->tx_pause = true;
-
 	/* Get the FM node */
 	dev_node = of_get_parent(mac_node);
 	if (unlikely(dev_node == NULL)) {
@@ -365,6 +358,26 @@ static int __cold mac_probe(struct platform_device *_of_dev)
 		goto _return_dev_set_drvdata;
 	}
 
+	/* pause frame autonegotiation enabled*/
+	mac_dev->autoneg_pause = true;
+
+	/* by intializing the values to false, force FMD to enable PAUSE frames
+	 * on RX and TX
+	 */
+	mac_dev->rx_pause = mac_dev->tx_pause = false;
+	/* does not ignore PAUSE frames */
+	_errno = set_mac_rx_pause(mac_dev, true);
+	if (unlikely(_errno < 0)) {
+		dev_err(dev, "set_mac_rx_pause() = %d\n", _errno);
+		return _errno;
+	}
+	/* transmits PAUSE frames when congested */
+	_errno = set_mac_tx_pause(mac_dev, true);
+	if (unlikely(_errno < 0)) {
+		dev_err(dev, "set_mac_tx_pause() = %d\n", _errno);
+		return _errno;
+	}
+
 	dev_info(dev,
 		"FMan MAC address: %02hx:%02hx:%02hx:%02hx:%02hx:%02hx\n",
 		     mac_dev->addr[0], mac_dev->addr[1], mac_dev->addr[2],
diff --git a/drivers/net/ethernet/freescale/dpa/mac.h b/drivers/net/ethernet/freescale/dpa/mac.h
index 8814c28..785877f 100644
--- a/drivers/net/ethernet/freescale/dpa/mac.h
+++ b/drivers/net/ethernet/freescale/dpa/mac.h
@@ -121,9 +121,7 @@ extern const char	*mac_driver_description;
 extern const size_t	 mac_sizeof_priv[];
 extern void (*const mac_setup[])(struct mac_device *mac_dev);
 
-int set_mac_rx_pause(struct mac_device *mac_dev,
-		struct fm_mac_dev *fm_mac_dev, bool en);
-int set_mac_tx_pause(struct mac_device *mac_dev,
-		struct fm_mac_dev *fm_mac_dev, bool en);
+int set_mac_rx_pause(struct mac_device *mac_dev, bool en);
+int set_mac_tx_pause(struct mac_device *mac_dev, bool en);
 
 #endif	/* __MAC_H */