3 files changed, 14 insertions, 2 deletions
diff --git a/drivers/net/wireless/p54/p54.h b/drivers/net/wireless/p54/p54.h
index 98d4f8e7d84d..36b8e584c6cf 100644
--- a/drivers/net/wireless/p54/p54.h
+++ b/drivers/net/wireless/p54/p54.h
@@ -87,6 +87,8 @@ struct p54_common {
 	void *cached_vdcf;
 	unsigned int fw_var;
 	unsigned int fw_interface;
+	u32 tsf_low32;
+	u32 tsf_high32;
 	struct ieee80211_tx_queue_stats tx_stats[8];
 	void *eeprom;
 	struct completion eeprom_comp;
diff --git a/drivers/net/wireless/p54/p54common.c b/drivers/net/wireless/p54/p54common.c
index f96f7c7e6af5..526d3a282b7a 100644
--- a/drivers/net/wireless/p54/p54common.c
+++ b/drivers/net/wireless/p54/p54common.c
@@ -426,10 +426,12 @@ EXPORT_SYMBOL_GPL(p54_parse_eeprom);
 
 static int p54_rx_data(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
+	struct p54_common *priv = dev->priv;
 	struct p54_rx_hdr *hdr = (struct p54_rx_hdr *) skb->data;
 	struct ieee80211_rx_status rx_status = {0};
 	u16 freq = le16_to_cpu(hdr->freq);
 	size_t header_len = sizeof(*hdr);
+	u32 tsf32;
 
 	rx_status.signal = hdr->rssi;
 	/* XX correct? */
@@ -438,7 +440,13 @@ static int p54_rx_data(struct ieee80211_hw *dev, struct sk_buff *skb)
 	rx_status.freq = freq;
 	rx_status.band = IEEE80211_BAND_2GHZ;
 	rx_status.antenna = hdr->antenna;
-	rx_status.mactime = le64_to_cpu(hdr->timestamp);
+
+	tsf32 = le32_to_cpu(hdr->tsf32);
+	if (tsf32 < priv->tsf_low32)
+		priv->tsf_high32++;
+	rx_status.mactime = ((u64)priv->tsf_high32) << 32 | tsf32;
+	priv->tsf_low32 = tsf32;
+
 	rx_status.flag |= RX_FLAG_TSFT;
 
 	if (hdr->magic & cpu_to_le16(0x4000))
@@ -1037,6 +1045,7 @@ static void p54_stop(struct ieee80211_hw *dev)
 	while ((skb = skb_dequeue(&priv->tx_queue)))
 		kfree_skb(skb);
 	priv->stop(dev);
+	priv->tsf_high32 = priv->tsf_low32 = 0;
 	priv->mode = IEEE80211_IF_TYPE_INVALID;
 }
 
diff --git a/drivers/net/wireless/p54/p54common.h b/drivers/net/wireless/p54/p54common.h
index 73a9a2c923dd..976cbf9689b5 100644
--- a/drivers/net/wireless/p54/p54common.h
+++ b/drivers/net/wireless/p54/p54common.h
@@ -185,7 +185,8 @@ struct p54_rx_hdr {
 	u8 rssi;
 	u8 quality;
 	u16 unknown2;
-	__le64 timestamp;
+	__le32 tsf32;
+	__le32 unalloc0;
 	u8 align[0];
 } __attribute__ ((packed));