4 files changed, 68 insertions, 4 deletions

diff --git a/drivers/net/wireless/realtek/rtw89/fw.c b/drivers/net/wireless/realtek/rtw89/fw.c
index 3d7b99e15967..544f6d4b6873 100644
--- a/drivers/net/wireless/realtek/rtw89/fw.c
+++ b/drivers/net/wireless/realtek/rtw89/fw.c
@@ -11,6 +11,9 @@
 #include "phy.h"
 #include "reg.h"
 
+static void rtw89_fw_c2h_cmd_handle(struct rtw89_dev *rtwdev,
+				    struct sk_buff *skb);
+
 static struct sk_buff *rtw89_fw_h2c_alloc_skb(struct rtw89_dev *rtwdev, u32 len,
 					      bool header)
 {
@@ -2382,8 +2385,43 @@ void rtw89_fw_free_all_early_h2c(struct rtw89_dev *rtwdev)
 	mutex_unlock(&rtwdev->mutex);
 }
 
+static void rtw89_fw_c2h_parse_attr(struct sk_buff *c2h)
+{
+	struct rtw89_fw_c2h_attr *attr = RTW89_SKB_C2H_CB(c2h);
+
+	attr->category = RTW89_GET_C2H_CATEGORY(c2h->data);
+	attr->class = RTW89_GET_C2H_CLASS(c2h->data);
+	attr->func = RTW89_GET_C2H_FUNC(c2h->data);
+	attr->len = RTW89_GET_C2H_LEN(c2h->data);
+}
+
+static bool rtw89_fw_c2h_chk_atomic(struct rtw89_dev *rtwdev,
+				    struct sk_buff *c2h)
+{
+	struct rtw89_fw_c2h_attr *attr = RTW89_SKB_C2H_CB(c2h);
+	u8 category = attr->category;
+	u8 class = attr->class;
+	u8 func = attr->func;
+
+	switch (category) {
+	default:
+		return false;
+	case RTW89_C2H_CAT_MAC:
+		return rtw89_mac_c2h_chk_atomic(rtwdev, class, func);
+	}
+}
+
 void rtw89_fw_c2h_irqsafe(struct rtw89_dev *rtwdev, struct sk_buff *c2h)
 {
+	rtw89_fw_c2h_parse_attr(c2h);
+	if (!rtw89_fw_c2h_chk_atomic(rtwdev, c2h))
+		goto enqueue;
+
+	rtw89_fw_c2h_cmd_handle(rtwdev, c2h);
+	dev_kfree_skb_any(c2h);
+	return;
+
+enqueue:
 	skb_queue_tail(&rtwdev->c2h_queue, c2h);
 	ieee80211_queue_work(rtwdev->hw, &rtwdev->c2h_work);
 }
@@ -2391,10 +2429,11 @@ void rtw89_fw_c2h_irqsafe(struct rtw89_dev *rtwdev, struct sk_buff *c2h)
 static void rtw89_fw_c2h_cmd_handle(struct rtw89_dev *rtwdev,
 				    struct sk_buff *skb)
 {
-	u8 category = RTW89_GET_C2H_CATEGORY(skb->data);
-	u8 class = RTW89_GET_C2H_CLASS(skb->data);
-	u8 func = RTW89_GET_C2H_FUNC(skb->data);
-	u16 len = RTW89_GET_C2H_LEN(skb->data);
+	struct rtw89_fw_c2h_attr *attr = RTW89_SKB_C2H_CB(skb);
+	u8 category = attr->category;
+	u8 class = attr->class;
+	u8 func = attr->func;
+	u16 len = attr->len;
 	bool dump = true;
 
 	if (!test_bit(RTW89_FLAG_RUNNING, rtwdev->flags))
diff --git a/drivers/net/wireless/realtek/rtw89/fw.h b/drivers/net/wireless/realtek/rtw89/fw.h
index 509a3eac5ffe..d76d0c80f025 100644
--- a/drivers/net/wireless/realtek/rtw89/fw.h
+++ b/drivers/net/wireless/realtek/rtw89/fw.h
@@ -2778,6 +2778,20 @@ static inline void RTW89_SET_FWCMD_TSF32_TOGL_EARLY(void *cmd, u32 val)
 #define RTW89_GET_C2H_LEN(c2h) \
 	le32_get_bits(*((const __le32 *)(c2h) + 1), GENMASK(13, 0))
 
+struct rtw89_fw_c2h_attr {
+	u8 category;
+	u8 class;
+	u8 func;
+	u16 len;
+};
+
+static inline struct rtw89_fw_c2h_attr *RTW89_SKB_C2H_CB(struct sk_buff *skb)
+{
+	static_assert(sizeof(skb->cb) >= sizeof(struct rtw89_fw_c2h_attr));
+
+	return (struct rtw89_fw_c2h_attr *)skb->cb;
+}
+
 #define RTW89_GET_C2H_LOG_SRT_PRT(c2h) (char *)((__le32 *)(c2h) + 2)
 #define RTW89_GET_C2H_LOG_LEN(len) ((len) - RTW89_C2H_HEADER_LEN)
 
diff --git a/drivers/net/wireless/realtek/rtw89/mac.c b/drivers/net/wireless/realtek/rtw89/mac.c
index 6587cdf6ba62..098637a84895 100644
--- a/drivers/net/wireless/realtek/rtw89/mac.c
+++ b/drivers/net/wireless/realtek/rtw89/mac.c
@@ -4208,6 +4208,16 @@ void (* const rtw89_mac_c2h_info_handler[])(struct rtw89_dev *rtwdev,
 	[RTW89_MAC_C2H_FUNC_BCN_CNT] = rtw89_mac_c2h_bcn_cnt,
 };
 
+bool rtw89_mac_c2h_chk_atomic(struct rtw89_dev *rtwdev, u8 class, u8 func)
+{
+	switch (class) {
+	default:
+		return false;
+	case RTW89_MAC_C2H_CLASS_MCC:
+		return true;
+	}
+}
+
 void rtw89_mac_c2h_handle(struct rtw89_dev *rtwdev, struct sk_buff *skb,
 			  u32 len, u8 class, u8 func)
 {
diff --git a/drivers/net/wireless/realtek/rtw89/mac.h b/drivers/net/wireless/realtek/rtw89/mac.h
index 045e8ec61a41..82b9e81fe474 100644
--- a/drivers/net/wireless/realtek/rtw89/mac.h
+++ b/drivers/net/wireless/realtek/rtw89/mac.h
@@ -894,6 +894,7 @@ static inline int rtw89_chip_disable_bb_rf(struct rtw89_dev *rtwdev)
 
 u32 rtw89_mac_get_err_status(struct rtw89_dev *rtwdev);
 int rtw89_mac_set_err_status(struct rtw89_dev *rtwdev, u32 err);
+bool rtw89_mac_c2h_chk_atomic(struct rtw89_dev *rtwdev, u8 class, u8 func);
 void rtw89_mac_c2h_handle(struct rtw89_dev *rtwdev, struct sk_buff *skb,
 			  u32 len, u8 class, u8 func);
 int rtw89_mac_setup_phycap(struct rtw89_dev *rtwdev);