drivers/net/wireless/rockchip_wlan/rtl8723ds/hal/rtl8723d/rtl8723d_lps_poff.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  *
  19  ******************************************************************************/
  20 #include <rtl8723d_hal.h>
  21
  22 #ifdef CONFIG_LPS_POFF
  23 /****************************************************************************
  24  * Function: constuct Register Setting for HW to Backup Before LPS
  25  *                  page 2/4/6/7/8~F and page 0x24(for NAN)
  26 *****************************************************************************/
  27 static bool hal_construct_poff_static_file(PADAPTER padapter)
  28 {
  29         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
  30         lps_poff_info_t *lps_poff_info = NULL;
  31         u8      *staticFile = NULL;
  32         u8      *start_at = NULL;
  33         u8      page_count = 0, page_offset = 0, round = 0;
  34         /*There are 256 bytes in each register bank, and  64 dwords*/
  35         u8      total = 256 / 4;
  36         u16     offset = 0, addr_value = 0;
  37
  38         if (pwrpriv->plps_poff_info == NULL) {
  39                 RTW_INFO("%s: please alloc plps_poff_info first!!\n", __func__);
  40                 return _FALSE;
  41         }
  42
  43         lps_poff_info = pwrpriv->plps_poff_info;
  44
  45         if (lps_poff_info->pStaticFile == NULL) {
  46                 RTW_INFO("%s: please alloc static configure file first!!\n",
  47                          __func__);
  48                 return _FALSE;
  49         }
  50         staticFile = lps_poff_info->pStaticFile + TXDESC_SIZE;
  51
  52         for (page_count = 2 ; page_count < 16 ; page_count++) {
  53                 page_offset = 0;
  54                 if (page_count == 3 || page_count == 5)
  55                         continue;
  56
  57                 offset = (round << 9);
  58
  59                 for (page_offset = 0 ; page_offset < total ; page_offset++) {
  60                         start_at = staticFile + offset + (page_offset << 3);
  61                         addr_value =
  62                                 ((page_count << 8) + (page_offset << 2)) >> 2;
  63
  64                         SET_HOIE_ENTRY_LOW_DATA(start_at, 0);
  65                         SET_HOIE_ENTRY_HIGH_DATA(start_at, 0);
  66                         SET_HOIE_ENTRY_MODE_SELECT(start_at, 0);
  67                         SET_HOIE_ENTRY_ADDRESS(start_at, addr_value);
  68                         SET_HOIE_ENTRY_BYTE_MASK(start_at, 0xF);
  69                         SET_HOIE_ENTRY_IO_LOCK(start_at, 0);
  70                         SET_HOIE_ENTRY_WR_EN(start_at, 1);
  71                         SET_HOIE_ENTRY_RD_EN(start_at, 1);
  72                         SET_HOIE_ENTRY_RAW_RW(start_at, 0);
  73                         SET_HOIE_ENTRY_RAW(start_at, 0);
  74                         SET_HOIE_ENTRY_IO_DELAY(start_at, 0);
  75                 }
  76
  77                 round++;
  78         }
  79
  80         /*construct page 24*/
  81         offset = (round << 9);
  82         for (page_offset = 0 ; page_offset < total ; page_offset++) {
  83                 start_at = staticFile + offset + (page_offset << 3);
  84                 addr_value = ((0x24 << 8) + (page_offset << 2)) >> 2;
  85
  86                 SET_HOIE_ENTRY_LOW_DATA(start_at, 0);
  87                 SET_HOIE_ENTRY_HIGH_DATA(start_at, 0);
  88                 SET_HOIE_ENTRY_MODE_SELECT(start_at, 0);
  89                 SET_HOIE_ENTRY_ADDRESS(start_at, addr_value);
  90                 SET_HOIE_ENTRY_BYTE_MASK(start_at, 0xF);
  91                 SET_HOIE_ENTRY_IO_LOCK(start_at, 0);
  92                 SET_HOIE_ENTRY_WR_EN(start_at, 1);
  93                 SET_HOIE_ENTRY_RD_EN(start_at, 1);
  94                 SET_HOIE_ENTRY_RAW_RW(start_at, 0);
  95                 SET_HOIE_ENTRY_RAW(start_at, 0);
  96                 SET_HOIE_ENTRY_IO_DELAY(start_at, 0);
  97         }
  98         RTW_INFO("%s: (round << 9) + (PageOffset << 3) = %#08x\n",
  99                  __func__, offset + (page_offset << 3));
 100
 101         start_at = staticFile + offset + (page_offset << 3);
 102         /* add last command: 00 00 00 00 00 40 30 00,suggested by DD */
 103         *(start_at) = 0;
 104         *(start_at + 1) = 0;
 105         *(start_at + 2) = 0;
 106         *(start_at + 3) = 0;
 107         *(start_at + 4) = 0;
 108         *(start_at + 5) = 0x40;
 109         *(start_at + 6) = 0x30;
 110         *(start_at + 7) = 0;
 111
 112         return _TRUE;
 113 }
 114
 115 /****************************************************************************
 116 Function: send Location of configuration file and other info for FW
 117 *****************************************************************************/
 118 static void rtl8723d_lps_poff_h2c_param(PADAPTER padapter, u8 tx_bndy, u16 len,
 119                                         bool isDynamic)
 120 {
 121         u8 lps_poff_param[H2C_LPS_POFF_PARAM_LEN] = {0};
 122         u8 start_addr_l = 0, start_addr_h = 0;
 123         u8 end_addr_l = 0, end_addr_h = 0;
 124         u16 start_addr = 0, end_addr = 0;
 125
 126         if (len < 8)
 127                 return;
 128         /*
 129         set start address, The parameter is entrys. every page has 16 entrys
 130         The Tx Descriptor is 40Byte which locate 5 entries
 131         */
 132
 133         start_addr = (tx_bndy << 4) + 5;
 134         end_addr = start_addr + (len >> 3) - 1;
 135
 136         RTW_INFO("%s: start_addr = %#02X, end_addr= %#02X\n",
 137                  __func__, start_addr, end_addr);
 138
 139         start_addr_l = (u8)start_addr;
 140         start_addr_h = (u8)(start_addr >> 8);
 141
 142         end_addr_l = (u8)end_addr;
 143         end_addr_h = (u8)(end_addr >> 8);
 144
 145         /*construct H2C Cmd*/
 146         if (isDynamic)
 147                 SET_H2CCMD_LPS_POFF_PARAM_RDVLD(lps_poff_param, 0);
 148         else
 149                 SET_H2CCMD_LPS_POFF_PARAM_RDVLD(lps_poff_param, 1);
 150
 151         SET_H2CCMD_LPS_POFF_PARAM_WRVLD(lps_poff_param, 1);
 152         SET_H2CCMD_LPS_POFF_PARAM_STARTADDL(lps_poff_param, start_addr_l);
 153         SET_H2CCMD_LPS_POFF_PARAM_STARTADDH(lps_poff_param, start_addr_h);
 154         SET_H2CCMD_LPS_POFF_PARAM_ENDADDL(lps_poff_param, end_addr_l);
 155         SET_H2CCMD_LPS_POFF_PARAM_ENDADDH(lps_poff_param, end_addr_h);
 156
 157         rtw_hal_fill_h2c_cmd(padapter, H2C_LPS_POFF_PARAM,
 158                              H2C_LPS_POFF_PARAM_LEN, lps_poff_param);
 159 }
 160
 161 /*************************************************************************
 162 Function: SET Location of Configuration File to FW
 163 **************************************************************************/
 164 static void rtl8723d_lps_poff_set_param(PADAPTER padapter)
 165 {
 166         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 167         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 168
 169         u8 static_tx_bndy = 0;
 170         u8 dynamic_tx_bndy = 0;
 171         u16 static_len = 0, dynamic_len = 0;
 172
 173         static_tx_bndy = plps_poff_info->tx_bndy_static;
 174         dynamic_tx_bndy = plps_poff_info->tx_bndy_dynamic;
 175
 176         dynamic_len = plps_poff_info->ConfLenForPTK +
 177                       plps_poff_info->ConfLenForGTK;
 178
 179         if (ATOMIC_READ(&plps_poff_info->bSetPOFFParm) == _TRUE)
 180                 return;
 181
 182         /* download static configuration */
 183         static_len = LPS_POFF_STATIC_FILE_LEN - TXDESC_SIZE;
 184         rtl8723d_lps_poff_h2c_param(padapter, static_tx_bndy,
 185                                     static_len, _FALSE);
 186
 187         /* download dynamic configuration */
 188         /* the length must be add more 8 Byte due to hard bug */
 189         if (dynamic_len != 0) {
 190                 dynamic_len += 8;
 191                 rtl8723d_lps_poff_h2c_param(padapter, dynamic_tx_bndy,
 192                                             dynamic_len, _TRUE);
 193         }
 194
 195         ATOMIC_SET(&plps_poff_info->bSetPOFFParm, _TRUE);
 196 }
 197
 198
 199 /****************************************************************************
 200 Function: change tx boundary
 201 *****************************************************************************/
 202 static void rtl8723d_lps_poff_set_tx_bndy(PADAPTER padapter, u8 tx_bndy)
 203 {
 204         u32     numHQ = 0x10;
 205         u32     numLQ = 0x10;
 206         u32     numPubQ = 0;
 207         u8      numNQ = 0;
 208
 209         u32     val32 = 0;
 210         u8      val8  = 0;
 211
 212 #if (DEV_BUS_TYPE == RT_PCI_INTERFACE)
 213         numHQ = 0x8;
 214         numLQ = 0x8;
 215 #endif
 216         numPubQ = tx_bndy - numHQ - numLQ - numNQ - 1;
 217         val8 = _NPQ(numNQ);
 218         val32 = _HPQ(numHQ) | _LPQ(numLQ) | _PUBQ(numPubQ) | LD_RQPN;
 219
 220         rtw_write8(padapter, REG_RQPN_NPQ, val8);
 221         rtw_write32(padapter, REG_RQPN, val32);
 222 }
 223
 224 /****************************************************************************
 225 Function: change tx boundary flow
 226 *****************************************************************************/
 227 static bool rtl8723d_lps_poff_tx_bndy_flow(PADAPTER padapter, bool enable)
 228 {
 229         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 230         struct xmit_priv *pxmitpriv;
 231         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 232         u8      tx_bndy = 0, tx_bndy_new = 0, count = 0, queue_pending = _FALSE;
 233         u8      val8 = 0;
 234         u16     val16 = 0;
 235         u32     val32 = 0;
 236
 237         pxmitpriv = &padapter->xmitpriv;
 238         rtw_hal_get_def_var(padapter, HAL_DEF_TX_PAGE_BOUNDARY, (u8 *)&tx_bndy);
 239
 240         RTW_INFO("%s: tx_bndy: %#X, tx_bndy_static: %#X\n",
 241                  __func__, tx_bndy, plps_poff_info->tx_bndy_static);
 242
 243         if (enable)
 244                 tx_bndy_new = plps_poff_info->tx_bndy_static + 1;
 245         else
 246                 tx_bndy_new = tx_bndy;
 247
 248         ATOMIC_SET(&plps_poff_info->bTxBoundInProgress, _TRUE);
 249
 250         /* stop os layer TX*/
 251         rtw_mi_netif_stop_queue(padapter, _FALSE);
 252
 253         val16 = rtw_read16(padapter, REG_TXPKT_EMPTY);
 254
 255         /* stop tx process and wait tx empty */
 256         while ((val16 & 0x05FF) != 0x05FF) {
 257                 rtw_mdelay_os(10);
 258                 val16 = rtw_read16(padapter, REG_TXPKT_EMPTY);
 259
 260                 count++;
 261                 if (count >= 100) {
 262                         val16 = rtw_read16(padapter, REG_TXPKT_EMPTY);
 263                         RTW_INFO("%s, txpkt_empty: %#04x\n", __func__, val16);
 264                         val8 = rtw_read8(padapter, REG_CPU_MGQ_INFORMATION);
 265                         RTW_INFO("%s, REG_CPU_MGQ_INFORMATION: %#02x\n",
 266                                  __func__, val8);
 267                         RTW_INFO("%s, wait for tx empty timeout!!\n", __func__);
 268                         ATOMIC_SET(&plps_poff_info->bTxBoundInProgress, _FALSE);
 269                         rtw_mi_netif_wake_queue(padapter);
 270                         return _FALSE;
 271                 }
 272         }
 273
 274         /* change tx boundary*/
 275         rtl8723d_lps_poff_set_tx_bndy(padapter, tx_bndy_new);
 276
 277         /* set free tail */
 278         val32 = rtw_read32(padapter, REG_FWHW_TXQ_CTRL);
 279         val32 |= BIT20;
 280         rtw_write32(padapter, REG_FWHW_TXQ_CTRL, val32);
 281         rtw_write8(padapter, REG_BCNQ_BDNY, tx_bndy_new);
 282
 283         RTW_INFO("%s: free tail = %#x after\n", __func__,
 284                  rtw_read8(padapter, REG_FW_FREE_TAIL_8723D));
 285
 286         /* set bcn head */
 287         val32 = rtw_read32(padapter, REG_FWHW_TXQ_CTRL);
 288         val32 &= ~BIT20;
 289         rtw_write32(padapter, REG_FWHW_TXQ_CTRL, val32);
 290         rtw_write8(padapter, REG_BCNQ_BDNY, tx_bndy);
 291
 292         /* reinit LLT */
 293         rtl8723d_InitLLTTable(padapter);
 294
 295         /* clear 0x210 ??*/
 296         val32 = rtw_read32(padapter, REG_TXDMA_STATUS);
 297
 298         if (val32 != 0) {
 299                 RTW_INFO("%s: REG_TXDMA_STATUS: %#08x\n", __func__, val32);
 300                 rtw_write32(padapter, REG_TXDMA_STATUS, val32);
 301         }
 302
 303         ATOMIC_SET(&plps_poff_info->bTxBoundInProgress, _FALSE);
 304
 305
 306         /* restart tx */
 307
 308         rtw_mi_netif_wake_queue(padapter);
 309
 310         return _TRUE;
 311 }
 312
 313 /****************************************************************************
 314 Function: Append extra bytes for dynamic confiure file
 315           Add one entry to fix hw bug,
 316           list command: 00 00 00 00 00 40 30 00, suggested by DD
 317 *****************************************************************************/
 318 static u8 rtl8723d_lps_poff_append_extra_info(PADAPTER padapter, u32 len)
 319 {
 320         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 321         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 322         u32 data_offset = len + plps_poff_info->ConfFileOffset;
 323
 324         *(plps_poff_info->pDynamicFile + (data_offset)) = 0x00;
 325         *(plps_poff_info->pDynamicFile + (data_offset + 1)) = 0x00;
 326         *(plps_poff_info->pDynamicFile + (data_offset + 2)) = 0x00;
 327         *(plps_poff_info->pDynamicFile + (data_offset + 3)) = 0x00;
 328         *(plps_poff_info->pDynamicFile + (data_offset + 4)) = 0x00;
 329         *(plps_poff_info->pDynamicFile + (data_offset + 5)) = 0x40;
 330         *(plps_poff_info->pDynamicFile + (data_offset + 6)) = 0x30;
 331         *(plps_poff_info->pDynamicFile + (data_offset + 7)) = 0x00;
 332         return 8;
 333 }
 334
 335 /****************************************************************************
 336 Function: xmit config file to write port.
 337 *****************************************************************************/
 338 static void rtl8723d_lps_poff_send_config_frame(PADAPTER padapter,
 339                 u8 *pFile, u32 len)
 340 {
 341         struct xmit_frame       *pcmdframe = NULL;
 342         struct pkt_attrib       *pattrib;
 343         struct xmit_priv        *pxmitpriv;
 344         int i = 0;
 345
 346         pxmitpriv = &padapter->xmitpriv;
 347         pcmdframe = rtw_alloc_cmdxmitframe(pxmitpriv);
 348
 349         if (pcmdframe == NULL)
 350                 RTW_INFO("%s: alloc cmdframe fail\n", __func__);
 351
 352         _rtw_memcpy(pcmdframe->buf_addr, pFile, len);
 353
 354         pattrib = &pcmdframe->attrib;
 355         update_mgntframe_attrib(padapter, pattrib);
 356         pattrib->qsel = QSLT_BEACON;
 357         pattrib->pktlen = len - TXDESC_OFFSET;
 358         pattrib->last_txcmdsz = len - TXDESC_OFFSET;
 359
 360         RTW_INFO("%s, len: %d, MAX_CMDBUF_SZ: %d\n", __func__, len,
 361                  MAX_CMDBUF_SZ);
 362 #ifdef CONFIG_PCI_HCI
 363         dump_mgntframe(padapter, pcmdframe);
 364 #else
 365         dump_mgntframe_and_wait(padapter, pcmdframe, 100);
 366 #endif
 367
 368 }
 369
 370 /****************************************************************************
 371 Function: download config file flow
 372 *****************************************************************************/
 373 static void rtl8723d_lps_poff_send_config_file(PADAPTER padapter,
 374                 u8 *pFile, u8 loc, u32 len)
 375 {
 376         HAL_DATA_TYPE   *pHalData = GET_HAL_DATA(padapter);
 377         bool bRecover = _FALSE, bcn_valid = _FALSE;
 378         u8 DLBcnCount = 0, val8 = 0, tx_bndy = 0;
 379         u32 poll = 0;
 380
 381         rtw_hal_get_def_var(padapter,
 382                             HAL_DEF_TX_PAGE_BOUNDARY, (u8 *)&tx_bndy);
 383
 384         /* set 0x100[8]=1 for SW beacon */
 385         val8 = rtw_read8(padapter, REG_CR + 1);
 386         val8 |= BIT(0);
 387         rtw_write8(padapter,  REG_CR + 1, val8);
 388
 389         /*set 0x422[6]=0 to disable beacon DMA pass to MACTx*/
 390
 391         if (pHalData->RegFwHwTxQCtrl & BIT(6))
 392                 bRecover = _TRUE;
 393
 394         rtw_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
 395                    pHalData->RegFwHwTxQCtrl & ~BIT(6));
 396         pHalData->RegFwHwTxQCtrl &= ~BIT(6);
 397
 398         /* Clear beacon valid check bit. */
 399         rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
 400         rtw_hal_set_hwreg(padapter, HW_VAR_DL_BCN_SEL, NULL);
 401
 402         /* set 0x209[7:0] beacon queue head page to start download location */
 403         rtw_write8(padapter, REG_TDECTRL_8723D + 1, loc);
 404
 405         DLBcnCount = 0;
 406         poll = 0;
 407
 408         do {
 409                 rtl8723d_lps_poff_send_config_frame(padapter, pFile, len);
 410                 DLBcnCount++;
 411                 do {
 412                         rtw_mdelay_os(10);
 413                         /*check rsvd page download OK.*/
 414                         rtw_hal_get_hwreg(padapter, HW_VAR_BCN_VALID,
 415                                           (u8 *)(&bcn_valid));
 416                         poll++;
 417                 } while (!bcn_valid && (poll % 10) != 0
 418                          && !RTW_CANNOT_RUN(padapter));
 419         } while (!bcn_valid && DLBcnCount <= 100 && !RTW_CANNOT_RUN(padapter));
 420
 421         if (!bcn_valid)
 422                 RTW_INFO(ADPT_FMT": 1 DL RSVD page failed! DLBcnCount:%u, poll:%u\n",
 423                          ADPT_ARG(padapter), DLBcnCount, poll);
 424         else
 425                 RTW_INFO(ADPT_FMT": 1 DL RSVD page success! DLBcnCount:%u, poll:%u\n",
 426                          ADPT_ARG(padapter), DLBcnCount, poll);
 427
 428         /*restore bcn operation*/
 429         rtw_write8(padapter, REG_TDECTRL_8723D + 1, tx_bndy);
 430
 431         /*restore 0x422[6]=1 for normal bcn*/
 432         if (bRecover) {
 433                 rtw_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
 434                            pHalData->RegFwHwTxQCtrl | BIT(6));
 435                 pHalData->RegFwHwTxQCtrl |= BIT(6);
 436         }
 437
 438         /*restore 0x100[8]=0 for SW beacon*/
 439         /* Clear CR[8] or beacon packet will not be send to TxBuf anymore.*/
 440 #ifndef CONFIG_PCI_HCI
 441         val8 = rtw_read8(padapter, REG_CR + 1);
 442         val8 &= ~BIT(0);
 443         rtw_write8(padapter, REG_CR + 1, val8);
 444 #endif
 445 }
 446
 447 /****************************************************************************
 448 Function: Prepare Confiure File
 449 *****************************************************************************/
 450 static void rtl8723d_lps_poff_dl_config_file(PADAPTER padapter)
 451 {
 452         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 453         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 454         u8 count = 0, append_len = 0;
 455         u32 offset = 0, static_file_len = 0, dynamic_file_len = 0;
 456         int i = 0, j = 0;
 457
 458         offset = plps_poff_info->ConfFileOffset;
 459
 460         static_file_len = LPS_POFF_STATIC_FILE_LEN - offset;
 461
 462         dynamic_file_len =
 463                 plps_poff_info->ConfLenForPTK + plps_poff_info->ConfLenForGTK;
 464
 465         RTW_INFO("%s: static_file_len: %d dynamic_file_len: %d, offset: %d\n",
 466                  __func__, static_file_len, dynamic_file_len, offset);
 467
 468         /*static file*/
 469         rtl8723d_lps_poff_send_config_file(padapter,
 470                                            plps_poff_info->pStaticFile + offset,
 471                                            plps_poff_info->tx_bndy_static,
 472                                            static_file_len);
 473
 474         /*dynamic file*/
 475         if (dynamic_file_len != 0) {
 476                 dynamic_file_len += TXDESC_SIZE - offset;
 477
 478                 append_len =
 479                         rtl8723d_lps_poff_append_extra_info(padapter,
 480                                         dynamic_file_len);
 481                 dynamic_file_len += append_len;
 482                 rtl8723d_lps_poff_send_config_file(padapter,
 483                                    plps_poff_info->pDynamicFile + offset,
 484                                            plps_poff_info->tx_bndy_dynamic,
 485                                                    dynamic_file_len);
 486         }
 487 }
 488
 489 static u8 rtl8723d_lps_poff_set_dynamic_file(u8 *pFile, u32 type, u32 wdata)
 490 {
 491         SET_HOIE_ENTRY_LOW_DATA(pFile, (u16)wdata);
 492         SET_HOIE_ENTRY_HIGH_DATA(pFile, (u16)(wdata >> 16));
 493         SET_HOIE_ENTRY_MODE_SELECT(pFile, 0);
 494         SET_HOIE_ENTRY_ADDRESS(pFile, (type >> 2));
 495         SET_HOIE_ENTRY_BYTE_MASK(pFile, 0xF);
 496         SET_HOIE_ENTRY_IO_LOCK(pFile, 0);
 497         SET_HOIE_ENTRY_WR_EN(pFile, 1);
 498         SET_HOIE_ENTRY_RD_EN(pFile, 0);
 499         SET_HOIE_ENTRY_RAW_RW(pFile, 0);
 500         SET_HOIE_ENTRY_RAW(pFile, 0);
 501         SET_HOIE_ENTRY_IO_DELAY(pFile, 0);
 502
 503         return 8;
 504 }
 505
 506 static void rtl8723d_lps_poff_dynamic_file(PADAPTER padapter, u8 index, u8 isGK)
 507 {
 508         struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
 509         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 510         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 511         u8 *ptkfile = NULL;
 512         u8 ret = 0;
 513         u32 tgt_cmd = 0, tgt_wdata = 0;
 514         int i = 0, j = 0;
 515
 516         for (i = 0 ; i < CAM_CONTENT_COUNT; i++) {
 517                 if (!isGK)
 518                         ptkfile = plps_poff_info->pDynamicFile +
 519                                   plps_poff_info->ConfLenForPTK + TXDESC_SIZE;
 520                 else
 521                         ptkfile = plps_poff_info->pDynamicFile +
 522                                   plps_poff_info->ConfLenForPTK +
 523                                   plps_poff_info->ConfLenForGTK + TXDESC_SIZE;
 524
 525                 tgt_cmd = i + (CAM_CONTENT_COUNT * index);
 526                 tgt_cmd = tgt_cmd | CAM_POLLINIG | CAM_WRITE;
 527
 528                 switch (i) {
 529                 case 0:
 530                         tgt_wdata = dvobj->cam_cache[index].ctrl |
 531                                     dvobj->cam_cache[index].mac[0] << 16 |
 532                                     dvobj->cam_cache[index].mac[1] << 24;
 533                         break;
 534                 case 1:
 535                         tgt_wdata = dvobj->cam_cache[index].mac[2] |
 536                                     dvobj->cam_cache[index].mac[3] << 8 |
 537                                     dvobj->cam_cache[index].mac[4] << 16 |
 538                                     dvobj->cam_cache[index].mac[5] << 24;
 539                         break;
 540                 default:
 541                         j = (i - 2) << 2;
 542                         tgt_wdata =
 543                                 dvobj->cam_cache[index].key[j + 3] << 24 |
 544                                 dvobj->cam_cache[index].key[j + 2] << 16 |
 545                                 dvobj->cam_cache[index].key[j + 1] << 8 |
 546                                 dvobj->cam_cache[index].key[j];
 547                         break;
 548                 }
 549
 550                 ret = rtl8723d_lps_poff_set_dynamic_file(ptkfile,
 551                                 WCAMI, tgt_wdata);
 552
 553                 if (!isGK) {
 554                         plps_poff_info->ConfLenForPTK += ret;
 555
 556                         ptkfile = plps_poff_info->pDynamicFile +
 557                                   plps_poff_info->ConfLenForPTK + TXDESC_SIZE;
 558                 } else {
 559                         plps_poff_info->ConfLenForGTK += ret;
 560                         ptkfile = plps_poff_info->pDynamicFile +
 561                                   plps_poff_info->ConfLenForPTK +
 562                                   plps_poff_info->ConfLenForGTK + TXDESC_SIZE;
 563                 }
 564
 565                 ret = rtl8723d_lps_poff_set_dynamic_file(ptkfile,
 566                                 RWCAM, tgt_cmd);
 567
 568                 if (!isGK)
 569                         plps_poff_info->ConfLenForPTK += ret;
 570                 else
 571                         plps_poff_info->ConfLenForGTK += ret;
 572
 573 #if 0
 574                 RTW_INFO("%s: tgt_wdata: %#08x\n", __func__, tgt_wdata);
 575 #endif
 576         }
 577 }
 578
 579 static void rtl8723d_lps_poff_sec_cam_opt(PADAPTER padapter)
 580 {
 581         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 582         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 583         struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
 584         struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
 585         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 586         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 587         int i = 0;
 588         u8 isValid = 0, isGK = 0, index = 0, key_id = 0xff, gk_start = 0xff;
 589         u16 val16 = 0, len = 0;
 590
 591         /*Using cam cache to create config file for FW use.*/
 592         /*1- deail with pairwise key*/
 593
 594         for (i = 0 ; i < cam_ctl->num ; i++) {
 595
 596                 val16 = dvobj->cam_cache[i].ctrl;
 597                 isValid = (val16 >> 15) & 0x01;
 598                 isGK = (val16 >> 6) & 0x01;
 599
 600                 if (isValid && !isGK) {
 601                         rtl8723d_lps_poff_dynamic_file(padapter, i, _FALSE);
 602                         RTW_INFO("%s: id: %2u, kid: %3u, ctrl: %#04x, "MAC_FMT""KEY_FMT"\n",
 603                                  __func__, i, (dvobj->cam_cache[i].ctrl) & 0x03,
 604                                  dvobj->cam_cache[i].ctrl,
 605                                  MAC_ARG(dvobj->cam_cache[i].mac),
 606                                  KEY_ARG(dvobj->cam_cache[i].key));
 607                 } else if (isGK) {
 608                         key_id = dvobj->cam_cache[i].ctrl & 0x03;
 609                         if (key_id == pmlmeinfo->key_index)
 610                                 gk_start = i;
 611                         RTW_INFO("%s: GK_start at %d\n", __func__, gk_start);
 612                         RTW_INFO("%s: id: %2u, kid: %3u, ctrl: %#04x, "MAC_FMT""KEY_FMT"\n",
 613                                  __func__, i, key_id, dvobj->cam_cache[i].ctrl,
 614                                  MAC_ARG(dvobj->cam_cache[i].mac),
 615                                  KEY_ARG(dvobj->cam_cache[i].key));
 616                 }
 617         }
 618
 619         /*2- deail with group key*/
 620         rtl8723d_lps_poff_dynamic_file(padapter, gk_start, _TRUE);
 621
 622         RTW_INFO("%s: ConfLenForPTK: %d, ConfLenForGTK: %d\n", __func__,
 623                  plps_poff_info->ConfLenForPTK, plps_poff_info->ConfLenForGTK);
 624 }
 625
 626 /****************************************************************************
 627 Function: Prepare enter LPS partial off status.
 628 change tx boundary, download configuration file if necessary and send info to FW
 629 *****************************************************************************/
 630 static bool rtl8723d_prepare_for_enter_poff(PADAPTER padapter, bool bEnterLPS)
 631 {
 632         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 633         struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
 634         struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
 635         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 636         bool ret = _FALSE;
 637         u8 i = 0, cam_cache_num = 0;
 638
 639         for (i = 0 ; i < cam_ctl->num; i++) {
 640                 if (dvobj->cam_cache[i].ctrl != 0)
 641                         cam_cache_num++;
 642         }
 643
 644         ret = rtl8723d_lps_poff_tx_bndy_flow(padapter, bEnterLPS);
 645
 646         if (ret == _TRUE) {
 647                 if (cam_cache_num > 0) {
 648                         rtl8723d_lps_poff_sec_cam_opt(padapter);
 649                 } else {
 650                         plps_poff_info->ConfLenForPTK = 0;
 651                         plps_poff_info->ConfLenForGTK = 0;
 652                 }
 653                 rtl8723d_lps_poff_dl_config_file(padapter);
 654                 rtl8723d_lps_poff_set_param(padapter);
 655         }
 656         return ret;
 657 }
 658
 659 /*************************************************************************
 660 Function: SET H2C To Enable Partial Off
 661 **************************************************************************/
 662 void rtl8723d_lps_poff_h2c_ctrl(PADAPTER padapter, u8 en)
 663 {
 664         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 665         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
 666         struct registry_priv *pregistrypriv = &padapter->registrypriv;
 667         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 668         u8 param = 0;
 669
 670         if (pregistrypriv->wifi_spec == 1)
 671                 return;
 672
 673         if (plps_poff_info->bEn == _FALSE)
 674                 return;
 675
 676         if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
 677                 if (en) {
 678                         RTW_INFO("%s: enable case\n", __func__);
 679                         SET_H2CCMD_LPS_POFF_CTRL_EN(&param, 1);
 680                 } else {
 681                         RTW_INFO("%s: disable case\n", __func__);
 682                         ATOMIC_SET(&plps_poff_info->bSetPOFFParm, _FALSE);
 683                         SET_H2CCMD_LPS_POFF_CTRL_EN(&param, 0);
 684                 }
 685                 rtw_hal_fill_h2c_cmd(padapter, H2C_LPS_POFF_CTRL,
 686                                      H2C_LPS_POFF_CTRL_LEN, &param);
 687         }
 688 }
 689
 690 /*************************************************************************
 691 Function: The operation to Enter or Leave FWLPS 32K when partial off enable
 692 **************************************************************************/
 693 void rtl8723d_lps_poff_set_ps_mode(PADAPTER padapter, bool bEnterLPS)
 694 {
 695
 696         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 697         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 698         struct registry_priv *pregistrypriv = &padapter->registrypriv;
 699         bool res = _FALSE;
 700
 701         if (pregistrypriv->wifi_spec == 1)
 702                 return;
 703
 704         if (plps_poff_info->bEn) {
 705
 706                 plps_poff_info->ConfLenForPTK = 0;
 707                 plps_poff_info->ConfLenForGTK = 0;
 708
 709                 if (bEnterLPS) {
 710                         res = rtl8723d_prepare_for_enter_poff(padapter,
 711                                                               bEnterLPS);
 712                         ATOMIC_SET(&plps_poff_info->bEnterPOFF, res);
 713                 } else
 714                         rtl8723d_lps_poff_tx_bndy_flow(padapter, bEnterLPS);
 715         }
 716 }
 717
 718 /*************************************************************************
 719 Function: Get LPS-POFF Enter Status
 720 **************************************************************************/
 721 bool rtl8723d_lps_poff_get_status(PADAPTER padapter)
 722 {
 723         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 724         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 725         struct registry_priv *pregistrypriv = &padapter->registrypriv;
 726
 727         if (pregistrypriv->wifi_spec == 1) {
 728                 RTW_INFO("%s: wifi_spec is enable\n", __func__);
 729                 return _FALSE;
 730         }
 731
 732         if (plps_poff_info->bEn == _FALSE) {
 733                 RTW_INFO("%s: POFF is disable\n", __func__);
 734                 return _FALSE;
 735         }
 736
 737         return ATOMIC_READ(&plps_poff_info->bEnterPOFF);
 738 }
 739
 740 /*************************************************************************
 741 Function: Get LPS-POFF change tx bndy status
 742 **************************************************************************/
 743 bool rtl8723d_lps_poff_get_txbndy_status(PADAPTER padapter)
 744 {
 745         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 746         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 747
 748         return ATOMIC_READ(&plps_poff_info->bTxBoundInProgress);
 749 }
 750
 751 /*************************************************************************
 752 Function: Get LPS-POFF initial
 753 **************************************************************************/
 754 void rtl8723d_lps_poff_init(PADAPTER padapter)
 755 {
 756         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 757         struct registry_priv *pregistrypriv = &padapter->registrypriv;
 758         lps_poff_info_t *lps_poff_info;
 759         u8 tx_bndy = 0, page_size = 0, total_page = 0, page_num = 0;
 760         u8 val = 0;
 761
 762         if (pregistrypriv->wifi_spec == 1)
 763                 return;
 764
 765         if (is_primary_adapter(padapter)) {
 766
 767                 rtw_hal_get_def_var(padapter,
 768                                     HAL_DEF_TX_PAGE_BOUNDARY, (u8 *)&val);
 769                 tx_bndy = val;
 770
 771                 rtw_hal_get_def_var(padapter,
 772                                     HAL_DEF_TX_PAGE_SIZE, (u8 *)&val);
 773                 page_size = val;
 774
 775                 total_page = PageNum(LPS_POFF_STATIC_FILE_LEN, page_size) +
 776                              PageNum(LPS_POFF_DYNAMIC_FILE_LEN, page_size);
 777
 778                 lps_poff_info =
 779                         (lps_poff_info_t *)rtw_zmalloc(sizeof(lps_poff_info_t));
 780
 781                 if (lps_poff_info != NULL) {
 782                         pwrpriv->plps_poff_info = lps_poff_info;
 783
 784                         lps_poff_info->pStaticFile =
 785                                 (u8 *)rtw_zmalloc(LPS_POFF_STATIC_FILE_LEN);
 786                         if (lps_poff_info->pStaticFile == NULL) {
 787                                 RTW_INFO("%s: alloc pStaticFile fail\n",
 788                                          __func__);
 789                                 goto alloc_static_conf_file_fail;
 790                         } else {
 791                                 pwrpriv->plps_poff_info->pStaticFile =
 792                                         lps_poff_info->pStaticFile;
 793                         }
 794
 795                         lps_poff_info->pDynamicFile =
 796                                 (u8 *)rtw_zmalloc(LPS_POFF_DYNAMIC_FILE_LEN);
 797                         if (lps_poff_info->pDynamicFile == NULL) {
 798                                 RTW_INFO("%s: alloc pDynamicFile fail\n",
 799                                          __func__);
 800                                 goto alloc_dynamic_conf_file_fail;
 801                         } else {
 802                                 pwrpriv->plps_poff_info->pDynamicFile =
 803                                         lps_poff_info->pDynamicFile;
 804                         }
 805
 806                         pwrpriv->plps_poff_info->bEn = _TRUE;
 807                         ATOMIC_SET(&pwrpriv->plps_poff_info->bEnterPOFF,
 808                                    _FALSE);
 809                         ATOMIC_SET(&pwrpriv->plps_poff_info->bSetPOFFParm,
 810                                    _FALSE);
 811                         ATOMIC_SET(&pwrpriv->plps_poff_info->bTxBoundInProgress,
 812                                    _FALSE);
 813 #ifdef CONFIG_PCI_HCI
 814                         pwrpriv->plps_poff_info->ConfFileOffset = 40;
 815 #else
 816                         pwrpriv->plps_poff_info->ConfFileOffset = 0;
 817 #endif
 818                         pwrpriv->plps_poff_info->tx_bndy_static =
 819                                 tx_bndy - total_page;
 820                         page_num = PageNum(LPS_POFF_DYNAMIC_FILE_LEN,
 821                                            page_size);
 822                         pwrpriv->plps_poff_info->tx_bndy_dynamic =
 823                                 tx_bndy - page_num;
 824                         /*
 825                            construct static DLConfiguration File
 826                         */
 827                         hal_construct_poff_static_file(padapter);
 828                         goto exit;
 829                 } else {
 830                         RTW_INFO("%s: alloc lps_poff_info fail\n",  __func__);
 831                         goto exit;
 832                 }
 833         }
 834
 835 alloc_dynamic_conf_file_fail:
 836         rtw_mfree((u8 *)pwrpriv->plps_poff_info->pStaticFile,
 837                   LPS_POFF_STATIC_FILE_LEN);
 838         pwrpriv->plps_poff_info->pStaticFile = NULL;
 839 alloc_static_conf_file_fail:
 840         rtw_mfree((u8 *)pwrpriv->plps_poff_info, sizeof(lps_poff_info_t));
 841         pwrpriv->plps_poff_info = NULL;
 842 exit:
 843         return;
 844 }
 845
 846 /*************************************************************************
 847 Function: Get LPS-POFF de-initial
 848 **************************************************************************/
 849 void rtl8723d_lps_poff_deinit(PADAPTER padapter)
 850 {
 851         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
 852         lps_poff_info_t *plps_poff_info = pwrpriv->plps_poff_info;
 853         struct registry_priv *pregistrypriv = &padapter->registrypriv;
 854
 855         if (pregistrypriv->wifi_spec == 1)
 856                 return;
 857
 858         if (is_primary_adapter(padapter)) {
 859                 if (plps_poff_info->pDynamicFile != NULL) {
 860                         rtw_mfree((u8 *)plps_poff_info->pDynamicFile,
 861                                   LPS_POFF_DYNAMIC_FILE_LEN);
 862                         plps_poff_info->pDynamicFile = NULL;
 863                 }
 864                 if (plps_poff_info->pStaticFile != NULL) {
 865                         rtw_mfree((u8 *)plps_poff_info->pStaticFile,
 866                                   LPS_POFF_STATIC_FILE_LEN);
 867                         plps_poff_info->pStaticFile = NULL;
 868                 }
 869                 if (plps_poff_info != NULL) {
 870                         rtw_mfree((u8 *)plps_poff_info,
 871                                   sizeof(lps_poff_info_t));
 872                         plps_poff_info = NULL;
 873                 }
 874         }
 875
 876 }
 877 #endif