drivers/net/wireless/realtek/rtw88/wow.c

   1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
   2 /* Copyright(c) 2018-2019  Realtek Corporation
   3  */
   4
   5 #include "main.h"
   6 #include "fw.h"
   7 #include "wow.h"
   8 #include "reg.h"
   9 #include "debug.h"
  10 #include "mac.h"
  11 #include "ps.h"
  12
  13 static void rtw_wow_show_wakeup_reason(struct rtw_dev *rtwdev)
  14 {
  15         u8 reason;
  16
  17         reason = rtw_read8(rtwdev, REG_WOWLAN_WAKE_REASON);
  18
  19         if (reason == RTW_WOW_RSN_RX_DEAUTH)
  20                 rtw_dbg(rtwdev, RTW_DBG_WOW, "WOW: Rx deauth\n");
  21         else if (reason == RTW_WOW_RSN_DISCONNECT)
  22                 rtw_dbg(rtwdev, RTW_DBG_WOW, "WOW: AP is off\n");
  23         else if (reason == RTW_WOW_RSN_RX_MAGIC_PKT)
  24                 rtw_dbg(rtwdev, RTW_DBG_WOW, "WOW: Rx magic packet\n");
  25         else if (reason == RTW_WOW_RSN_RX_GTK_REKEY)
  26                 rtw_dbg(rtwdev, RTW_DBG_WOW, "WOW: Rx gtk rekey\n");
  27         else if (reason == RTW_WOW_RSN_RX_PTK_REKEY)
  28                 rtw_dbg(rtwdev, RTW_DBG_WOW, "WOW: Rx ptk rekey\n");
  29         else if (reason == RTW_WOW_RSN_RX_PATTERN_MATCH)
  30                 rtw_dbg(rtwdev, RTW_DBG_WOW, "WOW: Rx pattern match packet\n");
  31         else if (reason == RTW_WOW_RSN_RX_NLO)
  32                 rtw_dbg(rtwdev, RTW_DBG_WOW, "Rx NLO\n");
  33         else
  34                 rtw_warn(rtwdev, "Unknown wakeup reason %x\n", reason);
  35 }
  36
  37 static void rtw_wow_pattern_write_cam(struct rtw_dev *rtwdev, u8 addr,
  38                                       u32 wdata)
  39 {
  40         rtw_write32(rtwdev, REG_WKFMCAM_RWD, wdata);
  41         rtw_write32(rtwdev, REG_WKFMCAM_CMD, BIT_WKFCAM_POLLING_V1 |
  42                     BIT_WKFCAM_WE | BIT_WKFCAM_ADDR_V2(addr));
  43
  44         if (!check_hw_ready(rtwdev, REG_WKFMCAM_CMD, BIT_WKFCAM_POLLING_V1, 0))
  45                 rtw_err(rtwdev, "failed to write pattern cam\n");
  46 }
  47
  48 static void rtw_wow_pattern_write_cam_ent(struct rtw_dev *rtwdev, u8 id,
  49                                           struct rtw_wow_pattern *rtw_pattern)
  50 {
  51         int i;
  52         u8 addr;
  53         u32 wdata;
  54
  55         for (i = 0; i < RTW_MAX_PATTERN_MASK_SIZE / 4; i++) {
  56                 addr = (id << 3) + i;
  57                 wdata = rtw_pattern->mask[i * 4];
  58                 wdata |= rtw_pattern->mask[i * 4 + 1] << 8;
  59                 wdata |= rtw_pattern->mask[i * 4 + 2] << 16;
  60                 wdata |= rtw_pattern->mask[i * 4 + 3] << 24;
  61                 rtw_wow_pattern_write_cam(rtwdev, addr, wdata);
  62         }
  63
  64         wdata = rtw_pattern->crc;
  65         addr = (id << 3) + RTW_MAX_PATTERN_MASK_SIZE / 4;
  66
  67         switch (rtw_pattern->type) {
  68         case RTW_PATTERN_BROADCAST:
  69                 wdata |= BIT_WKFMCAM_BC | BIT_WKFMCAM_VALID;
  70                 break;
  71         case RTW_PATTERN_MULTICAST:
  72                 wdata |= BIT_WKFMCAM_MC | BIT_WKFMCAM_VALID;
  73                 break;
  74         case RTW_PATTERN_UNICAST:
  75                 wdata |= BIT_WKFMCAM_UC | BIT_WKFMCAM_VALID;
  76                 break;
  77         default:
  78                 break;
  79         }
  80         rtw_wow_pattern_write_cam(rtwdev, addr, wdata);
  81 }
  82
  83 /* RTK internal CRC16 for Pattern Cam */
  84 static u16 __rtw_cal_crc16(u8 data, u16 crc)
  85 {
  86         u8 shift_in, data_bit;
  87         u8 crc_bit4, crc_bit11, crc_bit15;
  88         u16 crc_result;
  89         int index;
  90
  91         for (index = 0; index < 8; index++) {
  92                 crc_bit15 = ((crc & BIT(15)) ? 1 : 0);
  93                 data_bit = (data & (BIT(0) << index) ? 1 : 0);
  94                 shift_in = crc_bit15 ^ data_bit;
  95
  96                 crc_result = crc << 1;
  97
  98                 if (shift_in == 0)
  99                         crc_result &= (~BIT(0));
 100                 else
 101                         crc_result |= BIT(0);
 102
 103                 crc_bit11 = ((crc & BIT(11)) ? 1 : 0) ^ shift_in;
 104
 105                 if (crc_bit11 == 0)
 106                         crc_result &= (~BIT(12));
 107                 else
 108                         crc_result |= BIT(12);
 109
 110                 crc_bit4 = ((crc & BIT(4)) ? 1 : 0) ^ shift_in;
 111
 112                 if (crc_bit4 == 0)
 113                         crc_result &= (~BIT(5));
 114                 else
 115                         crc_result |= BIT(5);
 116
 117                 crc = crc_result;
 118         }
 119         return crc;
 120 }
 121
 122 static u16 rtw_calc_crc(u8 *pdata, int length)
 123 {
 124         u16 crc = 0xffff;
 125         int i;
 126
 127         for (i = 0; i < length; i++)
 128                 crc = __rtw_cal_crc16(pdata[i], crc);
 129
 130         /* get 1' complement */
 131         return ~crc;
 132 }
 133
 134 static void rtw_wow_pattern_generate(struct rtw_dev *rtwdev,
 135                                      struct rtw_vif *rtwvif,
 136                                      const struct cfg80211_pkt_pattern *pkt_pattern,
 137                                      struct rtw_wow_pattern *rtw_pattern)
 138 {
 139         const u8 *mask;
 140         const u8 *pattern;
 141         u8 mask_hw[RTW_MAX_PATTERN_MASK_SIZE] = {0};
 142         u8 content[RTW_MAX_PATTERN_SIZE] = {0};
 143         u8 mac_addr[ETH_ALEN] = {0};
 144         u8 mask_len;
 145         u16 count;
 146         int len;
 147         int i;
 148
 149         pattern = pkt_pattern->pattern;
 150         len = pkt_pattern->pattern_len;
 151         mask = pkt_pattern->mask;
 152
 153         ether_addr_copy(mac_addr, rtwvif->mac_addr);
 154         memset(rtw_pattern, 0, sizeof(*rtw_pattern));
 155
 156         mask_len = DIV_ROUND_UP(len, 8);
 157
 158         if (is_broadcast_ether_addr(pattern))
 159                 rtw_pattern->type = RTW_PATTERN_BROADCAST;
 160         else if (is_multicast_ether_addr(pattern))
 161                 rtw_pattern->type = RTW_PATTERN_MULTICAST;
 162         else if (ether_addr_equal(pattern, mac_addr))
 163                 rtw_pattern->type = RTW_PATTERN_UNICAST;
 164         else
 165                 rtw_pattern->type = RTW_PATTERN_INVALID;
 166
 167         /* translate mask from os to mask for hw
 168          * pattern from OS uses 'ethenet frame', like this:
 169          * |    6   |    6   |   2  |     20    |  Variable  |  4  |
 170          * |--------+--------+------+-----------+------------+-----|
 171          * |    802.3 Mac Header    | IP Header | TCP Packet | FCS |
 172          * |   DA   |   SA   | Type |
 173          *
 174          * BUT, packet catched by our HW is in '802.11 frame', begin from LLC
 175          * |     24 or 30      |    6   |   2  |     20    |  Variable  |  4  |
 176          * |-------------------+--------+------+-----------+------------+-----|
 177          * | 802.11 MAC Header |       LLC     | IP Header | TCP Packet | FCS |
 178          *                     | Others | Tpye |
 179          *
 180          * Therefore, we need translate mask_from_OS to mask_to_hw.
 181          * We should left-shift mask by 6 bits, then set the new bit[0~5] = 0,
 182          * because new mask[0~5] means 'SA', but our HW packet begins from LLC,
 183          * bit[0~5] corresponds to first 6 Bytes in LLC, they just don't match.
 184          */
 185
 186         /* Shift 6 bits */
 187         for (i = 0; i < mask_len - 1; i++) {
 188                 mask_hw[i] = u8_get_bits(mask[i], GENMASK(7, 6));
 189                 mask_hw[i] |= u8_get_bits(mask[i + 1], GENMASK(5, 0)) << 2;
 190         }
 191         mask_hw[i] = u8_get_bits(mask[i], GENMASK(7, 6));
 192
 193         /* Set bit 0-5 to zero */
 194         mask_hw[0] &= (~GENMASK(5, 0));
 195
 196         memcpy(rtw_pattern->mask, mask_hw, RTW_MAX_PATTERN_MASK_SIZE);
 197
 198         /* To get the wake up pattern from the mask.
 199          * We do not count first 12 bits which means
 200          * DA[6] and SA[6] in the pattern to match HW design.
 201          */
 202         count = 0;
 203         for (i = 12; i < len; i++) {
 204                 if ((mask[i / 8] >> (i % 8)) & 0x01) {
 205                         content[count] = pattern[i];
 206                         count++;
 207                 }
 208         }
 209
 210         rtw_pattern->crc = rtw_calc_crc(content, count);
 211 }
 212
 213 static void rtw_wow_pattern_clear_cam(struct rtw_dev *rtwdev)
 214 {
 215         bool ret;
 216
 217         rtw_write32(rtwdev, REG_WKFMCAM_CMD, BIT_WKFCAM_POLLING_V1 |
 218                     BIT_WKFCAM_CLR_V1);
 219
 220         ret = check_hw_ready(rtwdev, REG_WKFMCAM_CMD, BIT_WKFCAM_POLLING_V1, 0);
 221         if (!ret)
 222                 rtw_err(rtwdev, "failed to clean pattern cam\n");
 223 }
 224
 225 static void rtw_wow_pattern_write(struct rtw_dev *rtwdev)
 226 {
 227         struct rtw_wow_param *rtw_wow = &rtwdev->wow;
 228         struct rtw_wow_pattern *rtw_pattern = rtw_wow->patterns;
 229         int i = 0;
 230
 231         for (i = 0; i < rtw_wow->pattern_cnt; i++)
 232                 rtw_wow_pattern_write_cam_ent(rtwdev, i, rtw_pattern + i);
 233 }
 234
 235 static void rtw_wow_pattern_clear(struct rtw_dev *rtwdev)
 236 {
 237         struct rtw_wow_param *rtw_wow = &rtwdev->wow;
 238
 239         rtw_wow_pattern_clear_cam(rtwdev);
 240
 241         rtw_wow->pattern_cnt = 0;
 242         memset(rtw_wow->patterns, 0, sizeof(rtw_wow->patterns));
 243 }
 244
 245 static void rtw_wow_bb_stop(struct rtw_dev *rtwdev)
 246 {
 247         struct rtw_wow_param *rtw_wow = &rtwdev->wow;
 248
 249         /* wait 100ms for firmware to finish TX */
 250         msleep(100);
 251
 252         if (!rtw_read32_mask(rtwdev, REG_BCNQ_INFO, BIT_MGQ_CPU_EMPTY))
 253                 rtw_warn(rtwdev, "Wrong status of MGQ_CPU empty!\n");
 254
 255         rtw_wow->txpause = rtw_read8(rtwdev, REG_TXPAUSE);
 256         rtw_write8(rtwdev, REG_TXPAUSE, 0xff);
 257         rtw_write8_clr(rtwdev, REG_SYS_FUNC_EN, BIT_FEN_BB_RSTB);
 258 }
 259
 260 static void rtw_wow_bb_start(struct rtw_dev *rtwdev)
 261 {
 262         struct rtw_wow_param *rtw_wow = &rtwdev->wow;
 263
 264         rtw_write8_set(rtwdev, REG_SYS_FUNC_EN, BIT_FEN_BB_RSTB);
 265         rtw_write8(rtwdev, REG_TXPAUSE, rtw_wow->txpause);
 266 }
 267
 268 static void rtw_wow_rx_dma_stop(struct rtw_dev *rtwdev)
 269 {
 270         /* wait 100ms for HW to finish rx dma */
 271         msleep(100);
 272
 273         rtw_write32_set(rtwdev, REG_RXPKT_NUM, BIT_RW_RELEASE);
 274
 275         if (!check_hw_ready(rtwdev, REG_RXPKT_NUM, BIT_RXDMA_IDLE, 1))
 276                 rtw_err(rtwdev, "failed to stop rx dma\n");
 277 }
 278
 279 static void rtw_wow_rx_dma_start(struct rtw_dev *rtwdev)
 280 {
 281         rtw_write32_clr(rtwdev, REG_RXPKT_NUM, BIT_RW_RELEASE);
 282 }
 283
 284 static int rtw_wow_check_fw_status(struct rtw_dev *rtwdev, bool wow_enable)
 285 {
 286         /* wait 100ms for wow firmware to finish work */
 287         msleep(100);
 288
 289         if (wow_enable) {
 290                 if (rtw_read8(rtwdev, REG_WOWLAN_WAKE_REASON))
 291                         goto wow_fail;
 292         } else {
 293                 if (rtw_read32_mask(rtwdev, REG_FE1IMR, BIT_FS_RXDONE) ||
 294                     rtw_read32_mask(rtwdev, REG_RXPKT_NUM, BIT_RW_RELEASE))
 295                         goto wow_fail;
 296         }
 297
 298         return 0;
 299
 300 wow_fail:
 301         rtw_err(rtwdev, "failed to check wow status %s\n",
 302                 wow_enable ? "enabled" : "disabled");
 303         return -EBUSY;
 304 }
 305
 306 static void rtw_wow_fw_security_type_iter(struct ieee80211_hw *hw,
 307                                           struct ieee80211_vif *vif,
 308                                           struct ieee80211_sta *sta,
 309                                           struct ieee80211_key_conf *key,
 310                                           void *data)
 311 {
 312         struct rtw_fw_key_type_iter_data *iter_data = data;
 313         struct rtw_dev *rtwdev = hw->priv;
 314         u8 hw_key_type;
 315
 316         if (vif != rtwdev->wow.wow_vif)
 317                 return;
 318
 319         switch (key->cipher) {
 320         case WLAN_CIPHER_SUITE_WEP40:
 321                 hw_key_type = RTW_CAM_WEP40;
 322                 break;
 323         case WLAN_CIPHER_SUITE_WEP104:
 324                 hw_key_type = RTW_CAM_WEP104;
 325                 break;
 326         case WLAN_CIPHER_SUITE_TKIP:
 327                 hw_key_type = RTW_CAM_TKIP;
 328                 key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
 329                 break;
 330         case WLAN_CIPHER_SUITE_CCMP:
 331                 hw_key_type = RTW_CAM_AES;
 332                 key->flags |= IEEE80211_KEY_FLAG_SW_MGMT_TX;
 333                 break;
 334         default:
 335                 rtw_err(rtwdev, "Unsupported key type for wowlan mode\n");
 336                 hw_key_type = 0;
 337                 break;
 338         }
 339
 340         if (sta)
 341                 iter_data->pairwise_key_type = hw_key_type;
 342         else
 343                 iter_data->group_key_type = hw_key_type;
 344 }
 345
 346 static void rtw_wow_fw_security_type(struct rtw_dev *rtwdev)
 347 {
 348         struct rtw_fw_key_type_iter_data data = {};
 349         struct ieee80211_vif *wow_vif = rtwdev->wow.wow_vif;
 350
 351         data.rtwdev = rtwdev;
 352         rtw_iterate_keys(rtwdev, wow_vif,
 353                          rtw_wow_fw_security_type_iter, &data);
 354         rtw_fw_set_aoac_global_info_cmd(rtwdev, data.pairwise_key_type,
 355                                         data.group_key_type);
 356 }
 357
 358 static int rtw_wow_fw_start(struct rtw_dev *rtwdev)
 359 {
 360         if (rtw_wow_mgd_linked(rtwdev)) {
 361                 rtw_send_rsvd_page_h2c(rtwdev);
 362                 rtw_wow_pattern_write(rtwdev);
 363                 rtw_wow_fw_security_type(rtwdev);
 364                 rtw_fw_set_disconnect_decision_cmd(rtwdev, true);
 365                 rtw_fw_set_keep_alive_cmd(rtwdev, true);
 366         } else if (rtw_wow_no_link(rtwdev)) {
 367                 rtw_fw_set_nlo_info(rtwdev, true);
 368                 rtw_fw_update_pkt_probe_req(rtwdev, NULL);
 369                 rtw_fw_channel_switch(rtwdev, true);
 370         }
 371
 372         rtw_fw_set_wowlan_ctrl_cmd(rtwdev, true);
 373         rtw_fw_set_remote_wake_ctrl_cmd(rtwdev, true);
 374
 375         return rtw_wow_check_fw_status(rtwdev, true);
 376 }
 377
 378 static int rtw_wow_fw_stop(struct rtw_dev *rtwdev)
 379 {
 380         if (rtw_wow_mgd_linked(rtwdev)) {
 381                 rtw_fw_set_disconnect_decision_cmd(rtwdev, false);
 382                 rtw_fw_set_keep_alive_cmd(rtwdev, false);
 383                 rtw_wow_pattern_clear(rtwdev);
 384         } else if (rtw_wow_no_link(rtwdev)) {
 385                 rtw_fw_channel_switch(rtwdev, false);
 386                 rtw_fw_set_nlo_info(rtwdev, false);
 387         }
 388
 389         rtw_fw_set_wowlan_ctrl_cmd(rtwdev, false);
 390         rtw_fw_set_remote_wake_ctrl_cmd(rtwdev, false);
 391
 392         return rtw_wow_check_fw_status(rtwdev, false);
 393 }
 394
 395 static void rtw_wow_avoid_reset_mac(struct rtw_dev *rtwdev)
 396 {
 397         /* When resuming from wowlan mode, some hosts issue signal
 398          * (PCIE: PREST, USB: SE0RST) to device, and lead to reset
 399          * mac core. If it happens, the connection to AP will be lost.
 400          * Setting REG_RSV_CTRL Register can avoid this process.
 401          */
 402         switch (rtw_hci_type(rtwdev)) {
 403         case RTW_HCI_TYPE_PCIE:
 404         case RTW_HCI_TYPE_USB:
 405                 rtw_write8(rtwdev, REG_RSV_CTRL, BIT_WLOCK_1C_B6);
 406                 rtw_write8(rtwdev, REG_RSV_CTRL,
 407                            BIT_WLOCK_1C_B6 | BIT_R_DIS_PRST);
 408                 break;
 409         default:
 410                 rtw_warn(rtwdev, "Unsupported hci type to disable reset MAC\n");
 411                 break;
 412         }
 413 }
 414
 415 static void rtw_wow_fw_media_status_iter(void *data, struct ieee80211_sta *sta)
 416 {
 417         struct rtw_sta_info *si = (struct rtw_sta_info *)sta->drv_priv;
 418         struct rtw_fw_media_status_iter_data *iter_data = data;
 419         struct rtw_dev *rtwdev = iter_data->rtwdev;
 420
 421         rtw_fw_media_status_report(rtwdev, si->mac_id, iter_data->connect);
 422 }
 423
 424 static void rtw_wow_fw_media_status(struct rtw_dev *rtwdev, bool connect)
 425 {
 426         struct rtw_fw_media_status_iter_data data;
 427
 428         data.rtwdev = rtwdev;
 429         data.connect = connect;
 430
 431         rtw_iterate_stas_atomic(rtwdev, rtw_wow_fw_media_status_iter, &data);
 432 }
 433
 434 static void rtw_wow_config_pno_rsvd_page(struct rtw_dev *rtwdev,
 435                                          struct rtw_vif *rtwvif)
 436 {
 437         rtw_add_rsvd_page_pno(rtwdev, rtwvif);
 438 }
 439
 440 static void rtw_wow_config_linked_rsvd_page(struct rtw_dev *rtwdev,
 441                                            struct rtw_vif *rtwvif)
 442 {
 443         rtw_add_rsvd_page_sta(rtwdev, rtwvif);
 444 }
 445
 446 static void rtw_wow_config_rsvd_page(struct rtw_dev *rtwdev,
 447                                      struct rtw_vif *rtwvif)
 448 {
 449         rtw_remove_rsvd_page(rtwdev, rtwvif);
 450
 451         if (rtw_wow_mgd_linked(rtwdev)) {
 452                 rtw_wow_config_linked_rsvd_page(rtwdev, rtwvif);
 453         } else if (test_bit(RTW_FLAG_WOWLAN, rtwdev->flags) &&
 454                    rtw_wow_no_link(rtwdev)) {
 455                 rtw_wow_config_pno_rsvd_page(rtwdev, rtwvif);
 456         }
 457 }
 458
 459 static int rtw_wow_dl_fw_rsvd_page(struct rtw_dev *rtwdev)
 460 {
 461         struct ieee80211_vif *wow_vif = rtwdev->wow.wow_vif;
 462         struct rtw_vif *rtwvif = (struct rtw_vif *)wow_vif->drv_priv;
 463
 464         rtw_wow_config_rsvd_page(rtwdev, rtwvif);
 465
 466         return rtw_fw_download_rsvd_page(rtwdev);
 467 }
 468
 469 static int rtw_wow_swap_fw(struct rtw_dev *rtwdev, enum rtw_fw_type type)
 470 {
 471         struct rtw_fw_state *fw;
 472         int ret;
 473
 474         switch (type) {
 475         case RTW_WOWLAN_FW:
 476                 fw = &rtwdev->wow_fw;
 477                 break;
 478
 479         case RTW_NORMAL_FW:
 480                 fw = &rtwdev->fw;
 481                 break;
 482
 483         default:
 484                 rtw_warn(rtwdev, "unsupported firmware type to swap\n");
 485                 return -ENOENT;
 486         }
 487
 488         ret = rtw_download_firmware(rtwdev, fw);
 489         if (ret)
 490                 goto out;
 491
 492         rtw_fw_send_general_info(rtwdev);
 493         rtw_fw_send_phydm_info(rtwdev);
 494         rtw_wow_fw_media_status(rtwdev, true);
 495
 496 out:
 497         return ret;
 498 }
 499
 500 static void rtw_wow_check_pno(struct rtw_dev *rtwdev,
 501                               struct cfg80211_sched_scan_request *nd_config)
 502 {
 503         struct rtw_wow_param *rtw_wow = &rtwdev->wow;
 504         struct rtw_pno_request *pno_req = &rtw_wow->pno_req;
 505         struct ieee80211_channel *channel;
 506         int i, size;
 507
 508         if (!nd_config->n_match_sets || !nd_config->n_channels)
 509                 goto err;
 510
 511         pno_req->match_set_cnt = nd_config->n_match_sets;
 512         size = sizeof(*pno_req->match_sets) * pno_req->match_set_cnt;
 513         pno_req->match_sets = kmemdup(nd_config->match_sets, size, GFP_KERNEL);
 514         if (!pno_req->match_sets)
 515                 goto err;
 516
 517         pno_req->channel_cnt = nd_config->n_channels;
 518         size = sizeof(*nd_config->channels[0]) * nd_config->n_channels;
 519         pno_req->channels = kmalloc(size, GFP_KERNEL);
 520         if (!pno_req->channels)
 521                 goto channel_err;
 522
 523         for (i = 0 ; i < pno_req->channel_cnt; i++) {
 524                 channel = pno_req->channels + i;
 525                 memcpy(channel, nd_config->channels[i], sizeof(*channel));
 526         }
 527
 528         pno_req->scan_plan = *nd_config->scan_plans;
 529         pno_req->inited = true;
 530
 531         rtw_dbg(rtwdev, RTW_DBG_WOW, "WOW: net-detect is enabled\n");
 532
 533         return;
 534
 535 channel_err:
 536         kfree(pno_req->match_sets);
 537
 538 err:
 539         rtw_dbg(rtwdev, RTW_DBG_WOW, "WOW: net-detect is disabled\n");
 540 }
 541
 542 static int rtw_wow_leave_linked_ps(struct rtw_dev *rtwdev)
 543 {
 544         if (!test_bit(RTW_FLAG_WOWLAN, rtwdev->flags))
 545                 cancel_delayed_work_sync(&rtwdev->watch_dog_work);
 546
 547         rtw_leave_lps(rtwdev);
 548
 549         return 0;
 550 }
 551
 552 static int rtw_wow_leave_no_link_ps(struct rtw_dev *rtwdev)
 553 {
 554         struct rtw_wow_param *rtw_wow = &rtwdev->wow;
 555         int ret = 0;
 556
 557         if (test_bit(RTW_FLAG_WOWLAN, rtwdev->flags)) {
 558                 if (rtw_fw_lps_deep_mode)
 559                         rtw_leave_lps_deep(rtwdev);
 560         } else {
 561                 if (test_bit(RTW_FLAG_INACTIVE_PS, rtwdev->flags)) {
 562                         rtw_wow->ips_enabled = true;
 563                         ret = rtw_leave_ips(rtwdev);
 564                         if (ret)
 565                                 return ret;
 566                 }
 567         }
 568
 569         return 0;
 570 }
 571
 572 static int rtw_wow_leave_ps(struct rtw_dev *rtwdev)
 573 {
 574         int ret = 0;
 575
 576         if (rtw_wow_mgd_linked(rtwdev))
 577                 ret = rtw_wow_leave_linked_ps(rtwdev);
 578         else if (rtw_wow_no_link(rtwdev))
 579                 ret = rtw_wow_leave_no_link_ps(rtwdev);
 580
 581         return ret;
 582 }
 583
 584 static int rtw_wow_restore_ps(struct rtw_dev *rtwdev)
 585 {
 586         int ret = 0;
 587
 588         if (rtw_wow_no_link(rtwdev) && rtwdev->wow.ips_enabled)
 589                 ret = rtw_enter_ips(rtwdev);
 590
 591         return ret;
 592 }
 593
 594 static int rtw_wow_enter_linked_ps(struct rtw_dev *rtwdev)
 595 {
 596         struct rtw_wow_param *rtw_wow = &rtwdev->wow;
 597         struct ieee80211_vif *wow_vif = rtw_wow->wow_vif;
 598         struct rtw_vif *rtwvif = (struct rtw_vif *)wow_vif->drv_priv;
 599
 600         rtw_enter_lps(rtwdev, rtwvif->port);
 601
 602         return 0;
 603 }
 604
 605 static int rtw_wow_enter_no_link_ps(struct rtw_dev *rtwdev)
 606 {
 607         /* firmware enters deep ps by itself if supported */
 608         set_bit(RTW_FLAG_LEISURE_PS_DEEP, rtwdev->flags);
 609
 610         return 0;
 611 }
 612
 613 static int rtw_wow_enter_ps(struct rtw_dev *rtwdev)
 614 {
 615         int ret = 0;
 616
 617         if (rtw_wow_mgd_linked(rtwdev))
 618                 ret = rtw_wow_enter_linked_ps(rtwdev);
 619         else if (rtw_wow_no_link(rtwdev) && rtw_fw_lps_deep_mode)
 620                 ret = rtw_wow_enter_no_link_ps(rtwdev);
 621
 622         return ret;
 623 }
 624
 625 static void rtw_wow_stop_trx(struct rtw_dev *rtwdev)
 626 {
 627         rtw_wow_bb_stop(rtwdev);
 628         rtw_wow_rx_dma_stop(rtwdev);
 629 }
 630
 631 static int rtw_wow_start(struct rtw_dev *rtwdev)
 632 {
 633         int ret;
 634
 635         ret = rtw_wow_fw_start(rtwdev);
 636         if (ret)
 637                 goto out;
 638
 639         rtw_hci_stop(rtwdev);
 640         rtw_wow_bb_start(rtwdev);
 641         rtw_wow_avoid_reset_mac(rtwdev);
 642
 643 out:
 644         return ret;
 645 }
 646
 647 static int rtw_wow_enable(struct rtw_dev *rtwdev)
 648 {
 649         int ret = 0;
 650
 651         rtw_wow_stop_trx(rtwdev);
 652
 653         ret = rtw_wow_swap_fw(rtwdev, RTW_WOWLAN_FW);
 654         if (ret) {
 655                 rtw_err(rtwdev, "failed to swap wow fw\n");
 656                 goto error;
 657         }
 658
 659         set_bit(RTW_FLAG_WOWLAN, rtwdev->flags);
 660
 661         ret = rtw_wow_dl_fw_rsvd_page(rtwdev);
 662         if (ret) {
 663                 rtw_err(rtwdev, "failed to download wowlan rsvd page\n");
 664                 goto error;
 665         }
 666
 667         ret = rtw_wow_start(rtwdev);
 668         if (ret) {
 669                 rtw_err(rtwdev, "failed to start wow\n");
 670                 goto error;
 671         }
 672
 673         return ret;
 674
 675 error:
 676         clear_bit(RTW_FLAG_WOWLAN, rtwdev->flags);
 677         return ret;
 678 }
 679
 680 static int rtw_wow_stop(struct rtw_dev *rtwdev)
 681 {
 682         int ret;
 683
 684         /* some HCI related registers will be reset after resume,
 685          * need to set them again.
 686          */
 687         ret = rtw_hci_setup(rtwdev);
 688         if (ret) {
 689                 rtw_err(rtwdev, "failed to setup hci\n");
 690                 return ret;
 691         }
 692
 693         ret = rtw_hci_start(rtwdev);
 694         if (ret) {
 695                 rtw_err(rtwdev, "failed to start hci\n");
 696                 return ret;
 697         }
 698
 699         ret = rtw_wow_fw_stop(rtwdev);
 700         if (ret)
 701                 rtw_err(rtwdev, "failed to stop wowlan fw\n");
 702
 703         rtw_wow_bb_stop(rtwdev);
 704
 705         return ret;
 706 }
 707
 708 static void rtw_wow_resume_trx(struct rtw_dev *rtwdev)
 709 {
 710         rtw_wow_rx_dma_start(rtwdev);
 711         rtw_wow_bb_start(rtwdev);
 712         ieee80211_queue_delayed_work(rtwdev->hw, &rtwdev->watch_dog_work,
 713                                      RTW_WATCH_DOG_DELAY_TIME);
 714 }
 715
 716 static int rtw_wow_disable(struct rtw_dev *rtwdev)
 717 {
 718         int ret;
 719
 720         clear_bit(RTW_FLAG_WOWLAN, rtwdev->flags);
 721
 722         ret = rtw_wow_stop(rtwdev);
 723         if (ret) {
 724                 rtw_err(rtwdev, "failed to stop wow\n");
 725                 goto out;
 726         }
 727
 728         ret = rtw_wow_swap_fw(rtwdev, RTW_NORMAL_FW);
 729         if (ret) {
 730                 rtw_err(rtwdev, "failed to swap normal fw\n");
 731                 goto out;
 732         }
 733
 734         ret = rtw_wow_dl_fw_rsvd_page(rtwdev);
 735         if (ret)
 736                 rtw_err(rtwdev, "failed to download normal rsvd page\n");
 737
 738 out:
 739         rtw_wow_resume_trx(rtwdev);
 740         return ret;
 741 }
 742
 743 static void rtw_wow_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 744 {
 745         struct rtw_dev *rtwdev = data;
 746         struct rtw_vif *rtwvif = (struct rtw_vif *)vif->drv_priv;
 747         struct rtw_wow_param *rtw_wow = &rtwdev->wow;
 748
 749         /* Current wowlan function support setting of only one STATION vif.
 750          * So when one suitable vif is found, stop the iteration.
 751          */
 752         if (rtw_wow->wow_vif || vif->type != NL80211_IFTYPE_STATION)
 753                 return;
 754
 755         switch (rtwvif->net_type) {
 756         case RTW_NET_MGD_LINKED:
 757                 rtw_wow->wow_vif = vif;
 758                 break;
 759         case RTW_NET_NO_LINK:
 760                 if (rtw_wow->pno_req.inited)
 761                         rtwdev->wow.wow_vif = vif;
 762                 break;
 763         default:
 764                 break;
 765         }
 766 }
 767
 768 static int rtw_wow_set_wakeups(struct rtw_dev *rtwdev,
 769                                struct cfg80211_wowlan *wowlan)
 770 {
 771         struct rtw_wow_param *rtw_wow = &rtwdev->wow;
 772         struct rtw_wow_pattern *rtw_patterns = rtw_wow->patterns;
 773         struct rtw_vif *rtwvif;
 774         int i;
 775
 776         if (wowlan->disconnect)
 777                 set_bit(RTW_WOW_FLAG_EN_DISCONNECT, rtw_wow->flags);
 778         if (wowlan->magic_pkt)
 779                 set_bit(RTW_WOW_FLAG_EN_MAGIC_PKT, rtw_wow->flags);
 780         if (wowlan->gtk_rekey_failure)
 781                 set_bit(RTW_WOW_FLAG_EN_REKEY_PKT, rtw_wow->flags);
 782
 783         if (wowlan->nd_config)
 784                 rtw_wow_check_pno(rtwdev, wowlan->nd_config);
 785
 786         rtw_iterate_vifs_atomic(rtwdev, rtw_wow_vif_iter, rtwdev);
 787         if (!rtw_wow->wow_vif)
 788                 return -EPERM;
 789
 790         rtwvif = (struct rtw_vif *)rtw_wow->wow_vif->drv_priv;
 791         if (wowlan->n_patterns && wowlan->patterns) {
 792                 rtw_wow->pattern_cnt = wowlan->n_patterns;
 793                 for (i = 0; i < wowlan->n_patterns; i++)
 794                         rtw_wow_pattern_generate(rtwdev, rtwvif,
 795                                                  wowlan->patterns + i,
 796                                                  rtw_patterns + i);
 797         }
 798
 799         return 0;
 800 }
 801
 802 static void rtw_wow_clear_wakeups(struct rtw_dev *rtwdev)
 803 {
 804         struct rtw_wow_param *rtw_wow = &rtwdev->wow;
 805         struct rtw_pno_request *pno_req = &rtw_wow->pno_req;
 806
 807         if (pno_req->inited) {
 808                 kfree(pno_req->channels);
 809                 kfree(pno_req->match_sets);
 810         }
 811
 812         memset(rtw_wow, 0, sizeof(rtwdev->wow));
 813 }
 814
 815 int rtw_wow_suspend(struct rtw_dev *rtwdev, struct cfg80211_wowlan *wowlan)
 816 {
 817         int ret = 0;
 818
 819         ret = rtw_wow_set_wakeups(rtwdev, wowlan);
 820         if (ret) {
 821                 rtw_err(rtwdev, "failed to set wakeup event\n");
 822                 goto out;
 823         }
 824
 825         ret = rtw_wow_leave_ps(rtwdev);
 826         if (ret) {
 827                 rtw_err(rtwdev, "failed to leave ps from normal mode\n");
 828                 goto out;
 829         }
 830
 831         ret = rtw_wow_enable(rtwdev);
 832         if (ret) {
 833                 rtw_err(rtwdev, "failed to enable wow\n");
 834                 rtw_wow_restore_ps(rtwdev);
 835                 goto out;
 836         }
 837
 838         ret = rtw_wow_enter_ps(rtwdev);
 839         if (ret)
 840                 rtw_err(rtwdev, "failed to enter ps for wow\n");
 841
 842 out:
 843         return ret;
 844 }
 845
 846 int rtw_wow_resume(struct rtw_dev *rtwdev)
 847 {
 848         int ret;
 849
 850         /* If wowlan mode is not enabled, do nothing */
 851         if (!test_bit(RTW_FLAG_WOWLAN, rtwdev->flags)) {
 852                 rtw_err(rtwdev, "wow is not enabled\n");
 853                 ret = -EPERM;
 854                 goto out;
 855         }
 856
 857         ret = rtw_wow_leave_ps(rtwdev);
 858         if (ret) {
 859                 rtw_err(rtwdev, "failed to leave ps from wowlan mode\n");
 860                 goto out;
 861         }
 862
 863         rtw_wow_show_wakeup_reason(rtwdev);
 864
 865         ret = rtw_wow_disable(rtwdev);
 866         if (ret) {
 867                 rtw_err(rtwdev, "failed to disable wow\n");
 868                 goto out;
 869         }
 870
 871         ret = rtw_wow_restore_ps(rtwdev);
 872         if (ret)
 873                 rtw_err(rtwdev, "failed to restore ps to normal mode\n");
 874
 875 out:
 876         rtw_wow_clear_wakeups(rtwdev);
 877         return ret;
 878 }