Commit 3782b69d authored by Lorenzo Bianconi's avatar Lorenzo Bianconi Committed by Felix Fietkau
Browse files

mt76: mt7915: introduce mt7915_mac_add_twt_setup routine 



Introduce individual TWT support to mt7915 in AP mode.
Implement the two following mac80211 callbacks:
- add_twt_setup
- twt_teardown_request

Tested-by: default avatarPeter Chiu <chui-hao.chiu@mediatek.com>
Tested-by: default avatarEvelyn Tsai <evelyn.tsai@mediatek.com>
Signed-off-by: default avatarLorenzo Bianconi <lorenzo@kernel.org>
Signed-off-by: default avatarFelix Fietkau <nbd@nbd.name>
parent 179090a5

drivers/net/wireless/mediatek/mt76/mt7915/init.c

+1 −0
Original line number Diff line number Diff line
@@ -901,6 +901,7 @@ int mt7915_register_device(struct mt7915_dev *dev) 
	INIT_DELAYED_WORK(&dev->mphy.mac_work, mt7915_mac_work);

	INIT_LIST_HEAD(&dev->sta_rc_list);

	INIT_LIST_HEAD(&dev->sta_poll_list);

	INIT_LIST_HEAD(&dev->twt_list);

	spin_lock_init(&dev->sta_poll_lock);



	init_waitqueue_head(&dev->reset_wait);

drivers/net/wireless/mediatek/mt76/mt7915/mac.c

+180 −0
Original line number Diff line number Diff line
@@ -6,6 +6,7 @@ 
#include "mt7915.h"

#include "../dma.h"

#include "mac.h"

#include "mcu.h"



#define to_rssi(field, rxv)	((FIELD_GET(field, rxv) - 220) / 2)
@@ -2242,3 +2243,182 @@ int mt7915_dfs_init_radar_detector(struct mt7915_phy *phy) 
	mt7915_dfs_stop_radar_detector(phy);

	return 0;

}



static int

mt7915_mac_twt_duration_align(int duration)

{

	return duration << 8;

}



static u64

mt7915_mac_twt_sched_list_add(struct mt7915_dev *dev,

			      struct mt7915_twt_flow *flow)

{

	struct mt7915_twt_flow *iter, *iter_next;

	u32 duration = flow->duration << 8;

	u64 start_tsf;



	iter = list_first_entry_or_null(&dev->twt_list,

					struct mt7915_twt_flow, list);

	if (!iter || !iter->sched || iter->start_tsf > duration) {

		/* add flow as first entry in the list */

		list_add(&flow->list, &dev->twt_list);

		return 0;

	}



	list_for_each_entry_safe(iter, iter_next, &dev->twt_list, list) {

		start_tsf = iter->start_tsf +

			    mt7915_mac_twt_duration_align(iter->duration);

		if (list_is_last(&iter->list, &dev->twt_list))

			break;



		if (!iter_next->sched ||

		    iter_next->start_tsf > start_tsf + duration) {

			list_add(&flow->list, &iter->list);

			goto out;

		}

	}



	/* add flow as last entry in the list */

	list_add_tail(&flow->list, &dev->twt_list);

out:

	return start_tsf;

}



static int mt7915_mac_check_twt_req(struct ieee80211_twt_setup *twt)

{

	struct ieee80211_twt_params *twt_agrt;

	u64 interval, duration;

	u16 mantissa;

	u8 exp;



	/* only individual agreement supported */

	if (twt->control & IEEE80211_TWT_CONTROL_NEG_TYPE_BROADCAST)

		return -EOPNOTSUPP;



	/* only 256us unit supported */

	if (twt->control & IEEE80211_TWT_CONTROL_WAKE_DUR_UNIT)

		return -EOPNOTSUPP;



	twt_agrt = (struct ieee80211_twt_params *)twt->params;



	/* explicit agreement not supported */

	if (!(twt_agrt->req_type & cpu_to_le16(IEEE80211_TWT_REQTYPE_IMPLICIT)))

		return -EOPNOTSUPP;



	exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP,

			le16_to_cpu(twt_agrt->req_type));

	mantissa = le16_to_cpu(twt_agrt->mantissa);

	duration = twt_agrt->min_twt_dur << 8;



	interval = (u64)mantissa << exp;

	if (interval < duration)

		return -EOPNOTSUPP;



	return 0;

}



void mt7915_mac_add_twt_setup(struct ieee80211_hw *hw,

			      struct ieee80211_sta *sta,

			      struct ieee80211_twt_setup *twt)

{

	enum ieee80211_twt_setup_cmd setup_cmd = TWT_SETUP_CMD_REJECT;

	struct mt7915_sta *msta = (struct mt7915_sta *)sta->drv_priv;

	struct ieee80211_twt_params *twt_agrt = (void *)twt->params;

	u16 req_type = le16_to_cpu(twt_agrt->req_type);

	enum ieee80211_twt_setup_cmd sta_setup_cmd;

	struct mt7915_dev *dev = mt7915_hw_dev(hw);

	struct mt7915_twt_flow *flow;

	int flowid, table_id;

	u8 exp;



	if (mt7915_mac_check_twt_req(twt))

		goto out;



	mutex_lock(&dev->mt76.mutex);



	if (dev->twt.n_agrt == MT7915_MAX_TWT_AGRT)

		goto unlock;



	if (hweight8(msta->twt.flowid_mask) == ARRAY_SIZE(msta->twt.flow))

		goto unlock;



	flowid = ffs(~msta->twt.flowid_mask) - 1;

	le16p_replace_bits(&twt_agrt->req_type, flowid,

			   IEEE80211_TWT_REQTYPE_FLOWID);



	table_id = ffs(~dev->twt.table_mask) - 1;

	exp = FIELD_GET(IEEE80211_TWT_REQTYPE_WAKE_INT_EXP, req_type);

	sta_setup_cmd = FIELD_GET(IEEE80211_TWT_REQTYPE_SETUP_CMD, req_type);



	flow = &msta->twt.flow[flowid];

	memset(flow, 0, sizeof(*flow));

	INIT_LIST_HEAD(&flow->list);

	flow->wcid = msta->wcid.idx;

	flow->table_id = table_id;

	flow->id = flowid;

	flow->duration = twt_agrt->min_twt_dur;

	flow->mantissa = twt_agrt->mantissa;

	flow->exp = exp;

	flow->protection = !!(req_type & IEEE80211_TWT_REQTYPE_PROTECTION);

	flow->flowtype = !!(req_type & IEEE80211_TWT_REQTYPE_FLOWTYPE);

	flow->trigger = !!(req_type & IEEE80211_TWT_REQTYPE_TRIGGER);



	if (sta_setup_cmd == TWT_SETUP_CMD_REQUEST ||

	    sta_setup_cmd == TWT_SETUP_CMD_SUGGEST) {

		u64 interval = (u64)le16_to_cpu(twt_agrt->mantissa) << exp;

		u64 flow_tsf, curr_tsf;

		u32 rem;



		flow->sched = true;

		flow->start_tsf = mt7915_mac_twt_sched_list_add(dev, flow);

		curr_tsf = __mt7915_get_tsf(hw, msta->vif);

		div_u64_rem(curr_tsf - flow->start_tsf, interval, &rem);

		flow_tsf = curr_tsf + interval - rem;

		twt_agrt->twt = cpu_to_le64(flow_tsf);

	} else {

		list_add_tail(&flow->list, &dev->twt_list);

	}

	flow->tsf = le64_to_cpu(twt_agrt->twt);



	if (mt7915_mcu_twt_agrt_update(dev, msta->vif, flow, MCU_TWT_AGRT_ADD))

		goto unlock;



	setup_cmd = TWT_SETUP_CMD_ACCEPT;

	dev->twt.table_mask |= BIT(table_id);

	msta->twt.flowid_mask |= BIT(flowid);

	dev->twt.n_agrt++;



unlock:

	mutex_unlock(&dev->mt76.mutex);

out:

	le16p_replace_bits(&twt_agrt->req_type, setup_cmd,

			   IEEE80211_TWT_REQTYPE_SETUP_CMD);

	twt->control = (twt->control & IEEE80211_TWT_CONTROL_WAKE_DUR_UNIT) |

		       (twt->control & IEEE80211_TWT_CONTROL_RX_DISABLED);

}



void mt7915_mac_twt_teardown_flow(struct mt7915_dev *dev,

				  struct mt7915_sta *msta,

				  u8 flowid)

{

	struct mt7915_twt_flow *flow;



	lockdep_assert_held(&dev->mt76.mutex);



	if (flowid >= ARRAY_SIZE(msta->twt.flow))

		return;



	if (!(msta->twt.flowid_mask & BIT(flowid)))

		return;



	flow = &msta->twt.flow[flowid];

	if (mt7915_mcu_twt_agrt_update(dev, msta->vif, flow,

				       MCU_TWT_AGRT_DELETE))

		return;



	list_del_init(&flow->list);

	msta->twt.flowid_mask &= ~BIT(flowid);

	dev->twt.table_mask &= ~BIT(flow->table_id);

	dev->twt.n_agrt--;

}

drivers/net/wireless/mediatek/mt76/mt7915/main.c

+19 −0
Original line number Diff line number Diff line
@@ -670,6 +670,7 @@ void mt7915_mac_sta_remove(struct mt76_dev *mdev, struct ieee80211_vif *vif, 
{

	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);

	struct mt7915_sta *msta = (struct mt7915_sta *)sta->drv_priv;

	int i;



	mt7915_mcu_add_sta_adv(dev, vif, sta, false);

	mt7915_mcu_add_sta(dev, vif, sta, false);
@@ -677,6 +678,9 @@ void mt7915_mac_sta_remove(struct mt76_dev *mdev, struct ieee80211_vif *vif, 
	mt7915_mac_wtbl_update(dev, msta->wcid.idx,

			       MT_WTBL_UPDATE_ADM_COUNT_CLEAR);



	for (i = 0; i < ARRAY_SIZE(msta->twt.flow); i++)

		mt7915_mac_twt_teardown_flow(dev, msta, i);



	spin_lock_bh(&dev->sta_poll_lock);

	if (!list_empty(&msta->poll_list))

		list_del_init(&msta->poll_list);
@@ -1316,6 +1320,19 @@ void mt7915_get_et_stats(struct ieee80211_hw *hw, 
			ei, (int)MT7915_SSTATS_LEN);

}



static void

mt7915_twt_teardown_request(struct ieee80211_hw *hw,

			    struct ieee80211_sta *sta,

			    u8 flowid)

{

	struct mt7915_sta *msta = (struct mt7915_sta *)sta->drv_priv;

	struct mt7915_dev *dev = mt7915_hw_dev(hw);



	mutex_lock(&dev->mt76.mutex);

	mt7915_mac_twt_teardown_flow(dev, msta, flowid);

	mutex_unlock(&dev->mt76.mutex);

}



const struct ieee80211_ops mt7915_ops = {

	.tx = mt7915_tx,

	.start = mt7915_start,
@@ -1354,6 +1371,8 @@ const struct ieee80211_ops mt7915_ops = { 
	.sta_statistics = mt7915_sta_statistics,

	.sta_set_4addr = mt7915_sta_set_4addr,

	.sta_set_decap_offload = mt7915_sta_set_decap_offload,

	.add_twt_setup = mt7915_mac_add_twt_setup,

	.twt_teardown_request = mt7915_twt_teardown_request,

	CFG80211_TESTMODE_CMD(mt76_testmode_cmd)

	CFG80211_TESTMODE_DUMP(mt76_testmode_dump)

#ifdef CONFIG_MAC80211_DEBUGFS

drivers/net/wireless/mediatek/mt76/mt7915/mt7915.h

+20 −0
Original line number Diff line number Diff line
@@ -41,6 +41,9 @@ 


#define MT7915_SKU_RATE_NUM		161



#define MT7915_MAX_TWT_AGRT		16

#define MT7915_MAX_STA_TWT_AGRT		8



struct mt7915_vif;

struct mt7915_sta;

struct mt7915_dfs_pulse;
@@ -106,6 +109,11 @@ struct mt7915_sta { 
	struct mt7915_sta_stats stats;



	struct mt7915_sta_key_conf bip;



	struct {

		u8 flowid_mask;

		struct mt7915_twt_flow flow[MT7915_MAX_STA_TWT_AGRT];

	} twt;

};



struct mt7915_vif {
@@ -247,6 +255,7 @@ struct mt7915_dev { 


	struct list_head sta_rc_list;

	struct list_head sta_poll_list;

	struct list_head twt_list;

	spinlock_t sta_poll_lock;



	u32 hw_pattern;
@@ -257,6 +266,11 @@ struct mt7915_dev { 
	bool ibf;



	void *cal;



	struct {

		u8 table_mask;

		u8 n_agrt;

	} twt;

};



enum {
@@ -475,6 +489,12 @@ void mt7915_mac_work(struct work_struct *work); 
void mt7915_mac_reset_work(struct work_struct *work);

void mt7915_mac_sta_rc_work(struct work_struct *work);

int mt7915_mmio_init(struct mt76_dev *mdev, void __iomem *mem_base, int irq);

void mt7915_mac_twt_teardown_flow(struct mt7915_dev *dev,

				  struct mt7915_sta *msta,

				  u8 flowid);

void mt7915_mac_add_twt_setup(struct ieee80211_hw *hw,

			      struct ieee80211_sta *sta,

			      struct ieee80211_twt_setup *twt);

int mt7915_tx_prepare_skb(struct mt76_dev *mdev, void *txwi_ptr,

			  enum mt76_txq_id qid, struct mt76_wcid *wcid,

			  struct ieee80211_sta *sta,