diff --git a/drivers/net/wireless/mediatek/mt76/mt76x02_beacon.c b/drivers/net/wireless/mediatek/mt76/mt76x02_beacon.c
index d77088b7659e7884acb64e62a07d46fbc542937e..c0be90988f8253a657d2764498a4d052c93da77f 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76x02_beacon.c
+++ b/drivers/net/wireless/mediatek/mt76/mt76x02_beacon.c
@@ -38,6 +38,7 @@ const u16 mt76x02_beacon_offsets[16] = {
 	0xc000,
 	0xc000,
 };
+EXPORT_SYMBOL_GPL(mt76x02_beacon_offsets);
 
 static void mt76x02_set_beacon_offsets(struct mt76x02_dev *dev)
 {
@@ -134,6 +135,7 @@ int mt76x02_mac_set_beacon(struct mt76x02_dev *dev, u8 vif_idx,
 		       bcn_idx - 1);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(mt76x02_mac_set_beacon);
 
 static void
 __mt76x02_mac_set_beacon_enable(struct mt76x02_dev *dev, u8 vif_idx,
diff --git a/drivers/net/wireless/mediatek/mt76/mt76x02_usb_core.c b/drivers/net/wireless/mediatek/mt76/mt76x02_usb_core.c
index 82b78cc5b362d25432fc063d4d6d40e4c3d509ee..89249621bcec8c9a97381eedecd569101c1c25f7 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76x02_usb_core.c
+++ b/drivers/net/wireless/mediatek/mt76/mt76x02_usb_core.c
@@ -164,9 +164,32 @@ static void mt76x02u_pre_tbtt_work(struct work_struct *work)
 {
 	struct mt76x02_dev *dev =
 		container_of(work, struct mt76x02_dev, pre_tbtt_work);
+	int beacon_len = mt76x02_beacon_offsets[1] - mt76x02_beacon_offsets[0];
+	struct beacon_bc_data data = {};
+	struct sk_buff *skb;
+	int i, nbeacons;
 
 	if (!dev->beacon_mask)
 		return;
+
+	mt76x02_resync_beacon_timer(dev);
+
+	ieee80211_iterate_active_interfaces(mt76_hw(dev),
+		IEEE80211_IFACE_ITER_RESUME_ALL,
+		mt76x02_update_beacon_iter, dev);
+
+	nbeacons = hweight8(dev->beacon_mask);
+	mt76x02_enqueue_buffered_bc(dev, &data, 8 - nbeacons);
+
+	for (i = nbeacons; i < 8; i++) {
+		skb = __skb_dequeue(&data.q);
+		if (skb && skb->len >= beacon_len) {
+			dev_kfree_skb(skb);
+			skb = NULL;
+		}
+		mt76x02_mac_set_beacon(dev, i, skb);
+	}
+
 	mt76x02u_restart_pre_tbtt_timer(dev);
 }
 
@@ -190,13 +213,20 @@ static void mt76x02u_pre_tbtt_enable(struct mt76x02_dev *dev, bool en)
 
 static void mt76x02u_beacon_enable(struct mt76x02_dev *dev, bool en)
 {
+	int i;
+
 	if (WARN_ON_ONCE(!dev->beacon_int))
 		return;
 
-	if (en)
+	if (en) {
 		mt76x02u_start_pre_tbtt_timer(dev);
-
-	/* Nothing to do on disable as timer is already stopped */
+	} else {
+		/* Timer is already stopped, only clean up
+		 * PS buffered frames if any.
+		 */
+		for (i = 0; i < 8; i++)
+			mt76x02_mac_set_beacon(dev, i, NULL);
+	}
 }
 
 void mt76x02u_init_beacon_config(struct mt76x02_dev *dev)