drivers/net/wireless/rtl8192cu/core/rtw_mp.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2011 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 #define _RTW_MP_C_
  21
  22 #include <drv_types.h>
  23
  24 #ifdef PLATFORM_FREEBSD
  25 #include <sys/unistd.h>         /* for RFHIGHPID */
  26 #endif
  27
  28 #ifdef CONFIG_RTL8712
  29 #include <rtw_mp_phy_regdef.h>
  30 #endif
  31 #ifdef CONFIG_RTL8192C
  32 #include <rtl8192c_hal.h>
  33 #endif
  34 #ifdef CONFIG_RTL8192D
  35 #include <rtl8192d_hal.h>
  36 #endif
  37 #ifdef CONFIG_RTL8723A
  38 #include <rtl8723a_hal.h>
  39 #endif
  40
  41
  42 #ifdef CONFIG_MP_INCLUDED
  43
  44 u32 read_macreg(_adapter *padapter, u32 addr, u32 sz)
  45 {
  46         u32 val = 0;
  47
  48         switch(sz)
  49         {
  50                 case 1:
  51                         val = rtw_read8(padapter, addr);
  52                         break;
  53                 case 2:
  54                         val = rtw_read16(padapter, addr);
  55                         break;
  56                 case 4:
  57                         val = rtw_read32(padapter, addr);
  58                         break;
  59                 default:
  60                         val = 0xffffffff;
  61                         break;
  62         }
  63
  64         return val;
  65
  66 }
  67
  68 void write_macreg(_adapter *padapter, u32 addr, u32 val, u32 sz)
  69 {
  70         switch(sz)
  71         {
  72                 case 1:
  73                         rtw_write8(padapter, addr, (u8)val);
  74                         break;
  75                 case 2:
  76                         rtw_write16(padapter, addr, (u16)val);
  77                         break;
  78                 case 4:
  79                         rtw_write32(padapter, addr, val);
  80                         break;
  81                 default:
  82                         break;
  83         }
  84
  85 }
  86
  87 u32 read_bbreg(_adapter *padapter, u32 addr, u32 bitmask)
  88 {
  89         return rtw_hal_read_bbreg(padapter, addr, bitmask);
  90 }
  91
  92 void write_bbreg(_adapter *padapter, u32 addr, u32 bitmask, u32 val)
  93 {
  94         rtw_hal_write_bbreg(padapter, addr, bitmask, val);
  95 }
  96
  97 u32 _read_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 bitmask)
  98 {
  99         return rtw_hal_read_rfreg(padapter, (RF_RADIO_PATH_E)rfpath, addr, bitmask);
 100 }
 101
 102 void _write_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 bitmask, u32 val)
 103 {
 104         rtw_hal_write_rfreg(padapter, (RF_RADIO_PATH_E)rfpath, addr, bitmask, val);
 105 }
 106
 107 u32 read_rfreg(PADAPTER padapter, u8 rfpath, u32 addr)
 108 {
 109         return _read_rfreg(padapter, (RF_RADIO_PATH_E)rfpath, addr, bRFRegOffsetMask);
 110 }
 111
 112 void write_rfreg(PADAPTER padapter, u8 rfpath, u32 addr, u32 val)
 113 {
 114         _write_rfreg(padapter, (RF_RADIO_PATH_E)rfpath, addr, bRFRegOffsetMask, val);
 115 }
 116
 117 static void _init_mp_priv_(struct mp_priv *pmp_priv)
 118 {
 119         WLAN_BSSID_EX *pnetwork;
 120
 121         _rtw_memset(pmp_priv, 0, sizeof(struct mp_priv));
 122
 123         pmp_priv->mode = MP_OFF;
 124
 125         pmp_priv->channel = 1;
 126         pmp_priv->bandwidth = HT_CHANNEL_WIDTH_20;
 127         pmp_priv->prime_channel_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 128         pmp_priv->rateidx = MPT_RATE_1M;
 129         pmp_priv->txpoweridx = 0x2A;
 130
 131         pmp_priv->antenna_tx = ANTENNA_A;
 132         pmp_priv->antenna_rx = ANTENNA_AB;
 133
 134         pmp_priv->check_mp_pkt = 0;
 135
 136         pmp_priv->tx_pktcount = 0;
 137
 138         pmp_priv->rx_pktcount = 0;
 139         pmp_priv->rx_crcerrpktcount = 0;
 140
 141         pmp_priv->network_macaddr[0] = 0x00;
 142         pmp_priv->network_macaddr[1] = 0xE0;
 143         pmp_priv->network_macaddr[2] = 0x4C;
 144         pmp_priv->network_macaddr[3] = 0x87;
 145         pmp_priv->network_macaddr[4] = 0x66;
 146         pmp_priv->network_macaddr[5] = 0x55;
 147
 148         pnetwork = &pmp_priv->mp_network.network;
 149         _rtw_memcpy(pnetwork->MacAddress, pmp_priv->network_macaddr, ETH_ALEN);
 150
 151         pnetwork->Ssid.SsidLength = 8;
 152         _rtw_memcpy(pnetwork->Ssid.Ssid, "mp_871x", pnetwork->Ssid.SsidLength);
 153 }
 154
 155 #ifdef PLATFORM_WINDOWS
 156 /*
 157 void mp_wi_callback(
 158         IN NDIS_WORK_ITEM*      pwk_item,
 159         IN PVOID                        cntx
 160         )
 161 {
 162         _adapter* padapter =(_adapter *)cntx;
 163         struct mp_priv *pmppriv=&padapter->mppriv;
 164         struct mp_wi_cntx       *pmp_wi_cntx=&pmppriv->wi_cntx;
 165
 166         // Execute specified action.
 167         if(pmp_wi_cntx->curractfunc != NULL)
 168         {
 169                 LARGE_INTEGER   cur_time;
 170                 ULONGLONG start_time, end_time;
 171                 NdisGetCurrentSystemTime(&cur_time);    // driver version
 172                 start_time = cur_time.QuadPart/10; // The return value is in microsecond
 173
 174                 pmp_wi_cntx->curractfunc(padapter);
 175
 176                 NdisGetCurrentSystemTime(&cur_time);    // driver version
 177                 end_time = cur_time.QuadPart/10; // The return value is in microsecond
 178
 179                 RT_TRACE(_module_mp_, _drv_info_,
 180                          ("WorkItemActType: %d, time spent: %I64d us\n",
 181                           pmp_wi_cntx->param.act_type, (end_time-start_time)));
 182         }
 183
 184         NdisAcquireSpinLock(&(pmp_wi_cntx->mp_wi_lock));
 185         pmp_wi_cntx->bmp_wi_progress= _FALSE;
 186         NdisReleaseSpinLock(&(pmp_wi_cntx->mp_wi_lock));
 187
 188         if (pmp_wi_cntx->bmpdrv_unload)
 189         {
 190                 NdisSetEvent(&(pmp_wi_cntx->mp_wi_evt));
 191         }
 192
 193 }
 194 */
 195
 196 static int init_mp_priv_by_os(struct mp_priv *pmp_priv)
 197 {
 198         struct mp_wi_cntx *pmp_wi_cntx;
 199
 200         if (pmp_priv == NULL) return _FAIL;
 201
 202         pmp_priv->rx_testcnt = 0;
 203         pmp_priv->rx_testcnt1 = 0;
 204         pmp_priv->rx_testcnt2 = 0;
 205
 206         pmp_priv->tx_testcnt = 0;
 207         pmp_priv->tx_testcnt1 = 0;
 208
 209         pmp_wi_cntx = &pmp_priv->wi_cntx
 210         pmp_wi_cntx->bmpdrv_unload = _FALSE;
 211         pmp_wi_cntx->bmp_wi_progress = _FALSE;
 212         pmp_wi_cntx->curractfunc = NULL;
 213
 214         return _SUCCESS;
 215 }
 216 #endif
 217
 218 #ifdef PLATFORM_LINUX
 219 static int init_mp_priv_by_os(struct mp_priv *pmp_priv)
 220 {
 221         int i, res;
 222         struct mp_xmit_frame *pmp_xmitframe;
 223
 224         if (pmp_priv == NULL) return _FAIL;
 225
 226         _rtw_init_queue(&pmp_priv->free_mp_xmitqueue);
 227
 228         pmp_priv->pallocated_mp_xmitframe_buf = NULL;
 229         pmp_priv->pallocated_mp_xmitframe_buf = rtw_zmalloc(NR_MP_XMITFRAME * sizeof(struct mp_xmit_frame) + 4);
 230         if (pmp_priv->pallocated_mp_xmitframe_buf == NULL) {
 231                 res = _FAIL;
 232                 goto _exit_init_mp_priv;
 233         }
 234
 235         pmp_priv->pmp_xmtframe_buf = pmp_priv->pallocated_mp_xmitframe_buf + 4 - ((uint) (pmp_priv->pallocated_mp_xmitframe_buf) & 3);
 236
 237         pmp_xmitframe = (struct mp_xmit_frame*)pmp_priv->pmp_xmtframe_buf;
 238
 239         for (i = 0; i < NR_MP_XMITFRAME; i++)
 240         {
 241                 _rtw_init_listhead(&pmp_xmitframe->list);
 242                 rtw_list_insert_tail(&pmp_xmitframe->list, &pmp_priv->free_mp_xmitqueue.queue);
 243
 244                 pmp_xmitframe->pkt = NULL;
 245                 pmp_xmitframe->frame_tag = MP_FRAMETAG;
 246                 pmp_xmitframe->padapter = pmp_priv->papdater;
 247
 248                 pmp_xmitframe++;
 249         }
 250
 251         pmp_priv->free_mp_xmitframe_cnt = NR_MP_XMITFRAME;
 252
 253         res = _SUCCESS;
 254
 255 _exit_init_mp_priv:
 256
 257         return res;
 258 }
 259 #endif
 260
 261 static void mp_init_xmit_attrib(struct mp_tx *pmptx, PADAPTER padapter)
 262 {
 263         struct pkt_attrib *pattrib;
 264         struct tx_desc *desc;
 265
 266         // init xmitframe attribute
 267         pattrib = &pmptx->attrib;
 268         _rtw_memset(pattrib, 0, sizeof(struct pkt_attrib));
 269         desc = &pmptx->desc;
 270         _rtw_memset(desc, 0, TXDESC_SIZE);
 271
 272         pattrib->ether_type = 0x8712;
 273         //_rtw_memcpy(pattrib->src, padapter->eeprompriv.mac_addr, ETH_ALEN);
 274 //      _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
 275         _rtw_memset(pattrib->dst, 0xFF, ETH_ALEN);
 276 //      pattrib->pctrl = 0;
 277 //      pattrib->dhcp_pkt = 0;
 278 //      pattrib->pktlen = 0;
 279         pattrib->ack_policy = 0;
 280 //      pattrib->pkt_hdrlen = ETH_HLEN;
 281         pattrib->hdrlen = WLAN_HDR_A3_LEN;
 282         pattrib->subtype = WIFI_DATA;
 283         pattrib->priority = 0;
 284         pattrib->qsel = pattrib->priority;
 285 //      do_queue_select(padapter, pattrib);
 286         pattrib->nr_frags = 1;
 287         pattrib->encrypt = 0;
 288         pattrib->bswenc = _FALSE;
 289         pattrib->qos_en = _FALSE;
 290 }
 291
 292 s32 init_mp_priv(PADAPTER padapter)
 293 {
 294         struct mp_priv *pmppriv = &padapter->mppriv;
 295
 296         _init_mp_priv_(pmppriv);
 297         pmppriv->papdater = padapter;
 298
 299         pmppriv->tx.stop = 1;
 300         mp_init_xmit_attrib(&pmppriv->tx, padapter);
 301
 302         switch (padapter->registrypriv.rf_config) {
 303                 case RF_1T1R:
 304                         pmppriv->antenna_tx = ANTENNA_A;
 305                         pmppriv->antenna_rx = ANTENNA_A;
 306                         break;
 307                 case RF_1T2R:
 308                 default:
 309                         pmppriv->antenna_tx = ANTENNA_A;
 310                         pmppriv->antenna_rx = ANTENNA_AB;
 311                         break;
 312                 case RF_2T2R:
 313                 case RF_2T2R_GREEN:
 314                         pmppriv->antenna_tx = ANTENNA_AB;
 315                         pmppriv->antenna_rx = ANTENNA_AB;
 316                         break;
 317                 case RF_2T4R:
 318                         pmppriv->antenna_tx = ANTENNA_AB;
 319                         pmppriv->antenna_rx = ANTENNA_ABCD;
 320                         break;
 321         }
 322
 323         return _SUCCESS;
 324 }
 325
 326 void free_mp_priv(struct mp_priv *pmp_priv)
 327 {
 328         if (pmp_priv->pallocated_mp_xmitframe_buf) {
 329                 rtw_mfree(pmp_priv->pallocated_mp_xmitframe_buf, 0);
 330                 pmp_priv->pallocated_mp_xmitframe_buf = NULL;
 331         }
 332         pmp_priv->pmp_xmtframe_buf = NULL;
 333 }
 334
 335 #ifdef CONFIG_RTL8192C
 336 #define PHY_IQCalibrate(a,b)    rtl8192c_PHY_IQCalibrate(a,b)
 337 #define PHY_LCCalibrate(a)      rtl8192c_PHY_LCCalibrate(a)
 338 #define dm_CheckTXPowerTracking(a)      rtl8192c_dm_CheckTXPowerTracking(a)
 339 #define PHY_SetRFPathSwitch(a,b)        rtl8192c_PHY_SetRFPathSwitch(a,b)
 340 #endif
 341
 342 #ifdef CONFIG_RTL8192D
 343 #define PHY_IQCalibrate(a)      rtl8192d_PHY_IQCalibrate(a)
 344 #define PHY_LCCalibrate(a)      rtl8192d_PHY_LCCalibrate(a)
 345 #define dm_CheckTXPowerTracking(a)      rtl8192d_dm_CheckTXPowerTracking(a)
 346 #define PHY_SetRFPathSwitch(a,b)        rtl8192d_PHY_SetRFPathSwitch(a,b)
 347 #endif
 348
 349 s32
 350 MPT_InitializeAdapter(
 351         IN      PADAPTER                        pAdapter,
 352         IN      u8                              Channel
 353         )
 354 {
 355         HAL_DATA_TYPE   *pHalData = GET_HAL_DATA(pAdapter);
 356         s32             rtStatus = _SUCCESS;
 357         PMPT_CONTEXT    pMptCtx = &pAdapter->mppriv.MptCtx;
 358         u32             ledsetting;
 359
 360         //-------------------------------------------------------------------------
 361         // HW Initialization for 8190 MPT.
 362         //-------------------------------------------------------------------------
 363         //-------------------------------------------------------------------------
 364         // SW Initialization for 8190 MP.
 365         //-------------------------------------------------------------------------
 366         pMptCtx->bMptDrvUnload = _FALSE;
 367         pMptCtx->bMassProdTest = _FALSE;
 368         pMptCtx->bMptIndexEven = _TRUE; //default gain index is -6.0db
 369
 370         /* Init mpt event. */
 371 #if 0 // for Windows
 372         NdisInitializeEvent( &(pMptCtx->MptWorkItemEvent) );
 373         NdisAllocateSpinLock( &(pMptCtx->MptWorkItemSpinLock) );
 374
 375         PlatformInitializeWorkItem(
 376                 Adapter,
 377                 &(pMptCtx->MptWorkItem),
 378                 (RT_WORKITEM_CALL_BACK)MPT_WorkItemCallback,
 379                 (PVOID)Adapter,
 380                 "MptWorkItem");
 381 #endif
 382         pMptCtx->bMptWorkItemInProgress = _FALSE;
 383         pMptCtx->CurrMptAct = NULL;
 384         //-------------------------------------------------------------------------
 385
 386 #if 1
 387         // Don't accept any packets
 388         rtw_write32(pAdapter, REG_RCR, 0);
 389 #else
 390         // Accept CRC error and destination address
 391         pHalData->ReceiveConfig |= (RCR_ACRC32|RCR_AAP);
 392         rtw_write32(pAdapter, REG_RCR, pHalData->ReceiveConfig);
 393 #endif
 394
 395 #if 0
 396         // If EEPROM or EFUSE is empty,we assign as RF 2T2R for MP.
 397         if (pHalData->AutoloadFailFlag == TRUE)
 398         {
 399                 pHalData->RF_Type = RF_2T2R;
 400         }
 401 #endif
 402         ledsetting = rtw_read32(pAdapter, REG_LEDCFG0);
 403         rtw_write32(pAdapter, REG_LEDCFG0, ledsetting & ~LED0DIS);
 404
 405 #ifdef CONFIG_RTL8192C
 406         PHY_IQCalibrate(pAdapter, _FALSE);
 407         dm_CheckTXPowerTracking(pAdapter);      //trigger thermal meter
 408         PHY_LCCalibrate(pAdapter);
 409 #endif
 410
 411 #ifdef CONFIG_RTL8192D
 412         PHY_IQCalibrate(pAdapter);
 413         dm_CheckTXPowerTracking(pAdapter);      //trigger thermal meter
 414         PHY_LCCalibrate(pAdapter);
 415 #endif
 416
 417 #ifdef CONFIG_PCI_HCI
 418         PHY_SetRFPathSwitch(pAdapter, 1/*pHalData->bDefaultAntenna*/);  //Wifi default use Main
 419 #else
 420
 421 #ifdef CONFIG_RTL8192C
 422 #if 1
 423         if (pHalData->BoardType == BOARD_MINICARD)
 424                 PHY_SetRFPathSwitch(pAdapter, 1/*pHalData->bDefaultAntenna*/); //default use Main
 425 #else
 426         if(pAdapter->HalFunc.GetInterfaceSelectionHandler(pAdapter) == INTF_SEL2_MINICARD )
 427                 PHY_SetRFPathSwitch(Adapter, pAdapter->MgntInfo.bDefaultAntenna); //default use Main
 428 #endif
 429
 430 #endif
 431
 432 #endif
 433
 434         pMptCtx->backup0xc50 = (u1Byte)PHY_QueryBBReg(pAdapter, rOFDM0_XAAGCCore1, bMaskByte0);
 435         pMptCtx->backup0xc58 = (u1Byte)PHY_QueryBBReg(pAdapter, rOFDM0_XBAGCCore1, bMaskByte0);
 436         pMptCtx->backup0xc30 = (u1Byte)PHY_QueryBBReg(pAdapter, rOFDM0_RxDetector1, bMaskByte0);
 437
 438         return  rtStatus;
 439 }
 440
 441 /*-----------------------------------------------------------------------------
 442  * Function:    MPT_DeInitAdapter()
 443  *
 444  * Overview:    Extra DeInitialization for Mass Production Test.
 445  *
 446  * Input:               PADAPTER        pAdapter
 447  *
 448  * Output:              NONE
 449  *
 450  * Return:              NONE
 451  *
 452  * Revised History:
 453  *      When            Who             Remark
 454  *      05/08/2007      MHC             Create Version 0.
 455  *      05/18/2007      MHC             Add normal driver MPHalt code.
 456  *
 457  *---------------------------------------------------------------------------*/
 458 VOID
 459 MPT_DeInitAdapter(
 460         IN      PADAPTER        pAdapter
 461         )
 462 {
 463         PMPT_CONTEXT            pMptCtx = &pAdapter->mppriv.MptCtx;
 464
 465         pMptCtx->bMptDrvUnload = _TRUE;
 466 #if 0 // for Windows
 467         PlatformFreeWorkItem( &(pMptCtx->MptWorkItem) );
 468
 469         while(pMptCtx->bMptWorkItemInProgress)
 470         {
 471                 if(NdisWaitEvent(&(pMptCtx->MptWorkItemEvent), 50))
 472                 {
 473                         break;
 474                 }
 475         }
 476         NdisFreeSpinLock( &(pMptCtx->MptWorkItemSpinLock) );
 477 #endif
 478 }
 479
 480 static u8 mpt_ProStartTest(PADAPTER padapter)
 481 {
 482         PMPT_CONTEXT pMptCtx = &padapter->mppriv.MptCtx;
 483
 484         pMptCtx->bMassProdTest = _TRUE;
 485         pMptCtx->bStartContTx = _FALSE;
 486         pMptCtx->bCckContTx = _FALSE;
 487         pMptCtx->bOfdmContTx = _FALSE;
 488         pMptCtx->bSingleCarrier = _FALSE;
 489         pMptCtx->bCarrierSuppression = _FALSE;
 490         pMptCtx->bSingleTone = _FALSE;
 491
 492         return _SUCCESS;
 493 }
 494
 495 /*
 496  * General use
 497  */
 498 s32 SetPowerTracking(PADAPTER padapter, u8 enable)
 499 {
 500
 501         Hal_SetPowerTracking( padapter, enable );
 502         return 0;
 503 }
 504
 505 void GetPowerTracking(PADAPTER padapter, u8 *enable)
 506 {
 507         Hal_GetPowerTracking( padapter, enable );
 508 }
 509
 510 static void disable_dm(PADAPTER padapter)
 511 {
 512 #ifndef CONFIG_RTL8723A
 513         u8 v8;
 514 #endif
 515         HAL_DATA_TYPE   *pHalData = GET_HAL_DATA(padapter);
 516         struct dm_priv  *pdmpriv = &pHalData->dmpriv;
 517
 518
 519         //3 1. disable firmware dynamic mechanism
 520         // disable Power Training, Rate Adaptive
 521 #ifdef CONFIG_RTL8723A
 522         SetBcnCtrlReg(padapter, 0, EN_BCN_FUNCTION);
 523 #else
 524         v8 = rtw_read8(padapter, REG_BCN_CTRL);
 525         v8 &= ~EN_BCN_FUNCTION;
 526         rtw_write8(padapter, REG_BCN_CTRL, v8);
 527 #endif
 528
 529         //3 2. disable driver dynamic mechanism
 530         // disable Dynamic Initial Gain
 531         // disable High Power
 532         // disable Power Tracking
 533         Switch_DM_Func(padapter, DYNAMIC_FUNC_DISABLE, _FALSE);
 534
 535         // enable APK, LCK and IQK but disable power tracking
 536         pdmpriv->TxPowerTrackControl = _FALSE;
 537         Switch_DM_Func(padapter, DYNAMIC_FUNC_SS, _TRUE);
 538 }
 539
 540 //This function initializes the DUT to the MP test mode
 541 s32 mp_start_test(PADAPTER padapter)
 542 {
 543         WLAN_BSSID_EX bssid;
 544         struct sta_info *psta;
 545         u32 length;
 546         u8 val8;
 547
 548         _irqL irqL;
 549         s32 res = _SUCCESS;
 550
 551         struct mp_priv *pmppriv = &padapter->mppriv;
 552         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 553         struct wlan_network *tgt_network = &pmlmepriv->cur_network;
 554
 555
 556         //3 disable dynamic mechanism
 557         disable_dm(padapter);
 558
 559         //3 0. update mp_priv
 560 #if  defined (CONFIG_RTL8192C)  ||  defined (CONFIG_RTL8192D)
 561         if (padapter->registrypriv.rf_config == RF_819X_MAX_TYPE) {
 562 //              HAL_DATA_TYPE *phal = GET_HAL_DATA(padapter);
 563 //              switch (phal->rf_type) {
 564                 switch (GET_RF_TYPE(padapter)) {
 565                         case RF_1T1R:
 566                                 pmppriv->antenna_tx = ANTENNA_A;
 567                                 pmppriv->antenna_rx = ANTENNA_A;
 568                                 break;
 569                         case RF_1T2R:
 570                         default:
 571                                 pmppriv->antenna_tx = ANTENNA_A;
 572                                 pmppriv->antenna_rx = ANTENNA_AB;
 573                                 break;
 574                         case RF_2T2R:
 575                         case RF_2T2R_GREEN:
 576                                 pmppriv->antenna_tx = ANTENNA_AB;
 577                                 pmppriv->antenna_rx = ANTENNA_AB;
 578                                 break;
 579                         case RF_2T4R:
 580                                 pmppriv->antenna_tx = ANTENNA_AB;
 581                                 pmppriv->antenna_rx = ANTENNA_ABCD;
 582                                 break;
 583                 }
 584         }
 585 #endif
 586         mpt_ProStartTest(padapter);
 587
 588         //3 1. initialize a new WLAN_BSSID_EX
 589 //      _rtw_memset(&bssid, 0, sizeof(WLAN_BSSID_EX));
 590         _rtw_memcpy(bssid.MacAddress, pmppriv->network_macaddr, ETH_ALEN);
 591         bssid.Ssid.SsidLength = strlen("mp_pseudo_adhoc");
 592         _rtw_memcpy(bssid.Ssid.Ssid, (u8*)"mp_pseudo_adhoc", bssid.Ssid.SsidLength);
 593         bssid.InfrastructureMode = Ndis802_11IBSS;
 594         bssid.NetworkTypeInUse = Ndis802_11DS;
 595         bssid.IELength = 0;
 596
 597         length = get_WLAN_BSSID_EX_sz(&bssid);
 598         if (length % 4)
 599                 bssid.Length = ((length >> 2) + 1) << 2; //round up to multiple of 4 bytes.
 600         else
 601                 bssid.Length = length;
 602
 603         _enter_critical_bh(&pmlmepriv->lock, &irqL);
 604
 605         if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE)
 606                 goto end_of_mp_start_test;
 607
 608         //init mp_start_test status
 609         if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE) {
 610                 rtw_disassoc_cmd(padapter, 500, _TRUE);
 611                 rtw_indicate_disconnect(padapter);
 612                 rtw_free_assoc_resources(padapter, 1);
 613         }
 614         pmppriv->prev_fw_state = get_fwstate(pmlmepriv);
 615         pmlmepriv->fw_state = WIFI_MP_STATE;
 616 #if 0
 617         if (pmppriv->mode == _LOOPBOOK_MODE_) {
 618                 set_fwstate(pmlmepriv, WIFI_MP_LPBK_STATE); //append txdesc
 619                 RT_TRACE(_module_mp_, _drv_notice_, ("+start mp in Lookback mode\n"));
 620         } else {
 621                 RT_TRACE(_module_mp_, _drv_notice_, ("+start mp in normal mode\n"));
 622         }
 623 #endif
 624         set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 625
 626         //3 2. create a new psta for mp driver
 627         //clear psta in the cur_network, if any
 628         psta = rtw_get_stainfo(&padapter->stapriv, tgt_network->network.MacAddress);
 629         if (psta) rtw_free_stainfo(padapter, psta);
 630
 631         psta = rtw_alloc_stainfo(&padapter->stapriv, bssid.MacAddress);
 632         if (psta == NULL) {
 633                 RT_TRACE(_module_mp_, _drv_err_, ("mp_start_test: Can't alloc sta_info!\n"));
 634                 pmlmepriv->fw_state = pmppriv->prev_fw_state;
 635                 res = _FAIL;
 636                 goto end_of_mp_start_test;
 637         }
 638
 639         //3 3. join psudo AdHoc
 640         tgt_network->join_res = 1;
 641         tgt_network->aid = psta->aid = 1;
 642         _rtw_memcpy(&tgt_network->network, &bssid, length);
 643
 644         rtw_indicate_connect(padapter);
 645         _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 646
 647 end_of_mp_start_test:
 648
 649         _exit_critical_bh(&pmlmepriv->lock, &irqL);
 650
 651         if (res == _SUCCESS)
 652         {
 653                 // set MSR to WIFI_FW_ADHOC_STATE
 654 #if  defined (CONFIG_RTL8192C)  ||  defined (CONFIG_RTL8192D)
 655                 val8 = rtw_read8(padapter, MSR) & 0xFC; // 0x0102
 656                 val8 |= WIFI_FW_ADHOC_STATE;
 657                 rtw_write8(padapter, MSR, val8); // Link in ad hoc network
 658 #endif
 659
 660 #if  !defined (CONFIG_RTL8192C)  &&  !defined (CONFIG_RTL8192D)
 661                 rtw_write8(padapter, MSR, 1); // Link in ad hoc network
 662                 rtw_write8(padapter, RCR, 0); // RCR : disable all pkt, 0x10250048
 663                 rtw_write8(padapter, RCR+2, 0x57); // RCR disable Check BSSID, 0x1025004a
 664
 665                 // disable RX filter map , mgt frames will put in RX FIFO 0
 666                 rtw_write16(padapter, RXFLTMAP0, 0x0); // 0x10250116
 667
 668                 val8 = rtw_read8(padapter, EE_9346CR); // 0x1025000A
 669                 if (!(val8 & _9356SEL))//boot from EFUSE
 670                         efuse_change_max_size(padapter);
 671 #endif
 672         }
 673
 674         return res;
 675 }
 676 //------------------------------------------------------------------------------
 677 //This function change the DUT from the MP test mode into normal mode
 678 void mp_stop_test(PADAPTER padapter)
 679 {
 680         struct mp_priv *pmppriv = &padapter->mppriv;
 681         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 682         struct wlan_network *tgt_network = &pmlmepriv->cur_network;
 683         struct sta_info *psta;
 684
 685         _irqL irqL;
 686
 687
 688         _enter_critical_bh(&pmlmepriv->lock, &irqL);
 689
 690         if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _FALSE)
 691                 goto end_of_mp_stop_test;
 692
 693         //3 1. disconnect psudo AdHoc
 694         rtw_indicate_disconnect(padapter);
 695
 696         //3 2. clear psta used in mp test mode.
 697 //      rtw_free_assoc_resources(padapter, 1);
 698         psta = rtw_get_stainfo(&padapter->stapriv, tgt_network->network.MacAddress);
 699         if (psta) rtw_free_stainfo(padapter, psta);
 700
 701         //3 3. return to normal state (default:station mode)
 702         pmlmepriv->fw_state = pmppriv->prev_fw_state; // WIFI_STATION_STATE;
 703
 704         //flush the cur_network
 705         _rtw_memset(tgt_network, 0, sizeof(struct wlan_network));
 706
 707         _clr_fwstate_(pmlmepriv, WIFI_MP_STATE);
 708
 709 end_of_mp_stop_test:
 710
 711         _exit_critical_bh(&pmlmepriv->lock, &irqL);
 712 }
 713 /*---------------------------hal\rtl8192c\MPT_Phy.c---------------------------*/
 714 #if 0
 715 //#ifdef CONFIG_USB_HCI
 716 static VOID mpt_AdjustRFRegByRateByChan92CU(PADAPTER pAdapter, u8 RateIdx, u8 Channel, u8 BandWidthID)
 717 {
 718         u8              eRFPath;
 719         u32             rfReg0x26;
 720         HAL_DATA_TYPE   *pHalData = GET_HAL_DATA(pAdapter);
 721
 722
 723         if (RateIdx < MPT_RATE_6M) {    // CCK rate,for 88cu
 724                 rfReg0x26 = 0xf400;
 725         }
 726         else if ((RateIdx >= MPT_RATE_6M) && (RateIdx <= MPT_RATE_54M)) {// OFDM rate,for 88cu
 727                 if ((4 == Channel) || (8 == Channel) || (12 == Channel))
 728                         rfReg0x26 = 0xf000;
 729                 else if ((5 == Channel) || (7 == Channel) || (13 == Channel) || (14 == Channel))
 730                         rfReg0x26 = 0xf400;
 731                 else
 732                         rfReg0x26 = 0x4f200;
 733         }
 734         else if ((RateIdx >= MPT_RATE_MCS0) && (RateIdx <= MPT_RATE_MCS15)) {// MCS 20M ,for 88cu // MCS40M rate,for 88cu
 735
 736                 if (HT_CHANNEL_WIDTH_20 == BandWidthID) {
 737                         if ((4 == Channel) || (8 == Channel))
 738                                 rfReg0x26 = 0xf000;
 739                         else if ((5 == Channel) || (7 == Channel) || (13 == Channel) || (14 == Channel))
 740                                 rfReg0x26 = 0xf400;
 741                         else
 742                                 rfReg0x26 = 0x4f200;
 743                 }
 744                 else{
 745                         if ((4 == Channel) || (8 == Channel))
 746                                 rfReg0x26 = 0xf000;
 747                         else if ((5 == Channel) || (7 == Channel))
 748                                 rfReg0x26 = 0xf400;
 749                         else
 750                                 rfReg0x26 = 0x4f200;
 751                 }
 752         }
 753
 754 //      RT_TRACE(COMP_CMD, DBG_LOUD, ("\n mpt_AdjustRFRegByRateByChan92CU():Chan:%d Rate=%d rfReg0x26:0x%08x\n",Channel, RateIdx,rfReg0x26));
 755         for (eRFPath = 0; eRFPath < pHalData->NumTotalRFPath; eRFPath++) {
 756                 write_rfreg(pAdapter, eRFPath, RF_SYN_G2, rfReg0x26);
 757         }
 758 }
 759 #endif
 760 /*-----------------------------------------------------------------------------
 761  * Function:    mpt_SwitchRfSetting
 762  *
 763  * Overview:    Change RF Setting when we siwthc channel/rate/BW for MP.
 764  *
 765  * Input:       IN      PADAPTER                                pAdapter
 766  *
 767  * Output:      NONE
 768  *
 769  * Return:      NONE
 770  *
 771  * Revised History:
 772  * When                 Who             Remark
 773  * 01/08/2009   MHC             Suggestion from SD3 Willis for 92S series.
 774  * 01/09/2009   MHC             Add CCK modification for 40MHZ. Suggestion from SD3.
 775  *
 776  *---------------------------------------------------------------------------*/
 777 static void mpt_SwitchRfSetting(PADAPTER pAdapter)
 778 {
 779         Hal_mpt_SwitchRfSetting(pAdapter);
 780     }
 781
 782 /*---------------------------hal\rtl8192c\MPT_Phy.c---------------------------*/
 783 /*---------------------------hal\rtl8192c\MPT_HelperFunc.c---------------------------*/
 784 static void MPT_CCKTxPowerAdjust(PADAPTER Adapter, BOOLEAN bInCH14)
 785 {
 786         Hal_MPT_CCKTxPowerAdjust(Adapter,bInCH14);
 787 }
 788
 789 static void MPT_CCKTxPowerAdjustbyIndex(PADAPTER pAdapter, BOOLEAN beven)
 790 {
 791         Hal_MPT_CCKTxPowerAdjustbyIndex(pAdapter,beven);
 792         }
 793
 794 /*---------------------------hal\rtl8192c\MPT_HelperFunc.c---------------------------*/
 795
 796 /*
 797  * SetChannel
 798  * Description
 799  *      Use H2C command to change channel,
 800  *      not only modify rf register, but also other setting need to be done.
 801  */
 802 void SetChannel(PADAPTER pAdapter)
 803 {
 804         Hal_SetChannel(pAdapter);
 805
 806 }
 807
 808 /*
 809  * Notice
 810  *      Switch bandwitdth may change center frequency(channel)
 811  */
 812 void SetBandwidth(PADAPTER pAdapter)
 813 {
 814         Hal_SetBandwidth(pAdapter);
 815
 816 }
 817
 818 static void SetCCKTxPower(PADAPTER pAdapter, u8 *TxPower)
 819 {
 820         Hal_SetCCKTxPower(pAdapter,TxPower);
 821 }
 822
 823 static void SetOFDMTxPower(PADAPTER pAdapter, u8 *TxPower)
 824 {
 825         Hal_SetOFDMTxPower(pAdapter,TxPower);
 826         }
 827
 828
 829 void SetAntenna(PADAPTER pAdapter)
 830         {
 831         Hal_SetAntenna(pAdapter);
 832 }
 833
 834 void    SetAntennaPathPower(PADAPTER pAdapter)
 835 {
 836         Hal_SetAntennaPathPower(pAdapter);
 837 }
 838
 839 void SetTxPower(PADAPTER pAdapter)
 840 {
 841         Hal_SetTxPower(pAdapter);
 842         }
 843
 844 void SetTxAGCOffset(PADAPTER pAdapter, u32 ulTxAGCOffset)
 845 {
 846         u32 TxAGCOffset_B, TxAGCOffset_C, TxAGCOffset_D,tmpAGC;
 847
 848         TxAGCOffset_B = (ulTxAGCOffset&0x000000ff);
 849         TxAGCOffset_C = ((ulTxAGCOffset&0x0000ff00)>>8);
 850         TxAGCOffset_D = ((ulTxAGCOffset&0x00ff0000)>>16);
 851
 852         tmpAGC = (TxAGCOffset_D<<8 | TxAGCOffset_C<<4 | TxAGCOffset_B);
 853         write_bbreg(pAdapter, rFPGA0_TxGainStage,
 854                         (bXBTxAGC|bXCTxAGC|bXDTxAGC), tmpAGC);
 855 }
 856
 857 void SetDataRate(PADAPTER pAdapter)
 858 {
 859         Hal_SetDataRate(pAdapter);
 860 }
 861
 862 #if  !defined (CONFIG_RTL8192C)  &&  !defined (CONFIG_RTL8192D)
 863 /*------------------------------Define structure----------------------------*/
 864 typedef struct _R_ANTENNA_SELECT_OFDM {
 865         u32     r_tx_antenna:4;
 866         u32     r_ant_l:4;
 867         u32     r_ant_non_ht:4;
 868         u32     r_ant_ht1:4;
 869         u32     r_ant_ht2:4;
 870         u32     r_ant_ht_s1:4;
 871         u32     r_ant_non_ht_s1:4;
 872         u32     OFDM_TXSC:2;
 873         u32     Reserved:2;
 874 }R_ANTENNA_SELECT_OFDM;
 875
 876 typedef struct _R_ANTENNA_SELECT_CCK {
 877         u8      r_cckrx_enable_2:2;
 878         u8      r_cckrx_enable:2;
 879         u8      r_ccktx_enable:4;
 880 }R_ANTENNA_SELECT_CCK;
 881 #endif
 882
 883 s32 SetThermalMeter(PADAPTER pAdapter, u8 target_ther)
 884 {
 885         return Hal_SetThermalMeter( pAdapter, target_ther);
 886 }
 887
 888 static void TriggerRFThermalMeter(PADAPTER pAdapter)
 889 {
 890         Hal_TriggerRFThermalMeter(pAdapter);
 891 }
 892
 893 static u8 ReadRFThermalMeter(PADAPTER pAdapter)
 894 {
 895         return Hal_ReadRFThermalMeter(pAdapter);
 896 }
 897
 898 void GetThermalMeter(PADAPTER pAdapter, u8 *value)
 899 {
 900         Hal_GetThermalMeter(pAdapter,value);
 901 }
 902
 903 void SetSingleCarrierTx(PADAPTER pAdapter, u8 bStart)
 904 {
 905         Hal_SetSingleCarrierTx(pAdapter,bStart);
 906 }
 907
 908 void SetSingleToneTx(PADAPTER pAdapter, u8 bStart)
 909 {
 910         Hal_SetSingleToneTx(pAdapter,bStart);
 911 }
 912
 913 void SetCarrierSuppressionTx(PADAPTER pAdapter, u8 bStart)
 914 {
 915         Hal_SetCarrierSuppressionTx(pAdapter, bStart);
 916 }
 917
 918 void SetCCKContinuousTx(PADAPTER pAdapter, u8 bStart)
 919                 {
 920         Hal_SetCCKContinuousTx(pAdapter,bStart);
 921                 }
 922
 923 void SetOFDMContinuousTx(PADAPTER pAdapter, u8 bStart)
 924         {
 925    Hal_SetOFDMContinuousTx( pAdapter, bStart);
 926 }/* mpt_StartOfdmContTx */
 927
 928 void SetContinuousTx(PADAPTER pAdapter, u8 bStart)
 929 {
 930         Hal_SetContinuousTx(pAdapter,bStart);
 931 }
 932
 933 //------------------------------------------------------------------------------
 934 static void dump_mpframe(PADAPTER padapter, struct xmit_frame *pmpframe)
 935 {
 936         rtw_hal_mgnt_xmit(padapter, pmpframe);
 937 }
 938
 939 static struct xmit_frame *alloc_mp_xmitframe(struct xmit_priv *pxmitpriv)
 940 {
 941         struct xmit_frame       *pmpframe;
 942         struct xmit_buf *pxmitbuf;
 943
 944         if ((pmpframe = rtw_alloc_xmitframe(pxmitpriv)) == NULL)
 945         {
 946                 return NULL;
 947         }
 948
 949         if ((pxmitbuf = rtw_alloc_xmitbuf(pxmitpriv)) == NULL)
 950         {
 951                 rtw_free_xmitframe(pxmitpriv, pmpframe);
 952                 return NULL;
 953         }
 954
 955         pmpframe->frame_tag = MP_FRAMETAG;
 956
 957         pmpframe->pxmitbuf = pxmitbuf;
 958
 959         pmpframe->buf_addr = pxmitbuf->pbuf;
 960
 961         pxmitbuf->priv_data = pmpframe;
 962
 963         return pmpframe;
 964
 965 }
 966
 967 static thread_return mp_xmit_packet_thread(thread_context context)
 968 {
 969         struct xmit_frame       *pxmitframe;
 970         struct mp_tx            *pmptx;
 971         struct mp_priv  *pmp_priv;
 972         struct xmit_priv        *pxmitpriv;
 973         PADAPTER padapter;
 974
 975         pmp_priv = (struct mp_priv *)context;
 976         pmptx = &pmp_priv->tx;
 977         padapter = pmp_priv->papdater;
 978         pxmitpriv = &(padapter->xmitpriv);
 979
 980         thread_enter("RTW_MP_THREAD");
 981
 982         DBG_871X("%s:pkTx Start\n", __func__);
 983         while (1) {
 984                 pxmitframe = alloc_mp_xmitframe(pxmitpriv);
 985                 if (pxmitframe == NULL) {
 986                         if (pmptx->stop ||
 987                             padapter->bSurpriseRemoved ||
 988                             padapter->bDriverStopped) {
 989                                 goto exit;
 990                         }
 991                         else {
 992                                 rtw_msleep_os(1);
 993                                 continue;
 994                         }
 995                 }
 996
 997                 _rtw_memcpy((u8 *)(pxmitframe->buf_addr+TXDESC_OFFSET), pmptx->buf, pmptx->write_size);
 998                 _rtw_memcpy(&(pxmitframe->attrib), &(pmptx->attrib), sizeof(struct pkt_attrib));
 999
1000                 dump_mpframe(padapter, pxmitframe);
1001
1002                 pmptx->sended++;
1003                 pmp_priv->tx_pktcount++;
1004
1005                 if (pmptx->stop ||
1006                     padapter->bSurpriseRemoved ||
1007                     padapter->bDriverStopped)
1008                         goto exit;
1009                 if ((pmptx->count != 0) &&
1010                     (pmptx->count == pmptx->sended))
1011                         goto exit;
1012
1013                 flush_signals_thread();
1014         }
1015
1016 exit:
1017         //DBG_871X("%s:pkTx Exit\n", __func__);
1018         rtw_mfree(pmptx->pallocated_buf, pmptx->buf_size);
1019         pmptx->pallocated_buf = NULL;
1020         pmptx->stop = 1;
1021
1022         thread_exit();
1023 }
1024
1025 void fill_txdesc_for_mp(PADAPTER padapter, struct tx_desc *ptxdesc)
1026 {
1027         struct mp_priv *pmp_priv = &padapter->mppriv;
1028         _rtw_memcpy(ptxdesc, &(pmp_priv->tx.desc), TXDESC_SIZE);
1029 }
1030
1031 void SetPacketTx(PADAPTER padapter)
1032 {
1033         u8 *ptr, *pkt_start, *pkt_end;
1034         u32 pkt_size;
1035         struct tx_desc *desc;
1036         struct rtw_ieee80211_hdr *hdr;
1037         u8 payload;
1038         s32 bmcast;
1039         struct pkt_attrib *pattrib;
1040         struct mp_priv *pmp_priv;
1041
1042
1043         pmp_priv = &padapter->mppriv;
1044         if (pmp_priv->tx.stop) return;
1045         pmp_priv->tx.sended = 0;
1046         pmp_priv->tx.stop = 0;
1047         pmp_priv->tx_pktcount = 0;
1048
1049         //3 1. update_attrib()
1050         pattrib = &pmp_priv->tx.attrib;
1051         _rtw_memcpy(pattrib->src, padapter->eeprompriv.mac_addr, ETH_ALEN);
1052         _rtw_memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
1053         _rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
1054         bmcast = IS_MCAST(pattrib->ra);
1055         if (bmcast) {
1056                 pattrib->mac_id = 1;
1057                 pattrib->psta = rtw_get_bcmc_stainfo(padapter);
1058         } else {
1059                 pattrib->mac_id = 0;
1060                 pattrib->psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(&padapter->mlmepriv));
1061         }
1062
1063         pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->pktlen;
1064
1065         //3 2. allocate xmit buffer
1066         pkt_size = pattrib->last_txcmdsz;
1067
1068         if (pmp_priv->tx.pallocated_buf)
1069                 rtw_mfree(pmp_priv->tx.pallocated_buf, pmp_priv->tx.buf_size);
1070         pmp_priv->tx.write_size = pkt_size;
1071         pmp_priv->tx.buf_size = pkt_size + XMITBUF_ALIGN_SZ;
1072         pmp_priv->tx.pallocated_buf = rtw_zmalloc(pmp_priv->tx.buf_size);
1073         if (pmp_priv->tx.pallocated_buf == NULL) {
1074                 DBG_871X("%s: malloc(%d) fail!!\n", __func__, pmp_priv->tx.buf_size);
1075                 return;
1076         }
1077         pmp_priv->tx.buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pmp_priv->tx.pallocated_buf), XMITBUF_ALIGN_SZ);
1078         ptr = pmp_priv->tx.buf;
1079
1080         desc = &(pmp_priv->tx.desc);
1081         _rtw_memset(desc, 0, TXDESC_SIZE);
1082         pkt_start = ptr;
1083         pkt_end = pkt_start + pkt_size;
1084
1085         //3 3. init TX descriptor
1086         // offset 0
1087         //desc->txdw0 |= cpu_to_le32(pkt_size & 0x0000FFFF); // packet size
1088         //desc->txdw0 |= cpu_to_le32(OWN | FSG | LSG);
1089         //desc->txdw0 |= cpu_to_le32(((TXDESC_SIZE + OFFSET_SZ) << OFFSET_SHT) & 0x00FF0000); //32 bytes for TX Desc
1090         //if (bmcast) desc->txdw0 |= cpu_to_le32(BMC); // broadcast packet
1091
1092         // offset 4
1093         desc->txdw1 |= cpu_to_le32(BK); // don't aggregate(AMPDU)
1094         desc->txdw1 |= cpu_to_le32((pattrib->mac_id) & 0x1F); //CAM_ID(MAC_ID)
1095         desc->txdw1 |= cpu_to_le32((pattrib->qsel << QSEL_SHT) & 0x00001F00); // Queue Select, TID
1096         desc->txdw1 |= cpu_to_le32((pattrib->raid << Rate_ID_SHT) & 0x000F0000); // Rate Adaptive ID
1097
1098         // offset 8
1099         // offset 12
1100         //desc->txdw3 |= cpu_to_le32((pattrib->seqnum << SEQ_SHT) & 0xffff0000);
1101
1102         // offset 16
1103         //desc->txdw4 |= cpu_to_le32(QoS);
1104         desc->txdw4 |= cpu_to_le32(HW_SEQ_EN);
1105         desc->txdw4 |= cpu_to_le32(USERATE);
1106         desc->txdw4 |= cpu_to_le32(DISDATAFB);
1107
1108         if( pmp_priv->preamble ){
1109                 if (pmp_priv->rateidx <=  MPT_RATE_54M)
1110                         desc->txdw4 |= cpu_to_le32(DATA_SHORT); // CCK Short Preamble
1111         }
1112         if (pmp_priv->bandwidth == HT_CHANNEL_WIDTH_40)
1113                 desc->txdw4 |= cpu_to_le32(DATA_BW);
1114
1115         // offset 20
1116         desc->txdw5 |= cpu_to_le32(pmp_priv->rateidx & 0x0000001F);
1117
1118         if( pmp_priv->preamble ){
1119                 if (pmp_priv->rateidx > MPT_RATE_54M)
1120                         desc->txdw5 |= cpu_to_le32(SGI); // MCS Short Guard Interval
1121         }
1122         desc->txdw5 |= cpu_to_le32(0x0001FF00); // DATA/RTS Rate Fallback Limit
1123
1124         //3 4. make wlan header, make_wlanhdr()
1125         hdr = (struct rtw_ieee80211_hdr *)pkt_start;
1126         SetFrameSubType(&hdr->frame_ctl, pattrib->subtype);
1127         _rtw_memcpy(hdr->addr1, pattrib->dst, ETH_ALEN); // DA
1128         _rtw_memcpy(hdr->addr2, pattrib->src, ETH_ALEN); // SA
1129         _rtw_memcpy(hdr->addr3, get_bssid(&padapter->mlmepriv), ETH_ALEN); // RA, BSSID
1130
1131         //3 5. make payload
1132         ptr = pkt_start + pattrib->hdrlen;
1133
1134         switch (pmp_priv->tx.payload) {
1135                 case 0:
1136                         payload = 0x00;
1137                         break;
1138                 case 1:
1139                         payload = 0x5a;
1140                         break;
1141                 case 2:
1142                         payload = 0xa5;
1143                         break;
1144                 case 3:
1145                         payload = 0xff;
1146                         break;
1147                 default:
1148                         payload = 0x00;
1149                         break;
1150         }
1151
1152         _rtw_memset(ptr, payload, pkt_end - ptr);
1153
1154         //3 6. start thread
1155 #ifdef PLATFORM_LINUX
1156         pmp_priv->tx.PktTxThread = kthread_run(mp_xmit_packet_thread, pmp_priv, "RTW_MP_THREAD");
1157         if (IS_ERR(pmp_priv->tx.PktTxThread))
1158                 DBG_871X("Create PktTx Thread Fail !!!!!\n");
1159 #endif
1160 #ifdef PLATFORM_FREEBSD
1161 {
1162         struct proc *p;
1163         struct thread *td;
1164         pmp_priv->tx.PktTxThread = kproc_kthread_add(mp_xmit_packet_thread, pmp_priv,
1165                                         &p, &td, RFHIGHPID, 0, "MPXmitThread", "MPXmitThread");
1166
1167         if (pmp_priv->tx.PktTxThread < 0)
1168                 DBG_871X("Create PktTx Thread Fail !!!!!\n");
1169 }
1170 #endif
1171 }
1172
1173 void SetPacketRx(PADAPTER pAdapter, u8 bStartRx)
1174 {
1175         HAL_DATA_TYPE   *pHalData = GET_HAL_DATA(pAdapter);
1176
1177         if(bStartRx)
1178         {
1179                 pHalData->ReceiveConfig = AAP | APM | AM | AB | APP_ICV | ADF | AMF | HTC_LOC_CTRL | APP_MIC | APP_PHYSTS;
1180
1181                 pHalData->ReceiveConfig |= ACRC32;
1182
1183                 rtw_write32(pAdapter, REG_RCR, pHalData->ReceiveConfig);
1184
1185                 // Accept all data frames
1186                 rtw_write16(pAdapter, REG_RXFLTMAP2, 0xFFFF);
1187         }
1188         else
1189         {
1190                 rtw_write32(pAdapter, REG_RCR, 0);
1191         }
1192 }
1193
1194 void ResetPhyRxPktCount(PADAPTER pAdapter)
1195 {
1196         u32 i, phyrx_set = 0;
1197
1198         for (i = 0; i <= 0xF; i++) {
1199                 phyrx_set = 0;
1200                 phyrx_set |= _RXERR_RPT_SEL(i); //select
1201                 phyrx_set |= RXERR_RPT_RST;     // set counter to zero
1202                 rtw_write32(pAdapter, REG_RXERR_RPT, phyrx_set);
1203         }
1204 }
1205
1206 static u32 GetPhyRxPktCounts(PADAPTER pAdapter, u32 selbit)
1207 {
1208         //selection
1209         u32 phyrx_set = 0, count = 0;
1210
1211         phyrx_set = _RXERR_RPT_SEL(selbit & 0xF);
1212         rtw_write32(pAdapter, REG_RXERR_RPT, phyrx_set);
1213
1214         //Read packet count
1215         count = rtw_read32(pAdapter, REG_RXERR_RPT) & RXERR_COUNTER_MASK;
1216
1217         return count;
1218 }
1219
1220 u32 GetPhyRxPktReceived(PADAPTER pAdapter)
1221 {
1222         u32 OFDM_cnt = 0, CCK_cnt = 0, HT_cnt = 0;
1223
1224         OFDM_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_OFDM_MPDU_OK);
1225         CCK_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_CCK_MPDU_OK);
1226         HT_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_HT_MPDU_OK);
1227
1228         return OFDM_cnt + CCK_cnt + HT_cnt;
1229 }
1230
1231 u32 GetPhyRxPktCRC32Error(PADAPTER pAdapter)
1232 {
1233         u32 OFDM_cnt = 0, CCK_cnt = 0, HT_cnt = 0;
1234
1235         OFDM_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_OFDM_MPDU_FAIL);
1236         CCK_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_CCK_MPDU_FAIL);
1237         HT_cnt = GetPhyRxPktCounts(pAdapter, RXERR_TYPE_HT_MPDU_FAIL);
1238
1239         return OFDM_cnt + CCK_cnt + HT_cnt;
1240 }
1241
1242 //reg 0x808[9:0]: FFT data x
1243 //reg 0x808[22]:  0  -->  1  to get 1 FFT data y
1244 //reg 0x8B4[15:0]: FFT data y report
1245 static u32 GetPSDData(PADAPTER pAdapter, u32 point)
1246 {
1247         int psd_val;
1248
1249
1250         psd_val = rtw_read32(pAdapter, 0x808);
1251         psd_val &= 0xFFBFFC00;
1252         psd_val |= point;
1253
1254         rtw_write32(pAdapter, 0x808, psd_val);
1255         rtw_mdelay_os(1);
1256         psd_val |= 0x00400000;
1257
1258         rtw_write32(pAdapter, 0x808, psd_val);
1259         rtw_mdelay_os(1);
1260         psd_val = rtw_read32(pAdapter, 0x8B4);
1261
1262         psd_val &= 0x0000FFFF;
1263
1264         return psd_val;
1265 }
1266
1267 /*
1268  * pts  start_point_min         stop_point_max
1269  * 128  64                      64 + 128 = 192
1270  * 256  128                     128 + 256 = 384
1271  * 512  256                     256 + 512 = 768
1272  * 1024 512                     512 + 1024 = 1536
1273  *
1274  */
1275 u32 mp_query_psd(PADAPTER pAdapter, u8 *data)
1276 {
1277         u32 i, psd_pts=0, psd_start=0, psd_stop=0;
1278         u32 psd_data=0;
1279
1280 #ifdef PLATFORM_LINUX
1281         if (!netif_running(pAdapter->pnetdev)) {
1282                 RT_TRACE(_module_mp_, _drv_warning_, ("mp_query_psd: Fail! interface not opened!\n"));
1283                 return 0;
1284         }
1285 #endif
1286
1287         if (check_fwstate(&pAdapter->mlmepriv, WIFI_MP_STATE) == _FALSE) {
1288                 RT_TRACE(_module_mp_, _drv_warning_, ("mp_query_psd: Fail! not in MP mode!\n"));
1289                 return 0;
1290         }
1291
1292         if (strlen(data) == 0) { //default value
1293                 psd_pts = 128;
1294                 psd_start = 64;
1295                 psd_stop = 128;
1296         } else {
1297                 sscanf(data, "pts=%d,start=%d,stop=%d", &psd_pts, &psd_start, &psd_stop);
1298         }
1299
1300         _rtw_memset(data, '\0', sizeof(data));
1301
1302         i = psd_start;
1303         while (i < psd_stop)
1304         {
1305                 if (i >= psd_pts) {
1306                         psd_data = GetPSDData(pAdapter, i-psd_pts);
1307                 } else {
1308                         psd_data = GetPSDData(pAdapter, i);
1309                 }
1310                 sprintf(data, "%s%x ", data, psd_data);
1311                 i++;
1312         }
1313
1314         #ifdef CONFIG_LONG_DELAY_ISSUE
1315         rtw_msleep_os(100);
1316         #else
1317         rtw_mdelay_os(100);
1318         #endif
1319
1320         return strlen(data)+1;
1321 }
1322
1323 #endif
1324