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