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 7a83d9a..58a83b8 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
@@ -758,7 +758,7 @@
 		add_timer(&hnat_priv->hnat_reset_timestamp_timer);
 	}
 
-	if (qos_toggle && IS_GMAC1_MODE)
+	if (IS_HQOS_MODE && IS_GMAC1_MODE)
 		dev_add_pack(&mtk_pack_type);
 
 	err = hnat_roaming_enable();
@@ -801,7 +801,7 @@
 	if (hnat_priv->data->version == MTK_HNAT_V3)
 		del_timer_sync(&hnat_priv->hnat_reset_timestamp_timer);
 
-	if (qos_toggle && IS_GMAC1_MODE)
+	if (IS_HQOS_MODE && IS_GMAC1_MODE)
 		dev_remove_pack(&mtk_pack_type);
 
 	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 0e1e91b..31a060f 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
@@ -123,6 +123,7 @@
 #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_WFQ_EN		BIT(15)
 
 /*--------------------------------------------------------------------------*/
 /* 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 17ab837..a5403a8 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
@@ -2017,28 +2017,28 @@
 static void hnat_qos_disable(void)
 {
 	struct mtk_hnat *h = hnat_priv;
-	u32 id;
+	u32 id, cfg;
 
 	for (id = 0; id < MAX_PPPQ_PORT_NUM; id++) {
 		hnat_qos_shaper_ebl(id, 0);
-		writel(0, h->fe_base + QTX_CFG(id % NUM_OF_Q_PER_PAGE));
+		writel((4 << QTX_CFG_HW_RESV_CNT_OFFSET) |
+		       (4 << QTX_CFG_SW_RESV_CNT_OFFSET),
+		       h->fe_base + QTX_CFG(id % NUM_OF_Q_PER_PAGE));
 	}
 
-	writel((4 << QTX_CFG_HW_RESV_CNT_OFFSET) |
-	       (4 << QTX_CFG_SW_RESV_CNT_OFFSET), h->fe_base + QTX_CFG(0));
-
+	cfg = (QDMA_TX_SCH_WFQ_EN) | (QDMA_TX_SCH_WFQ_EN << 16);
 	for (id = 0; id < h->data->num_of_sch; id += 2) {
 		if (h->data->num_of_sch == 4)
-			writel(0, h->fe_base + QDMA_TX_4SCH_BASE(id));
+			writel(cfg, h->fe_base + QDMA_TX_4SCH_BASE(id));
 		else
-			writel(0, h->fe_base + QDMA_TX_2SCH_BASE);
+			writel(cfg, h->fe_base + QDMA_TX_2SCH_BASE);
 	}
 }
 
 static void hnat_qos_pppq_enable(void)
 {
 	struct mtk_hnat *h = hnat_priv;
-	u32 id;
+	u32 id, cfg;
 
 	for (id = 0; id < MAX_PPPQ_PORT_NUM; id++) {
 		if (hook_toggle)
@@ -2051,11 +2051,12 @@
 		       h->fe_base + QTX_CFG(id % NUM_OF_Q_PER_PAGE));
 	}
 
+	cfg = (QDMA_TX_SCH_WFQ_EN) | (QDMA_TX_SCH_WFQ_EN << 16);
 	for (id = 0; id < h->data->num_of_sch; id+= 2) {
 		if (h->data->num_of_sch == 4)
-                        writel(0, h->fe_base + QDMA_TX_4SCH_BASE(id));
+                        writel(cfg, h->fe_base + QDMA_TX_4SCH_BASE(id));
                 else
-                        writel(0, h->fe_base + QDMA_TX_2SCH_BASE);
+                        writel(cfg, h->fe_base + QDMA_TX_2SCH_BASE);
 	}
 }
 
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 c23b8ed..1570981 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,7 @@
 
 	skb->dev = get_dev_from_index(index);
 
-	if (qos_toggle && eth_hdr(skb)->h_proto == HQOS_MAGIC_TAG) {
+	if (IS_HQOS_MODE && eth_hdr(skb)->h_proto == HQOS_MAGIC_TAG) {
 		skb = skb_unshare(skb, GFP_ATOMIC);
 		if (!skb)
 			return NF_ACCEPT;
@@ -806,7 +806,7 @@
 {
 	struct vlan_ethhdr *veth;
 
-	if (qos_toggle && hnat_priv->data->whnat) {
+	if (IS_HQOS_MODE && hnat_priv->data->whnat) {
 		veth = (struct vlan_ethhdr *)skb_mac_header(skb);
 
 		if (eth_hdr(skb)->h_proto == HQOS_MAGIC_TAG) {
@@ -1357,7 +1357,7 @@
 					foe->ipv4_hnapt.new_dip;
 				entry.ipv4_hnapt.etype = htons(ETH_P_IP);
 
-				if (qos_toggle) {
+				if (IS_HQOS_MODE) {
 					entry.ipv4_hnapt.iblk2.qid =
 						(hnat_priv->data->version == MTK_HNAT_V4) ?
 						 skb->mark & 0x7f : skb->mark & 0xf;
@@ -1479,7 +1479,7 @@
 
 	if (IS_HQOS_MODE)
 		qid = skb->mark & (MTK_QDMA_TX_MASK);
-	else if (IS_PPPQ_MODE)
+	else if (IS_PPPQ_MODE && (IS_DSA_LAN(dev) || IS_DSA_WAN(dev)))
 		qid = port_id & MTK_QDMA_TX_MASK;
 	else
 		qid = 0;
@@ -1509,7 +1509,8 @@
 				}
 			}
 
-			if (FROM_EXT(skb) || skb_hnat_sport(skb) == NR_QDMA_PORT)
+			if (FROM_EXT(skb) || skb_hnat_sport(skb) == NR_QDMA_PORT ||
+			    (IS_PPPQ_MODE && !IS_DSA_LAN(dev) && !IS_DSA_WAN(dev)))
 				entry.ipv4_hnapt.iblk2.fqos = 0;
 			else
 				entry.ipv4_hnapt.iblk2.fqos = 1;
@@ -1540,7 +1541,8 @@
 				}
 			}
 
-			if (FROM_EXT(skb))
+			if (FROM_EXT(skb) ||
+			    (IS_PPPQ_MODE && !IS_DSA_LAN(dev) && !IS_DSA_WAN(dev)))
 				entry.ipv6_5t_route.iblk2.fqos = 0;
 			else
 				entry.ipv6_5t_route.iblk2.fqos = 1;
@@ -1664,7 +1666,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)
-			entry->ipv4_hnapt.iblk2.fqos = (qos_toggle) ? 1 : 0;
+			entry->ipv4_hnapt.iblk2.fqos = (IS_HQOS_MODE) ? 1 : 0;
 			entry->ipv4_hnapt.iblk2.rxid = skb_hnat_rx_id(skb);
 			entry->ipv4_hnapt.iblk2.winfoi = 1;
 #else
@@ -1683,7 +1685,7 @@
 					entry->ipv4_hnapt.vlan1 = 2;
 			}
 
-			if (qos_toggle &&
+			if (IS_HQOS_MODE &&
 			    (FROM_GE_LAN(skb) || FROM_GE_WAN(skb) || FROM_GE_VIRTUAL(skb))) {
 				entry->bfib1.vpm = 0;
 				entry->bfib1.vlan_layer = 1;
@@ -1702,7 +1704,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)
-			entry->ipv6_5t_route.iblk2.fqos = (qos_toggle) ? 1 : 0;
+			entry->ipv6_5t_route.iblk2.fqos = (IS_HQOS_MODE) ? 1 : 0;
 			entry->ipv6_5t_route.iblk2.rxid = skb_hnat_rx_id(skb);
 			entry->ipv6_5t_route.iblk2.winfoi = 1;
 #else
@@ -1721,7 +1723,7 @@
 					entry->ipv6_5t_route.vlan1 = 2;
 			}
 
-			if (qos_toggle &&
+			if (IS_HQOS_MODE &&
 			    (FROM_GE_LAN(skb) || FROM_GE_WAN(skb) || FROM_GE_VIRTUAL(skb))) {
 				entry->bfib1.vpm = 0;
 				entry->bfib1.vlan_layer = 1;
@@ -2083,7 +2085,7 @@
 {
 	struct vlan_ethhdr *veth = (struct vlan_ethhdr *)skb_mac_header(skb);
 
-	if (qos_toggle && eth_hdr(skb)->h_proto == HQOS_MAGIC_TAG) {
+	if (IS_HQOS_MODE && 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;
 	}