@@ -127,6 +127,51 @@ static void rtw_fw_ra_report_handle(struct rtw_dev *rtwdev, u8 *payload,
rtw_iterate_stas_atomic(rtwdev, rtw_fw_ra_report_iter, &ra_data);
}
+struct rtw_beacon_filter_iter_data {
+ struct rtw_dev *rtwdev;
+ u8 *payload;
+};
+
+static void rtw_fw_bcn_filter_notify_vif_iter(void *data, u8 *mac,
+ struct ieee80211_vif *vif)
+{
+ struct rtw_beacon_filter_iter_data *iter_data = data;
+ struct rtw_dev *rtwdev = iter_data->rtwdev;
+ u8 *payload = iter_data->payload;
+ u8 type = GET_BCN_FILTER_NOTIFY_TYPE(payload);
+ u8 event = GET_BCN_FILTER_NOTIFY_EVENT(payload);
+ s8 sig = (s8)GET_BCN_FILTER_NOTIFY_RSSI(payload);
+
+ switch (type) {
+ case BCN_FILTER_NOTIFY_SIGNAL_CHANGE:
+ event = event ? NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH :
+ NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW;
+ ieee80211_cqm_rssi_notify(vif, event, sig, GFP_KERNEL);
+ break;
+ case BCN_FILTER_CONNECTION_LOSS:
+ ieee80211_connection_loss(vif);
+ break;
+ case BCN_FILTER_CONNECTED:
+ rtwdev->beacon_loss = false;
+ break;
+ case BCN_FILTER_NOTIFY_BEACON_LOSS:
+ rtwdev->beacon_loss = true;
+ rtw_leave_lps(rtwdev);
+ break;
+ }
+}
+
+static void rtw_fw_bcn_filter_notify(struct rtw_dev *rtwdev, u8 *payload,
+ u8 length)
+{
+ struct rtw_beacon_filter_iter_data dev_iter_data;
+
+ dev_iter_data.rtwdev = rtwdev;
+ dev_iter_data.payload = payload;
+ rtw_iterate_vifs(rtwdev, rtw_fw_bcn_filter_notify_vif_iter,
+ &dev_iter_data);
+}
+
void rtw_fw_c2h_cmd_handle(struct rtw_dev *rtwdev, struct sk_buff *skb)
{
struct rtw_c2h_cmd *c2h;
@@ -152,6 +197,9 @@ void rtw_fw_c2h_cmd_handle(struct rtw_dev *rtwdev, struct sk_buff *skb)
case C2H_WLAN_INFO:
rtw_coex_wl_fwdbginfo_notify(rtwdev, c2h->payload, len);
break;
+ case C2H_BCN_FILTER_NOTIFY:
+ rtw_fw_bcn_filter_notify(rtwdev, c2h->payload, len);
+ break;
case C2H_HALMAC:
rtw_fw_c2h_cmd_handle_ext(rtwdev, skb);
break;
@@ -527,6 +575,49 @@ void rtw_fw_update_wl_phy_info(struct rtw_dev *rtwdev)
rtw_fw_send_h2c_command(rtwdev, h2c_pkt);
}
+void rtw_fw_beacon_filter_config(struct rtw_dev *rtwdev, bool connect,
+ struct ieee80211_vif *vif)
+{
+ struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+ struct ieee80211_sta *sta = ieee80211_find_sta(vif, bss_conf->bssid);
+ static const u8 rssi_min = 0, rssi_max = 100, rssi_offset = 100;
+ struct rtw_sta_info *si =
+ sta ? (struct rtw_sta_info *)sta->drv_priv : NULL;
+ s32 threshold = bss_conf->cqm_rssi_thold + rssi_offset;
+ struct rtw_fw_state *fw = &rtwdev->fw;
+ u8 h2c_pkt[H2C_PKT_SIZE] = {0};
+
+ if (!(fw->feature & FW_FEATURE_BCN_FILTER))
+ return;
+
+ if (!connect) {
+ SET_H2C_CMD_ID_CLASS(h2c_pkt, H2C_CMD_BCN_FILTER_OFFLOAD_P1);
+ SET_BCN_FILTER_OFFLOAD_P1_ENABLE(h2c_pkt, connect);
+ rtw_fw_send_h2c_command(rtwdev, h2c_pkt);
+
+ return;
+ }
+ SET_H2C_CMD_ID_CLASS(h2c_pkt, H2C_CMD_BCN_FILTER_OFFLOAD_P0);
+ ether_addr_copy(&h2c_pkt[1], bss_conf->bssid);
+ rtw_fw_send_h2c_command(rtwdev, h2c_pkt);
+
+ memset(h2c_pkt, 0, sizeof(h2c_pkt));
+ threshold = clamp_t(s32, threshold, rssi_min, rssi_max);
+ SET_H2C_CMD_ID_CLASS(h2c_pkt, H2C_CMD_BCN_FILTER_OFFLOAD_P1);
+ SET_BCN_FILTER_OFFLOAD_P1_ENABLE(h2c_pkt, connect);
+ SET_BCN_FILTER_OFFLOAD_P1_OFFLOAD_MODE(h2c_pkt,
+ BCN_FILTER_OFFLOAD_MODE_DEFAULT);
+ SET_BCN_FILTER_OFFLOAD_P1_THRESHOLD(h2c_pkt, (u8)threshold);
+ SET_BCN_FILTER_OFFLOAD_P1_BCN_LOSS_CNT(h2c_pkt, BCN_LOSS_CNT);
+ if (si)
+ SET_BCN_FILTER_OFFLOAD_P1_MACID(h2c_pkt, si->mac_id);
+ else
+ rtw_warn(rtwdev, "CQM config with station not found\n");
+ SET_BCN_FILTER_OFFLOAD_P1_HYST(h2c_pkt, bss_conf->cqm_rssi_hyst);
+ SET_BCN_FILTER_OFFLOAD_P1_BCN_INTERVAL(h2c_pkt, bss_conf->beacon_int);
+ rtw_fw_send_h2c_command(rtwdev, h2c_pkt);
+}
+
void rtw_fw_set_pwr_mode(struct rtw_dev *rtwdev)
{
struct rtw_lps_conf *conf = &rtwdev->lps_conf;
@@ -24,6 +24,12 @@
#define DLFW_BLK_SIZE_LEGACY 4
#define FW_START_ADDR_LEGACY 0x1000
+#define BCN_LOSS_CNT 10
+#define BCN_FILTER_NOTIFY_SIGNAL_CHANGE 0
+#define BCN_FILTER_CONNECTION_LOSS 1
+#define BCN_FILTER_CONNECTED 2
+#define BCN_FILTER_NOTIFY_BEACON_LOSS 3
+
enum rtw_c2h_cmd_id {
C2H_CCX_TX_RPT = 0x03,
C2H_BT_INFO = 0x09,
@@ -32,6 +38,7 @@ enum rtw_c2h_cmd_id {
C2H_HW_FEATURE_REPORT = 0x19,
C2H_WLAN_INFO = 0x27,
C2H_WLAN_RFON = 0x32,
+ C2H_BCN_FILTER_NOTIFY = 0x36,
C2H_HW_FEATURE_DUMP = 0xfd,
C2H_HALMAC = 0xff,
};
@@ -78,9 +85,19 @@ enum rtw_fw_feature {
FW_FEATURE_LPS_C2H = BIT(1),
FW_FEATURE_LCLK = BIT(2),
FW_FEATURE_PG = BIT(3),
+ FW_FEATURE_BCN_FILTER = BIT(5),
FW_FEATURE_MAX = BIT(31),
};
+enum rtw_beacon_filter_offload_mode {
+ BCN_FILTER_OFFLOAD_MODE_0 = 0,
+ BCN_FILTER_OFFLOAD_MODE_1,
+ BCN_FILTER_OFFLOAD_MODE_2,
+ BCN_FILTER_OFFLOAD_MODE_3,
+
+ BCN_FILTER_OFFLOAD_MODE_DEFAULT = BCN_FILTER_OFFLOAD_MODE_1,
+};
+
struct rtw_coex_info_req {
u8 seq;
u8 op_code;
@@ -237,6 +254,10 @@ struct rtw_fw_hdr_legacy {
#define GET_RA_REPORT_BW(c2h_payload) (c2h_payload[6])
#define GET_RA_REPORT_MACID(c2h_payload) (c2h_payload[1])
+#define GET_BCN_FILTER_NOTIFY_TYPE(c2h_payload) (c2h_payload[1] & 0xf)
+#define GET_BCN_FILTER_NOTIFY_EVENT(c2h_payload) (c2h_payload[1] & 0x10)
+#define GET_BCN_FILTER_NOTIFY_RSSI(c2h_payload) (c2h_payload[2] - 100)
+
/* PKT H2C */
#define H2C_PKT_CMD_ID 0xFF
#define H2C_PKT_CATEGORY 0x01
@@ -345,6 +366,8 @@ static inline void rtw_h2c_pkt_set_header(u8 *h2c_pkt, u8 sub_id)
#define H2C_CMD_LPS_PG_INFO 0x2b
#define H2C_CMD_RA_INFO 0x40
#define H2C_CMD_RSSI_MONITOR 0x42
+#define H2C_CMD_BCN_FILTER_OFFLOAD_P0 0x56
+#define H2C_CMD_BCN_FILTER_OFFLOAD_P1 0x57
#define H2C_CMD_WL_PHY_INFO 0x58
#define H2C_CMD_COEX_TDMA_TYPE 0x60
@@ -381,6 +404,20 @@ static inline void rtw_h2c_pkt_set_header(u8 *h2c_pkt, u8 sub_id)
le32p_replace_bits((__le32 *)(h2c_pkt) + 0x01, value, GENMASK(15, 8))
#define SET_WL_PHY_INFO_RX_EVM(h2c_pkt, value) \
le32p_replace_bits((__le32 *)(h2c_pkt) + 0x01, value, GENMASK(23, 16))
+#define SET_BCN_FILTER_OFFLOAD_P1_MACID(h2c_pkt, value) \
+ le32p_replace_bits((__le32 *)(h2c_pkt) + 0x00, value, GENMASK(15, 8))
+#define SET_BCN_FILTER_OFFLOAD_P1_ENABLE(h2c_pkt, value) \
+ le32p_replace_bits((__le32 *)(h2c_pkt) + 0x00, value, BIT(16))
+#define SET_BCN_FILTER_OFFLOAD_P1_HYST(h2c_pkt, value) \
+ le32p_replace_bits((__le32 *)(h2c_pkt) + 0x00, value, GENMASK(20, 17))
+#define SET_BCN_FILTER_OFFLOAD_P1_OFFLOAD_MODE(h2c_pkt, value) \
+ le32p_replace_bits((__le32 *)(h2c_pkt) + 0x00, value, GENMASK(23, 21))
+#define SET_BCN_FILTER_OFFLOAD_P1_THRESHOLD(h2c_pkt, value) \
+ le32p_replace_bits((__le32 *)(h2c_pkt) + 0x00, value, GENMASK(31, 24))
+#define SET_BCN_FILTER_OFFLOAD_P1_BCN_LOSS_CNT(h2c_pkt, value) \
+ le32p_replace_bits((__le32 *)(h2c_pkt) + 0x01, value, GENMASK(3, 0))
+#define SET_BCN_FILTER_OFFLOAD_P1_BCN_INTERVAL(h2c_pkt, value) \
+ le32p_replace_bits((__le32 *)(h2c_pkt) + 0x01, value, GENMASK(13, 4))
#define SET_PWR_MODE_SET_MODE(h2c_pkt, value) \
le32p_replace_bits((__le32 *)(h2c_pkt) + 0x00, value, GENMASK(14, 8))
@@ -577,6 +614,8 @@ void rtw_fw_send_rssi_info(struct rtw_dev *rtwdev, struct rtw_sta_info *si);
void rtw_fw_send_ra_info(struct rtw_dev *rtwdev, struct rtw_sta_info *si);
void rtw_fw_media_status_report(struct rtw_dev *rtwdev, u8 mac_id, bool conn);
void rtw_fw_update_wl_phy_info(struct rtw_dev *rtwdev);
+void rtw_fw_beacon_filter_config(struct rtw_dev *rtwdev, bool connect,
+ struct ieee80211_vif *vif);
int rtw_fw_write_data_rsvd_page(struct rtw_dev *rtwdev, u16 pg_addr,
u8 *buf, u32 size);
void rtw_remove_rsvd_page(struct rtw_dev *rtwdev,
@@ -148,11 +148,15 @@ static int rtw_ops_add_interface(struct ieee80211_hw *hw,
{
struct rtw_dev *rtwdev = hw->priv;
struct rtw_vif *rtwvif = (struct rtw_vif *)vif->drv_priv;
+ struct rtw_fw_state *fw = &rtwdev->fw;
enum rtw_net_type net_type;
u32 config = 0;
u8 port = 0;
u8 bcn_ctrl = 0;
+ if (fw->feature & FW_FEATURE_BCN_FILTER)
+ vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER |
+ IEEE80211_VIF_SUPPORTS_CQM_RSSI;
rtwvif->port = port;
rtwvif->stats.tx_unicast = 0;
rtwvif->stats.rx_unicast = 0;
@@ -399,6 +403,8 @@ static void rtw_ops_bss_info_changed(struct ieee80211_hw *hw,
rtw_write32_clr(rtwdev, REG_FWHW_TXQ_CTRL,
BIT_EN_BCNQ_DL);
}
+ if (changed & BSS_CHANGED_CQM)
+ rtw_fw_beacon_filter_config(rtwdev, true, vif);
if (changed & BSS_CHANGED_MU_GROUPS)
rtw_chip_set_gid_table(rtwdev, vif, conf);
@@ -450,6 +456,7 @@ static int rtw_ops_sta_remove(struct ieee80211_hw *hw,
{
struct rtw_dev *rtwdev = hw->priv;
+ rtw_fw_beacon_filter_config(rtwdev, false, vif);
mutex_lock(&rtwdev->mutex);
rtw_sta_remove(rtwdev, sta, true);
mutex_unlock(&rtwdev->mutex);
@@ -239,7 +239,8 @@ static void rtw_watch_dog_work(struct work_struct *work)
* get that vif and check if device is having traffic more than the
* threshold.
*/
- if (rtwdev->ps_enabled && data.rtwvif && !ps_active)
+ if (rtwdev->ps_enabled && data.rtwvif && !ps_active &&
+ !rtwdev->beacon_loss)
rtw_enter_lps(rtwdev, data.rtwvif->port);
rtwdev->watch_dog_cnt++;
@@ -292,6 +293,7 @@ int rtw_sta_add(struct rtw_dev *rtwdev, struct ieee80211_sta *sta,
rtw_fw_media_status_report(rtwdev, si->mac_id, true);
rtwdev->sta_cnt++;
+ rtwdev->beacon_loss = false;
rtw_info(rtwdev, "sta %pM joined with macid %d\n",
sta->addr, si->mac_id);
@@ -1837,6 +1837,7 @@ struct rtw_dev {
/* lps power state & handler work */
struct rtw_lps_conf lps_conf;
bool ps_enabled;
+ bool beacon_loss;
struct completion lps_leave_check;
struct dentry *debugfs;