diff --git a/mt76x02_txrx.c b/mt76x02_txrx.c
index dd7d04b9..00d14f09 100644
--- a/mt76x02_txrx.c
+++ b/mt76x02_txrx.c
@@ -154,13 +154,14 @@ int mt76x02_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,
 	struct mt76x02_dev *dev = container_of(mdev, struct mt76x02_dev, mt76);
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 	struct mt76x02_txwi *txwi = txwi_ptr;
-	int hdrlen, len, pid, qsel = MT_QSEL_EDCA;
+	int hdrlen, pad, len, pid, qsel = MT_QSEL_EDCA;
 
 	if (qid == MT_TXQ_PSD && wcid && wcid->idx < 128)
 		mt76x02_mac_wcid_set_drop(dev, wcid->idx, false);
 
 	hdrlen = ieee80211_hdrlen(hdr->frame_control);
-	len = skb->len - (hdrlen & 2);
+	pad = skb->len > hdrlen ? (hdrlen & 2) : 0;
+	len = skb->len - pad;
 	mt76x02_mac_write_txwi(dev, txwi, skb, wcid, sta, len);
 
 	pid = mt76_tx_status_skb_add(mdev, wcid, skb);
diff --git a/mt76x02_usb_core.c b/mt76x02_usb_core.c
index c470e5ea..4cdfe31e 100644
--- a/mt76x02_usb_core.c
+++ b/mt76x02_usb_core.c
@@ -77,11 +77,14 @@ int mt76x02u_tx_prepare_skb(struct mt76_dev *mdev, void *data,
 			    struct mt76_tx_info *tx_info)
 {
 	struct mt76x02_dev *dev = container_of(mdev, struct mt76x02_dev, mt76);
-	int pid, len = skb->len, ep = q2ep(mdev->q_tx[qid].q->hw_idx);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	int pid, hdrlen, len = skb->len, ep = q2ep(mdev->q_tx[qid].q->hw_idx);
 	struct mt76x02_txwi *txwi;
 	enum mt76_qsel qsel;
 	u32 flags;
 
+	hdrlen = ieee80211_hdrlen(hdr->frame_control);
+	len -= skb->len > hdrlen ? (hdrlen & 2) : 0;
 	txwi = (struct mt76x02_txwi *)(skb->data - sizeof(struct mt76x02_txwi));
 	mt76x02_mac_write_txwi(dev, txwi, skb, wcid, sta, len);
 	skb_push(skb, sizeof(struct mt76x02_txwi));