diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/Kconfig b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/Kconfig
index b097f52..42e6b38 100755
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/Kconfig
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/Kconfig
@@ -28,14 +28,4 @@
 	  in the MediaTek MT7986/MT2701/MT7622/MT7629/MT7621 chipset
 	  family.
 
-config NET_MEDIATEK_HW_QOS
-	bool "Mediatek HW QoS support"
-	depends on NET_MEDIATEK_HNAT
-	default n
-	---help---
-	  This driver supports the hardward
-	  quality of service (QoS) control
-	  for the hardware NAT in the
-	  MediaTek chipset family.
-
 endif #NET_VENDOR_MEDIATEK
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c
index 658f1ab..98397c0 100755
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_eth_soc.c
@@ -1015,9 +1015,7 @@
 
 	nr_frags = skb_shinfo(skb)->nr_frags;
 
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
         qid = skb->mark & (MTK_QDMA_TX_MASK);
-#endif
 
 	if (MTK_HAS_CAPS(eth->soc->caps, MTK_NETSYS_V2)) {
 		u32 txd5 = 0, txd6 = 0;
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat.c
index c10e7ed..2479b3d 100644
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat.c
@@ -599,12 +599,10 @@
 	return 0;
 }
 
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 static struct packet_type mtk_pack_type __read_mostly = {
 	.type   = HQOS_MAGIC_TAG,
 	.func   = mtk_hqos_ptype_cb,
 };
-#endif
 
 static int hnat_probe(struct platform_device *pdev)
 {
@@ -756,10 +754,9 @@
 		add_timer(&hnat_priv->hnat_reset_timestamp_timer);
 	}
 
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 	if (qos_toggle && IS_GMAC1_MODE)
 		dev_add_pack(&mtk_pack_type);
-#endif
+
 	err = hnat_roaming_enable();
 	if (err)
 		pr_info("hnat roaming work fail\n");
@@ -800,10 +797,8 @@
 	if (hnat_priv->data->version == MTK_HNAT_V3)
 		del_timer_sync(&hnat_priv->hnat_reset_timestamp_timer);
 
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 	if (qos_toggle && IS_GMAC1_MODE)
 		dev_remove_pack(&mtk_pack_type);
-#endif
 
 	return 0;
 }
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat.h b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat.h
index ce5af47..f10b169 100644
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat.h
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat.h
@@ -104,12 +104,20 @@
 #define GDMA1_FWD_CFG 0x500
 #define GDMA2_FWD_CFG 0x1500
 
+/* QDMA Tx queue configuration */
 #define QTX_CFG(x)		(QDMA_BASE + ((x) * 0x10))
 #define QTX_SCH(x)		(QDMA_BASE + 0x4 + ((x) * 0x10))
+#define QTX_TX_SCH_SEL_OFFSET	(30)
+
+/* QDMA Tx scheduler configuration */
 #define QDMA_PAGE		(QDMA_BASE + 0x1f0)
 #define QDMA_TX_2SCH_BASE	(QDMA_BASE + 0x214)
 #define QTX_MIB_IF		(QDMA_BASE + 0x2bc)
 #define QDMA_TX_4SCH_BASE(x)	(QDMA_BASE + 0x398 + (((x) >> 1) * 0x4))
+#define QDMA_TX_SCH_MAX_WFQ	BIT(15)
+#define QDMA_TX_SCH_RATE_EN	BIT(11)
+#define QDMA_RATE_MAN_OFFSET	(4)
+#define QDMA_RATE_EXP_OFFSET	(0)
 
 /*--------------------------------------------------------------------------*/
 /* Register Mask*/
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_debugfs.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_debugfs.c
index 4a95c10..c6978fa 100644
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_debugfs.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_debugfs.c
@@ -1986,6 +1986,29 @@
 	return single_open(file, hnat_qos_toggle_read, file->private_data);
 }
 
+static void hnat_qos_pppq_enable(void)
+{
+	u32 cfg, id;
+
+	for (id = 0; id < hnat_priv->data->num_of_sch; id++) {
+		writel(id << QTX_TX_SCH_SEL_OFFSET,
+		       hnat_priv->fe_base + QTX_SCH(id % NUM_OF_Q_PER_PAGE));
+
+		if (id & 0x1) {
+			cfg = 0;
+			cfg |= QDMA_TX_SCH_MAX_WFQ | QDMA_TX_SCH_RATE_EN;
+			cfg |= 25 << QDMA_RATE_MAN_OFFSET;
+			cfg |= 5 << QDMA_RATE_EXP_OFFSET;
+			cfg |= cfg << 16;
+
+			if (hnat_priv->data->num_of_sch == 4)
+				writel(cfg, hnat_priv->fe_base + QDMA_TX_4SCH_BASE(id));
+			else
+				writel(cfg, hnat_priv->fe_base + QDMA_TX_2SCH_BASE);
+		}
+	}
+}
+
 static ssize_t hnat_qos_toggle_write(struct file *file, const char __user *buffer,
 				     size_t count, loff_t *data)
 {
@@ -1995,15 +2018,16 @@
 	if ((len > 8) || copy_from_user(buf, buffer, len))
 		return -EFAULT;
 
-	if (buf[0] == '0' && qos_toggle != 0) {
+	if (buf[0] == '0') {
 		pr_info("HQoS is going to be disabled !\n");
 		qos_toggle = 0;
-	} else if (buf[0] == '1' && qos_toggle != 1) {
+	} else if (buf[0] == '1') {
 		pr_info("HQoS mode is going to be enabled !\n");
 		qos_toggle = 1;
-	} else if (buf[0] == '2' && qos_toggle != 2) {
+	} else if (buf[0] == '2') {
 		pr_info("Per-port-per-queue mode is going to be enabled !\n");
 		qos_toggle = 2;
+		hnat_qos_pppq_enable();
 	}
 
 	return len;
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_nf_hook.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_nf_hook.c
index fe81bf3..da97b89 100644
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_nf_hook.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_nf_hook.c
@@ -476,7 +476,6 @@
 
 	skb->dev = get_dev_from_index(index);
 
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 	if (qos_toggle && eth_hdr(skb)->h_proto == HQOS_MAGIC_TAG) {
 		skb = skb_unshare(skb, GFP_ATOMIC);
 		if (!skb)
@@ -490,7 +489,6 @@
 		memmove(skb->data - ETH_HLEN, skb->data - ETH_HLEN - VLAN_HLEN,
 			2 * ETH_ALEN);
 	}
-#endif
 
 	if (skb->dev) {
 		skb_set_network_header(skb, 0);
@@ -806,7 +804,6 @@
 mtk_hnat_br_nf_local_in(void *priv, struct sk_buff *skb,
 			const struct nf_hook_state *state)
 {
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 	struct vlan_ethhdr *veth;
 
 	if (qos_toggle && hnat_priv->data->whnat) {
@@ -817,7 +814,6 @@
 			skb_hnat_reason(skb) = HIT_BIND_FORCE_TO_CPU;
 		}
 	}
-#endif
 
 	if (!HAS_HQOS_MAGIC_TAG(skb) && !is_ppe_support_type(skb)) {
 		hnat_set_head_frags(state, skb, 1, hnat_set_alg);
@@ -852,12 +848,8 @@
 			clr_from_extge(skb);
 
 		/* packets from external devices -> xxx ,step 2, learning stage */
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 		if (do_ext2ge_fast_learn(state->in, skb) && (!qos_toggle ||
 		    (qos_toggle && eth_hdr(skb)->h_proto != HQOS_MAGIC_TAG))) {
-#else
-		if (do_ext2ge_fast_learn(state->in, skb)) {
-#endif
 			if (!do_hnat_ext_to_ge2(skb, __func__))
 				return NF_STOLEN;
 			goto drop;
@@ -1055,7 +1047,7 @@
 		}
 
 		entry.ipv4_hnapt.iblk2.port_ag =
-			(hnat_priv->data->version == MTK_HNAT_V4) ? 0x3f : 0xf;
+			(hnat_priv->data->version == MTK_HNAT_V4) ? 0xf : 0x3f;
 		break;
 	case IPV4_DSLITE:
 	case IPV4_MAP_E:
@@ -1074,7 +1066,7 @@
 		}
 
 		entry.ipv6_5t_route.iblk2.port_ag =
-			(hnat_priv->data->version == MTK_HNAT_V4) ? 0x3f : 0xf;
+			(hnat_priv->data->version == MTK_HNAT_V4) ? 0xf : 0x3f;
 		break;
 	}
 	return entry;
@@ -1365,14 +1357,12 @@
 					foe->ipv4_hnapt.new_dip;
 				entry.ipv4_hnapt.etype = htons(ETH_P_IP);
 
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 				if (qos_toggle) {
 					entry.ipv4_hnapt.iblk2.qid =
 						(hnat_priv->data->version == MTK_HNAT_V4) ?
 						 skb->mark & 0x7f : skb->mark & 0xf;
 					entry.ipv4_hnapt.iblk2.fqos = 1;
 				}
-#endif
 
 				entry.ipv4_hnapt.bfib1.udp =
 					foe->ipv4_hnapt.bfib1.udp;
@@ -1498,7 +1488,7 @@
 		entry.ipv4_hnapt.iblk2.dp = gmac;
 		entry.ipv4_hnapt.iblk2.port_mg =
 			(hnat_priv->data->version == MTK_HNAT_V1) ? 0x3f : 0;
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
+
 		if (qos_toggle) {
 			if (hnat_priv->data->version == MTK_HNAT_V4) {
 				entry.ipv4_hnapt.iblk2.qid = qid & 0x7f;
@@ -1526,14 +1516,11 @@
 		} else {
 			entry.ipv4_hnapt.iblk2.fqos = 0;
 		}
-#else
-		entry.ipv4_hnapt.iblk2.fqos = 0;
-#endif
 	} else {
 		entry.ipv6_5t_route.iblk2.dp = gmac;
 		entry.ipv6_5t_route.iblk2.port_mg =
 			(hnat_priv->data->version == MTK_HNAT_V1) ? 0x3f : 0;
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
+
 		if (qos_toggle) {
 			if (hnat_priv->data->version == MTK_HNAT_V4) {
 				entry.ipv6_5t_route.iblk2.qid = qid & 0x7f;
@@ -1560,9 +1547,6 @@
 		} else {
 			entry.ipv6_5t_route.iblk2.fqos = 0;
 		}
-#else
-		entry.ipv6_5t_route.iblk2.fqos = 0;
-#endif
 	}
 
 	/* The INFO2.port_mg and 2nd VLAN ID fields of PPE entry are redefined
@@ -1680,10 +1664,7 @@
 			entry->ipv4_hnapt.winfo.bssid = skb_hnat_bss_id(skb);
 			entry->ipv4_hnapt.winfo.wcid = skb_hnat_wc_id(skb);
 #if defined(CONFIG_MEDIATEK_NETSYS_V2)
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
-			if (qos_toggle)
-				entry->ipv4_hnapt.iblk2.fqos = 1;
-#endif
+			entry->ipv4_hnapt.iblk2.fqos = (qos_toggle) ? 1 : 0;
 			entry->ipv4_hnapt.iblk2.rxid = skb_hnat_rx_id(skb);
 			entry->ipv4_hnapt.iblk2.winfoi = 1;
 #else
@@ -1702,7 +1683,6 @@
 					entry->ipv4_hnapt.vlan1 = 2;
 			}
 
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 			if (qos_toggle &&
 			    (FROM_GE_LAN(skb) || FROM_GE_WAN(skb) || FROM_GE_VIRTUAL(skb))) {
 				entry->bfib1.vpm = 0;
@@ -1711,7 +1691,6 @@
 				entry->ipv4_hnapt.vlan1 = skb_hnat_entry(skb);
 				entry->ipv4_hnapt.iblk2.fqos = 1;
 			}
-#endif
 		}
 		entry->ipv4_hnapt.iblk2.dp = gmac_no;
 	} else {
@@ -1723,10 +1702,7 @@
 			entry->ipv6_5t_route.winfo.bssid = skb_hnat_bss_id(skb);
 			entry->ipv6_5t_route.winfo.wcid = skb_hnat_wc_id(skb);
 #if defined(CONFIG_MEDIATEK_NETSYS_V2)
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
-			if (qos_toggle)
-				entry->ipv6_5t_route.iblk2.fqos = 1;
-#endif
+			entry->ipv6_5t_route.iblk2.fqos = (qos_toggle) ? 1 : 0;
 			entry->ipv6_5t_route.iblk2.rxid = skb_hnat_rx_id(skb);
 			entry->ipv6_5t_route.iblk2.winfoi = 1;
 #else
@@ -1745,7 +1721,6 @@
 					entry->ipv6_5t_route.vlan1 = 2;
 			}
 
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 			if (qos_toggle &&
 			    (FROM_GE_LAN(skb) || FROM_GE_WAN(skb) || FROM_GE_VIRTUAL(skb))) {
 				entry->bfib1.vpm = 0;
@@ -1754,7 +1729,6 @@
 				entry->ipv6_5t_route.vlan1 = skb_hnat_entry(skb);
 				entry->ipv6_5t_route.iblk2.fqos = 1;
 			}
-#endif
 		}
 		entry->ipv6_5t_route.iblk2.dp = gmac_no;
 	}
@@ -2106,25 +2080,19 @@
 mtk_pong_hqos_handler(void *priv, struct sk_buff *skb,
 		      const struct nf_hook_state *state)
 {
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 	struct vlan_ethhdr *veth = (struct vlan_ethhdr *)skb_mac_header(skb);
 
 	if (qos_toggle && eth_hdr(skb)->h_proto == HQOS_MAGIC_TAG) {
 		skb_hnat_entry(skb) = ntohs(veth->h_vlan_TCI) & 0x3fff;
 		skb_hnat_reason(skb) = HIT_BIND_FORCE_TO_CPU;
 	}
-#endif
 
 	if (skb_hnat_iface(skb) == FOE_MAGIC_EXT)
 		clr_from_extge(skb);
 
 	/* packets from external devices -> xxx ,step 2, learning stage */
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 	if (do_ext2ge_fast_learn(state->in, skb) && (!qos_toggle ||
 	    (qos_toggle && eth_hdr(skb)->h_proto != HQOS_MAGIC_TAG))) {
-#else
-	if (do_ext2ge_fast_learn(state->in, skb)) {
-#endif
 		if (!do_hnat_ext_to_ge2(skb, __func__))
 			return NF_STOLEN;
 		goto drop;
@@ -2301,7 +2269,6 @@
 	return 0;
 }
 
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 int mtk_hqos_ptype_cb(struct sk_buff *skb, struct net_device *dev,
 		      struct packet_type *pt, struct net_device *unused)
 {
@@ -2314,5 +2281,4 @@
 
 	return 0;
 }
-#endif
 
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_stag.c b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_stag.c
index 6e96429..73de3e9 100644
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_stag.c
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/hnat_stag.c
@@ -26,7 +26,7 @@
 
 	port_reg = of_get_property(ndev->dev.of_node, "reg", NULL);
 	if (unlikely(!port_reg))
-		return;
+		return -EINVAL;
 
 	port_index = be32_to_cpup(port_reg);
 	sp_tag = BIT(port_index);
diff --git a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/nf_hnat_mtk.h b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/nf_hnat_mtk.h
index 0493337..96bbe06 100644
--- a/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/nf_hnat_mtk.h
+++ b/target/linux/mediatek/files-5.4/drivers/net/ethernet/mediatek/mtk_hnat/nf_hnat_mtk.h
@@ -57,12 +57,8 @@
 } __packed;
 #endif
 
-#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
 #define HQOS_MAGIC_TAG 0x5678
-#define HAS_HQOS_MAGIC_TAG(skb) (skb->protocol == HQOS_MAGIC_TAG)
-#else
-#define HAS_HQOS_MAGIC_TAG(skb) NULL
-#endif
+#define HAS_HQOS_MAGIC_TAG(skb) (qos_toggle && skb->protocol == HQOS_MAGIC_TAG)
 
 #define HNAT_MAGIC_TAG 0x6789
 #define HNAT_INFO_FILLED 0x7