drivers/staging/rtl8188eu/core/rtw_cmd.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 #define _RTW_CMD_C_
  21
  22 #include <osdep_service.h>
  23 #include <drv_types.h>
  24 #include <recv_osdep.h>
  25 #include <cmd_osdep.h>
  26 #include <mlme_osdep.h>
  27 #include <rtw_br_ext.h>
  28 #include <rtw_mlme_ext.h>
  29
  30 /*
  31 Caller and the rtw_cmd_thread can protect cmd_q by spin_lock.
  32 No irqsave is necessary.
  33 */
  34
  35 int     _rtw_init_cmd_priv (struct      cmd_priv *pcmdpriv)
  36 {
  37         int res = _SUCCESS;
  38
  39 _func_enter_;
  40
  41         sema_init(&(pcmdpriv->cmd_queue_sema), 0);
  42         /* sema_init(&(pcmdpriv->cmd_done_sema), 0); */
  43         sema_init(&(pcmdpriv->terminate_cmdthread_sema), 0);
  44
  45
  46         _rtw_init_queue(&(pcmdpriv->cmd_queue));
  47
  48         /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
  49
  50         pcmdpriv->cmd_seq = 1;
  51
  52         pcmdpriv->cmd_allocated_buf = rtw_zmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
  53
  54         if (pcmdpriv->cmd_allocated_buf == NULL) {
  55                 res = _FAIL;
  56                 goto exit;
  57         }
  58
  59         pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf  +  CMDBUFF_ALIGN_SZ - ((size_t)(pcmdpriv->cmd_allocated_buf) & (CMDBUFF_ALIGN_SZ-1));
  60
  61         pcmdpriv->rsp_allocated_buf = rtw_zmalloc(MAX_RSPSZ + 4);
  62
  63         if (pcmdpriv->rsp_allocated_buf == NULL) {
  64                 res = _FAIL;
  65                 goto exit;
  66         }
  67
  68         pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 - ((size_t)(pcmdpriv->rsp_allocated_buf) & 3);
  69
  70         pcmdpriv->cmd_issued_cnt = 0;
  71         pcmdpriv->cmd_done_cnt = 0;
  72         pcmdpriv->rsp_cnt = 0;
  73 exit:
  74 _func_exit_;
  75         return res;
  76 }
  77
  78 static void c2h_wk_callback(struct work_struct *work);
  79
  80 int _rtw_init_evt_priv(struct evt_priv *pevtpriv)
  81 {
  82         int res = _SUCCESS;
  83
  84 _func_enter_;
  85
  86         /* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
  87         atomic_set(&pevtpriv->event_seq, 0);
  88         pevtpriv->evt_done_cnt = 0;
  89
  90         _init_workitem(&pevtpriv->c2h_wk, c2h_wk_callback, NULL);
  91         pevtpriv->c2h_wk_alive = false;
  92         pevtpriv->c2h_queue = rtw_cbuf_alloc(C2H_QUEUE_MAX_LEN+1);
  93
  94 _func_exit_;
  95
  96         return res;
  97 }
  98
  99 void rtw_free_evt_priv(struct   evt_priv *pevtpriv)
 100 {
 101 _func_enter_;
 102
 103         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+rtw_free_evt_priv\n"));
 104
 105         _cancel_workitem_sync(&pevtpriv->c2h_wk);
 106         while (pevtpriv->c2h_wk_alive)
 107                 msleep(10);
 108
 109         while (!rtw_cbuf_empty(pevtpriv->c2h_queue)) {
 110                 void *c2h = rtw_cbuf_pop(pevtpriv->c2h_queue);
 111                 if (c2h != NULL && c2h != (void *)pevtpriv)
 112                         kfree(c2h);
 113         }
 114         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("-rtw_free_evt_priv\n"));
 115
 116 _func_exit_;
 117 }
 118
 119 void _rtw_free_cmd_priv (struct cmd_priv *pcmdpriv)
 120 {
 121 _func_enter_;
 122
 123         if (pcmdpriv) {
 124                 _rtw_free_sema(&(pcmdpriv->cmd_queue_sema));
 125                 _rtw_free_sema(&(pcmdpriv->terminate_cmdthread_sema));
 126
 127                 if (pcmdpriv->cmd_allocated_buf)
 128                         kfree(pcmdpriv->cmd_allocated_buf);
 129
 130                 if (pcmdpriv->rsp_allocated_buf)
 131                         kfree(pcmdpriv->rsp_allocated_buf);
 132         }
 133 _func_exit_;
 134 }
 135
 136 /*
 137 Calling Context:
 138
 139 rtw_enqueue_cmd can only be called between kernel thread,
 140 since only spin_lock is used.
 141
 142 ISR/Call-Back functions can't call this sub-function.
 143
 144 */
 145
 146 int     _rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj)
 147 {
 148         unsigned long irqL;
 149
 150 _func_enter_;
 151
 152         if (obj == NULL)
 153                 goto exit;
 154
 155         spin_lock_irqsave(&queue->lock, irqL);
 156
 157         rtw_list_insert_tail(&obj->list, &queue->queue);
 158
 159         spin_unlock_irqrestore(&queue->lock, irqL);
 160
 161 exit:
 162
 163 _func_exit_;
 164
 165         return _SUCCESS;
 166 }
 167
 168 struct  cmd_obj *_rtw_dequeue_cmd(struct __queue *queue)
 169 {
 170         unsigned long irqL;
 171         struct cmd_obj *obj;
 172
 173 _func_enter_;
 174
 175         spin_lock_irqsave(&queue->lock, irqL);
 176         if (rtw_is_list_empty(&(queue->queue))) {
 177                 obj = NULL;
 178         } else {
 179                 obj = LIST_CONTAINOR(get_next(&(queue->queue)), struct cmd_obj, list);
 180                 rtw_list_delete(&obj->list);
 181         }
 182
 183         spin_unlock_irqrestore(&queue->lock, irqL);
 184
 185 _func_exit_;
 186
 187         return obj;
 188 }
 189
 190 u32     rtw_init_cmd_priv(struct cmd_priv *pcmdpriv)
 191 {
 192         u32     res;
 193 _func_enter_;
 194         res = _rtw_init_cmd_priv (pcmdpriv);
 195 _func_exit_;
 196         return res;
 197 }
 198
 199 u32     rtw_init_evt_priv (struct       evt_priv *pevtpriv)
 200 {
 201         int     res;
 202 _func_enter_;
 203         res = _rtw_init_evt_priv(pevtpriv);
 204 _func_exit_;
 205         return res;
 206 }
 207
 208 void rtw_free_cmd_priv(struct   cmd_priv *pcmdpriv)
 209 {
 210 _func_enter_;
 211         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("rtw_free_cmd_priv\n"));
 212         _rtw_free_cmd_priv(pcmdpriv);
 213 _func_exit_;
 214 }
 215
 216 static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 217 {
 218         u8 bAllow = false; /* set to true to allow enqueuing cmd when hw_init_completed is false */
 219
 220         /* To decide allow or not */
 221         if ((pcmdpriv->padapter->pwrctrlpriv.bHWPwrPindetect) &&
 222             (!pcmdpriv->padapter->registrypriv.usbss_enable)) {
 223                 if (cmd_obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
 224                         struct drvextra_cmd_parm        *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)cmd_obj->parmbuf;
 225                         if (pdrvextra_cmd_parm->ec_id == POWER_SAVING_CTRL_WK_CID)
 226                                 bAllow = true;
 227                 }
 228         }
 229
 230         if (cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
 231                 bAllow = true;
 232
 233         if ((!pcmdpriv->padapter->hw_init_completed && !bAllow) ||
 234             !pcmdpriv->cmdthd_running)  /* com_thread not running */
 235                 return _FAIL;
 236         return _SUCCESS;
 237 }
 238
 239 u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 240 {
 241         int res = _FAIL;
 242         struct adapter *padapter = pcmdpriv->padapter;
 243
 244 _func_enter_;
 245
 246         if (cmd_obj == NULL)
 247                 goto exit;
 248
 249         cmd_obj->padapter = padapter;
 250
 251         res = rtw_cmd_filter(pcmdpriv, cmd_obj);
 252         if (_FAIL == res) {
 253                 rtw_free_cmd_obj(cmd_obj);
 254                 goto exit;
 255         }
 256
 257         res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj);
 258
 259         if (res == _SUCCESS)
 260                 _rtw_up_sema(&pcmdpriv->cmd_queue_sema);
 261
 262 exit:
 263
 264 _func_exit_;
 265
 266         return res;
 267 }
 268
 269 struct  cmd_obj *rtw_dequeue_cmd(struct cmd_priv *pcmdpriv)
 270 {
 271         struct cmd_obj *cmd_obj;
 272
 273 _func_enter_;
 274
 275         cmd_obj = _rtw_dequeue_cmd(&pcmdpriv->cmd_queue);
 276
 277 _func_exit_;
 278         return cmd_obj;
 279 }
 280
 281 void rtw_cmd_clr_isr(struct     cmd_priv *pcmdpriv)
 282 {
 283 _func_enter_;
 284         pcmdpriv->cmd_done_cnt++;
 285         /* _rtw_up_sema(&(pcmdpriv->cmd_done_sema)); */
 286 _func_exit_;
 287 }
 288
 289 void rtw_free_cmd_obj(struct cmd_obj *pcmd)
 290 {
 291 _func_enter_;
 292
 293         if ((pcmd->cmdcode != _JoinBss_CMD_) && (pcmd->cmdcode != _CreateBss_CMD_)) {
 294                 /* free parmbuf in cmd_obj */
 295                 kfree(pcmd->parmbuf);
 296         }
 297
 298         if (pcmd->rsp != NULL) {
 299                 if (pcmd->rspsz != 0) {
 300                         /* free rsp in cmd_obj */
 301                         kfree(pcmd->rsp);
 302                 }
 303         }
 304
 305         /* free cmd_obj */
 306         kfree(pcmd);
 307
 308 _func_exit_;
 309 }
 310
 311 int rtw_cmd_thread(void *context)
 312 {
 313         u8 ret;
 314         struct cmd_obj *pcmd;
 315         u8 *pcmdbuf;
 316         u8 (*cmd_hdl)(struct adapter *padapter, u8 *pbuf);
 317         void (*pcmd_callback)(struct adapter *dev, struct cmd_obj *pcmd);
 318         struct adapter *padapter = (struct adapter *)context;
 319         struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
 320
 321 _func_enter_;
 322
 323         thread_enter("RTW_CMD_THREAD");
 324
 325         pcmdbuf = pcmdpriv->cmd_buf;
 326
 327         pcmdpriv->cmdthd_running = true;
 328         _rtw_up_sema(&pcmdpriv->terminate_cmdthread_sema);
 329
 330         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("start r871x rtw_cmd_thread !!!!\n"));
 331
 332         while (1) {
 333                 if (_rtw_down_sema(&pcmdpriv->cmd_queue_sema) == _FAIL)
 334                         break;
 335
 336                 if (padapter->bDriverStopped ||
 337                     padapter->bSurpriseRemoved) {
 338                         DBG_88E("%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
 339                                 __func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
 340                         break;
 341                 }
 342 _next:
 343                 if (padapter->bDriverStopped ||
 344                     padapter->bSurpriseRemoved) {
 345                         DBG_88E("%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
 346                                 __func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
 347                         break;
 348                 }
 349
 350                 pcmd = rtw_dequeue_cmd(pcmdpriv);
 351                 if (!pcmd)
 352                         continue;
 353
 354                 if (_FAIL == rtw_cmd_filter(pcmdpriv, pcmd)) {
 355                         pcmd->res = H2C_DROPPED;
 356                         goto post_process;
 357                 }
 358
 359                 pcmdpriv->cmd_issued_cnt++;
 360
 361                 pcmd->cmdsz = _RND4((pcmd->cmdsz));/* _RND4 */
 362
 363                 memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
 364
 365                 if (pcmd->cmdcode < ARRAY_SIZE(wlancmds)) {
 366                         cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
 367
 368                         if (cmd_hdl) {
 369                                 ret = cmd_hdl(pcmd->padapter, pcmdbuf);
 370                                 pcmd->res = ret;
 371                         }
 372
 373                         pcmdpriv->cmd_seq++;
 374                 } else {
 375                         pcmd->res = H2C_PARAMETERS_ERROR;
 376                 }
 377
 378                 cmd_hdl = NULL;
 379
 380 post_process:
 381
 382                 /* call callback function for post-processed */
 383                 if (pcmd->cmdcode < ARRAY_SIZE(rtw_cmd_callback)) {
 384                         pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
 385                         if (pcmd_callback == NULL) {
 386                                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("mlme_cmd_hdl(): pcmd_callback = 0x%p, cmdcode = 0x%x\n", pcmd_callback, pcmd->cmdcode));
 387                                 rtw_free_cmd_obj(pcmd);
 388                         } else {
 389                                 /* todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!= NULL) */
 390                                 pcmd_callback(pcmd->padapter, pcmd);/* need conider that free cmd_obj in rtw_cmd_callback */
 391                         }
 392                 } else {
 393                         RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("%s: cmdcode = 0x%x callback not defined!\n", __func__, pcmd->cmdcode));
 394                         rtw_free_cmd_obj(pcmd);
 395                 }
 396
 397                 flush_signals_thread();
 398
 399                 goto _next;
 400         }
 401         pcmdpriv->cmdthd_running = false;
 402
 403         /*  free all cmd_obj resources */
 404         do {
 405                 pcmd = rtw_dequeue_cmd(pcmdpriv);
 406                 if (pcmd == NULL)
 407                         break;
 408
 409                 /* DBG_88E("%s: leaving... drop cmdcode:%u\n", __func__, pcmd->cmdcode); */
 410
 411                 rtw_free_cmd_obj(pcmd);
 412         } while (1);
 413
 414         _rtw_up_sema(&pcmdpriv->terminate_cmdthread_sema);
 415
 416 _func_exit_;
 417
 418         complete_and_exit(NULL, 0);
 419 }
 420
 421 u8 rtw_setstandby_cmd(struct adapter *padapter, uint action)
 422 {
 423         struct cmd_obj *ph2c;
 424         struct usb_suspend_parm *psetusbsuspend;
 425         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 426
 427         u8 ret = _SUCCESS;
 428
 429 _func_enter_;
 430
 431         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 432         if (ph2c == NULL) {
 433                 ret = _FAIL;
 434                 goto exit;
 435         }
 436
 437         psetusbsuspend = (struct usb_suspend_parm *)rtw_zmalloc(sizeof(struct usb_suspend_parm));
 438         if (psetusbsuspend == NULL) {
 439                 kfree(ph2c);
 440                 ret = _FAIL;
 441                 goto exit;
 442         }
 443
 444         psetusbsuspend->action = action;
 445
 446         init_h2fwcmd_w_parm_no_rsp(ph2c, psetusbsuspend, GEN_CMD_CODE(_SetUsbSuspend));
 447
 448         ret = rtw_enqueue_cmd(pcmdpriv, ph2c);
 449
 450 exit:
 451
 452 _func_exit_;
 453
 454         return ret;
 455 }
 456
 457 /*
 458 rtw_sitesurvey_cmd(~)
 459         ### NOTE:#### (!!!!)
 460         MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
 461 */
 462 u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid, int ssid_num,
 463         struct rtw_ieee80211_channel *ch, int ch_num)
 464 {
 465         u8 res = _FAIL;
 466         struct cmd_obj          *ph2c;
 467         struct sitesurvey_parm  *psurveyPara;
 468         struct cmd_priv         *pcmdpriv = &padapter->cmdpriv;
 469         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
 470
 471 _func_enter_;
 472         if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
 473                 rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1);
 474         }
 475
 476         if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
 477                 p2p_ps_wk_cmd(padapter, P2P_PS_SCAN, 1);
 478         }
 479
 480         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 481         if (ph2c == NULL)
 482                 return _FAIL;
 483
 484         psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm));
 485         if (psurveyPara == NULL) {
 486                 kfree(ph2c);
 487                 return _FAIL;
 488         }
 489
 490         rtw_free_network_queue(padapter, false);
 491
 492         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("%s: flush network queue\n", __func__));
 493
 494         init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
 495
 496         /* psurveyPara->bsslimit = 48; */
 497         psurveyPara->scan_mode = pmlmepriv->scan_mode;
 498
 499         /* prepare ssid list */
 500         if (ssid) {
 501                 int i;
 502                 for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) {
 503                         if (ssid[i].SsidLength) {
 504                                 memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(struct ndis_802_11_ssid));
 505                                 psurveyPara->ssid_num++;
 506                                 if (0)
 507                                 DBG_88E(FUNC_ADPT_FMT" ssid:(%s, %d)\n", FUNC_ADPT_ARG(padapter),
 508                                         psurveyPara->ssid[i].Ssid, psurveyPara->ssid[i].SsidLength);
 509                         }
 510                 }
 511         }
 512
 513         /* prepare channel list */
 514         if (ch) {
 515                 int i;
 516                 for (i = 0; i < ch_num && i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
 517                         if (ch[i].hw_value && !(ch[i].flags & RTW_IEEE80211_CHAN_DISABLED)) {
 518                                 memcpy(&psurveyPara->ch[i], &ch[i], sizeof(struct rtw_ieee80211_channel));
 519                                 psurveyPara->ch_num++;
 520                                 if (0)
 521                                 DBG_88E(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter),
 522                                         psurveyPara->ch[i].hw_value);
 523                         }
 524                 }
 525         }
 526
 527         set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
 528
 529         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 530
 531         if (res == _SUCCESS) {
 532                 pmlmepriv->scan_start_time = jiffies;
 533
 534                 _set_timer(&pmlmepriv->scan_to_timer, SCANNING_TIMEOUT);
 535
 536                 rtw_led_control(padapter, LED_CTL_SITE_SURVEY);
 537
 538                 pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
 539         } else {
 540                 _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 541         }
 542
 543 _func_exit_;
 544
 545         return res;
 546 }
 547
 548 u8 rtw_setdatarate_cmd(struct adapter *padapter, u8 *rateset)
 549 {
 550         struct cmd_obj *ph2c;
 551         struct setdatarate_parm *pbsetdataratepara;
 552         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 553         u8      res = _SUCCESS;
 554
 555 _func_enter_;
 556
 557         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 558         if (ph2c == NULL) {
 559                 res = _FAIL;
 560                 goto exit;
 561         }
 562
 563         pbsetdataratepara = (struct setdatarate_parm *)rtw_zmalloc(sizeof(struct setdatarate_parm));
 564         if (pbsetdataratepara == NULL) {
 565                 kfree(ph2c);
 566                 res = _FAIL;
 567                 goto exit;
 568         }
 569
 570         init_h2fwcmd_w_parm_no_rsp(ph2c, pbsetdataratepara, GEN_CMD_CODE(_SetDataRate));
 571         pbsetdataratepara->mac_id = 5;
 572         memcpy(pbsetdataratepara->datarates, rateset, NumRates);
 573         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 574 exit:
 575
 576 _func_exit_;
 577
 578         return res;
 579 }
 580
 581 u8 rtw_setbasicrate_cmd(struct adapter *padapter, u8 *rateset)
 582 {
 583         struct cmd_obj *ph2c;
 584         struct setbasicrate_parm *pssetbasicratepara;
 585         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 586         u8      res = _SUCCESS;
 587
 588 _func_enter_;
 589
 590         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 591         if (ph2c == NULL) {
 592                 res = _FAIL;
 593                 goto exit;
 594         }
 595         pssetbasicratepara = (struct setbasicrate_parm *)rtw_zmalloc(sizeof(struct setbasicrate_parm));
 596
 597         if (pssetbasicratepara == NULL) {
 598                 kfree(ph2c);
 599                 res = _FAIL;
 600                 goto exit;
 601         }
 602
 603         init_h2fwcmd_w_parm_no_rsp(ph2c, pssetbasicratepara, _SetBasicRate_CMD_);
 604
 605         memcpy(pssetbasicratepara->basicrates, rateset, NumRates);
 606
 607         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 608 exit:
 609
 610 _func_exit_;
 611
 612         return res;
 613 }
 614
 615
 616 /*
 617 unsigned char rtw_setphy_cmd(unsigned char  *adapter)
 618
 619 1.  be called only after rtw_update_registrypriv_dev_network(~) or mp testing program
 620 2.  for AdHoc/Ap mode or mp mode?
 621
 622 */
 623 u8 rtw_setphy_cmd(struct adapter *padapter, u8 modem, u8 ch)
 624 {
 625         struct cmd_obj *ph2c;
 626         struct setphy_parm *psetphypara;
 627         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 628         u8      res = _SUCCESS;
 629
 630 _func_enter_;
 631
 632         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 633         if (ph2c == NULL) {
 634                 res = _FAIL;
 635                 goto exit;
 636                 }
 637         psetphypara = (struct setphy_parm *)rtw_zmalloc(sizeof(struct setphy_parm));
 638
 639         if (psetphypara == NULL) {
 640                 kfree(ph2c);
 641                 res = _FAIL;
 642                 goto exit;
 643         }
 644
 645         init_h2fwcmd_w_parm_no_rsp(ph2c, psetphypara, _SetPhy_CMD_);
 646
 647         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("CH =%d, modem =%d", ch, modem));
 648
 649         psetphypara->modem = modem;
 650         psetphypara->rfchannel = ch;
 651
 652         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 653 exit:
 654 _func_exit_;
 655         return res;
 656 }
 657
 658 u8 rtw_setbbreg_cmd(struct adapter *padapter, u8 offset, u8 val)
 659 {
 660         struct cmd_obj *ph2c;
 661         struct writeBB_parm *pwritebbparm;
 662         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 663         u8      res = _SUCCESS;
 664
 665 _func_enter_;
 666         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 667         if (ph2c == NULL) {
 668                 res = _FAIL;
 669                 goto exit;
 670                 }
 671         pwritebbparm = (struct writeBB_parm *)rtw_zmalloc(sizeof(struct writeBB_parm));
 672
 673         if (pwritebbparm == NULL) {
 674                 kfree(ph2c);
 675                 res = _FAIL;
 676                 goto exit;
 677         }
 678
 679         init_h2fwcmd_w_parm_no_rsp(ph2c, pwritebbparm, GEN_CMD_CODE(_SetBBReg));
 680
 681         pwritebbparm->offset = offset;
 682         pwritebbparm->value = val;
 683
 684         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 685 exit:
 686 _func_exit_;
 687         return res;
 688 }
 689
 690 u8 rtw_getbbreg_cmd(struct adapter  *padapter, u8 offset, u8 *pval)
 691 {
 692         struct cmd_obj *ph2c;
 693         struct readBB_parm *prdbbparm;
 694         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 695         u8      res = _SUCCESS;
 696
 697 _func_enter_;
 698         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 699         if (ph2c == NULL) {
 700                 res = _FAIL;
 701                 goto exit;
 702                 }
 703         prdbbparm = (struct readBB_parm *)rtw_zmalloc(sizeof(struct readBB_parm));
 704
 705         if (prdbbparm == NULL) {
 706                 kfree(ph2c);
 707                 return _FAIL;
 708         }
 709
 710         _rtw_init_listhead(&ph2c->list);
 711         ph2c->cmdcode = GEN_CMD_CODE(_GetBBReg);
 712         ph2c->parmbuf = (unsigned char *)prdbbparm;
 713         ph2c->cmdsz =  sizeof(struct readBB_parm);
 714         ph2c->rsp = pval;
 715         ph2c->rspsz = sizeof(struct readBB_rsp);
 716
 717         prdbbparm->offset = offset;
 718
 719         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 720 exit:
 721 _func_exit_;
 722         return res;
 723 }
 724
 725 u8 rtw_setrfreg_cmd(struct adapter  *padapter, u8 offset, u32 val)
 726 {
 727         struct cmd_obj *ph2c;
 728         struct writeRF_parm *pwriterfparm;
 729         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 730         u8      res = _SUCCESS;
 731 _func_enter_;
 732         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 733         if (ph2c == NULL) {
 734                 res = _FAIL;
 735                 goto exit;
 736         }
 737         pwriterfparm = (struct writeRF_parm *)rtw_zmalloc(sizeof(struct writeRF_parm));
 738
 739         if (pwriterfparm == NULL) {
 740                 kfree(ph2c);
 741                 res = _FAIL;
 742                 goto exit;
 743         }
 744
 745         init_h2fwcmd_w_parm_no_rsp(ph2c, pwriterfparm, GEN_CMD_CODE(_SetRFReg));
 746
 747         pwriterfparm->offset = offset;
 748         pwriterfparm->value = val;
 749
 750         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 751 exit:
 752 _func_exit_;
 753         return res;
 754 }
 755
 756 u8 rtw_getrfreg_cmd(struct adapter  *padapter, u8 offset, u8 *pval)
 757 {
 758         struct cmd_obj *ph2c;
 759         struct readRF_parm *prdrfparm;
 760         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 761         u8      res = _SUCCESS;
 762
 763 _func_enter_;
 764
 765         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 766         if (ph2c == NULL) {
 767                 res = _FAIL;
 768                 goto exit;
 769         }
 770
 771         prdrfparm = (struct readRF_parm *)rtw_zmalloc(sizeof(struct readRF_parm));
 772         if (prdrfparm == NULL) {
 773                 kfree(ph2c);
 774                 res = _FAIL;
 775                 goto exit;
 776         }
 777
 778         _rtw_init_listhead(&ph2c->list);
 779         ph2c->cmdcode = GEN_CMD_CODE(_GetRFReg);
 780         ph2c->parmbuf = (unsigned char *)prdrfparm;
 781         ph2c->cmdsz =  sizeof(struct readRF_parm);
 782         ph2c->rsp = pval;
 783         ph2c->rspsz = sizeof(struct readRF_rsp);
 784
 785         prdrfparm->offset = offset;
 786
 787         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 788
 789 exit:
 790
 791 _func_exit_;
 792
 793         return res;
 794 }
 795
 796 void rtw_getbbrfreg_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
 797 {
 798  _func_enter_;
 799
 800         kfree(pcmd->parmbuf);
 801         kfree(pcmd);
 802
 803         if (padapter->registrypriv.mp_mode == 1)
 804                 padapter->mppriv.workparam.bcompleted = true;
 805 _func_exit_;
 806 }
 807
 808 void rtw_readtssi_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
 809 {
 810  _func_enter_;
 811
 812         kfree(pcmd->parmbuf);
 813         kfree(pcmd);
 814
 815         if (padapter->registrypriv.mp_mode == 1)
 816                 padapter->mppriv.workparam.bcompleted = true;
 817 _func_exit_;
 818 }
 819
 820 u8 rtw_createbss_cmd(struct adapter  *padapter)
 821 {
 822         struct cmd_obj *pcmd;
 823         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 824         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 825         struct wlan_bssid_ex *pdev_network = &padapter->registrypriv.dev_network;
 826         u8      res = _SUCCESS;
 827
 828 _func_enter_;
 829
 830         rtw_led_control(padapter, LED_CTL_START_TO_LINK);
 831
 832         if (pmlmepriv->assoc_ssid.SsidLength == 0)
 833                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for Any SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
 834         else
 835                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
 836
 837         pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 838         if (pcmd == NULL) {
 839                 res = _FAIL;
 840                 goto exit;
 841         }
 842
 843         _rtw_init_listhead(&pcmd->list);
 844         pcmd->cmdcode = _CreateBss_CMD_;
 845         pcmd->parmbuf = (unsigned char *)pdev_network;
 846         pcmd->cmdsz = get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network);
 847         pcmd->rsp = NULL;
 848         pcmd->rspsz = 0;
 849         pdev_network->Length = pcmd->cmdsz;
 850         res = rtw_enqueue_cmd(pcmdpriv, pcmd);
 851 exit:
 852
 853 _func_exit_;
 854
 855         return res;
 856 }
 857
 858 u8 rtw_createbss_cmd_ex(struct adapter  *padapter, unsigned char *pbss, unsigned int sz)
 859 {
 860         struct cmd_obj *pcmd;
 861         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 862         u8      res = _SUCCESS;
 863
 864 _func_enter_;
 865
 866         pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 867         if (pcmd == NULL) {
 868                 res = _FAIL;
 869                 goto exit;
 870         }
 871
 872         _rtw_init_listhead(&pcmd->list);
 873         pcmd->cmdcode = GEN_CMD_CODE(_CreateBss);
 874         pcmd->parmbuf = pbss;
 875         pcmd->cmdsz =  sz;
 876         pcmd->rsp = NULL;
 877         pcmd->rspsz = 0;
 878
 879         res = rtw_enqueue_cmd(pcmdpriv, pcmd);
 880
 881 exit:
 882
 883 _func_exit_;
 884
 885         return res;
 886 }
 887
 888 u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
 889 {
 890         u8      res = _SUCCESS;
 891         uint    t_len = 0;
 892         struct wlan_bssid_ex            *psecnetwork;
 893         struct cmd_obj          *pcmd;
 894         struct cmd_priv         *pcmdpriv = &padapter->cmdpriv;
 895         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
 896         struct qos_priv         *pqospriv = &pmlmepriv->qospriv;
 897         struct security_priv    *psecuritypriv = &padapter->securitypriv;
 898         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
 899         struct ht_priv          *phtpriv = &pmlmepriv->htpriv;
 900         enum ndis_802_11_network_infra ndis_network_mode = pnetwork->network.InfrastructureMode;
 901         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 902         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 903
 904 _func_enter_;
 905
 906         rtw_led_control(padapter, LED_CTL_START_TO_LINK);
 907
 908         if (pmlmepriv->assoc_ssid.SsidLength == 0) {
 909                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+Join cmd: Any SSid\n"));
 910         } else {
 911                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+Join cmd: SSid =[%s]\n", pmlmepriv->assoc_ssid.Ssid));
 912         }
 913
 914         pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
 915         if (pcmd == NULL) {
 916                 res = _FAIL;
 917                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd: memory allocate for cmd_obj fail!!!\n"));
 918                 goto exit;
 919         }
 920         /* for IEs is fix buf size */
 921         t_len = sizeof(struct wlan_bssid_ex);
 922
 923
 924         /* for hidden ap to set fw_state here */
 925         if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE)) {
 926                 switch (ndis_network_mode) {
 927                 case Ndis802_11IBSS:
 928                         set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
 929                         break;
 930                 case Ndis802_11Infrastructure:
 931                         set_fwstate(pmlmepriv, WIFI_STATION_STATE);
 932                         break;
 933                 case Ndis802_11APMode:
 934                 case Ndis802_11AutoUnknown:
 935                 case Ndis802_11InfrastructureMax:
 936                         break;
 937                 }
 938         }
 939
 940         psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss;
 941         if (psecnetwork == NULL) {
 942                 if (pcmd != NULL)
 943                         kfree(pcmd);
 944
 945                 res = _FAIL;
 946
 947                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd :psecnetwork == NULL!!!\n"));
 948
 949                 goto exit;
 950         }
 951
 952         _rtw_memset(psecnetwork, 0, t_len);
 953
 954         memcpy(psecnetwork, &pnetwork->network, get_wlan_bssid_ex_sz(&pnetwork->network));
 955
 956         psecuritypriv->authenticator_ie[0] = (unsigned char)psecnetwork->IELength;
 957
 958         if ((psecnetwork->IELength-12) < (256-1)) {
 959                 memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], psecnetwork->IELength-12);
 960         } else {
 961                 memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], (256-1));
 962         }
 963
 964         psecnetwork->IELength = 0;
 965         /*  Added by Albert 2009/02/18 */
 966         /*  If the the driver wants to use the bssid to create the connection. */
 967         /*  If not,  we have to copy the connecting AP's MAC address to it so that */
 968         /*  the driver just has the bssid information for PMKIDList searching. */
 969
 970         if (!pmlmepriv->assoc_by_bssid)
 971                 memcpy(&pmlmepriv->assoc_bssid[0], &pnetwork->network.MacAddress[0], ETH_ALEN);
 972
 973         psecnetwork->IELength = rtw_restruct_sec_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength);
 974
 975
 976         pqospriv->qos_option = 0;
 977
 978         if (pregistrypriv->wmm_enable) {
 979                 u32 tmp_len;
 980
 981                 tmp_len = rtw_restruct_wmm_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength, psecnetwork->IELength);
 982
 983                 if (psecnetwork->IELength != tmp_len) {
 984                         psecnetwork->IELength = tmp_len;
 985                         pqospriv->qos_option = 1; /* There is WMM IE in this corresp. beacon */
 986                 } else {
 987                         pqospriv->qos_option = 0;/* There is no WMM IE in this corresp. beacon */
 988                 }
 989         }
 990
 991         phtpriv->ht_option = false;
 992         if (pregistrypriv->ht_enable) {
 993                 /*      Added by Albert 2010/06/23 */
 994                 /*      For the WEP mode, we will use the bg mode to do the connection to avoid some IOT issue. */
 995                 /*      Especially for Realtek 8192u SoftAP. */
 996                 if ((padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_) &&
 997                     (padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_) &&
 998                     (padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_)) {
 999                         /* rtw_restructure_ht_ie */
1000                         rtw_restructure_ht_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0],
1001                                                                         pnetwork->network.IELength, &psecnetwork->IELength);
1002                 }
1003         }
1004
1005         pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength);
1006
1007         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_TENDA)
1008                 padapter->pwrctrlpriv.smart_ps = 0;
1009         else
1010                 padapter->pwrctrlpriv.smart_ps = padapter->registrypriv.smart_ps;
1011
1012         DBG_88E("%s: smart_ps =%d\n", __func__, padapter->pwrctrlpriv.smart_ps);
1013
1014         pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork);/* get cmdsz before endian conversion */
1015
1016         _rtw_init_listhead(&pcmd->list);
1017         pcmd->cmdcode = _JoinBss_CMD_;/* GEN_CMD_CODE(_JoinBss) */
1018         pcmd->parmbuf = (unsigned char *)psecnetwork;
1019         pcmd->rsp = NULL;
1020         pcmd->rspsz = 0;
1021
1022         res = rtw_enqueue_cmd(pcmdpriv, pcmd);
1023
1024 exit:
1025
1026 _func_exit_;
1027
1028         return res;
1029 }
1030
1031 u8 rtw_disassoc_cmd(struct adapter *padapter, u32 deauth_timeout_ms, bool enqueue) /* for sta_mode */
1032 {
1033         struct cmd_obj *cmdobj = NULL;
1034         struct disconnect_parm *param = NULL;
1035         struct cmd_priv *cmdpriv = &padapter->cmdpriv;
1036         u8 res = _SUCCESS;
1037
1038 _func_enter_;
1039
1040         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_disassoc_cmd\n"));
1041
1042         /* prepare cmd parameter */
1043         param = (struct disconnect_parm *)rtw_zmalloc(sizeof(*param));
1044         if (param == NULL) {
1045                 res = _FAIL;
1046                 goto exit;
1047         }
1048         param->deauth_timeout_ms = deauth_timeout_ms;
1049
1050         if (enqueue) {
1051                 /* need enqueue, prepare cmd_obj and enqueue */
1052                 cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
1053                 if (cmdobj == NULL) {
1054                         res = _FAIL;
1055                         kfree(param);
1056                         goto exit;
1057                 }
1058                 init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_);
1059                 res = rtw_enqueue_cmd(cmdpriv, cmdobj);
1060         } else {
1061                 /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
1062                 if (H2C_SUCCESS != disconnect_hdl(padapter, (u8 *)param))
1063                         res = _FAIL;
1064                 kfree(param);
1065         }
1066
1067 exit:
1068
1069 _func_exit_;
1070
1071         return res;
1072 }
1073
1074 u8 rtw_setopmode_cmd(struct adapter  *padapter, enum ndis_802_11_network_infra networktype)
1075 {
1076         struct  cmd_obj *ph2c;
1077         struct  setopmode_parm *psetop;
1078
1079         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
1080         u8      res = _SUCCESS;
1081
1082 _func_enter_;
1083
1084         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1085         if (ph2c == NULL) {
1086                 res = false;
1087                 goto exit;
1088         }
1089         psetop = (struct setopmode_parm *)rtw_zmalloc(sizeof(struct setopmode_parm));
1090
1091         if (psetop == NULL) {
1092                 kfree(ph2c);
1093                 res = false;
1094                 goto exit;
1095         }
1096
1097         init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
1098         psetop->mode = (u8)networktype;
1099
1100         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1101
1102 exit:
1103
1104 _func_exit_;
1105
1106         return res;
1107 }
1108
1109 u8 rtw_setstakey_cmd(struct adapter *padapter, u8 *psta, u8 unicast_key)
1110 {
1111         struct cmd_obj *ph2c;
1112         struct set_stakey_parm *psetstakey_para;
1113         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1114         struct set_stakey_rsp *psetstakey_rsp = NULL;
1115
1116         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1117         struct security_priv *psecuritypriv = &padapter->securitypriv;
1118         struct sta_info *sta = (struct sta_info *)psta;
1119         u8      res = _SUCCESS;
1120
1121 _func_enter_;
1122
1123         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1124         if (ph2c == NULL) {
1125                 res = _FAIL;
1126                 goto exit;
1127         }
1128
1129         psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm));
1130         if (psetstakey_para == NULL) {
1131                 kfree(ph2c);
1132                 res = _FAIL;
1133                 goto exit;
1134         }
1135
1136         psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp));
1137         if (psetstakey_rsp == NULL) {
1138                 kfree(ph2c);
1139                 kfree(psetstakey_para);
1140                 res = _FAIL;
1141                 goto exit;
1142         }
1143
1144         init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
1145         ph2c->rsp = (u8 *)psetstakey_rsp;
1146         ph2c->rspsz = sizeof(struct set_stakey_rsp);
1147
1148         memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
1149
1150         if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
1151                 psetstakey_para->algorithm = (unsigned char) psecuritypriv->dot11PrivacyAlgrthm;
1152         else
1153                 GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, false);
1154
1155         if (unicast_key)
1156                 memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
1157         else
1158                 memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16);
1159
1160         /* jeff: set this because at least sw key is ready */
1161         padapter->securitypriv.busetkipkey = true;
1162
1163         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1164
1165 exit:
1166
1167 _func_exit_;
1168
1169         return res;
1170 }
1171
1172 u8 rtw_clearstakey_cmd(struct adapter *padapter, u8 *psta, u8 entry, u8 enqueue)
1173 {
1174         struct cmd_obj *ph2c;
1175         struct set_stakey_parm  *psetstakey_para;
1176         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1177         struct set_stakey_rsp *psetstakey_rsp = NULL;
1178         struct sta_info *sta = (struct sta_info *)psta;
1179         u8      res = _SUCCESS;
1180
1181 _func_enter_;
1182
1183         if (!enqueue) {
1184                 clear_cam_entry(padapter, entry);
1185         } else {
1186                 ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1187                 if (ph2c == NULL) {
1188                         res = _FAIL;
1189                         goto exit;
1190                 }
1191
1192                 psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm));
1193                 if (psetstakey_para == NULL) {
1194                         kfree(ph2c);
1195                         res = _FAIL;
1196                         goto exit;
1197                 }
1198
1199                 psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp));
1200                 if (psetstakey_rsp == NULL) {
1201                         kfree(ph2c);
1202                         kfree(psetstakey_para);
1203                         res = _FAIL;
1204                         goto exit;
1205                 }
1206
1207                 init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
1208                 ph2c->rsp = (u8 *)psetstakey_rsp;
1209                 ph2c->rspsz = sizeof(struct set_stakey_rsp);
1210
1211                 memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
1212
1213                 psetstakey_para->algorithm = _NO_PRIVACY_;
1214
1215                 psetstakey_para->id = entry;
1216
1217                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1218         }
1219 exit:
1220
1221 _func_exit_;
1222
1223         return res;
1224 }
1225
1226 u8 rtw_setrttbl_cmd(struct adapter  *padapter, struct setratable_parm *prate_table)
1227 {
1228         struct cmd_obj *ph2c;
1229         struct setratable_parm *psetrttblparm;
1230         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1231         u8      res = _SUCCESS;
1232 _func_enter_;
1233
1234         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1235         if (ph2c == NULL) {
1236                 res = _FAIL;
1237                 goto exit;
1238         }
1239         psetrttblparm = (struct setratable_parm *)rtw_zmalloc(sizeof(struct setratable_parm));
1240
1241         if (psetrttblparm == NULL) {
1242                 kfree(ph2c);
1243                 res = _FAIL;
1244                 goto exit;
1245         }
1246
1247         init_h2fwcmd_w_parm_no_rsp(ph2c, psetrttblparm, GEN_CMD_CODE(_SetRaTable));
1248
1249         memcpy(psetrttblparm, prate_table, sizeof(struct setratable_parm));
1250
1251         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1252 exit:
1253 _func_exit_;
1254         return res;
1255 }
1256
1257 u8 rtw_getrttbl_cmd(struct adapter  *padapter, struct getratable_rsp *pval)
1258 {
1259         struct cmd_obj *ph2c;
1260         struct getratable_parm *pgetrttblparm;
1261         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1262         u8      res = _SUCCESS;
1263 _func_enter_;
1264
1265         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1266         if (ph2c == NULL) {
1267                 res = _FAIL;
1268                 goto exit;
1269         }
1270         pgetrttblparm = (struct getratable_parm *)rtw_zmalloc(sizeof(struct getratable_parm));
1271
1272         if (pgetrttblparm == NULL) {
1273                 kfree(ph2c);
1274                 res = _FAIL;
1275                 goto exit;
1276         }
1277
1278 /*      init_h2fwcmd_w_parm_no_rsp(ph2c, psetrttblparm, GEN_CMD_CODE(_SetRaTable)); */
1279
1280         _rtw_init_listhead(&ph2c->list);
1281         ph2c->cmdcode = GEN_CMD_CODE(_GetRaTable);
1282         ph2c->parmbuf = (unsigned char *)pgetrttblparm;
1283         ph2c->cmdsz =  sizeof(struct getratable_parm);
1284         ph2c->rsp = (u8 *)pval;
1285         ph2c->rspsz = sizeof(struct getratable_rsp);
1286
1287         pgetrttblparm->rsvd = 0x0;
1288
1289         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1290 exit:
1291 _func_exit_;
1292         return res;
1293 }
1294
1295 u8 rtw_setassocsta_cmd(struct adapter  *padapter, u8 *mac_addr)
1296 {
1297         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1298         struct cmd_obj *ph2c;
1299         struct set_assocsta_parm *psetassocsta_para;
1300         struct set_stakey_rsp *psetassocsta_rsp = NULL;
1301
1302         u8      res = _SUCCESS;
1303
1304 _func_enter_;
1305
1306         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1307         if (ph2c == NULL) {
1308                 res = _FAIL;
1309                 goto exit;
1310         }
1311
1312         psetassocsta_para = (struct set_assocsta_parm *)rtw_zmalloc(sizeof(struct set_assocsta_parm));
1313         if (psetassocsta_para == NULL) {
1314                 kfree(ph2c);
1315                 res = _FAIL;
1316                 goto exit;
1317         }
1318
1319         psetassocsta_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_assocsta_rsp));
1320         if (psetassocsta_rsp == NULL) {
1321                 kfree(ph2c);
1322                 kfree(psetassocsta_para);
1323                 return _FAIL;
1324         }
1325
1326         init_h2fwcmd_w_parm_no_rsp(ph2c, psetassocsta_para, _SetAssocSta_CMD_);
1327         ph2c->rsp = (u8 *)psetassocsta_rsp;
1328         ph2c->rspsz = sizeof(struct set_assocsta_rsp);
1329
1330         memcpy(psetassocsta_para->addr, mac_addr, ETH_ALEN);
1331
1332         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1333
1334 exit:
1335
1336 _func_exit_;
1337
1338         return res;
1339  }
1340
1341 u8 rtw_addbareq_cmd(struct adapter *padapter, u8 tid, u8 *addr)
1342 {
1343         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1344         struct cmd_obj *ph2c;
1345         struct addBaReq_parm *paddbareq_parm;
1346         u8      res = _SUCCESS;
1347
1348 _func_enter_;
1349
1350         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1351         if (ph2c == NULL) {
1352                 res = _FAIL;
1353                 goto exit;
1354         }
1355
1356         paddbareq_parm = (struct addBaReq_parm *)rtw_zmalloc(sizeof(struct addBaReq_parm));
1357         if (paddbareq_parm == NULL) {
1358                 kfree(ph2c);
1359                 res = _FAIL;
1360                 goto exit;
1361         }
1362
1363         paddbareq_parm->tid = tid;
1364         memcpy(paddbareq_parm->addr, addr, ETH_ALEN);
1365
1366         init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, GEN_CMD_CODE(_AddBAReq));
1367
1368         /* DBG_88E("rtw_addbareq_cmd, tid =%d\n", tid); */
1369
1370         /* rtw_enqueue_cmd(pcmdpriv, ph2c); */
1371         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1372
1373 exit:
1374
1375 _func_exit_;
1376
1377         return res;
1378 }
1379
1380 u8 rtw_dynamic_chk_wk_cmd(struct adapter *padapter)
1381 {
1382         struct cmd_obj *ph2c;
1383         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
1384         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1385         u8      res = _SUCCESS;
1386
1387 _func_enter_;
1388
1389         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1390         if (ph2c == NULL) {
1391                 res = _FAIL;
1392                 goto exit;
1393         }
1394
1395         pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
1396         if (pdrvextra_cmd_parm == NULL) {
1397                 kfree(ph2c);
1398                 res = _FAIL;
1399                 goto exit;
1400         }
1401
1402         pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID;
1403         pdrvextra_cmd_parm->type_size = 0;
1404         pdrvextra_cmd_parm->pbuf = (u8 *)padapter;
1405
1406         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
1407
1408
1409         /* rtw_enqueue_cmd(pcmdpriv, ph2c); */
1410         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1411 exit:
1412 _func_exit_;
1413         return res;
1414 }
1415
1416 u8 rtw_set_ch_cmd(struct adapter *padapter, u8 ch, u8 bw, u8 ch_offset, u8 enqueue)
1417 {
1418         struct cmd_obj *pcmdobj;
1419         struct set_ch_parm *set_ch_parm;
1420         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1421
1422         u8 res = _SUCCESS;
1423
1424 _func_enter_;
1425
1426         DBG_88E(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
1427                 FUNC_NDEV_ARG(padapter->pnetdev), ch, bw, ch_offset);
1428
1429         /* check input parameter */
1430
1431         /* prepare cmd parameter */
1432         set_ch_parm = (struct set_ch_parm *)rtw_zmalloc(sizeof(*set_ch_parm));
1433         if (set_ch_parm == NULL) {
1434                 res = _FAIL;
1435                 goto exit;
1436         }
1437         set_ch_parm->ch = ch;
1438         set_ch_parm->bw = bw;
1439         set_ch_parm->ch_offset = ch_offset;
1440
1441         if (enqueue) {
1442                 /* need enqueue, prepare cmd_obj and enqueue */
1443                 pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct   cmd_obj));
1444                 if (pcmdobj == NULL) {
1445                         kfree(set_ch_parm);
1446                         res = _FAIL;
1447                         goto exit;
1448                 }
1449
1450                 init_h2fwcmd_w_parm_no_rsp(pcmdobj, set_ch_parm, GEN_CMD_CODE(_SetChannel));
1451                 res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
1452         } else {
1453                 /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
1454                 if (H2C_SUCCESS != set_ch_hdl(padapter, (u8 *)set_ch_parm))
1455                         res = _FAIL;
1456
1457                 kfree(set_ch_parm);
1458         }
1459
1460         /* do something based on res... */
1461
1462 exit:
1463
1464         DBG_88E(FUNC_NDEV_FMT" res:%u\n", FUNC_NDEV_ARG(padapter->pnetdev), res);
1465
1466 _func_exit_;
1467
1468         return res;
1469 }
1470
1471 u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue)
1472 {
1473         struct  cmd_obj *pcmdobj;
1474         struct  SetChannelPlan_param *setChannelPlan_param;
1475         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
1476
1477         u8      res = _SUCCESS;
1478
1479 _func_enter_;
1480
1481         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_chplan_cmd\n"));
1482
1483         /* check input parameter */
1484         if (!rtw_is_channel_plan_valid(chplan)) {
1485                 res = _FAIL;
1486                 goto exit;
1487         }
1488
1489         /* prepare cmd parameter */
1490         setChannelPlan_param = (struct  SetChannelPlan_param *)rtw_zmalloc(sizeof(struct SetChannelPlan_param));
1491         if (setChannelPlan_param == NULL) {
1492                 res = _FAIL;
1493                 goto exit;
1494         }
1495         setChannelPlan_param->channel_plan = chplan;
1496
1497         if (enqueue) {
1498                 /* need enqueue, prepare cmd_obj and enqueue */
1499                 pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct   cmd_obj));
1500                 if (pcmdobj == NULL) {
1501                         kfree(setChannelPlan_param);
1502                         res = _FAIL;
1503                         goto exit;
1504                 }
1505
1506                 init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelPlan_param, GEN_CMD_CODE(_SetChannelPlan));
1507                 res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
1508         } else {
1509                 /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
1510                 if (H2C_SUCCESS != set_chplan_hdl(padapter, (unsigned char *)setChannelPlan_param))
1511                         res = _FAIL;
1512
1513                 kfree(setChannelPlan_param);
1514         }
1515
1516         /* do something based on res... */
1517         if (res == _SUCCESS)
1518                 padapter->mlmepriv.ChannelPlan = chplan;
1519
1520 exit:
1521
1522 _func_exit_;
1523
1524         return res;
1525 }
1526
1527 u8 rtw_led_blink_cmd(struct adapter *padapter, struct LED_871x *pLed)
1528 {
1529         struct  cmd_obj *pcmdobj;
1530         struct  LedBlink_param *ledBlink_param;
1531         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
1532
1533         u8      res = _SUCCESS;
1534
1535 _func_enter_;
1536
1537         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_led_blink_cmd\n"));
1538
1539         pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct   cmd_obj));
1540         if (pcmdobj == NULL) {
1541                 res = _FAIL;
1542                 goto exit;
1543         }
1544
1545         ledBlink_param = (struct        LedBlink_param *)rtw_zmalloc(sizeof(struct      LedBlink_param));
1546         if (ledBlink_param == NULL) {
1547                 kfree(pcmdobj);
1548                 res = _FAIL;
1549                 goto exit;
1550         }
1551
1552         ledBlink_param->pLed = pLed;
1553
1554         init_h2fwcmd_w_parm_no_rsp(pcmdobj, ledBlink_param, GEN_CMD_CODE(_LedBlink));
1555         res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
1556
1557 exit:
1558
1559 _func_exit_;
1560
1561         return res;
1562 }
1563
1564 u8 rtw_set_csa_cmd(struct adapter *padapter, u8 new_ch_no)
1565 {
1566         struct  cmd_obj *pcmdobj;
1567         struct  SetChannelSwitch_param *setChannelSwitch_param;
1568         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
1569
1570         u8      res = _SUCCESS;
1571
1572 _func_enter_;
1573
1574         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_csa_cmd\n"));
1575
1576         pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct   cmd_obj));
1577         if (pcmdobj == NULL) {
1578                 res = _FAIL;
1579                 goto exit;
1580         }
1581
1582         setChannelSwitch_param = (struct SetChannelSwitch_param *)rtw_zmalloc(sizeof(struct     SetChannelSwitch_param));
1583         if (setChannelSwitch_param == NULL) {
1584                 kfree(pcmdobj);
1585                 res = _FAIL;
1586                 goto exit;
1587         }
1588
1589         setChannelSwitch_param->new_ch_no = new_ch_no;
1590
1591         init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelSwitch_param, GEN_CMD_CODE(_SetChannelSwitch));
1592         res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
1593
1594 exit:
1595
1596 _func_exit_;
1597
1598         return res;
1599 }
1600
1601 u8 rtw_tdls_cmd(struct adapter *padapter, u8 *addr, u8 option)
1602 {
1603         return _SUCCESS;
1604 }
1605
1606 static void traffic_status_watchdog(struct adapter *padapter)
1607 {
1608         u8      bEnterPS;
1609         u8      bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false;
1610         u8      bHigherBusyTraffic = false, bHigherBusyRxTraffic = false, bHigherBusyTxTraffic = false;
1611         struct mlme_priv                *pmlmepriv = &(padapter->mlmepriv);
1612
1613         /*  */
1614         /*  Determine if our traffic is busy now */
1615         /*  */
1616         if (check_fwstate(pmlmepriv, _FW_LINKED)) {
1617                 if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 100 ||
1618                     pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 100) {
1619                         bBusyTraffic = true;
1620
1621                         if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
1622                                 bRxBusyTraffic = true;
1623                         else
1624                                 bTxBusyTraffic = true;
1625                 }
1626
1627                 /*  Higher Tx/Rx data. */
1628                 if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 ||
1629                     pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000) {
1630                         bHigherBusyTraffic = true;
1631
1632                         if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
1633                                 bHigherBusyRxTraffic = true;
1634                         else
1635                                 bHigherBusyTxTraffic = true;
1636                 }
1637
1638                 /*  check traffic for  powersaving. */
1639                 if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
1640                     (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2))
1641                         bEnterPS = false;
1642                 else
1643                         bEnterPS = true;
1644
1645                 /*  LeisurePS only work in infra mode. */
1646                 if (bEnterPS)
1647                         LPS_Enter(padapter);
1648                 else
1649                         LPS_Leave(padapter);
1650         } else {
1651                 LPS_Leave(padapter);
1652         }
1653
1654         pmlmepriv->LinkDetectInfo.NumRxOkInPeriod = 0;
1655         pmlmepriv->LinkDetectInfo.NumTxOkInPeriod = 0;
1656         pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
1657         pmlmepriv->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
1658         pmlmepriv->LinkDetectInfo.bTxBusyTraffic = bTxBusyTraffic;
1659         pmlmepriv->LinkDetectInfo.bRxBusyTraffic = bRxBusyTraffic;
1660         pmlmepriv->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic;
1661         pmlmepriv->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic;
1662         pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
1663 }
1664
1665 static void dynamic_chk_wk_hdl(struct adapter *padapter, u8 *pbuf, int sz)
1666 {
1667         struct mlme_priv *pmlmepriv;
1668
1669         padapter = (struct adapter *)pbuf;
1670         pmlmepriv = &(padapter->mlmepriv);
1671
1672 #ifdef CONFIG_88EU_AP_MODE
1673         if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
1674                 expire_timeout_chk(padapter);
1675 #endif
1676
1677         rtw_hal_sreset_xmit_status_check(padapter);
1678
1679         linked_status_chk(padapter);
1680         traffic_status_watchdog(padapter);
1681
1682         rtw_hal_dm_watchdog(padapter);
1683 }
1684
1685 static void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type)
1686 {
1687         struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
1688         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
1689         u8      mstatus;
1690
1691 _func_enter_;
1692
1693         if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
1694             (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
1695                 return;
1696
1697         switch (lps_ctrl_type) {
1698         case LPS_CTRL_SCAN:
1699                 if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
1700                         /* connect */
1701                         LPS_Leave(padapter);
1702                 }
1703                 break;
1704         case LPS_CTRL_JOINBSS:
1705                 LPS_Leave(padapter);
1706                 break;
1707         case LPS_CTRL_CONNECT:
1708                 mstatus = 1;/* connect */
1709                 /*  Reset LPS Setting */
1710                 padapter->pwrctrlpriv.LpsIdleCount = 0;
1711                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
1712                 break;
1713         case LPS_CTRL_DISCONNECT:
1714                 mstatus = 0;/* disconnect */
1715                 LPS_Leave(padapter);
1716                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
1717                 break;
1718         case LPS_CTRL_SPECIAL_PACKET:
1719                 /* DBG_88E("LPS_CTRL_SPECIAL_PACKET\n"); */
1720                 pwrpriv->DelayLPSLastTimeStamp = jiffies;
1721                 LPS_Leave(padapter);
1722                 break;
1723         case LPS_CTRL_LEAVE:
1724                 LPS_Leave(padapter);
1725                 break;
1726         default:
1727                 break;
1728         }
1729
1730 _func_exit_;
1731 }
1732
1733 u8 rtw_lps_ctrl_wk_cmd(struct adapter *padapter, u8 lps_ctrl_type, u8 enqueue)
1734 {
1735         struct cmd_obj  *ph2c;
1736         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
1737         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1738         /* struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv; */
1739         u8      res = _SUCCESS;
1740
1741 _func_enter_;
1742
1743         /* if (!pwrctrlpriv->bLeisurePs) */
1744         /*      return res; */
1745
1746         if (enqueue) {
1747                 ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1748                 if (ph2c == NULL) {
1749                         res = _FAIL;
1750                         goto exit;
1751                 }
1752
1753                 pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
1754                 if (pdrvextra_cmd_parm == NULL) {
1755                         kfree(ph2c);
1756                         res = _FAIL;
1757                         goto exit;
1758                 }
1759
1760                 pdrvextra_cmd_parm->ec_id = LPS_CTRL_WK_CID;
1761                 pdrvextra_cmd_parm->type_size = lps_ctrl_type;
1762                 pdrvextra_cmd_parm->pbuf = NULL;
1763
1764                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
1765
1766                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1767         } else {
1768                 lps_ctrl_wk_hdl(padapter, lps_ctrl_type);
1769         }
1770
1771 exit:
1772
1773 _func_exit_;
1774
1775         return res;
1776 }
1777
1778 static void rpt_timer_setting_wk_hdl(struct adapter *padapter, u16 min_time)
1779 {
1780         rtw_hal_set_hwreg(padapter, HW_VAR_RPT_TIMER_SETTING, (u8 *)(&min_time));
1781 }
1782
1783 u8 rtw_rpt_timer_cfg_cmd(struct adapter *padapter, u16 min_time)
1784 {
1785         struct cmd_obj          *ph2c;
1786         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
1787         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1788
1789         u8      res = _SUCCESS;
1790
1791 _func_enter_;
1792         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1793         if (ph2c == NULL) {
1794                 res = _FAIL;
1795                 goto exit;
1796         }
1797
1798         pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
1799         if (pdrvextra_cmd_parm == NULL) {
1800                 kfree(ph2c);
1801                 res = _FAIL;
1802                 goto exit;
1803         }
1804
1805         pdrvextra_cmd_parm->ec_id = RTP_TIMER_CFG_WK_CID;
1806         pdrvextra_cmd_parm->type_size = min_time;
1807         pdrvextra_cmd_parm->pbuf = NULL;
1808         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
1809         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1810 exit:
1811
1812 _func_exit_;
1813
1814         return res;
1815 }
1816
1817 static void antenna_select_wk_hdl(struct adapter *padapter, u8 antenna)
1818 {
1819         rtw_hal_set_hwreg(padapter, HW_VAR_ANTENNA_DIVERSITY_SELECT, (u8 *)(&antenna));
1820 }
1821
1822 u8 rtw_antenna_select_cmd(struct adapter *padapter, u8 antenna, u8 enqueue)
1823 {
1824         struct cmd_obj          *ph2c;
1825         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
1826         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1827         u8      support_ant_div;
1828         u8      res = _SUCCESS;
1829
1830 _func_enter_;
1831         rtw_hal_get_def_var(padapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &support_ant_div);
1832         if (!support_ant_div)
1833                 return res;
1834
1835         if (enqueue) {
1836                 ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1837                 if (ph2c == NULL) {
1838                         res = _FAIL;
1839                         goto exit;
1840                 }
1841
1842                 pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
1843                 if (pdrvextra_cmd_parm == NULL) {
1844                         kfree(ph2c);
1845                         res = _FAIL;
1846                         goto exit;
1847                 }
1848
1849                 pdrvextra_cmd_parm->ec_id = ANT_SELECT_WK_CID;
1850                 pdrvextra_cmd_parm->type_size = antenna;
1851                 pdrvextra_cmd_parm->pbuf = NULL;
1852                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
1853
1854                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1855         } else {
1856                 antenna_select_wk_hdl(padapter, antenna);
1857         }
1858 exit:
1859
1860 _func_exit_;
1861
1862         return res;
1863 }
1864
1865 static void power_saving_wk_hdl(struct adapter *padapter, u8 *pbuf, int sz)
1866 {
1867          rtw_ps_processor(padapter);
1868 }
1869
1870 #ifdef CONFIG_88EU_P2P
1871 u8 p2p_protocol_wk_cmd(struct adapter *padapter, int intCmdType)
1872 {
1873         struct cmd_obj  *ph2c;
1874         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
1875         struct wifidirect_info  *pwdinfo = &(padapter->wdinfo);
1876         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1877         u8      res = _SUCCESS;
1878
1879 _func_enter_;
1880
1881         if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
1882                 return res;
1883
1884         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1885         if (ph2c == NULL) {
1886                 res = _FAIL;
1887                 goto exit;
1888         }
1889
1890         pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
1891         if (pdrvextra_cmd_parm == NULL) {
1892                 kfree(ph2c);
1893                 res = _FAIL;
1894                 goto exit;
1895         }
1896
1897         pdrvextra_cmd_parm->ec_id = P2P_PROTO_WK_CID;
1898         pdrvextra_cmd_parm->type_size = intCmdType;     /*      As the command tppe. */
1899         pdrvextra_cmd_parm->pbuf = NULL;                /*      Must be NULL here */
1900
1901         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
1902
1903         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1904
1905 exit:
1906
1907 _func_exit_;
1908
1909         return res;
1910 }
1911 #endif /* CONFIG_88EU_P2P */
1912
1913 u8 rtw_ps_cmd(struct adapter *padapter)
1914 {
1915         struct cmd_obj          *ppscmd;
1916         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
1917         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1918
1919         u8      res = _SUCCESS;
1920 _func_enter_;
1921
1922         ppscmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1923         if (ppscmd == NULL) {
1924                 res = _FAIL;
1925                 goto exit;
1926         }
1927
1928         pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
1929         if (pdrvextra_cmd_parm == NULL) {
1930                 kfree(ppscmd);
1931                 res = _FAIL;
1932                 goto exit;
1933         }
1934
1935         pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID;
1936         pdrvextra_cmd_parm->pbuf = NULL;
1937         init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
1938
1939         res = rtw_enqueue_cmd(pcmdpriv, ppscmd);
1940
1941 exit:
1942
1943 _func_exit_;
1944
1945         return res;
1946 }
1947
1948 #ifdef CONFIG_88EU_AP_MODE
1949
1950 static void rtw_chk_hi_queue_hdl(struct adapter *padapter)
1951 {
1952         int cnt = 0;
1953         struct sta_info *psta_bmc;
1954         struct sta_priv *pstapriv = &padapter->stapriv;
1955
1956         psta_bmc = rtw_get_bcmc_stainfo(padapter);
1957         if (!psta_bmc)
1958                 return;
1959
1960         if (psta_bmc->sleepq_len == 0) {
1961                 u8 val = 0;
1962
1963                 /* while ((rtw_read32(padapter, 0x414)&0x00ffff00)!= 0) */
1964                 /* while ((rtw_read32(padapter, 0x414)&0x0000ff00)!= 0) */
1965
1966                 rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
1967
1968                 while (!val) {
1969                         msleep(100);
1970
1971                         cnt++;
1972
1973                         if (cnt > 10)
1974                                 break;
1975
1976                         rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
1977                 }
1978
1979                 if (cnt <= 10) {
1980                         pstapriv->tim_bitmap &= ~BIT(0);
1981                         pstapriv->sta_dz_bitmap &= ~BIT(0);
1982
1983                         update_beacon(padapter, _TIM_IE_, NULL, false);
1984                 } else { /* re check again */
1985                         rtw_chk_hi_queue_cmd(padapter);
1986                 }
1987         }
1988 }
1989
1990 u8 rtw_chk_hi_queue_cmd(struct adapter *padapter)
1991 {
1992         struct cmd_obj  *ph2c;
1993         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
1994         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1995         u8      res = _SUCCESS;
1996
1997         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
1998         if (ph2c == NULL) {
1999                 res = _FAIL;
2000                 goto exit;
2001         }
2002
2003         pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
2004         if (pdrvextra_cmd_parm == NULL) {
2005                 kfree(ph2c);
2006                 res = _FAIL;
2007                 goto exit;
2008         }
2009
2010         pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID;
2011         pdrvextra_cmd_parm->type_size = 0;
2012         pdrvextra_cmd_parm->pbuf = NULL;
2013
2014         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
2015
2016         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
2017 exit:
2018         return res;
2019 }
2020 #endif
2021
2022 u8 rtw_c2h_wk_cmd(struct adapter *padapter, u8 *c2h_evt)
2023 {
2024         struct cmd_obj *ph2c;
2025         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
2026         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
2027         u8      res = _SUCCESS;
2028
2029         ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
2030         if (ph2c == NULL) {
2031                 res = _FAIL;
2032                 goto exit;
2033         }
2034
2035         pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
2036         if (pdrvextra_cmd_parm == NULL) {
2037                 kfree(ph2c);
2038                 res = _FAIL;
2039                 goto exit;
2040         }
2041
2042         pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
2043         pdrvextra_cmd_parm->type_size = c2h_evt ? 16 : 0;
2044         pdrvextra_cmd_parm->pbuf = c2h_evt;
2045
2046         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
2047
2048         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
2049
2050 exit:
2051
2052         return res;
2053 }
2054
2055 static s32 c2h_evt_hdl(struct adapter *adapter, struct c2h_evt_hdr *c2h_evt, c2h_id_filter filter)
2056 {
2057         s32 ret = _FAIL;
2058         u8 buf[16];
2059
2060         if (!c2h_evt) {
2061                 /* No c2h event in cmd_obj, read c2h event before handling*/
2062                 if (c2h_evt_read(adapter, buf) == _SUCCESS) {
2063                         c2h_evt = (struct c2h_evt_hdr *)buf;
2064
2065                         if (filter && filter(c2h_evt->id) == false)
2066                                 goto exit;
2067
2068                         ret = rtw_hal_c2h_handler(adapter, c2h_evt);
2069                 }
2070         } else {
2071                 if (filter && filter(c2h_evt->id) == false)
2072                         goto exit;
2073
2074                 ret = rtw_hal_c2h_handler(adapter, c2h_evt);
2075         }
2076 exit:
2077         return ret;
2078 }
2079
2080 static void c2h_wk_callback(struct work_struct *work)
2081 {
2082         struct evt_priv *evtpriv = container_of(work, struct evt_priv, c2h_wk);
2083         struct adapter *adapter = container_of(evtpriv, struct adapter, evtpriv);
2084         struct c2h_evt_hdr *c2h_evt;
2085         c2h_id_filter ccx_id_filter = rtw_hal_c2h_id_filter_ccx(adapter);
2086
2087         evtpriv->c2h_wk_alive = true;
2088
2089         while (!rtw_cbuf_empty(evtpriv->c2h_queue)) {
2090                 if ((c2h_evt = (struct c2h_evt_hdr *)rtw_cbuf_pop(evtpriv->c2h_queue)) != NULL) {
2091                         /* This C2H event is read, clear it */
2092                         c2h_evt_clear(adapter);
2093                 } else if ((c2h_evt = (struct c2h_evt_hdr *)rtw_malloc(16)) != NULL) {
2094                         /* This C2H event is not read, read & clear now */
2095                         if (c2h_evt_read(adapter, (u8 *)c2h_evt) != _SUCCESS)
2096                                 continue;
2097                 }
2098
2099                 /* Special pointer to trigger c2h_evt_clear only */
2100                 if ((void *)c2h_evt == (void *)evtpriv)
2101                         continue;
2102
2103                 if (!c2h_evt_exist(c2h_evt)) {
2104                         kfree(c2h_evt);
2105                         continue;
2106                 }
2107
2108                 if (ccx_id_filter(c2h_evt->id) == true) {
2109                         /* Handle CCX report here */
2110                         rtw_hal_c2h_handler(adapter, c2h_evt);
2111                         kfree(c2h_evt);
2112                 } else {
2113 #ifdef CONFIG_88EU_P2P
2114                         /* Enqueue into cmd_thread for others */
2115                         rtw_c2h_wk_cmd(adapter, (u8 *)c2h_evt);
2116 #endif
2117                 }
2118         }
2119
2120         evtpriv->c2h_wk_alive = false;
2121 }
2122
2123 u8 rtw_drvextra_cmd_hdl(struct adapter *padapter, unsigned char *pbuf)
2124 {
2125         struct drvextra_cmd_parm *pdrvextra_cmd;
2126
2127         if (!pbuf)
2128                 return H2C_PARAMETERS_ERROR;
2129
2130         pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf;
2131
2132         switch (pdrvextra_cmd->ec_id) {
2133         case DYNAMIC_CHK_WK_CID:
2134                 dynamic_chk_wk_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type_size);
2135                 break;
2136         case POWER_SAVING_CTRL_WK_CID:
2137                 power_saving_wk_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type_size);
2138                 break;
2139         case LPS_CTRL_WK_CID:
2140                 lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type_size);
2141                 break;
2142         case RTP_TIMER_CFG_WK_CID:
2143                 rpt_timer_setting_wk_hdl(padapter, pdrvextra_cmd->type_size);
2144                 break;
2145         case ANT_SELECT_WK_CID:
2146                 antenna_select_wk_hdl(padapter, pdrvextra_cmd->type_size);
2147                 break;
2148 #ifdef CONFIG_88EU_P2P
2149         case P2P_PS_WK_CID:
2150                 p2p_ps_wk_hdl(padapter, pdrvextra_cmd->type_size);
2151                 break;
2152         case P2P_PROTO_WK_CID:
2153                 /*      Commented by Albert 2011/07/01 */
2154                 /*      I used the type_size as the type command */
2155                 p2p_protocol_wk_hdl(padapter, pdrvextra_cmd->type_size);
2156                 break;
2157 #endif
2158 #ifdef CONFIG_88EU_AP_MODE
2159         case CHECK_HIQ_WK_CID:
2160                 rtw_chk_hi_queue_hdl(padapter);
2161                 break;
2162 #endif /* CONFIG_88EU_AP_MODE */
2163         case C2H_WK_CID:
2164                 c2h_evt_hdl(padapter, (struct c2h_evt_hdr *)pdrvextra_cmd->pbuf, NULL);
2165                 break;
2166         default:
2167                 break;
2168         }
2169
2170         if (pdrvextra_cmd->pbuf && pdrvextra_cmd->type_size > 0)
2171                 kfree(pdrvextra_cmd->pbuf);
2172
2173         return H2C_SUCCESS;
2174 }
2175
2176 void rtw_survey_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
2177 {
2178         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
2179
2180 _func_enter_;
2181
2182         if (pcmd->res == H2C_DROPPED) {
2183                 /* TODO: cancel timer and do timeout handler directly... */
2184                 /* need to make timeout handlerOS independent */
2185                 _set_timer(&pmlmepriv->scan_to_timer, 1);
2186                 } else if (pcmd->res != H2C_SUCCESS) {
2187                 _set_timer(&pmlmepriv->scan_to_timer, 1);
2188                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: MgntActrtw_set_802_11_bssid_LIST_SCAN Fail ************\n\n."));
2189         }
2190
2191         /*  free cmd */
2192         rtw_free_cmd_obj(pcmd);
2193
2194 _func_exit_;
2195 }
2196 void rtw_disassoc_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
2197 {
2198         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
2199
2200 _func_enter_;
2201
2202         if (pcmd->res != H2C_SUCCESS) {
2203                 spin_lock_bh(&pmlmepriv->lock);
2204                 set_fwstate(pmlmepriv, _FW_LINKED);
2205                 spin_unlock_bh(&pmlmepriv->lock);
2206
2207                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ***Error: disconnect_cmd_callback Fail ***\n."));
2208
2209                 goto exit;
2210         } else /* clear bridge database */
2211                 nat25_db_cleanup(padapter);
2212
2213         /*  free cmd */
2214         rtw_free_cmd_obj(pcmd);
2215
2216 exit:
2217
2218 _func_exit_;
2219 }
2220
2221 void rtw_joinbss_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
2222 {
2223         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
2224
2225 _func_enter_;
2226
2227         if (pcmd->res == H2C_DROPPED) {
2228                 /* TODO: cancel timer and do timeout handler directly... */
2229                 /* need to make timeout handlerOS independent */
2230                 _set_timer(&pmlmepriv->assoc_timer, 1);
2231         } else if (pcmd->res != H2C_SUCCESS) {
2232                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("********Error:rtw_select_and_join_from_scanned_queue Wait Sema  Fail ************\n"));
2233                 _set_timer(&pmlmepriv->assoc_timer, 1);
2234         }
2235
2236         rtw_free_cmd_obj(pcmd);
2237
2238 _func_exit_;
2239 }
2240
2241 void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
2242 {
2243         u8 timer_cancelled;
2244         struct sta_info *psta = NULL;
2245         struct wlan_network *pwlan = NULL;
2246         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
2247         struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)pcmd->parmbuf;
2248         struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
2249
2250 _func_enter_;
2251
2252         if ((pcmd->res != H2C_SUCCESS)) {
2253                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: rtw_createbss_cmd_callback  Fail ************\n\n."));
2254                 _set_timer(&pmlmepriv->assoc_timer, 1);
2255         }
2256
2257         _cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
2258
2259         spin_lock_bh(&pmlmepriv->lock);
2260
2261         if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
2262                 psta = rtw_get_stainfo(&padapter->stapriv, pnetwork->MacAddress);
2263                 if (!psta) {
2264                         psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress);
2265                         if (psta == NULL) {
2266                                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nCan't alloc sta_info when createbss_cmd_callback\n"));
2267                                 goto createbss_cmd_fail ;
2268                         }
2269                 }
2270
2271                 rtw_indicate_connect(padapter);
2272         } else {
2273                 pwlan = _rtw_alloc_network(pmlmepriv);
2274                 spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
2275                 if (pwlan == NULL) {
2276                         pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
2277                         if (pwlan == NULL) {
2278                                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n Error:  can't get pwlan in rtw_joinbss_event_callback\n"));
2279                                 spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
2280                                 goto createbss_cmd_fail;
2281                         }
2282                         pwlan->last_scanned = jiffies;
2283                 } else {
2284                         rtw_list_insert_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue);
2285                 }
2286
2287                 pnetwork->Length = get_wlan_bssid_ex_sz(pnetwork);
2288                 memcpy(&(pwlan->network), pnetwork, pnetwork->Length);
2289
2290                 memcpy(&tgt_network->network, pnetwork, (get_wlan_bssid_ex_sz(pnetwork)));
2291
2292                 _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
2293
2294                 spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
2295                 /*  we will set _FW_LINKED when there is one more sat to join us (rtw_stassoc_event_callback) */
2296         }
2297
2298 createbss_cmd_fail:
2299
2300         spin_unlock_bh(&pmlmepriv->lock);
2301
2302         rtw_free_cmd_obj(pcmd);
2303
2304 _func_exit_;
2305 }
2306
2307 void rtw_setstaKey_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
2308 {
2309         struct sta_priv *pstapriv = &padapter->stapriv;
2310         struct set_stakey_rsp *psetstakey_rsp = (struct set_stakey_rsp *)(pcmd->rsp);
2311         struct sta_info *psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr);
2312
2313 _func_enter_;
2314
2315         if (psta == NULL) {
2316                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: rtw_setstaKey_cmdrsp_callback => can't get sta_info\n\n"));
2317                 goto exit;
2318         }
2319 exit:
2320         rtw_free_cmd_obj(pcmd);
2321 _func_exit_;
2322 }
2323
2324 void rtw_setassocsta_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
2325 {
2326         struct sta_priv *pstapriv = &padapter->stapriv;
2327         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2328         struct set_assocsta_parm *passocsta_parm = (struct set_assocsta_parm *)(pcmd->parmbuf);
2329         struct set_assocsta_rsp *passocsta_rsp = (struct set_assocsta_rsp *)(pcmd->rsp);
2330         struct sta_info *psta = rtw_get_stainfo(pstapriv, passocsta_parm->addr);
2331
2332 _func_enter_;
2333
2334         if (psta == NULL) {
2335                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: setassocsta_cmdrsp_callbac => can't get sta_info\n\n"));
2336                 goto exit;
2337         }
2338
2339         psta->aid = passocsta_rsp->cam_id;
2340         psta->mac_id = passocsta_rsp->cam_id;
2341
2342         spin_lock_bh(&pmlmepriv->lock);
2343
2344         if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) && (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true))
2345                 _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
2346
2347         set_fwstate(pmlmepriv, _FW_LINKED);
2348         spin_unlock_bh(&pmlmepriv->lock);
2349
2350 exit:
2351         rtw_free_cmd_obj(pcmd);
2352
2353 _func_exit_;
2354 }