diff --git a/drivers/net/ethernet/freescale/fec.h b/drivers/net/ethernet/freescale/fec.h
index 5ba1e0d71c68..5383681ac273 100644
--- a/drivers/net/ethernet/freescale/fec.h
+++ b/drivers/net/ethernet/freescale/fec.h
@@ -340,6 +340,10 @@ struct bufdesc_ex {
 #define RCMR_CMP(X)		(((X) == 1) ? RCMR_CMP_1 : RCMR_CMP_2)
 #define FEC_TX_BD_FTYPE(X)	(((X) & 0xf) << 20)
 
+#define FEC_QOS_TX_SHEME_MASK	GENMASK(2, 0)
+#define CREDIT_BASED_SCHEME	0
+#define ROUND_ROBIN_SCHEME	1
+
 /* The number of Tx and Rx buffers.  These are allocated from the page
  * pool.  The code may assume these are power of two, so it it best
  * to keep them that size.
@@ -571,6 +575,12 @@ struct fec_stop_mode_gpr {
 	u8 bit;
 };
 
+struct fec_cbs_params {
+	bool enable[FEC_ENET_MAX_TX_QS];
+	int idleslope[FEC_ENET_MAX_TX_QS];
+	int sendslope[FEC_ENET_MAX_TX_QS];
+};
+
 /* The FEC buffer descriptors track the ring buffers.  The rx_bd_base and
  * tx_bd_base always point to the base of the buffer descriptors.  The
  * cur_rx and cur_tx point to the currently available buffer.
@@ -679,6 +689,9 @@ struct fec_enet_private {
 	/* XDP BPF Program */
 	struct bpf_prog *xdp_prog;
 
+	/* CBS parameters */
+	struct fec_cbs_params cbs;
+
 	u64 ethtool_stats[];
 };
 
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index c73e25f8995e..91394ad05121 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -66,6 +66,7 @@
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
 #include <soc/imx/cpuidle.h>
+#include <net/pkt_sched.h>
 #include <linux/filter.h>
 #include <linux/bpf.h>
 
@@ -1023,6 +1024,174 @@ static void fec_enet_reset_skb(struct net_device *ndev)
 	}
 }
 
+static u32 fec_enet_get_idle_slope(u8 bw)
+{
+	int msb, power;
+	u32 idle_slope;
+
+	if (bw >= 100)
+		return 0;
+
+	/* Convert bw to hardware idle slope */
+	idle_slope = (512 * bw) / (100 - bw);
+
+	if (idle_slope >= 128) {
+		/* For values greater than or equal to 128, idle_slope
+		 * rounded to the nearest multiple of 128.
+		 */
+		idle_slope = DIV_ROUND_CLOSEST(idle_slope, 128U) * 128U;
+
+		return idle_slope;
+	}
+
+	/* For values less than 128, idle_slope is rounded to
+	 * nearst power of 2.
+	 */
+	if (idle_slope <= 1)
+		return 1;
+
+	msb = __fls(idle_slope);
+	power = BIT(msb);
+	idle_slope = DIV_ROUND_CLOSEST(idle_slope, power) * power;
+
+	return idle_slope;
+}
+
+static void fec_enet_set_cbs_idle_slope(struct fec_enet_private *fep)
+{
+	u32 bw, val, idle_slope;
+	int speed = fep->speed;
+	int idle_slope_sum = 0;
+	int i;
+
+	if (!speed)
+		return;
+
+	for (i = 1; i < FEC_ENET_MAX_TX_QS; i++) {
+		int port_tx_rate;
+
+		/* As defined in IEEE 802.1Q-2014 Section 8.6.8.2 item:
+		 *       sendslope = idleslope -  port_tx_rate
+		 * So we need to check whether port_tx_rate is equal to
+		 * the current link rate.
+		 */
+		port_tx_rate = fep->cbs.idleslope[i] - fep->cbs.sendslope[i];
+		if (port_tx_rate != speed * 1000)
+			return;
+
+		idle_slope_sum += fep->cbs.idleslope[i];
+	}
+
+	/* The all bandwidth of Queue 1 and Queue 2 can't greater than
+	 * the link rate.
+	 */
+	if (idle_slope_sum > speed * 1000)
+		return;
+
+	/* idleslope is in kilobits per second.
+	 * speed is the port rate in megabits per second.
+	 * So bandwidth the ratio, bw, is idleslope / (speed * 1000) * 100,
+	 * the unit of bw is percentage.
+	 */
+	for (i = 1; i < FEC_ENET_MAX_TX_QS; i++) {
+		bw = fep->cbs.idleslope[i] / (speed * 10);
+		idle_slope = fec_enet_get_idle_slope(bw);
+
+		val = readl(fep->hwp + FEC_DMA_CFG(i));
+		val &= ~IDLE_SLOPE_MASK;
+		val |= idle_slope & IDLE_SLOPE_MASK;
+		writel(val, fep->hwp + FEC_DMA_CFG(i));
+	}
+
+	/* Enable Credit-based shaper. */
+	val = readl(fep->hwp + FEC_QOS_SCHEME);
+	val &= ~FEC_QOS_TX_SHEME_MASK;
+	val |= CREDIT_BASED_SCHEME;
+	writel(val, fep->hwp + FEC_QOS_SCHEME);
+}
+
+static int fec_enet_setup_tc_cbs(struct net_device *ndev, void *type_data)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	struct tc_cbs_qopt_offload *cbs = type_data;
+	int queue = cbs->queue;
+	int speed = fep->speed;
+	int queue2;
+
+	if (!(fep->quirks & FEC_QUIRK_HAS_AVB))
+		return -EOPNOTSUPP;
+
+	/* Queue 1 for Class A, Queue 2 for Class B, so the ENET must
+	 * have three queues.
+	 */
+	if (fep->num_tx_queues != FEC_ENET_MAX_TX_QS)
+		return -EOPNOTSUPP;
+
+	if (!speed) {
+		netdev_err(ndev, "Link speed is 0!\n");
+		return -ECANCELED;
+	}
+
+	/* Queue 0 is not AVB capable */
+	if (queue <= 0 || queue >= fep->num_tx_queues) {
+		netdev_err(ndev, "The queue: %d is invalid!\n", queue);
+		return -EINVAL;
+	}
+
+	if (!cbs->enable) {
+		u32 val;
+
+		val = readl(fep->hwp + FEC_QOS_SCHEME);
+		val &= ~FEC_QOS_TX_SHEME_MASK;
+		val |= ROUND_ROBIN_SCHEME;
+		writel(val, fep->hwp + FEC_QOS_SCHEME);
+
+		memset(&fep->cbs, 0, sizeof(fep->cbs));
+
+		return 0;
+	}
+
+	if (cbs->idleslope - cbs->sendslope != speed * 1000 ||
+	    cbs->idleslope <= 0 || cbs->sendslope >= 0)
+		return -EINVAL;
+
+	/* Another AVB queue */
+	queue2 = (queue == 1) ? 2 : 1;
+	if (cbs->idleslope + fep->cbs.idleslope[queue2] > speed * 1000) {
+		netdev_err(ndev,
+			   "The sum of all idle slope can't exceed link speed!\n");
+		return -EINVAL;
+	}
+
+	fep->cbs.enable[queue] = true;
+	fep->cbs.idleslope[queue] = cbs->idleslope;
+	fep->cbs.sendslope[queue] = cbs->sendslope;
+	/* We need to configure the credit-based shaper of hardware after
+	 * the CBS parameters of queue 1 and queue 2 are both configured.
+	 * Avoid parameter conflicts between queue 1 and queue 2, causing
+	 * one of the queues to fail to be configured. Additionally, once
+	 * the FEC_QOS_SCHEME field is set to credit-based scheme, queue 1
+	 * and queue 2 are taking effective as AVB queues immediately. So
+	 * it's better to set credit-based shaper after both queues are
+	 * configured.
+	 */
+	if (fep->cbs.enable[queue2])
+		fec_enet_set_cbs_idle_slope(fep);
+
+	return 0;
+}
+
+static int fec_enet_setup_tc(struct net_device *ndev, enum tc_setup_type type,
+			     void *type_data)
+{
+	switch (type) {
+	case TC_SETUP_QDISC_CBS:
+		return fec_enet_setup_tc_cbs(ndev, type_data);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 /*
  * This function is called to start or restart the FEC during a link
  * change, transmit timeout, or to reconfigure the FEC.  The network
@@ -1173,6 +1342,15 @@ fec_restart(struct net_device *ndev)
 
 	writel(rcntl, fep->hwp + FEC_R_CNTRL);
 
+	/* We need to reconfigure the CBS due to some events will cause
+	 * the MAC reset such as link change, transmit timeout, checksum
+	 * feature change and so on.
+	 */
+	if (fep->quirks & FEC_QUIRK_HAS_AVB &&
+	    fep->num_tx_queues == FEC_ENET_MAX_TX_QS &&
+	    fep->cbs.enable[1] && fep->cbs.enable[2])
+		fec_enet_set_cbs_idle_slope(fep);
+
 	/* Setup multicast filter. */
 	set_multicast_list(ndev);
 #ifndef CONFIG_M5272
@@ -3882,6 +4060,7 @@ static const struct net_device_ops fec_netdev_ops = {
 	.ndo_tx_timeout		= fec_timeout,
 	.ndo_set_mac_address	= fec_set_mac_address,
 	.ndo_eth_ioctl		= fec_enet_ioctl,
+	.ndo_setup_tc	= fec_enet_setup_tc,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= fec_poll_controller,
 #endif
