From b1f0f4ab8ba05879b5f67d007c8a83ba8bd1b3c4 Mon Sep 17 00:00:00 2001
From: Felix Fietkau <nbd@openwrt.org>
Date: Mon, 1 Feb 2016 00:52:19 +0100
Subject: [PATCH] mt7603: init sta wtbl entries

Signed-off-by: Felix Fietkau <nbd@openwrt.org>
---
 mt7603.h      |  1 +
 mt7603_mac.c  | 16 ++++++++++++++++
 mt7603_main.c | 34 ++++++++++++++++++++++++++++++++--
 3 files changed, 49 insertions(+), 2 deletions(-)

diff --git a/mt7603.h b/mt7603.h
index 249a3e89..8e814841 100644
--- a/mt7603.h
+++ b/mt7603.h
@@ -137,6 +137,7 @@ int mt7603_mcu_exit(struct mt7603_dev *dev);
 
 void mt7603_wtbl_init(struct mt7603_dev *dev, int idx, const u8 *addr);
 void mt7603_wtbl_clear(struct mt7603_dev *dev, int idx);
+void mt7603_wtbl_update_cap(struct mt7603_dev *dev, struct ieee80211_sta *sta);
 
 int mt7603_mac_write_txwi(struct mt76_dev *mdev, void *txwi_ptr,
 			  struct sk_buff *skb, struct mt76_wcid *wcid,
diff --git a/mt7603_mac.c b/mt7603_mac.c
index d34343d7..495fedbf 100644
--- a/mt7603_mac.c
+++ b/mt7603_mac.c
@@ -115,6 +115,22 @@ void mt7603_wtbl_clear(struct mt7603_dev *dev, int idx)
 	mt7603_wtbl_update(dev, idx, MT_WTBL_UPDATE_ADM_COUNT_CLEAR);
 }
 
+void mt7603_wtbl_update_cap(struct mt7603_dev *dev, struct ieee80211_sta *sta)
+{
+	struct mt7603_sta *msta = (struct mt7603_sta *) sta->drv_priv;
+	u32 addr = mt7603_wtbl2_addr(msta->wcid.idx);
+	u32 val;
+
+	val = mt76_rr(dev, addr + 9 * 4);
+	val &= ~(MT_WTBL2_W9_SHORT_GI_20 | MT_WTBL2_W9_SHORT_GI_40 |
+		 MT_WTBL2_W9_SHORT_GI_80);
+	if (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20)
+		val |= MT_WTBL2_W9_SHORT_GI_20;
+	if (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40)
+		val |= MT_WTBL2_W9_SHORT_GI_40;
+	mt76_wr(dev, addr + 9 * 4, val);
+}
+
 static int
 mt7603_get_rate(struct mt7603_dev *dev, struct ieee80211_supported_band *sband,
 		int idx, bool cck)
diff --git a/mt7603_main.c b/mt7603_main.c
index 8d597e4f..b0a1f337 100644
--- a/mt7603_main.c
+++ b/mt7603_main.c
@@ -158,14 +158,44 @@ static int
 mt7603_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	     struct ieee80211_sta *sta)
 {
-	return -EINVAL;
+	struct mt7603_dev *dev = hw->priv;
+	struct mt7603_sta *msta = (struct mt7603_sta *) sta->drv_priv;
+	int ret, idx;
+
+	mutex_lock(&dev->mutex);
+
+	idx = mt76_wcid_alloc(dev->wcid_mask, ARRAY_SIZE(dev->wcid) - 1);
+	if (idx < 0) {
+		ret = -ENOSPC;
+		goto out;
+	}
+
+	msta->wcid.idx = idx;
+	mt7603_wtbl_update_cap(dev, sta);
+
+	mt7603_wtbl_init(dev, idx, sta->addr);
+	rcu_assign_pointer(dev->wcid[idx], &msta->wcid);
+
+out:
+	mutex_unlock(&dev->mutex);
+
+	return ret;
 }
 
 static int
 mt7603_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		struct ieee80211_sta *sta)
 {
-	return -EINVAL;
+	struct mt7603_dev *dev = hw->priv;
+	struct mt7603_sta *msta = (struct mt7603_sta *) sta->drv_priv;
+	int idx = msta->wcid.idx;
+
+	mutex_lock(&dev->mutex);
+	rcu_assign_pointer(dev->wcid[idx], NULL);
+	mt7603_wtbl_clear(dev, idx);
+	mutex_unlock(&dev->mutex);
+
+	return 0;
 }
 
 static void