From 732b74a65c2f6cdbbdfd8d2553fff8d2b856c8d6 Mon Sep 17 00:00:00 2001
From: Felix Fietkau <nbd@openwrt.org>
Date: Wed, 16 Dec 2015 10:53:58 +0100
Subject: [PATCH] mt7603: fill in rx frequency information

Signed-off-by: Felix Fietkau <nbd@openwrt.org>
---
 mt7603_core.c |  6 ++++--
 mt7603_mac.c  | 11 ++++++++++-
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/mt7603_core.c b/mt7603_core.c
index ba29831a..b45c4098 100644
--- a/mt7603_core.c
+++ b/mt7603_core.c
@@ -76,7 +76,7 @@ u32 mt7603_reg_map(struct mt7603_dev *dev, u32 addr)
 
 int mt7603_set_channel(struct mt7603_dev *dev, struct cfg80211_chan_def *def)
 {
-	int ret;
+	int idx, ret;
 
 	u8 bw = MT_BW_20;
 
@@ -88,7 +88,9 @@ int mt7603_set_channel(struct mt7603_dev *dev, struct cfg80211_chan_def *def)
 	if (ret)
 		return ret;
 
-	mt76_set(dev, MT_WF_RMAC_CH_FREQ, 1);
+	idx = def->chan->band == IEEE80211_BAND_5GHZ;
+	idx |= (def->chan - mt76_hw(dev)->wiphy->bands[def->chan->band]->channels) << 1;
+	mt76_set(dev, MT_WF_RMAC_CH_FREQ, idx);
 	mt7603_mac_start(dev);
 
 	return 0;
diff --git a/mt7603_mac.c b/mt7603_mac.c
index 70f935b7..b7c4452e 100644
--- a/mt7603_mac.c
+++ b/mt7603_mac.c
@@ -112,8 +112,18 @@ int
 mt7603_mac_fill_rx(struct mt7603_dev *dev, struct sk_buff *skb)
 {
 	struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
+	struct ieee80211_supported_band *sband;
 	__le32 *rxd = (__le32 *) skb->data;
 	u32 rxd0 = le32_to_cpu(rxd[0]);
+	int idx;
+
+	memset(status, 0, sizeof(*status));
+
+	idx = MT76_GET(MT_RXD1_NORMAL_CH_FREQ, rxd[1]);
+	sband = (idx & 1) ? &dev->mt76.sband_5g : &dev->mt76.sband_2g;
+	idx >>= 1;
+	if (idx < sband->n_channels)
+		status->freq = sband->channels[idx].center_freq;
 
 	rxd += 4;
 	if (rxd0 & MT_RXD0_NORMAL_GROUP_4) {
@@ -137,7 +147,6 @@ mt7603_mac_fill_rx(struct mt7603_dev *dev, struct sk_buff *skb)
 			return -EINVAL;
 	}
 
-	memset(status, 0, sizeof(*status));
 	skb_pull(skb, (u8 *) rxd - skb->data);
 
 	return 0;