drivers/net/wireless/rockchip_wlan/rtl8189es/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 <drv_types.h>
  23
  24 /*
  25 Caller and the rtw_cmd_thread can protect cmd_q by spin_lock.
  26 No irqsave is necessary.
  27 */
  28
  29 sint    _rtw_init_cmd_priv (struct      cmd_priv *pcmdpriv)
  30 {
  31         sint res=_SUCCESS;
  32
  33 _func_enter_;
  34
  35         _rtw_init_sema(&(pcmdpriv->cmd_queue_sema), 0);
  36         //_rtw_init_sema(&(pcmdpriv->cmd_done_sema), 0);
  37         _rtw_init_sema(&(pcmdpriv->terminate_cmdthread_sema), 0);
  38
  39
  40         _rtw_init_queue(&(pcmdpriv->cmd_queue));
  41
  42         //allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf
  43
  44         pcmdpriv->cmd_seq = 1;
  45
  46         pcmdpriv->cmd_allocated_buf = rtw_zmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
  47
  48         if (pcmdpriv->cmd_allocated_buf == NULL){
  49                 res= _FAIL;
  50                 goto exit;
  51         }
  52
  53         pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf  +  CMDBUFF_ALIGN_SZ - ( (SIZE_PTR)(pcmdpriv->cmd_allocated_buf) & (CMDBUFF_ALIGN_SZ-1));
  54
  55         pcmdpriv->rsp_allocated_buf = rtw_zmalloc(MAX_RSPSZ + 4);
  56
  57         if (pcmdpriv->rsp_allocated_buf == NULL){
  58                 res= _FAIL;
  59                 goto exit;
  60         }
  61
  62         pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 - ( (SIZE_PTR)(pcmdpriv->rsp_allocated_buf) & 3);
  63
  64         pcmdpriv->cmd_issued_cnt = pcmdpriv->cmd_done_cnt = pcmdpriv->rsp_cnt = 0;
  65
  66         _rtw_mutex_init(&pcmdpriv->sctx_mutex);
  67 exit:
  68
  69 _func_exit_;
  70
  71         return res;
  72
  73 }
  74
  75 #ifdef CONFIG_C2H_WK
  76 static void c2h_wk_callback(_workitem *work);
  77 #endif
  78 sint _rtw_init_evt_priv(struct evt_priv *pevtpriv)
  79 {
  80         sint res=_SUCCESS;
  81
  82 _func_enter_;
  83
  84 #ifdef CONFIG_H2CLBK
  85         _rtw_init_sema(&(pevtpriv->lbkevt_done), 0);
  86         pevtpriv->lbkevt_limit = 0;
  87         pevtpriv->lbkevt_num = 0;
  88         pevtpriv->cmdevt_parm = NULL;
  89 #endif
  90
  91         //allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf
  92         ATOMIC_SET(&pevtpriv->event_seq, 0);
  93         pevtpriv->evt_done_cnt = 0;
  94
  95 #ifdef CONFIG_EVENT_THREAD_MODE
  96
  97         _rtw_init_sema(&(pevtpriv->evt_notify), 0);
  98         _rtw_init_sema(&(pevtpriv->terminate_evtthread_sema), 0);
  99
 100         pevtpriv->evt_allocated_buf = rtw_zmalloc(MAX_EVTSZ + 4);
 101         if (pevtpriv->evt_allocated_buf == NULL){
 102                 res= _FAIL;
 103                 goto exit;
 104                 }
 105         pevtpriv->evt_buf = pevtpriv->evt_allocated_buf  +  4 - ((unsigned int)(pevtpriv->evt_allocated_buf) & 3);
 106
 107
 108 #if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
 109         pevtpriv->allocated_c2h_mem = rtw_zmalloc(C2H_MEM_SZ +4);
 110
 111         if (pevtpriv->allocated_c2h_mem == NULL){
 112                 res= _FAIL;
 113                 goto exit;
 114         }
 115
 116         pevtpriv->c2h_mem = pevtpriv->allocated_c2h_mem +  4\
 117         - ( (u32)(pevtpriv->allocated_c2h_mem) & 3);
 118 #ifdef PLATFORM_OS_XP
 119         pevtpriv->pc2h_mdl= IoAllocateMdl((u8 *)pevtpriv->c2h_mem, C2H_MEM_SZ , FALSE, FALSE, NULL);
 120
 121         if(pevtpriv->pc2h_mdl == NULL){
 122                 res= _FAIL;
 123                 goto exit;
 124         }
 125         MmBuildMdlForNonPagedPool(pevtpriv->pc2h_mdl);
 126 #endif
 127 #endif //end of CONFIG_SDIO_HCI
 128
 129         _rtw_init_queue(&(pevtpriv->evt_queue));
 130
 131 exit:
 132
 133 #endif //end of CONFIG_EVENT_THREAD_MODE
 134
 135 #ifdef CONFIG_C2H_WK
 136         _init_workitem(&pevtpriv->c2h_wk, c2h_wk_callback, NULL);
 137         pevtpriv->c2h_wk_alive = _FALSE;
 138         pevtpriv->c2h_queue = rtw_cbuf_alloc(C2H_QUEUE_MAX_LEN+1);
 139 #endif
 140
 141 _func_exit_;
 142
 143         return res;
 144 }
 145
 146 void _rtw_free_evt_priv (struct evt_priv *pevtpriv)
 147 {
 148 _func_enter_;
 149
 150         RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("+_rtw_free_evt_priv \n"));
 151
 152 #ifdef CONFIG_EVENT_THREAD_MODE
 153         _rtw_free_sema(&(pevtpriv->evt_notify));
 154         _rtw_free_sema(&(pevtpriv->terminate_evtthread_sema));
 155
 156
 157         if (pevtpriv->evt_allocated_buf)
 158                 rtw_mfree(pevtpriv->evt_allocated_buf, MAX_EVTSZ + 4);
 159 #endif
 160
 161 #ifdef CONFIG_C2H_WK
 162         _cancel_workitem_sync(&pevtpriv->c2h_wk);
 163         while(pevtpriv->c2h_wk_alive)
 164                 rtw_msleep_os(10);
 165
 166         while (!rtw_cbuf_empty(pevtpriv->c2h_queue)) {
 167                 void *c2h;
 168                 if ((c2h = rtw_cbuf_pop(pevtpriv->c2h_queue)) != NULL
 169                         && c2h != (void *)pevtpriv) {
 170                         rtw_mfree(c2h, 16);
 171                 }
 172         }
 173         rtw_cbuf_free(pevtpriv->c2h_queue);
 174 #endif
 175
 176         RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("-_rtw_free_evt_priv \n"));
 177
 178 _func_exit_;
 179
 180 }
 181
 182 void _rtw_free_cmd_priv (struct cmd_priv *pcmdpriv)
 183 {
 184 _func_enter_;
 185
 186         if(pcmdpriv){
 187                 _rtw_spinlock_free(&(pcmdpriv->cmd_queue.lock));
 188                 _rtw_free_sema(&(pcmdpriv->cmd_queue_sema));
 189                 //_rtw_free_sema(&(pcmdpriv->cmd_done_sema));
 190                 _rtw_free_sema(&(pcmdpriv->terminate_cmdthread_sema));
 191
 192                 if (pcmdpriv->cmd_allocated_buf)
 193                         rtw_mfree(pcmdpriv->cmd_allocated_buf, MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
 194
 195                 if (pcmdpriv->rsp_allocated_buf)
 196                         rtw_mfree(pcmdpriv->rsp_allocated_buf, MAX_RSPSZ + 4);
 197
 198                 _rtw_mutex_free(&pcmdpriv->sctx_mutex);
 199         }
 200 _func_exit_;
 201 }
 202
 203 /*
 204 Calling Context:
 205
 206 rtw_enqueue_cmd can only be called between kernel thread,
 207 since only spin_lock is used.
 208
 209 ISR/Call-Back functions can't call this sub-function.
 210
 211 */
 212 #ifdef DBG_CMD_QUEUE
 213 extern u8 dump_cmd_id;
 214 #endif
 215
 216 sint    _rtw_enqueue_cmd(_queue *queue, struct cmd_obj *obj)
 217 {
 218         _irqL irqL;
 219
 220 _func_enter_;
 221
 222         if (obj == NULL)
 223                 goto exit;
 224
 225         if(obj->cmdsz > MAX_CMDSZ ){
 226                 DBG_871X("%s failed due to obj->cmdsz(%d) > MAX_CMDSZ(%d) \n",__FUNCTION__, obj->cmdsz,MAX_CMDSZ);
 227                 goto exit;
 228         }
 229         //_enter_critical_bh(&queue->lock, &irqL);
 230         _enter_critical(&queue->lock, &irqL);
 231
 232         rtw_list_insert_tail(&obj->list, &queue->queue);
 233
 234         #ifdef DBG_CMD_QUEUE
 235         if(dump_cmd_id){
 236                 printk("%s===> cmdcode:0x%02x\n",__FUNCTION__,obj->cmdcode);
 237                 if(obj->cmdcode == GEN_CMD_CODE(_Set_MLME_EVT)){
 238                         if(obj->parmbuf){
 239                                 struct C2HEvent_Header *pc2h_evt_hdr = (struct C2HEvent_Header *)(obj->parmbuf);
 240                                 printk("pc2h_evt_hdr->ID:0x%02x(%d)\n",pc2h_evt_hdr->ID,pc2h_evt_hdr->ID);
 241                         }
 242                 }
 243                 if(obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)){
 244                         if(obj->parmbuf){
 245                                 struct drvextra_cmd_parm *pdrvextra_cmd_parm =(struct drvextra_cmd_parm*)(obj->parmbuf);
 246                                 printk("pdrvextra_cmd_parm->ec_id:0x%02x\n",pdrvextra_cmd_parm->ec_id);
 247                         }
 248                 }
 249         }
 250
 251         if (queue->queue.prev->next != &queue->queue)
 252         {
 253                 DBG_871X("[%d] head %p, tail %p, tail->prev->next %p[tail], tail->next %p[head]\n", __LINE__,
 254             &queue->queue, queue->queue.prev, queue->queue.prev->prev->next, queue->queue.prev->next);
 255
 256                 DBG_871X("==========%s============\n",__FUNCTION__);
 257                 DBG_871X("head:%p,obj_addr:%p\n",&queue->queue,obj);
 258                 DBG_871X("padapter: %p\n",obj->padapter);
 259                 DBG_871X("cmdcode: 0x%02x\n",obj->cmdcode);
 260                 DBG_871X("res: %d\n",obj->res);
 261                 DBG_871X("parmbuf: %p\n",obj->parmbuf);
 262                 DBG_871X("cmdsz: %d\n",obj->cmdsz);
 263                 DBG_871X("rsp: %p\n",obj->rsp);
 264                 DBG_871X("rspsz: %d\n",obj->rspsz);
 265                 DBG_871X("sctx: %p\n",obj->sctx);
 266                 DBG_871X("list->next: %p\n",obj->list.next);
 267                 DBG_871X("list->prev: %p\n",obj->list.prev);
 268         }
 269         #endif //DBG_CMD_QUEUE
 270
 271         //_exit_critical_bh(&queue->lock, &irqL);
 272         _exit_critical(&queue->lock, &irqL);
 273
 274 exit:
 275
 276 _func_exit_;
 277
 278         return _SUCCESS;
 279 }
 280
 281 struct  cmd_obj *_rtw_dequeue_cmd(_queue *queue)
 282 {
 283         _irqL irqL;
 284         struct cmd_obj *obj;
 285
 286 _func_enter_;
 287
 288         //_enter_critical_bh(&(queue->lock), &irqL);
 289         _enter_critical(&queue->lock, &irqL);
 290
 291         #ifdef DBG_CMD_QUEUE
 292         if (queue->queue.prev->next != &queue->queue)
 293         {
 294                  DBG_871X("[%d] head %p, tail %p, tail->prev->next %p[tail], tail->next %p[head]\n", __LINE__,
 295             &queue->queue, queue->queue.prev, queue->queue.prev->prev->next, queue->queue.prev->next);
 296         }
 297         #endif //DBG_CMD_QUEUE
 298
 299
 300         if (rtw_is_list_empty(&(queue->queue))){
 301                 obj = NULL;
 302         }
 303         else
 304         {
 305                 obj = LIST_CONTAINOR(get_next(&(queue->queue)), struct cmd_obj, list);
 306
 307                 #ifdef DBG_CMD_QUEUE
 308                 if (queue->queue.prev->next != &queue->queue){
 309                                 DBG_871X("==========%s============\n",__FUNCTION__);
 310                           DBG_871X("head:%p,obj_addr:%p\n",&queue->queue,obj);
 311                                 DBG_871X("padapter: %p\n",obj->padapter);
 312                                 DBG_871X("cmdcode: 0x%02x\n",obj->cmdcode);
 313                                 DBG_871X("res: %d\n",obj->res);
 314                                 DBG_871X("parmbuf: %p\n",obj->parmbuf);
 315                                 DBG_871X("cmdsz: %d\n",obj->cmdsz);
 316                                 DBG_871X("rsp: %p\n",obj->rsp);
 317                                 DBG_871X("rspsz: %d\n",obj->rspsz);
 318                                 DBG_871X("sctx: %p\n",obj->sctx);
 319                                 DBG_871X("list->next: %p\n",obj->list.next);
 320                                 DBG_871X("list->prev: %p\n",obj->list.prev);
 321                 }
 322
 323                 if(dump_cmd_id){
 324                         DBG_871X("%s===> cmdcode:0x%02x\n",__FUNCTION__,obj->cmdcode);
 325                         if(obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)){
 326                                 if(obj->parmbuf){
 327                                 struct drvextra_cmd_parm *pdrvextra_cmd_parm =(struct drvextra_cmd_parm*)(obj->parmbuf);
 328                                 printk("pdrvextra_cmd_parm->ec_id:0x%02x\n",pdrvextra_cmd_parm->ec_id);
 329                         }
 330                         }
 331
 332                 }
 333                 #endif //DBG_CMD_QUEUE
 334
 335                 rtw_list_delete(&obj->list);
 336         }
 337
 338         //_exit_critical_bh(&(queue->lock), &irqL);
 339         _exit_critical(&queue->lock, &irqL);
 340
 341 _func_exit_;
 342
 343         return obj;
 344 }
 345
 346 u32     rtw_init_cmd_priv(struct cmd_priv *pcmdpriv)
 347 {
 348         u32     res;
 349 _func_enter_;
 350         res = _rtw_init_cmd_priv (pcmdpriv);
 351 _func_exit_;
 352         return res;
 353 }
 354
 355 u32     rtw_init_evt_priv (struct       evt_priv *pevtpriv)
 356 {
 357         int     res;
 358 _func_enter_;
 359         res = _rtw_init_evt_priv(pevtpriv);
 360 _func_exit_;
 361         return res;
 362 }
 363
 364 void rtw_free_evt_priv (struct  evt_priv *pevtpriv)
 365 {
 366 _func_enter_;
 367         RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("rtw_free_evt_priv\n"));
 368         _rtw_free_evt_priv(pevtpriv);
 369 _func_exit_;
 370 }
 371
 372 void rtw_free_cmd_priv (struct  cmd_priv *pcmdpriv)
 373 {
 374 _func_enter_;
 375         RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("rtw_free_cmd_priv\n"));
 376         _rtw_free_cmd_priv(pcmdpriv);
 377 _func_exit_;
 378 }
 379
 380 int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj);
 381 int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 382 {
 383         u8 bAllow = _FALSE; //set to _TRUE to allow enqueuing cmd when hw_init_completed is _FALSE
 384
 385         #ifdef SUPPORT_HW_RFOFF_DETECTED
 386         //To decide allow or not
 387         if( (adapter_to_pwrctl(pcmdpriv->padapter)->bHWPwrPindetect)
 388                 &&(!pcmdpriv->padapter->registrypriv.usbss_enable)
 389         )
 390         {
 391                 if(cmd_obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra) )
 392                 {
 393                         struct drvextra_cmd_parm        *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)cmd_obj->parmbuf;
 394                         if(pdrvextra_cmd_parm->ec_id == POWER_SAVING_CTRL_WK_CID)
 395                         {
 396                                 //DBG_871X("==>enqueue POWER_SAVING_CTRL_WK_CID\n");
 397                                 bAllow = _TRUE;
 398                         }
 399                 }
 400         }
 401         #endif
 402
 403 #ifndef CONFIG_C2H_PACKET_EN
 404         /* C2H should be always allowed */
 405         if(cmd_obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
 406                 struct drvextra_cmd_parm *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)cmd_obj->parmbuf;
 407                 if(pdrvextra_cmd_parm->ec_id == C2H_WK_CID) {
 408                         bAllow = _TRUE;
 409                 }
 410         }
 411 #endif
 412
 413         if(cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
 414                 bAllow = _TRUE;
 415
 416         if( (pcmdpriv->padapter->hw_init_completed ==_FALSE && bAllow == _FALSE)
 417                 || ATOMIC_READ(&(pcmdpriv->cmdthd_running)) == _FALSE   //com_thread not running
 418         )
 419         {
 420                 //DBG_871X("%s:%s: drop cmdcode:%u, hw_init_completed:%u, cmdthd_running:%u\n", caller_func, __FUNCTION__,
 421                 //      cmd_obj->cmdcode,
 422                 //      pcmdpriv->padapter->hw_init_completed,
 423                 //      pcmdpriv->cmdthd_running
 424                 //);
 425
 426                 return _FAIL;
 427         }
 428         return _SUCCESS;
 429 }
 430
 431
 432
 433 u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 434 {
 435         int res = _FAIL;
 436         PADAPTER padapter = pcmdpriv->padapter;
 437
 438 _func_enter_;
 439
 440         if (cmd_obj == NULL) {
 441                 goto exit;
 442         }
 443
 444         cmd_obj->padapter = padapter;
 445
 446 #ifdef CONFIG_CONCURRENT_MODE
 447         //change pcmdpriv to primary's pcmdpriv
 448         if (padapter->adapter_type != PRIMARY_ADAPTER && padapter->pbuddy_adapter)
 449                 pcmdpriv = &(padapter->pbuddy_adapter->cmdpriv);
 450 #endif
 451
 452         if( _FAIL == (res=rtw_cmd_filter(pcmdpriv, cmd_obj)) ) {
 453                 rtw_free_cmd_obj(cmd_obj);
 454                 goto exit;
 455         }
 456
 457         res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj);
 458
 459         if(res == _SUCCESS)
 460                 _rtw_up_sema(&pcmdpriv->cmd_queue_sema);
 461
 462 exit:
 463
 464 _func_exit_;
 465
 466         return res;
 467 }
 468
 469 struct  cmd_obj *rtw_dequeue_cmd(struct cmd_priv *pcmdpriv)
 470 {
 471         struct cmd_obj *cmd_obj;
 472
 473 _func_enter_;
 474
 475         cmd_obj = _rtw_dequeue_cmd(&pcmdpriv->cmd_queue);
 476
 477 _func_exit_;
 478         return cmd_obj;
 479 }
 480
 481 void rtw_cmd_clr_isr(struct     cmd_priv *pcmdpriv)
 482 {
 483 _func_enter_;
 484         pcmdpriv->cmd_done_cnt++;
 485         //_rtw_up_sema(&(pcmdpriv->cmd_done_sema));
 486 _func_exit_;
 487 }
 488
 489 void rtw_free_cmd_obj(struct cmd_obj *pcmd)
 490 {
 491         struct drvextra_cmd_parm *extra_parm = NULL;
 492 _func_enter_;
 493
 494         if(pcmd->parmbuf != NULL){
 495                 if((pcmd->cmdcode!=_JoinBss_CMD_) &&(pcmd->cmdcode!= _CreateBss_CMD_))
 496                 {
 497                         //free parmbuf in cmd_obj
 498                         rtw_mfree((unsigned char*)pcmd->parmbuf, pcmd->cmdsz);
 499                 }
 500         }
 501         if(pcmd->rsp!=NULL)
 502         {
 503                 if(pcmd->rspsz!= 0)
 504                 {
 505                         //free rsp in cmd_obj
 506                         rtw_mfree((unsigned char*)pcmd->rsp, pcmd->rspsz);
 507                 }
 508         }
 509
 510         //free cmd_obj
 511         rtw_mfree((unsigned char*)pcmd, sizeof(struct cmd_obj));
 512
 513 _func_exit_;
 514 }
 515
 516
 517 void rtw_stop_cmd_thread(_adapter *adapter)
 518 {
 519         if(adapter->cmdThread &&
 520                 ATOMIC_READ(&(adapter->cmdpriv.cmdthd_running)) == _TRUE &&
 521                 adapter->cmdpriv.stop_req == 0)
 522         {
 523                 adapter->cmdpriv.stop_req = 1;
 524                 _rtw_up_sema(&adapter->cmdpriv.cmd_queue_sema);
 525                 _rtw_down_sema(&adapter->cmdpriv.terminate_cmdthread_sema);
 526         }
 527 }
 528
 529 thread_return rtw_cmd_thread(thread_context context)
 530 {
 531         u8 ret;
 532         struct cmd_obj *pcmd;
 533         u8 *pcmdbuf, *prspbuf;
 534         u32 cmd_start_time;
 535         u32 cmd_process_time;
 536         u8 (*cmd_hdl)(_adapter *padapter, u8* pbuf);
 537         void (*pcmd_callback)(_adapter *dev, struct cmd_obj *pcmd);
 538         PADAPTER padapter = (PADAPTER)context;
 539         struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
 540         struct drvextra_cmd_parm *extra_parm = NULL;
 541         _irqL irqL;
 542 _func_enter_;
 543
 544         thread_enter("RTW_CMD_THREAD");
 545
 546         pcmdbuf = pcmdpriv->cmd_buf;
 547         prspbuf = pcmdpriv->rsp_buf;
 548
 549         pcmdpriv->stop_req = 0;
 550         ATOMIC_SET(&(pcmdpriv->cmdthd_running), _TRUE);
 551         _rtw_up_sema(&pcmdpriv->terminate_cmdthread_sema);
 552
 553         RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("start r871x rtw_cmd_thread !!!!\n"));
 554
 555         while(1)
 556         {
 557                 if (_rtw_down_sema(&pcmdpriv->cmd_queue_sema) == _FAIL) {
 558                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" _rtw_down_sema(&pcmdpriv->cmd_queue_sema) return _FAIL, break\n", FUNC_ADPT_ARG(padapter));
 559                         break;
 560                 }
 561
 562                 if ((padapter->bDriverStopped == _TRUE)||(padapter->bSurpriseRemoved == _TRUE))
 563                 {
 564                         DBG_871X_LEVEL(_drv_always_, "%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
 565                                 __FUNCTION__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
 566                         break;
 567                 }
 568
 569                 if (pcmdpriv->stop_req) {
 570                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" stop_req:%u, break\n", FUNC_ADPT_ARG(padapter), pcmdpriv->stop_req);
 571                         break;
 572                 }
 573
 574                 _enter_critical(&pcmdpriv->cmd_queue.lock, &irqL);
 575                 if(rtw_is_list_empty(&(pcmdpriv->cmd_queue.queue)))
 576                 {
 577                         //DBG_871X("%s: cmd queue is empty!\n", __func__);
 578                         _exit_critical(&pcmdpriv->cmd_queue.lock, &irqL);
 579                         continue;
 580                 }
 581                 _exit_critical(&pcmdpriv->cmd_queue.lock, &irqL);
 582
 583 #ifdef CONFIG_LPS_LCLK
 584                 if (rtw_register_cmd_alive(padapter) != _SUCCESS)
 585                 {
 586                         RT_TRACE(_module_hal_xmit_c_, _drv_notice_,
 587                                          ("%s: wait to leave LPS_LCLK\n", __FUNCTION__));
 588                         continue;
 589                 }
 590 #endif
 591
 592 _next:
 593                 if ((padapter->bDriverStopped == _TRUE)||(padapter->bSurpriseRemoved== _TRUE))
 594                 {
 595                         DBG_871X_LEVEL(_drv_always_, "%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
 596                                 __FUNCTION__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
 597                         break;
 598                 }
 599
 600                 if(!(pcmd = rtw_dequeue_cmd(pcmdpriv))) {
 601 #ifdef CONFIG_LPS_LCLK
 602                         rtw_unregister_cmd_alive(padapter);
 603 #endif
 604                         continue;
 605                 }
 606
 607                 cmd_start_time = rtw_get_current_time();
 608
 609                 if( _FAIL == rtw_cmd_filter(pcmdpriv, pcmd) )
 610                 {
 611                         pcmd->res = H2C_DROPPED;
 612                         if (pcmd->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
 613                                 extra_parm = (struct drvextra_cmd_parm *)pcmd->parmbuf;
 614                                 if (extra_parm && extra_parm->pbuf && extra_parm->size > 0)
 615                                         rtw_mfree(extra_parm->pbuf, extra_parm->size);
 616                         }
 617                         goto post_process;
 618                 }
 619
 620                 pcmdpriv->cmd_issued_cnt++;
 621
 622                 pcmd->cmdsz = _RND4((pcmd->cmdsz));//_RND4
 623
 624                 if(pcmd->cmdsz > MAX_CMDSZ ){
 625                         DBG_871X("%s cmdsz:%d > MAX_CMDSZ:%d\n",__FUNCTION__,pcmd->cmdsz,MAX_CMDSZ);
 626                 }
 627
 628                 _rtw_memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
 629
 630                 if(pcmd->cmdcode < (sizeof(wlancmds) /sizeof(struct cmd_hdl)))
 631                 {
 632                         cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
 633
 634                         if (cmd_hdl)
 635                         {
 636                                 ret = cmd_hdl(pcmd->padapter, pcmdbuf);
 637                                 pcmd->res = ret;
 638                         }
 639
 640                         pcmdpriv->cmd_seq++;
 641                 }
 642                 else
 643                 {
 644                         pcmd->res = H2C_PARAMETERS_ERROR;
 645                 }
 646
 647                 cmd_hdl = NULL;
 648
 649 post_process:
 650
 651                 _enter_critical_mutex(&(pcmd->padapter->cmdpriv.sctx_mutex), NULL);
 652                 if (pcmd->sctx) {
 653                         if (0)
 654                         DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" pcmd->sctx\n",
 655                                 FUNC_ADPT_ARG(pcmd->padapter));
 656                         if (pcmd->res == H2C_SUCCESS)
 657                                 rtw_sctx_done(&pcmd->sctx);
 658                         else
 659                                 rtw_sctx_done_err(&pcmd->sctx, RTW_SCTX_DONE_CMD_ERROR);
 660                 }
 661                 _exit_critical_mutex(&(pcmd->padapter->cmdpriv.sctx_mutex), NULL);
 662
 663
 664                 if((cmd_process_time = rtw_get_passing_time_ms(cmd_start_time)) > 1000)
 665                 {
 666                         if (pcmd->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
 667                                 struct drvextra_cmd_parm *drvextra_parm = (struct drvextra_cmd_parm *)pcmdbuf;
 668                                 DBG_871X(ADPT_FMT" cmd=%d,%d,%d process_time=%d > 1 sec\n",
 669                                         ADPT_ARG(pcmd->padapter), pcmd->cmdcode, drvextra_parm->ec_id, drvextra_parm->type, cmd_process_time);
 670                                 //rtw_warn_on(1);
 671                         } else if(pcmd->cmdcode == GEN_CMD_CODE(_Set_MLME_EVT)){
 672                                 struct C2HEvent_Header *pc2h_evt_hdr = (struct C2HEvent_Header *)pcmdbuf;
 673                                 DBG_871X(ADPT_FMT" cmd=%d,%d, process_time=%d > 1 sec\n",
 674                                         ADPT_ARG(pcmd->padapter), pcmd->cmdcode, pc2h_evt_hdr->ID, cmd_process_time);
 675                                 //rtw_warn_on(1);
 676                         } else {
 677                                 DBG_871X(ADPT_FMT" cmd=%d, process_time=%d > 1 sec\n",
 678                                         ADPT_ARG(pcmd->padapter), pcmd->cmdcode, cmd_process_time);
 679                                 //rtw_warn_on(1);
 680                         }
 681                 }
 682
 683                 //call callback function for post-processed
 684                 if(pcmd->cmdcode < (sizeof(rtw_cmd_callback) /sizeof(struct _cmd_callback)))
 685                 {
 686                         pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
 687                         if(pcmd_callback == NULL)
 688                         {
 689                                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("mlme_cmd_hdl(): pcmd_callback=0x%p, cmdcode=0x%x\n", pcmd_callback, pcmd->cmdcode));
 690                                 rtw_free_cmd_obj(pcmd);
 691                         }
 692                         else
 693                         {
 694                                 //todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!=NULL)
 695                                 pcmd_callback(pcmd->padapter, pcmd);//need conider that free cmd_obj in rtw_cmd_callback
 696                         }
 697                 }
 698                 else
 699                 {
 700                         RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("%s: cmdcode=0x%x callback not defined!\n", __FUNCTION__, pcmd->cmdcode));
 701                         rtw_free_cmd_obj(pcmd);
 702                 }
 703
 704                 flush_signals_thread();
 705
 706                 goto _next;
 707
 708         }
 709
 710         // free all cmd_obj resources
 711         do{
 712                 pcmd = rtw_dequeue_cmd(pcmdpriv);
 713                 if(pcmd==NULL){
 714 #ifdef CONFIG_LPS_LCLK
 715                         rtw_unregister_cmd_alive(padapter);
 716 #endif
 717                         break;
 718                 }
 719                 //DBG_871X("%s: leaving... drop cmdcode:%u size:%d\n", __FUNCTION__, pcmd->cmdcode, pcmd->cmdsz);
 720
 721                 if (pcmd->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
 722                         extra_parm = (struct drvextra_cmd_parm *)pcmd->parmbuf;
 723                         if(extra_parm->pbuf && extra_parm->size > 0) {
 724                                 rtw_mfree(extra_parm->pbuf, extra_parm->size);
 725                         }
 726                 }
 727
 728                 rtw_free_cmd_obj(pcmd);
 729         }while(1);
 730
 731         _rtw_up_sema(&pcmdpriv->terminate_cmdthread_sema);
 732         ATOMIC_SET(&(pcmdpriv->cmdthd_running), _FALSE);
 733
 734 _func_exit_;
 735
 736         thread_exit();
 737
 738 }
 739
 740
 741 #ifdef CONFIG_EVENT_THREAD_MODE
 742 u32 rtw_enqueue_evt(struct evt_priv *pevtpriv, struct evt_obj *obj)
 743 {
 744         _irqL irqL;
 745         int     res;
 746         _queue *queue = &pevtpriv->evt_queue;
 747
 748 _func_enter_;
 749
 750         res = _SUCCESS;
 751
 752         if (obj == NULL) {
 753                 res = _FAIL;
 754                 goto exit;
 755         }
 756
 757         _enter_critical_bh(&queue->lock, &irqL);
 758
 759         rtw_list_insert_tail(&obj->list, &queue->queue);
 760
 761         _exit_critical_bh(&queue->lock, &irqL);
 762
 763         //rtw_evt_notify_isr(pevtpriv);
 764
 765 exit:
 766
 767 _func_exit_;
 768
 769         return res;
 770 }
 771
 772 struct evt_obj *rtw_dequeue_evt(_queue *queue)
 773 {
 774         _irqL irqL;
 775         struct  evt_obj *pevtobj;
 776
 777 _func_enter_;
 778
 779         _enter_critical_bh(&queue->lock, &irqL);
 780
 781         if (rtw_is_list_empty(&(queue->queue)))
 782                 pevtobj = NULL;
 783         else
 784         {
 785                 pevtobj = LIST_CONTAINOR(get_next(&(queue->queue)), struct evt_obj, list);
 786                 rtw_list_delete(&pevtobj->list);
 787         }
 788
 789         _exit_critical_bh(&queue->lock, &irqL);
 790
 791 _func_exit_;
 792
 793         return pevtobj;
 794 }
 795
 796 void rtw_free_evt_obj(struct evt_obj *pevtobj)
 797 {
 798 _func_enter_;
 799
 800         if(pevtobj->parmbuf)
 801                 rtw_mfree((unsigned char*)pevtobj->parmbuf, pevtobj->evtsz);
 802
 803         rtw_mfree((unsigned char*)pevtobj, sizeof(struct evt_obj));
 804
 805 _func_exit_;
 806 }
 807
 808 void rtw_evt_notify_isr(struct evt_priv *pevtpriv)
 809 {
 810 _func_enter_;
 811         pevtpriv->evt_done_cnt++;
 812         _rtw_up_sema(&(pevtpriv->evt_notify));
 813 _func_exit_;
 814 }
 815 #endif
 816
 817
 818 /*
 819 u8 rtw_setstandby_cmd(unsigned char  *adapter)
 820 */
 821 u8 rtw_setstandby_cmd(_adapter *padapter, uint action)
 822 {
 823         struct cmd_obj*                 ph2c;
 824         struct usb_suspend_parm*        psetusbsuspend;
 825         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
 826
 827         u8 ret = _SUCCESS;
 828
 829 _func_enter_;
 830
 831         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
 832         if (ph2c == NULL) {
 833                 ret = _FAIL;
 834                 goto exit;
 835         }
 836
 837         psetusbsuspend = (struct usb_suspend_parm*)rtw_zmalloc(sizeof(struct usb_suspend_parm));
 838         if (psetusbsuspend == NULL) {
 839                 rtw_mfree((u8 *) ph2c, sizeof(struct    cmd_obj));
 840                 ret = _FAIL;
 841                 goto exit;
 842         }
 843
 844         psetusbsuspend->action = action;
 845
 846         init_h2fwcmd_w_parm_no_rsp(ph2c, psetusbsuspend, GEN_CMD_CODE(_SetUsbSuspend));
 847
 848         ret = rtw_enqueue_cmd(pcmdpriv, ph2c);
 849
 850 exit:
 851
 852 _func_exit_;
 853
 854         return ret;
 855 }
 856
 857 /*
 858 rtw_sitesurvey_cmd(~)
 859         ### NOTE:#### (!!!!)
 860         MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
 861 */
 862 u8 rtw_sitesurvey_cmd(_adapter  *padapter, NDIS_802_11_SSID *ssid, int ssid_num,
 863         struct rtw_ieee80211_channel *ch, int ch_num)
 864 {
 865         u8 res = _FAIL;
 866 #ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
 867         u8 wireless_mode = 0;
 868         u32 mlmext_info_state = 0;
 869 #endif
 870         struct cmd_obj          *ph2c;
 871         struct sitesurvey_parm  *psurveyPara;
 872         struct cmd_priv         *pcmdpriv = &padapter->cmdpriv;
 873         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
 874 #ifdef CONFIG_P2P
 875         struct wifidirect_info *pwdinfo= &(padapter->wdinfo);
 876 #endif //CONFIG_P2P
 877
 878 _func_enter_;
 879
 880 #ifdef CONFIG_LPS
 881         if(check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE){
 882                 rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1);
 883         }
 884 #endif
 885
 886 #ifdef CONFIG_P2P_PS
 887         if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE) {
 888                 p2p_ps_wk_cmd(padapter, P2P_PS_SCAN, 1);
 889         }
 890 #endif //CONFIG_P2P_PS
 891
 892         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
 893         if (ph2c == NULL)
 894                 return _FAIL;
 895
 896         psurveyPara = (struct sitesurvey_parm*)rtw_zmalloc(sizeof(struct sitesurvey_parm));
 897         if (psurveyPara == NULL) {
 898                 rtw_mfree((unsigned char*) ph2c, sizeof(struct cmd_obj));
 899                 return _FAIL;
 900         }
 901
 902         rtw_free_network_queue(padapter, _FALSE);
 903
 904         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("%s: flush network queue\n", __FUNCTION__));
 905
 906         init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
 907
 908         /* psurveyPara->bsslimit = 48; */
 909         psurveyPara->scan_mode = pmlmepriv->scan_mode;
 910
 911         /* prepare ssid list */
 912         if (ssid) {
 913                 int i;
 914                 for (i=0; i<ssid_num && i< RTW_SSID_SCAN_AMOUNT; i++) {
 915                         if (ssid[i].SsidLength) {
 916                                 _rtw_memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(NDIS_802_11_SSID));
 917                                 psurveyPara->ssid_num++;
 918                                 if (0)
 919                                 DBG_871X(FUNC_ADPT_FMT" ssid:(%s, %d)\n", FUNC_ADPT_ARG(padapter),
 920                                         psurveyPara->ssid[i].Ssid, psurveyPara->ssid[i].SsidLength);
 921                         }
 922                 }
 923         }
 924
 925         /* prepare channel list */
 926         if (ch) {
 927                 int i;
 928                 for (i=0; i<ch_num && i< RTW_CHANNEL_SCAN_AMOUNT; i++) {
 929                         if (ch[i].hw_value && !(ch[i].flags & RTW_IEEE80211_CHAN_DISABLED)) {
 930                                 _rtw_memcpy(&psurveyPara->ch[i], &ch[i], sizeof(struct rtw_ieee80211_channel));
 931                                 psurveyPara->ch_num++;
 932                                 if (0)
 933                                 DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter),
 934                                         psurveyPara->ch[i].hw_value);
 935                         }
 936                 }
 937         }
 938
 939         set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
 940
 941         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 942
 943         if(res == _SUCCESS) {
 944
 945                 pmlmepriv->scan_start_time = rtw_get_current_time();
 946 #ifdef CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
 947                 mlmext_info_state =
 948                         padapter->pbuddy_adapter->mlmeextpriv.mlmext_info.state;
 949                 wireless_mode = padapter->registrypriv.wireless_mode;
 950
 951                 if((mlmext_info_state & 0x03) == WIFI_FW_AP_STATE) {
 952                         if(IsSupported5G(wireless_mode) && IsSupported24G(wireless_mode))
 953                                 _set_timer(&pmlmepriv->scan_to_timer,
 954                                                 CONC_SCANNING_TIMEOUT_DUAL_BAND);
 955                         else
 956                                 _set_timer(&pmlmepriv->scan_to_timer,
 957                                                 CONC_SCANNING_TIMEOUT_SINGLE_BAND);
 958                 } else
 959 #endif //CONFIG_STA_MODE_SCAN_UNDER_AP_MODE
 960                         _set_timer(&pmlmepriv->scan_to_timer, SCANNING_TIMEOUT);
 961
 962                 rtw_led_control(padapter, LED_CTL_SITE_SURVEY);
 963         } else {
 964                 _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 965         }
 966
 967 _func_exit_;
 968
 969         return res;
 970 }
 971
 972 u8 rtw_setdatarate_cmd(_adapter *padapter, u8 *rateset)
 973 {
 974         struct cmd_obj*                 ph2c;
 975         struct setdatarate_parm*        pbsetdataratepara;
 976         struct cmd_priv*                pcmdpriv = &padapter->cmdpriv;
 977         u8      res = _SUCCESS;
 978
 979 _func_enter_;
 980
 981         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
 982         if (ph2c == NULL) {
 983                 res = _FAIL;
 984                 goto exit;
 985         }
 986
 987         pbsetdataratepara = (struct setdatarate_parm*)rtw_zmalloc(sizeof(struct setdatarate_parm));
 988         if (pbsetdataratepara == NULL) {
 989                 rtw_mfree((u8 *) ph2c, sizeof(struct cmd_obj));
 990                 res = _FAIL;
 991                 goto exit;
 992         }
 993
 994         init_h2fwcmd_w_parm_no_rsp(ph2c, pbsetdataratepara, GEN_CMD_CODE(_SetDataRate));
 995 #ifdef MP_FIRMWARE_OFFLOAD
 996         pbsetdataratepara->curr_rateidx = *(u32*)rateset;
 997 //      _rtw_memcpy(pbsetdataratepara, rateset, sizeof(u32));
 998 #else
 999         pbsetdataratepara->mac_id = 5;
1000         _rtw_memcpy(pbsetdataratepara->datarates, rateset, NumRates);
1001 #endif
1002         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1003 exit:
1004
1005 _func_exit_;
1006
1007         return res;
1008 }
1009
1010 u8 rtw_setbasicrate_cmd(_adapter *padapter, u8 *rateset)
1011 {
1012         struct cmd_obj*                 ph2c;
1013         struct setbasicrate_parm*       pssetbasicratepara;
1014         struct cmd_priv*                pcmdpriv=&padapter->cmdpriv;
1015         u8      res = _SUCCESS;
1016
1017 _func_enter_;
1018
1019         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1020         if (ph2c == NULL) {
1021                 res= _FAIL;
1022                 goto exit;
1023         }
1024         pssetbasicratepara = (struct setbasicrate_parm*)rtw_zmalloc(sizeof(struct setbasicrate_parm));
1025
1026         if (pssetbasicratepara == NULL) {
1027                 rtw_mfree((u8*) ph2c, sizeof(struct cmd_obj));
1028                 res = _FAIL;
1029                 goto exit;
1030         }
1031
1032         init_h2fwcmd_w_parm_no_rsp(ph2c, pssetbasicratepara, _SetBasicRate_CMD_);
1033
1034         _rtw_memcpy(pssetbasicratepara->basicrates, rateset, NumRates);
1035
1036         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1037 exit:
1038
1039 _func_exit_;
1040
1041         return res;
1042 }
1043
1044
1045 /*
1046 unsigned char rtw_setphy_cmd(unsigned char  *adapter)
1047
1048 1.  be called only after rtw_update_registrypriv_dev_network( ~) or mp testing program
1049 2.  for AdHoc/Ap mode or mp mode?
1050
1051 */
1052 u8 rtw_setphy_cmd(_adapter *padapter, u8 modem, u8 ch)
1053 {
1054         struct cmd_obj*                 ph2c;
1055         struct setphy_parm*             psetphypara;
1056         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
1057 //      struct mlme_priv                        *pmlmepriv = &padapter->mlmepriv;
1058 //      struct registry_priv*           pregistry_priv = &padapter->registrypriv;
1059         u8      res=_SUCCESS;
1060
1061 _func_enter_;
1062
1063         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1064         if(ph2c==NULL){
1065                 res= _FAIL;
1066                 goto exit;
1067                 }
1068         psetphypara = (struct setphy_parm*)rtw_zmalloc(sizeof(struct setphy_parm));
1069
1070         if(psetphypara==NULL){
1071                 rtw_mfree((u8 *) ph2c, sizeof(struct    cmd_obj));
1072                 res= _FAIL;
1073                 goto exit;
1074         }
1075
1076         init_h2fwcmd_w_parm_no_rsp(ph2c, psetphypara, _SetPhy_CMD_);
1077
1078         RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,("CH=%d, modem=%d", ch, modem));
1079
1080         psetphypara->modem = modem;
1081         psetphypara->rfchannel = ch;
1082
1083         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1084 exit:
1085 _func_exit_;
1086         return res;
1087 }
1088
1089 u8 rtw_setbbreg_cmd(_adapter*padapter, u8 offset, u8 val)
1090 {
1091         struct cmd_obj*                 ph2c;
1092         struct writeBB_parm*            pwritebbparm;
1093         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
1094         u8      res=_SUCCESS;
1095 _func_enter_;
1096         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1097         if(ph2c==NULL){
1098                 res= _FAIL;
1099                 goto exit;
1100                 }
1101         pwritebbparm = (struct writeBB_parm*)rtw_zmalloc(sizeof(struct writeBB_parm));
1102
1103         if(pwritebbparm==NULL){
1104                 rtw_mfree((u8 *) ph2c, sizeof(struct    cmd_obj));
1105                 res= _FAIL;
1106                 goto exit;
1107         }
1108
1109         init_h2fwcmd_w_parm_no_rsp(ph2c, pwritebbparm, GEN_CMD_CODE(_SetBBReg));
1110
1111         pwritebbparm->offset = offset;
1112         pwritebbparm->value = val;
1113
1114         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1115 exit:
1116 _func_exit_;
1117         return res;
1118 }
1119
1120 u8 rtw_getbbreg_cmd(_adapter  *padapter, u8 offset, u8 *pval)
1121 {
1122         struct cmd_obj*                 ph2c;
1123         struct readBB_parm*             prdbbparm;
1124         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
1125         u8      res=_SUCCESS;
1126
1127 _func_enter_;
1128         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1129         if(ph2c==NULL){
1130                 res=_FAIL;
1131                 goto exit;
1132                 }
1133         prdbbparm = (struct readBB_parm*)rtw_zmalloc(sizeof(struct readBB_parm));
1134
1135         if(prdbbparm ==NULL){
1136                 rtw_mfree((unsigned char *) ph2c, sizeof(struct cmd_obj));
1137                 return _FAIL;
1138         }
1139
1140         _rtw_init_listhead(&ph2c->list);
1141         ph2c->cmdcode =GEN_CMD_CODE(_GetBBReg);
1142         ph2c->parmbuf = (unsigned char *)prdbbparm;
1143         ph2c->cmdsz =  sizeof(struct readBB_parm);
1144         ph2c->rsp = pval;
1145         ph2c->rspsz = sizeof(struct readBB_rsp);
1146
1147         prdbbparm ->offset = offset;
1148
1149         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1150 exit:
1151 _func_exit_;
1152         return res;
1153 }
1154
1155 u8 rtw_setrfreg_cmd(_adapter  *padapter, u8 offset, u32 val)
1156 {
1157         struct cmd_obj*                 ph2c;
1158         struct writeRF_parm*            pwriterfparm;
1159         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
1160         u8      res=_SUCCESS;
1161 _func_enter_;
1162         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1163         if(ph2c==NULL){
1164                 res= _FAIL;
1165                 goto exit;
1166         }
1167         pwriterfparm = (struct writeRF_parm*)rtw_zmalloc(sizeof(struct writeRF_parm));
1168
1169         if(pwriterfparm==NULL){
1170                 rtw_mfree((u8 *) ph2c, sizeof(struct    cmd_obj));
1171                 res= _FAIL;
1172                 goto exit;
1173         }
1174
1175         init_h2fwcmd_w_parm_no_rsp(ph2c, pwriterfparm, GEN_CMD_CODE(_SetRFReg));
1176
1177         pwriterfparm->offset = offset;
1178         pwriterfparm->value = val;
1179
1180         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1181 exit:
1182 _func_exit_;
1183         return res;
1184 }
1185
1186 u8 rtw_getrfreg_cmd(_adapter  *padapter, u8 offset, u8 *pval)
1187 {
1188         struct cmd_obj*                 ph2c;
1189         struct readRF_parm*             prdrfparm;
1190         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
1191         u8      res=_SUCCESS;
1192
1193 _func_enter_;
1194
1195         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1196         if(ph2c==NULL){
1197                 res= _FAIL;
1198                 goto exit;
1199         }
1200
1201         prdrfparm = (struct readRF_parm*)rtw_zmalloc(sizeof(struct readRF_parm));
1202         if(prdrfparm ==NULL){
1203                 rtw_mfree((u8 *) ph2c, sizeof(struct    cmd_obj));
1204                 res= _FAIL;
1205                 goto exit;
1206         }
1207
1208         _rtw_init_listhead(&ph2c->list);
1209         ph2c->cmdcode =GEN_CMD_CODE(_GetRFReg);
1210         ph2c->parmbuf = (unsigned char *)prdrfparm;
1211         ph2c->cmdsz =  sizeof(struct readRF_parm);
1212         ph2c->rsp = pval;
1213         ph2c->rspsz = sizeof(struct readRF_rsp);
1214
1215         prdrfparm ->offset = offset;
1216
1217         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1218
1219 exit:
1220
1221 _func_exit_;
1222
1223         return res;
1224 }
1225
1226 void rtw_getbbrfreg_cmdrsp_callback(_adapter*   padapter,  struct cmd_obj *pcmd)
1227 {
1228  _func_enter_;
1229
1230         //rtw_free_cmd_obj(pcmd);
1231         rtw_mfree((unsigned char*) pcmd->parmbuf, pcmd->cmdsz);
1232         rtw_mfree((unsigned char*) pcmd, sizeof(struct cmd_obj));
1233
1234 #ifdef CONFIG_MP_INCLUDED
1235         if (padapter->registrypriv.mp_mode == 1)
1236                 padapter->mppriv.workparam.bcompleted= _TRUE;
1237 #endif
1238 _func_exit_;
1239 }
1240
1241 void rtw_readtssi_cmdrsp_callback(_adapter*     padapter,  struct cmd_obj *pcmd)
1242 {
1243  _func_enter_;
1244
1245         rtw_mfree((unsigned char*) pcmd->parmbuf, pcmd->cmdsz);
1246         rtw_mfree((unsigned char*) pcmd, sizeof(struct cmd_obj));
1247
1248 #ifdef CONFIG_MP_INCLUDED
1249         if (padapter->registrypriv.mp_mode == 1)
1250                 padapter->mppriv.workparam.bcompleted= _TRUE;
1251 #endif
1252
1253 _func_exit_;
1254 }
1255
1256 u8 rtw_createbss_cmd(_adapter  *padapter)
1257 {
1258         struct cmd_obj*                 pcmd;
1259         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
1260         struct mlme_priv                        *pmlmepriv = &padapter->mlmepriv;
1261         WLAN_BSSID_EX           *pdev_network = &padapter->registrypriv.dev_network;
1262         u8      res=_SUCCESS;
1263
1264 _func_enter_;
1265
1266         rtw_led_control(padapter, LED_CTL_START_TO_LINK);
1267
1268         if (pmlmepriv->assoc_ssid.SsidLength == 0){
1269                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,(" createbss for Any SSid:%s\n",pmlmepriv->assoc_ssid.Ssid));
1270         } else {
1271                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_info_,(" createbss for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
1272         }
1273
1274         pcmd = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1275         if(pcmd==NULL){
1276                 res= _FAIL;
1277                 goto exit;
1278         }
1279
1280         _rtw_init_listhead(&pcmd->list);
1281         pcmd->cmdcode = _CreateBss_CMD_;
1282         pcmd->parmbuf = (unsigned char *)pdev_network;
1283         pcmd->cmdsz = get_WLAN_BSSID_EX_sz((WLAN_BSSID_EX*)pdev_network);
1284         pcmd->rsp = NULL;
1285         pcmd->rspsz = 0;
1286
1287         pdev_network->Length = pcmd->cmdsz;
1288
1289 #ifdef CONFIG_RTL8712
1290         //notes: translate IELength & Length after assign the Length to cmdsz;
1291         pdev_network->Length = cpu_to_le32(pcmd->cmdsz);
1292         pdev_network->IELength = cpu_to_le32(pdev_network->IELength);
1293         pdev_network->Ssid.SsidLength = cpu_to_le32(pdev_network->Ssid.SsidLength);
1294 #endif
1295
1296         res = rtw_enqueue_cmd(pcmdpriv, pcmd);
1297
1298 exit:
1299
1300 _func_exit_;
1301
1302         return res;
1303 }
1304
1305 u8 rtw_createbss_cmd_ex(_adapter  *padapter, unsigned char *pbss, unsigned int sz)
1306 {
1307         struct cmd_obj* pcmd;
1308         struct cmd_priv         *pcmdpriv=&padapter->cmdpriv;
1309         u8      res=_SUCCESS;
1310
1311 _func_enter_;
1312
1313         pcmd = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1314         if(pcmd==NULL){
1315                 res= _FAIL;
1316                 goto exit;
1317         }
1318
1319         _rtw_init_listhead(&pcmd->list);
1320         pcmd->cmdcode = GEN_CMD_CODE(_CreateBss);
1321         pcmd->parmbuf = pbss;
1322         pcmd->cmdsz =  sz;
1323         pcmd->rsp = NULL;
1324         pcmd->rspsz = 0;
1325
1326         res = rtw_enqueue_cmd(pcmdpriv, pcmd);
1327
1328 exit:
1329
1330 _func_exit_;
1331
1332         return res;
1333 }
1334
1335 u8 rtw_startbss_cmd(_adapter  *padapter, int flags)
1336 {
1337         struct cmd_obj* pcmd;
1338         struct cmd_priv  *pcmdpriv=&padapter->cmdpriv;
1339         struct submit_ctx sctx;
1340         u8 res=_SUCCESS;
1341
1342 _func_enter_;
1343
1344         if (flags & RTW_CMDF_DIRECTLY) {
1345                 /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
1346                 start_bss_network(padapter, (u8*)&(padapter->mlmepriv.cur_network.network));
1347         } else {
1348                 /* need enqueue, prepare cmd_obj and enqueue */
1349                 pcmd = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1350                 if (pcmd == NULL) {
1351                         res = _FAIL;
1352                         goto exit;
1353                 }
1354
1355                 _rtw_init_listhead(&pcmd->list);
1356                 pcmd->cmdcode = GEN_CMD_CODE(_CreateBss);
1357                 pcmd->parmbuf = NULL;
1358                 pcmd->cmdsz =  0;
1359                 pcmd->rsp = NULL;
1360                 pcmd->rspsz = 0;
1361
1362                 if (flags & RTW_CMDF_WAIT_ACK) {
1363                         pcmd->sctx = &sctx;
1364                         rtw_sctx_init(&sctx, 2000);
1365                 }
1366
1367                 res = rtw_enqueue_cmd(pcmdpriv, pcmd);
1368
1369                 if (res == _SUCCESS && (flags & RTW_CMDF_WAIT_ACK)) {
1370                         rtw_sctx_wait(&sctx, __func__);
1371                         _enter_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
1372                         if (sctx.status == RTW_SCTX_SUBMITTED)
1373                                 pcmd->sctx = NULL;
1374                         _exit_critical_mutex(&pcmdpriv->sctx_mutex, NULL);
1375                 }
1376         }
1377
1378 exit:
1379
1380 _func_exit_;
1381
1382         return res;
1383 }
1384
1385 u8 rtw_joinbss_cmd(_adapter  *padapter, struct wlan_network* pnetwork)
1386 {
1387         u8      *auth, res = _SUCCESS;
1388         uint    t_len = 0;
1389         WLAN_BSSID_EX           *psecnetwork;
1390         struct cmd_obj          *pcmd;
1391         struct cmd_priv         *pcmdpriv=&padapter->cmdpriv;
1392         struct mlme_priv                *pmlmepriv = &padapter->mlmepriv;
1393         struct qos_priv         *pqospriv= &pmlmepriv->qospriv;
1394         struct security_priv    *psecuritypriv=&padapter->securitypriv;
1395         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
1396 #ifdef CONFIG_80211N_HT
1397         struct ht_priv                  *phtpriv = &pmlmepriv->htpriv;
1398 #endif //CONFIG_80211N_HT
1399 #ifdef CONFIG_80211AC_VHT
1400         struct vht_priv         *pvhtpriv = &pmlmepriv->vhtpriv;
1401 #endif //CONFIG_80211AC_VHT
1402         NDIS_802_11_NETWORK_INFRASTRUCTURE ndis_network_mode = pnetwork->network.InfrastructureMode;
1403         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1404         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
1405         u32 tmp_len;
1406         u8 *ptmp=NULL;
1407 _func_enter_;
1408
1409         rtw_led_control(padapter, LED_CTL_START_TO_LINK);
1410
1411         if (pmlmepriv->assoc_ssid.SsidLength == 0){
1412                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+Join cmd: Any SSid\n"));
1413         } else {
1414                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+Join cmd: SSid=[%s]\n", pmlmepriv->assoc_ssid.Ssid));
1415         }
1416
1417         pcmd = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1418         if(pcmd==NULL){
1419                 res=_FAIL;
1420                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd: memory allocate for cmd_obj fail!!!\n"));
1421                 goto exit;
1422         }
1423         /* // for IEs is pointer
1424         t_len = sizeof (ULONG) + sizeof (NDIS_802_11_MAC_ADDRESS) + 2 +
1425                         sizeof (NDIS_802_11_SSID) + sizeof (ULONG) +
1426                         sizeof (NDIS_802_11_RSSI) + sizeof (NDIS_802_11_NETWORK_TYPE) +
1427                         sizeof (NDIS_802_11_CONFIGURATION) +
1428                         sizeof (NDIS_802_11_NETWORK_INFRASTRUCTURE) +
1429                         sizeof (NDIS_802_11_RATES_EX)+ sizeof(WLAN_PHY_INFO)+ sizeof (ULONG) + MAX_IE_SZ;
1430         */
1431         //for IEs is fix buf size
1432         t_len = sizeof(WLAN_BSSID_EX);
1433
1434
1435         //for hidden ap to set fw_state here
1436         if (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) != _TRUE)
1437         {
1438                 switch(ndis_network_mode)
1439                 {
1440                         case Ndis802_11IBSS:
1441                                 set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
1442                                 break;
1443
1444                         case Ndis802_11Infrastructure:
1445                                 set_fwstate(pmlmepriv, WIFI_STATION_STATE);
1446                                 break;
1447
1448                         case Ndis802_11APMode:
1449                         case Ndis802_11AutoUnknown:
1450                         case Ndis802_11InfrastructureMax:
1451                                 break;
1452
1453                 }
1454         }
1455
1456         pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength);
1457
1458         psecnetwork=(WLAN_BSSID_EX *)&psecuritypriv->sec_bss;
1459         if(psecnetwork==NULL)
1460         {
1461                 if(pcmd !=NULL)
1462                         rtw_mfree((unsigned char *)pcmd, sizeof(struct  cmd_obj));
1463
1464                 res=_FAIL;
1465
1466                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd :psecnetwork==NULL!!!\n"));
1467
1468                 goto exit;
1469         }
1470
1471         _rtw_memset(psecnetwork, 0, t_len);
1472
1473         _rtw_memcpy(psecnetwork, &pnetwork->network, get_WLAN_BSSID_EX_sz(&pnetwork->network));
1474
1475         auth=&psecuritypriv->authenticator_ie[0];
1476         psecuritypriv->authenticator_ie[0]=(unsigned char)psecnetwork->IELength;
1477
1478         if((psecnetwork->IELength-12) < (256-1)) {
1479                 _rtw_memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], psecnetwork->IELength-12);
1480         } else {
1481                 _rtw_memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], (256-1));
1482         }
1483
1484         psecnetwork->IELength = 0;
1485         // Added by Albert 2009/02/18
1486         // If the the driver wants to use the bssid to create the connection.
1487         // If not,  we have to copy the connecting AP's MAC address to it so that
1488         // the driver just has the bssid information for PMKIDList searching.
1489
1490         if ( pmlmepriv->assoc_by_bssid == _FALSE )
1491         {
1492                 _rtw_memcpy( &pmlmepriv->assoc_bssid[ 0 ], &pnetwork->network.MacAddress[ 0 ], ETH_ALEN );
1493         }
1494
1495         psecnetwork->IELength = rtw_restruct_sec_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength);
1496
1497
1498         pqospriv->qos_option = 0;
1499
1500         if(pregistrypriv->wmm_enable)
1501         {
1502                 tmp_len = rtw_restruct_wmm_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength, psecnetwork->IELength);
1503
1504                 if (psecnetwork->IELength != tmp_len)
1505                 {
1506                         psecnetwork->IELength = tmp_len;
1507                         pqospriv->qos_option = 1; //There is WMM IE in this corresp. beacon
1508                 }
1509                 else
1510                 {
1511                         pqospriv->qos_option = 0;//There is no WMM IE in this corresp. beacon
1512                 }
1513         }
1514
1515 #ifdef CONFIG_80211N_HT
1516         phtpriv->ht_option = _FALSE;
1517         ptmp = rtw_get_ie(&pnetwork->network.IEs[12], _HT_CAPABILITY_IE_, &tmp_len, pnetwork->network.IELength-12);
1518         if(pregistrypriv->ht_enable && ptmp && tmp_len>0)
1519         {
1520                 //      Added by Albert 2010/06/23
1521                 //      For the WEP mode, we will use the bg mode to do the connection to avoid some IOT issue.
1522                 //      Especially for Realtek 8192u SoftAP.
1523                 if (    ( padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_ ) &&
1524                         ( padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_ ) &&
1525                         ( padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_ ))
1526                 {
1527                         rtw_ht_use_default_setting(padapter);
1528
1529                         rtw_build_wmm_ie_ht(padapter, &psecnetwork->IEs[0], &psecnetwork->IELength);
1530
1531                         //rtw_restructure_ht_ie
1532                         rtw_restructure_ht_ie(padapter, &pnetwork->network.IEs[12], &psecnetwork->IEs[0],
1533                                                                         pnetwork->network.IELength-12, &psecnetwork->IELength,
1534                                                                         pnetwork->network.Configuration.DSConfig);
1535                 }
1536         }
1537
1538 #ifdef CONFIG_80211AC_VHT
1539         pvhtpriv->vht_option = _FALSE;
1540         if (phtpriv->ht_option && pregistrypriv->vht_enable) {
1541                 rtw_restructure_vht_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0],
1542                                                                 pnetwork->network.IELength, &psecnetwork->IELength);
1543         }
1544 #endif
1545
1546         rtw_append_exented_cap(padapter, &psecnetwork->IEs[0], &psecnetwork->IELength);
1547
1548 #endif //CONFIG_80211N_HT
1549
1550         #if 0
1551         psecuritypriv->supplicant_ie[0]=(u8)psecnetwork->IELength;
1552
1553         if(psecnetwork->IELength < (256-1))
1554         {
1555                 _rtw_memcpy(&psecuritypriv->supplicant_ie[1], &psecnetwork->IEs[0], psecnetwork->IELength);
1556         }
1557         else
1558         {
1559                 _rtw_memcpy(&psecuritypriv->supplicant_ie[1], &psecnetwork->IEs[0], (256-1));
1560         }
1561         #endif
1562
1563         pcmd->cmdsz = get_WLAN_BSSID_EX_sz(psecnetwork);//get cmdsz before endian conversion
1564
1565 #ifdef CONFIG_RTL8712
1566         //wlan_network endian conversion
1567         psecnetwork->Length = cpu_to_le32(psecnetwork->Length);
1568         psecnetwork->Ssid.SsidLength= cpu_to_le32(psecnetwork->Ssid.SsidLength);
1569         psecnetwork->Privacy = cpu_to_le32(psecnetwork->Privacy);
1570         psecnetwork->Rssi = cpu_to_le32(psecnetwork->Rssi);
1571         psecnetwork->NetworkTypeInUse = cpu_to_le32(psecnetwork->NetworkTypeInUse);
1572         psecnetwork->Configuration.ATIMWindow = cpu_to_le32(psecnetwork->Configuration.ATIMWindow);
1573         psecnetwork->Configuration.BeaconPeriod = cpu_to_le32(psecnetwork->Configuration.BeaconPeriod);
1574         psecnetwork->Configuration.DSConfig = cpu_to_le32(psecnetwork->Configuration.DSConfig);
1575         psecnetwork->Configuration.FHConfig.DwellTime=cpu_to_le32(psecnetwork->Configuration.FHConfig.DwellTime);
1576         psecnetwork->Configuration.FHConfig.HopPattern=cpu_to_le32(psecnetwork->Configuration.FHConfig.HopPattern);
1577         psecnetwork->Configuration.FHConfig.HopSet=cpu_to_le32(psecnetwork->Configuration.FHConfig.HopSet);
1578         psecnetwork->Configuration.FHConfig.Length=cpu_to_le32(psecnetwork->Configuration.FHConfig.Length);
1579         psecnetwork->Configuration.Length = cpu_to_le32(psecnetwork->Configuration.Length);
1580         psecnetwork->InfrastructureMode = cpu_to_le32(psecnetwork->InfrastructureMode);
1581         psecnetwork->IELength = cpu_to_le32(psecnetwork->IELength);
1582 #endif
1583
1584         _rtw_init_listhead(&pcmd->list);
1585         pcmd->cmdcode = _JoinBss_CMD_;//GEN_CMD_CODE(_JoinBss)
1586         pcmd->parmbuf = (unsigned char *)psecnetwork;
1587         pcmd->rsp = NULL;
1588         pcmd->rspsz = 0;
1589
1590         res = rtw_enqueue_cmd(pcmdpriv, pcmd);
1591
1592 exit:
1593
1594 _func_exit_;
1595
1596         return res;
1597 }
1598
1599 u8 rtw_disassoc_cmd(_adapter*padapter, u32 deauth_timeout_ms, bool enqueue) /* for sta_mode */
1600 {
1601         struct cmd_obj *cmdobj = NULL;
1602         struct disconnect_parm *param = NULL;
1603         struct cmd_priv *cmdpriv = &padapter->cmdpriv;
1604         u8 res = _SUCCESS;
1605
1606 _func_enter_;
1607
1608         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_disassoc_cmd\n"));
1609
1610         /* prepare cmd parameter */
1611         param = (struct disconnect_parm *)rtw_zmalloc(sizeof(*param));
1612         if (param == NULL) {
1613                 res = _FAIL;
1614                 goto exit;
1615         }
1616         param->deauth_timeout_ms = deauth_timeout_ms;
1617
1618         if (enqueue) {
1619                 /* need enqueue, prepare cmd_obj and enqueue */
1620                 cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
1621                 if (cmdobj == NULL) {
1622                         res = _FAIL;
1623                         rtw_mfree((u8 *)param, sizeof(*param));
1624                         goto exit;
1625                 }
1626                 init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_);
1627                 res = rtw_enqueue_cmd(cmdpriv, cmdobj);
1628         } else {
1629                 /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
1630                 if (H2C_SUCCESS != disconnect_hdl(padapter, (u8 *)param))
1631                         res = _FAIL;
1632                 rtw_mfree((u8 *)param, sizeof(*param));
1633         }
1634
1635 exit:
1636
1637 _func_exit_;
1638
1639         return res;
1640 }
1641
1642 u8 rtw_setopmode_cmd(_adapter  *padapter, NDIS_802_11_NETWORK_INFRASTRUCTURE networktype, bool enqueue)
1643 {
1644         struct  cmd_obj*        ph2c;
1645         struct  setopmode_parm* psetop;
1646
1647         struct  cmd_priv   *pcmdpriv= &padapter->cmdpriv;
1648         u8      res=_SUCCESS;
1649
1650 _func_enter_;
1651         psetop = (struct setopmode_parm*)rtw_zmalloc(sizeof(struct setopmode_parm));
1652
1653         if(psetop==NULL){
1654                 res=_FAIL;
1655                 goto exit;
1656         }
1657         psetop->mode = (u8)networktype;
1658
1659         if(enqueue){
1660                 ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1661                 if(ph2c==NULL){
1662                         rtw_mfree((u8 *)psetop, sizeof(*psetop));
1663                         res= _FAIL;
1664                         goto exit;
1665                 }
1666
1667                 init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
1668                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1669         }
1670         else{
1671                 setopmode_hdl(padapter, (u8 *)psetop);
1672                 rtw_mfree((u8 *)psetop, sizeof(*psetop));
1673         }
1674 exit:
1675
1676 _func_exit_;
1677
1678         return res;
1679 }
1680
1681 u8 rtw_setstakey_cmd(_adapter *padapter, struct sta_info *sta, u8 unicast_key, bool enqueue)
1682 {
1683         struct cmd_obj*                 ph2c;
1684         struct set_stakey_parm  *psetstakey_para;
1685         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
1686         struct set_stakey_rsp           *psetstakey_rsp = NULL;
1687
1688         struct mlme_priv                        *pmlmepriv = &padapter->mlmepriv;
1689         struct security_priv            *psecuritypriv = &padapter->securitypriv;
1690         u8      res=_SUCCESS;
1691
1692 _func_enter_;
1693
1694         psetstakey_para = (struct set_stakey_parm*)rtw_zmalloc(sizeof(struct set_stakey_parm));
1695         if(psetstakey_para==NULL){
1696                 res=_FAIL;
1697                 goto exit;
1698         }
1699
1700         _rtw_memcpy(psetstakey_para->addr, sta->hwaddr,ETH_ALEN);
1701
1702         if(check_fwstate(pmlmepriv, WIFI_STATION_STATE)){
1703 #ifdef CONFIG_TDLS
1704                 if(sta->tdls_sta_state&TDLS_LINKED_STATE)
1705                         psetstakey_para->algorithm=(u8)sta->dot118021XPrivacy;
1706                 else
1707 #endif //CONFIG_TDLS
1708                         psetstakey_para->algorithm =(unsigned char) psecuritypriv->dot11PrivacyAlgrthm;
1709         }else{
1710                 GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, _FALSE);
1711         }
1712
1713         if (unicast_key == _TRUE) {
1714 #ifdef CONFIG_TDLS
1715                 if((sta->tdls_sta_state&TDLS_LINKED_STATE)==TDLS_LINKED_STATE)
1716                         _rtw_memcpy(&psetstakey_para->key, sta->tpk.tk, 16);
1717                 else
1718 #endif //CONFIG_TDLS
1719                         _rtw_memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
1720         }
1721         else {
1722                 _rtw_memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16);
1723        }
1724
1725         //jeff: set this becasue at least sw key is ready
1726         padapter->securitypriv.busetkipkey=_TRUE;
1727
1728         if(enqueue)
1729         {
1730                 ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1731                 if ( ph2c == NULL){
1732                         rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm));
1733                         res= _FAIL;
1734                         goto exit;
1735                 }
1736
1737                 psetstakey_rsp = (struct set_stakey_rsp*)rtw_zmalloc(sizeof(struct set_stakey_rsp));
1738                 if(psetstakey_rsp == NULL){
1739                         rtw_mfree((u8 *) ph2c, sizeof(struct cmd_obj));
1740                         rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm));
1741                         res=_FAIL;
1742                         goto exit;
1743                 }
1744
1745                 init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
1746                 ph2c->rsp = (u8 *) psetstakey_rsp;
1747                 ph2c->rspsz = sizeof(struct set_stakey_rsp);
1748                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1749         }
1750         else{
1751                 set_stakey_hdl(padapter, (u8 *)psetstakey_para);
1752                 rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm));
1753         }
1754 exit:
1755
1756 _func_exit_;
1757
1758         return res;
1759 }
1760
1761 u8 rtw_clearstakey_cmd(_adapter *padapter, struct sta_info *sta, u8 enqueue)
1762 {
1763         struct cmd_obj*                 ph2c;
1764         struct set_stakey_parm  *psetstakey_para;
1765         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
1766         struct set_stakey_rsp           *psetstakey_rsp = NULL;
1767         struct mlme_priv                        *pmlmepriv = &padapter->mlmepriv;
1768         struct security_priv            *psecuritypriv = &padapter->securitypriv;
1769         s16 cam_id = 0;
1770         u8      res=_SUCCESS;
1771
1772 _func_enter_;
1773
1774         if(!enqueue)
1775         {
1776                 while((cam_id = rtw_camid_search(padapter, sta->hwaddr, -1)) >= 0) {
1777                         DBG_871X_LEVEL(_drv_always_, "clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(sta->hwaddr), cam_id);
1778                         clear_cam_entry(padapter, cam_id);
1779                         rtw_camid_free(padapter, cam_id);
1780                 }
1781         }
1782         else
1783         {
1784                 ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1785                 if ( ph2c == NULL){
1786                         res= _FAIL;
1787                         goto exit;
1788                 }
1789
1790                 psetstakey_para = (struct set_stakey_parm*)rtw_zmalloc(sizeof(struct set_stakey_parm));
1791                 if(psetstakey_para==NULL){
1792                         rtw_mfree((u8 *) ph2c, sizeof(struct    cmd_obj));
1793                         res=_FAIL;
1794                         goto exit;
1795                 }
1796
1797                 psetstakey_rsp = (struct set_stakey_rsp*)rtw_zmalloc(sizeof(struct set_stakey_rsp));
1798                 if(psetstakey_rsp == NULL){
1799                         rtw_mfree((u8 *) ph2c, sizeof(struct    cmd_obj));
1800                         rtw_mfree((u8 *) psetstakey_para, sizeof(struct set_stakey_parm));
1801                         res=_FAIL;
1802                         goto exit;
1803                 }
1804
1805                 init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
1806                 ph2c->rsp = (u8 *) psetstakey_rsp;
1807                 ph2c->rspsz = sizeof(struct set_stakey_rsp);
1808
1809                 _rtw_memcpy(psetstakey_para->addr, sta->hwaddr, ETH_ALEN);
1810
1811                 psetstakey_para->algorithm = _NO_PRIVACY_;
1812
1813                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1814
1815         }
1816
1817 exit:
1818
1819 _func_exit_;
1820
1821         return res;
1822 }
1823
1824 u8 rtw_setrttbl_cmd(_adapter  *padapter, struct setratable_parm *prate_table)
1825 {
1826         struct cmd_obj*                 ph2c;
1827         struct setratable_parm *        psetrttblparm;
1828         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
1829         u8      res=_SUCCESS;
1830 _func_enter_;
1831
1832         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1833         if(ph2c==NULL){
1834                 res= _FAIL;
1835                 goto exit;
1836                 }
1837         psetrttblparm = (struct setratable_parm*)rtw_zmalloc(sizeof(struct setratable_parm));
1838
1839         if(psetrttblparm==NULL){
1840                 rtw_mfree((unsigned char *) ph2c, sizeof(struct cmd_obj));
1841                 res= _FAIL;
1842                 goto exit;
1843         }
1844
1845         init_h2fwcmd_w_parm_no_rsp(ph2c, psetrttblparm, GEN_CMD_CODE(_SetRaTable));
1846
1847         _rtw_memcpy(psetrttblparm,prate_table,sizeof(struct setratable_parm));
1848
1849         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1850 exit:
1851 _func_exit_;
1852         return res;
1853
1854 }
1855
1856 u8 rtw_getrttbl_cmd(_adapter  *padapter, struct getratable_rsp *pval)
1857 {
1858         struct cmd_obj*                 ph2c;
1859         struct getratable_parm *        pgetrttblparm;
1860         struct cmd_priv                         *pcmdpriv=&padapter->cmdpriv;
1861         u8      res=_SUCCESS;
1862 _func_enter_;
1863
1864         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1865         if(ph2c==NULL){
1866                 res= _FAIL;
1867                 goto exit;
1868         }
1869         pgetrttblparm = (struct getratable_parm*)rtw_zmalloc(sizeof(struct getratable_parm));
1870
1871         if(pgetrttblparm==NULL){
1872                 rtw_mfree((unsigned char *) ph2c, sizeof(struct cmd_obj));
1873                 res= _FAIL;
1874                 goto exit;
1875         }
1876
1877 //      init_h2fwcmd_w_parm_no_rsp(ph2c, psetrttblparm, GEN_CMD_CODE(_SetRaTable));
1878
1879         _rtw_init_listhead(&ph2c->list);
1880         ph2c->cmdcode =GEN_CMD_CODE(_GetRaTable);
1881         ph2c->parmbuf = (unsigned char *)pgetrttblparm;
1882         ph2c->cmdsz =  sizeof(struct getratable_parm);
1883         ph2c->rsp = (u8*)pval;
1884         ph2c->rspsz = sizeof(struct getratable_rsp);
1885
1886         pgetrttblparm ->rsvd = 0x0;
1887
1888         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1889 exit:
1890 _func_exit_;
1891         return res;
1892
1893 }
1894
1895 u8 rtw_setassocsta_cmd(_adapter  *padapter, u8 *mac_addr)
1896 {
1897         struct cmd_priv                 *pcmdpriv = &padapter->cmdpriv;
1898         struct cmd_obj*                 ph2c;
1899         struct set_assocsta_parm        *psetassocsta_para;
1900         struct set_stakey_rsp           *psetassocsta_rsp = NULL;
1901
1902         u8      res=_SUCCESS;
1903
1904 _func_enter_;
1905
1906         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1907         if(ph2c==NULL){
1908                 res= _FAIL;
1909                 goto exit;
1910         }
1911
1912         psetassocsta_para = (struct set_assocsta_parm*)rtw_zmalloc(sizeof(struct set_assocsta_parm));
1913         if(psetassocsta_para==NULL){
1914                 rtw_mfree((u8 *) ph2c, sizeof(struct    cmd_obj));
1915                 res=_FAIL;
1916                 goto exit;
1917         }
1918
1919         psetassocsta_rsp = (struct set_stakey_rsp*)rtw_zmalloc(sizeof(struct set_assocsta_rsp));
1920         if(psetassocsta_rsp==NULL){
1921                 rtw_mfree((u8 *) ph2c, sizeof(struct    cmd_obj));
1922                 rtw_mfree((u8 *) psetassocsta_para, sizeof(struct set_assocsta_parm));
1923                 return _FAIL;
1924         }
1925
1926         init_h2fwcmd_w_parm_no_rsp(ph2c, psetassocsta_para, _SetAssocSta_CMD_);
1927         ph2c->rsp = (u8 *) psetassocsta_rsp;
1928         ph2c->rspsz = sizeof(struct set_assocsta_rsp);
1929
1930         _rtw_memcpy(psetassocsta_para->addr, mac_addr,ETH_ALEN);
1931
1932         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1933
1934 exit:
1935
1936 _func_exit_;
1937
1938         return res;
1939  }
1940
1941 u8 rtw_addbareq_cmd(_adapter*padapter, u8 tid, u8 *addr)
1942 {
1943         struct cmd_priv         *pcmdpriv = &padapter->cmdpriv;
1944         struct cmd_obj*         ph2c;
1945         struct addBaReq_parm    *paddbareq_parm;
1946
1947         u8      res=_SUCCESS;
1948
1949 _func_enter_;
1950
1951         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1952         if(ph2c==NULL){
1953                 res= _FAIL;
1954                 goto exit;
1955         }
1956
1957         paddbareq_parm = (struct addBaReq_parm*)rtw_zmalloc(sizeof(struct addBaReq_parm));
1958         if(paddbareq_parm==NULL){
1959                 rtw_mfree((unsigned char *)ph2c, sizeof(struct  cmd_obj));
1960                 res= _FAIL;
1961                 goto exit;
1962         }
1963
1964         paddbareq_parm->tid = tid;
1965         _rtw_memcpy(paddbareq_parm->addr, addr, ETH_ALEN);
1966
1967         init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, GEN_CMD_CODE(_AddBAReq));
1968
1969         //DBG_871X("rtw_addbareq_cmd, tid=%d\n", tid);
1970
1971         //rtw_enqueue_cmd(pcmdpriv, ph2c);
1972         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1973
1974 exit:
1975
1976 _func_exit_;
1977
1978         return res;
1979 }
1980 //add for CONFIG_IEEE80211W, none 11w can use it
1981 u8 rtw_reset_securitypriv_cmd(_adapter*padapter)
1982 {
1983         struct cmd_obj*         ph2c;
1984         struct drvextra_cmd_parm  *pdrvextra_cmd_parm;
1985         struct cmd_priv *pcmdpriv=&padapter->cmdpriv;
1986         u8      res=_SUCCESS;
1987
1988 _func_enter_;
1989
1990         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
1991         if(ph2c==NULL){
1992                 res= _FAIL;
1993                 goto exit;
1994         }
1995
1996         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
1997         if(pdrvextra_cmd_parm==NULL){
1998                 rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
1999                 res= _FAIL;
2000                 goto exit;
2001         }
2002
2003         pdrvextra_cmd_parm->ec_id = RESET_SECURITYPRIV;
2004         pdrvextra_cmd_parm->type = 0;
2005         pdrvextra_cmd_parm->size = 0;
2006         pdrvextra_cmd_parm->pbuf = NULL;
2007
2008         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
2009
2010
2011         //rtw_enqueue_cmd(pcmdpriv, ph2c);
2012         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
2013
2014 exit:
2015
2016 _func_exit_;
2017
2018         return res;
2019
2020 }
2021
2022 u8 rtw_free_assoc_resources_cmd(_adapter*padapter)
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 _func_enter_;
2030
2031         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
2032         if(ph2c==NULL){
2033                 res= _FAIL;
2034                 goto exit;
2035         }
2036
2037         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
2038         if(pdrvextra_cmd_parm==NULL){
2039                 rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
2040                 res= _FAIL;
2041                 goto exit;
2042         }
2043
2044         pdrvextra_cmd_parm->ec_id = FREE_ASSOC_RESOURCES;
2045         pdrvextra_cmd_parm->type = 0;
2046         pdrvextra_cmd_parm->size = 0;
2047         pdrvextra_cmd_parm->pbuf = NULL;
2048
2049         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
2050
2051
2052         //rtw_enqueue_cmd(pcmdpriv, ph2c);
2053         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
2054
2055 exit:
2056
2057 _func_exit_;
2058
2059         return res;
2060
2061 }
2062
2063 u8 rtw_dynamic_chk_wk_cmd(_adapter*padapter)
2064 {
2065         struct cmd_obj*         ph2c;
2066         struct drvextra_cmd_parm  *pdrvextra_cmd_parm;
2067         struct cmd_priv *pcmdpriv=&padapter->cmdpriv;
2068         u8      res=_SUCCESS;
2069
2070 _func_enter_;
2071
2072         //only  primary padapter does this cmd
2073 /*
2074 #ifdef CONFIG_CONCURRENT_MODE
2075         if (padapter->adapter_type != PRIMARY_ADAPTER && padapter->pbuddy_adapter)
2076                 pcmdpriv = &(padapter->pbuddy_adapter->cmdpriv);
2077 #endif
2078 */
2079
2080         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
2081         if(ph2c==NULL){
2082                 res= _FAIL;
2083                 goto exit;
2084         }
2085
2086         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
2087         if(pdrvextra_cmd_parm==NULL){
2088                 rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
2089                 res= _FAIL;
2090                 goto exit;
2091         }
2092
2093         pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID;
2094         pdrvextra_cmd_parm->type = 0;
2095         pdrvextra_cmd_parm->size = 0;
2096         pdrvextra_cmd_parm->pbuf = NULL;
2097         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
2098
2099
2100         //rtw_enqueue_cmd(pcmdpriv, ph2c);
2101         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
2102
2103 exit:
2104
2105 _func_exit_;
2106
2107         return res;
2108
2109 }
2110
2111 u8 rtw_set_ch_cmd(_adapter*padapter, u8 ch, u8 bw, u8 ch_offset, u8 enqueue)
2112 {
2113         struct cmd_obj *pcmdobj;
2114         struct set_ch_parm *set_ch_parm;
2115         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
2116
2117         u8 res=_SUCCESS;
2118
2119 _func_enter_;
2120
2121         DBG_871X(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
2122                 FUNC_NDEV_ARG(padapter->pnetdev), ch, bw, ch_offset);
2123
2124         /* check input parameter */
2125
2126         /* prepare cmd parameter */
2127         set_ch_parm = (struct set_ch_parm *)rtw_zmalloc(sizeof(*set_ch_parm));
2128         if (set_ch_parm == NULL) {
2129                 res= _FAIL;
2130                 goto exit;
2131         }
2132         set_ch_parm->ch = ch;
2133         set_ch_parm->bw = bw;
2134         set_ch_parm->ch_offset = ch_offset;
2135
2136         if (enqueue) {
2137                 /* need enqueue, prepare cmd_obj and enqueue */
2138                 pcmdobj = (struct cmd_obj*)rtw_zmalloc(sizeof(struct    cmd_obj));
2139                 if(pcmdobj == NULL){
2140                         rtw_mfree((u8 *)set_ch_parm, sizeof(*set_ch_parm));
2141                         res=_FAIL;
2142                         goto exit;
2143                 }
2144
2145                 init_h2fwcmd_w_parm_no_rsp(pcmdobj, set_ch_parm, GEN_CMD_CODE(_SetChannel));
2146                 res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
2147         } else {
2148                 /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
2149                 if( H2C_SUCCESS !=set_ch_hdl(padapter, (u8 *)set_ch_parm) )
2150                         res = _FAIL;
2151
2152                 rtw_mfree((u8 *)set_ch_parm, sizeof(*set_ch_parm));
2153         }
2154
2155         /* do something based on res... */
2156
2157 exit:
2158
2159         DBG_871X(FUNC_NDEV_FMT" res:%u\n", FUNC_NDEV_ARG(padapter->pnetdev), res);
2160
2161 _func_exit_;
2162
2163         return res;
2164 }
2165
2166 u8 rtw_set_chplan_cmd(_adapter*padapter, u8 chplan, u8 enqueue, u8 swconfig)
2167 {
2168         struct  cmd_obj*        pcmdobj;
2169         struct  SetChannelPlan_param *setChannelPlan_param;
2170         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
2171
2172         u8      res=_SUCCESS;
2173
2174 _func_enter_;
2175
2176         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_chplan_cmd\n"));
2177
2178         // check if allow software config
2179         if (swconfig && rtw_hal_is_disable_sw_channel_plan(padapter) == _TRUE)
2180         {
2181                 res = _FAIL;
2182                 goto exit;
2183         }
2184
2185         //check input parameter
2186         if(!rtw_is_channel_plan_valid(chplan)) {
2187                 res = _FAIL;
2188                 goto exit;
2189         }
2190
2191         //prepare cmd parameter
2192         setChannelPlan_param = (struct  SetChannelPlan_param *)rtw_zmalloc(sizeof(struct SetChannelPlan_param));
2193         if(setChannelPlan_param == NULL) {
2194                 res= _FAIL;
2195                 goto exit;
2196         }
2197         setChannelPlan_param->channel_plan=chplan;
2198
2199         if(enqueue)
2200         {
2201                 //need enqueue, prepare cmd_obj and enqueue
2202                 pcmdobj = (struct       cmd_obj*)rtw_zmalloc(sizeof(struct      cmd_obj));
2203                 if(pcmdobj == NULL){
2204                         rtw_mfree((u8 *)setChannelPlan_param, sizeof(struct SetChannelPlan_param));
2205                         res=_FAIL;
2206                         goto exit;
2207                 }
2208
2209                 init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelPlan_param, GEN_CMD_CODE(_SetChannelPlan));
2210                 res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
2211         }
2212         else
2213         {
2214                 //no need to enqueue, do the cmd hdl directly and free cmd parameter
2215                 if( H2C_SUCCESS != set_chplan_hdl(padapter, (unsigned char *)setChannelPlan_param) )
2216                         res = _FAIL;
2217
2218                 rtw_mfree((u8 *)setChannelPlan_param, sizeof(struct SetChannelPlan_param));
2219         }
2220
2221 exit:
2222
2223 _func_exit_;
2224
2225         return res;
2226 }
2227
2228 u8 rtw_led_blink_cmd(_adapter*padapter, PVOID pLed)
2229 {
2230         struct  cmd_obj*        pcmdobj;
2231         struct  LedBlink_param *ledBlink_param;
2232         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
2233
2234         u8      res=_SUCCESS;
2235
2236 _func_enter_;
2237
2238         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_led_blink_cmd\n"));
2239
2240         pcmdobj = (struct       cmd_obj*)rtw_zmalloc(sizeof(struct      cmd_obj));
2241         if(pcmdobj == NULL){
2242                 res=_FAIL;
2243                 goto exit;
2244         }
2245
2246         ledBlink_param = (struct        LedBlink_param *)rtw_zmalloc(sizeof(struct      LedBlink_param));
2247         if(ledBlink_param == NULL) {
2248                 rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
2249                 res= _FAIL;
2250                 goto exit;
2251         }
2252
2253         ledBlink_param->pLed=pLed;
2254
2255         init_h2fwcmd_w_parm_no_rsp(pcmdobj, ledBlink_param, GEN_CMD_CODE(_LedBlink));
2256         res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
2257
2258 exit:
2259
2260 _func_exit_;
2261
2262         return res;
2263 }
2264
2265 u8 rtw_set_csa_cmd(_adapter*padapter, u8 new_ch_no)
2266 {
2267         struct  cmd_obj*        pcmdobj;
2268         struct  SetChannelSwitch_param*setChannelSwitch_param;
2269         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
2270         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
2271
2272         u8      res=_SUCCESS;
2273
2274 _func_enter_;
2275
2276         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_csa_cmd\n"));
2277
2278         pcmdobj = (struct       cmd_obj*)rtw_zmalloc(sizeof(struct      cmd_obj));
2279         if(pcmdobj == NULL){
2280                 res=_FAIL;
2281                 goto exit;
2282         }
2283
2284         setChannelSwitch_param = (struct SetChannelSwitch_param *)rtw_zmalloc(sizeof(struct     SetChannelSwitch_param));
2285         if(setChannelSwitch_param == NULL) {
2286                 rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
2287                 res= _FAIL;
2288                 goto exit;
2289         }
2290
2291         setChannelSwitch_param->new_ch_no=new_ch_no;
2292
2293         init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelSwitch_param, GEN_CMD_CODE(_SetChannelSwitch));
2294         res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
2295
2296 exit:
2297
2298 _func_exit_;
2299
2300         return res;
2301 }
2302
2303 u8 rtw_tdls_cmd(_adapter *padapter, u8 *addr, u8 option)
2304 {
2305         struct  cmd_obj*        pcmdobj;
2306         struct  TDLSoption_param        *TDLSoption;
2307         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
2308         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
2309
2310         u8      res=_SUCCESS;
2311
2312 _func_enter_;
2313
2314 #ifdef CONFIG_TDLS
2315
2316         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_tdls_cmd\n"));
2317
2318         pcmdobj = (struct       cmd_obj*)rtw_zmalloc(sizeof(struct      cmd_obj));
2319         if(pcmdobj == NULL){
2320                 res=_FAIL;
2321                 goto exit;
2322         }
2323
2324         TDLSoption= (struct TDLSoption_param *)rtw_zmalloc(sizeof(struct TDLSoption_param));
2325         if(TDLSoption == NULL) {
2326                 rtw_mfree((u8 *)pcmdobj, sizeof(struct cmd_obj));
2327                 res= _FAIL;
2328                 goto exit;
2329         }
2330
2331         _rtw_spinlock(&(padapter->tdlsinfo.cmd_lock));
2332         _rtw_memcpy(TDLSoption->addr, addr, 6);
2333         TDLSoption->option = option;
2334         _rtw_spinunlock(&(padapter->tdlsinfo.cmd_lock));
2335         init_h2fwcmd_w_parm_no_rsp(pcmdobj, TDLSoption, GEN_CMD_CODE(_TDLS));
2336         res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
2337
2338 #endif  //CONFIG_TDLS
2339
2340 exit:
2341
2342
2343 _func_exit_;
2344
2345         return res;
2346 }
2347
2348 static void collect_traffic_statistics(_adapter *padapter)
2349 {
2350         struct dvobj_priv       *pdvobjpriv = adapter_to_dvobj(padapter);
2351
2352 #ifdef CONFIG_CONCURRENT_MODE
2353         if (padapter->adapter_type != PRIMARY_ADAPTER)
2354                 return;
2355 #endif
2356
2357         // Tx
2358         pdvobjpriv->traffic_stat.tx_bytes = padapter->xmitpriv.tx_bytes;
2359         pdvobjpriv->traffic_stat.tx_pkts = padapter->xmitpriv.tx_pkts;
2360         pdvobjpriv->traffic_stat.tx_drop = padapter->xmitpriv.tx_drop;
2361
2362         // Rx
2363         pdvobjpriv->traffic_stat.rx_bytes = padapter->recvpriv.rx_bytes;
2364         pdvobjpriv->traffic_stat.rx_pkts = padapter->recvpriv.rx_pkts;
2365         pdvobjpriv->traffic_stat.rx_drop = padapter->recvpriv.rx_drop;
2366
2367 #ifdef CONFIG_CONCURRENT_MODE
2368         // Add secondary adapter statistics
2369         if(rtw_buddy_adapter_up(padapter))
2370         {
2371                 // Tx
2372                 pdvobjpriv->traffic_stat.tx_bytes += padapter->pbuddy_adapter->xmitpriv.tx_bytes;
2373                 pdvobjpriv->traffic_stat.tx_pkts += padapter->pbuddy_adapter->xmitpriv.tx_pkts;
2374                 pdvobjpriv->traffic_stat.tx_drop += padapter->pbuddy_adapter->xmitpriv.tx_drop;
2375
2376                 // Rx
2377                 pdvobjpriv->traffic_stat.rx_bytes += padapter->pbuddy_adapter->recvpriv.rx_bytes;
2378                 pdvobjpriv->traffic_stat.rx_pkts += padapter->pbuddy_adapter->recvpriv.rx_pkts;
2379                 pdvobjpriv->traffic_stat.rx_drop += padapter->pbuddy_adapter->recvpriv.rx_drop;
2380         }
2381 #endif
2382
2383         // Calculate throughput in last interval
2384         pdvobjpriv->traffic_stat.cur_tx_bytes = pdvobjpriv->traffic_stat.tx_bytes - pdvobjpriv->traffic_stat.last_tx_bytes;
2385         pdvobjpriv->traffic_stat.cur_rx_bytes = pdvobjpriv->traffic_stat.rx_bytes - pdvobjpriv->traffic_stat.last_rx_bytes;
2386         pdvobjpriv->traffic_stat.last_tx_bytes = pdvobjpriv->traffic_stat.tx_bytes;
2387         pdvobjpriv->traffic_stat.last_rx_bytes = pdvobjpriv->traffic_stat.rx_bytes;
2388
2389         pdvobjpriv->traffic_stat.cur_tx_tp = (u32)(pdvobjpriv->traffic_stat.cur_tx_bytes *8/2/1024/1024);
2390         pdvobjpriv->traffic_stat.cur_rx_tp = (u32)(pdvobjpriv->traffic_stat.cur_rx_bytes *8/2/1024/1024);
2391 }
2392
2393 //from_timer == 1 means driver is in LPS
2394 u8 traffic_status_watchdog(_adapter *padapter, u8 from_timer)
2395 {
2396         u8      bEnterPS = _FALSE;
2397 #ifdef CONFIG_BT_COEXIST
2398         u16     BusyThresholdHigh = 25;
2399         u16     BusyThresholdLow = 10;
2400 #else
2401         u16     BusyThresholdHigh = 100;
2402         u16     BusyThresholdLow = 75;
2403 #endif
2404         u16     BusyThreshold = BusyThresholdHigh;
2405         u8      bBusyTraffic = _FALSE, bTxBusyTraffic = _FALSE, bRxBusyTraffic = _FALSE;
2406         u8      bHigherBusyTraffic = _FALSE, bHigherBusyRxTraffic = _FALSE, bHigherBusyTxTraffic = _FALSE;
2407
2408         struct mlme_priv                *pmlmepriv = &(padapter->mlmepriv);
2409 #ifdef CONFIG_TDLS
2410         struct tdls_info *ptdlsinfo = &(padapter->tdlsinfo);
2411         struct tdls_txmgmt txmgmt;
2412         u8 baddr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
2413 #endif //CONFIG_TDLS
2414
2415         RT_LINK_DETECT_T * link_detect = &pmlmepriv->LinkDetectInfo;
2416
2417         collect_traffic_statistics(padapter);
2418
2419         //
2420         // Determine if our traffic is busy now
2421         //
2422         if((check_fwstate(pmlmepriv, _FW_LINKED)== _TRUE)
2423                 /*&& !MgntInitAdapterInProgress(pMgntInfo)*/)
2424         {
2425                 // if we raise bBusyTraffic in last watchdog, using lower threshold.
2426                 if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
2427                                 BusyThreshold = BusyThresholdLow;
2428
2429                 if( pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > BusyThreshold ||
2430                         pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold )
2431                 {
2432                         bBusyTraffic = _TRUE;
2433
2434                         if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
2435                                 bRxBusyTraffic = _TRUE;
2436                         else
2437                                 bTxBusyTraffic = _TRUE;
2438                 }
2439
2440                 // Higher Tx/Rx data.
2441                 if( pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 ||
2442                         pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000 )
2443                 {
2444                         bHigherBusyTraffic = _TRUE;
2445
2446                         if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
2447                                 bHigherBusyRxTraffic = _TRUE;
2448                         else
2449                                 bHigherBusyTxTraffic = _TRUE;
2450                 }
2451
2452 #ifdef CONFIG_TRAFFIC_PROTECT
2453 #define TX_ACTIVE_TH 10
2454 #define RX_ACTIVE_TH 20
2455 #define TRAFFIC_PROTECT_PERIOD_MS 4500
2456
2457         if (link_detect->NumTxOkInPeriod > TX_ACTIVE_TH
2458                 || link_detect->NumRxUnicastOkInPeriod > RX_ACTIVE_TH) {
2459
2460                 DBG_871X_LEVEL(_drv_info_, FUNC_ADPT_FMT" acqiure wake_lock for %u ms(tx:%d,rx_unicast:%d)\n",
2461                         FUNC_ADPT_ARG(padapter),
2462                         TRAFFIC_PROTECT_PERIOD_MS,
2463                         link_detect->NumTxOkInPeriod,
2464                         link_detect->NumRxUnicastOkInPeriod);
2465
2466                 rtw_lock_traffic_suspend_timeout(TRAFFIC_PROTECT_PERIOD_MS);
2467         }
2468 #endif
2469
2470 #ifdef CONFIG_TDLS
2471 #ifdef CONFIG_TDLS_AUTOSETUP
2472                 if( ( ptdlsinfo->watchdog_count % TDLS_WATCHDOG_PERIOD ) == 0 ) //10 * 2sec, periodically sending
2473                 {
2474                         _rtw_memcpy(txmgmt.peer, baddr, ETH_ALEN);
2475                         issue_tdls_dis_req( padapter, &txmgmt );
2476                 }
2477                 ptdlsinfo->watchdog_count++;
2478 #endif //CONFIG_TDLS_AUTOSETUP
2479 #endif //CONFIG_TDLS
2480
2481 #ifdef CONFIG_LPS
2482                 // check traffic for  powersaving.
2483                 if( ((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8 ) ||
2484 #ifdef CONFIG_LPS_SLOW_TRANSITION
2485                         (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2)
2486 #else //CONFIG_LPS_SLOW_TRANSITION
2487                         (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 4)
2488 #endif //CONFIG_LPS_SLOW_TRANSITION
2489                         )
2490                 {
2491                         //DBG_871X("(-)Tx = %d, Rx = %d \n",pmlmepriv->LinkDetectInfo.NumTxOkInPeriod,pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod);
2492                         bEnterPS= _FALSE;
2493 #ifdef CONFIG_LPS_SLOW_TRANSITION
2494                         if(bBusyTraffic == _TRUE)
2495                         {
2496                                 if(pmlmepriv->LinkDetectInfo.TrafficTransitionCount <= 4)
2497                                         pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 4;
2498
2499                                 pmlmepriv->LinkDetectInfo.TrafficTransitionCount++;
2500
2501                                 //DBG_871X("Set TrafficTransitionCount to %d\n", pmlmepriv->LinkDetectInfo.TrafficTransitionCount);
2502
2503                                 if(pmlmepriv->LinkDetectInfo.TrafficTransitionCount > 30/*TrafficTransitionLevel*/)
2504                                 {
2505                                         pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 30;
2506                                 }
2507                         }
2508 #endif //CONFIG_LPS_SLOW_TRANSITION
2509
2510                 }
2511                 else
2512                 {
2513                         //DBG_871X("(+)Tx = %d, Rx = %d \n",pmlmepriv->LinkDetectInfo.NumTxOkInPeriod,pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod);
2514 #ifdef CONFIG_LPS_SLOW_TRANSITION
2515                         if(pmlmepriv->LinkDetectInfo.TrafficTransitionCount>=2)
2516                                 pmlmepriv->LinkDetectInfo.TrafficTransitionCount -=2;
2517                         else
2518                                 pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
2519
2520                         if(pmlmepriv->LinkDetectInfo.TrafficTransitionCount == 0)
2521                                 bEnterPS= _TRUE;
2522 #else //CONFIG_LPS_SLOW_TRANSITION
2523                                 bEnterPS= _TRUE;
2524 #endif //CONFIG_LPS_SLOW_TRANSITION
2525                 }
2526
2527 #ifdef CONFIG_DYNAMIC_DTIM
2528                 if(pmlmepriv->LinkDetectInfo.LowPowerTransitionCount == 8)
2529                         bEnterPS= _FALSE;
2530
2531                 DBG_871X("LowPowerTransitionCount=%d\n", pmlmepriv->LinkDetectInfo.LowPowerTransitionCount);
2532 #endif //CONFIG_DYNAMIC_DTIM
2533
2534                 // LeisurePS only work in infra mode.
2535                 if(bEnterPS)
2536                 {
2537                         if(!from_timer)
2538                         {
2539 #ifdef CONFIG_DYNAMIC_DTIM
2540                                 if(pmlmepriv->LinkDetectInfo.LowPowerTransitionCount < 8)
2541                                 {
2542                                         adapter_to_pwrctl(padapter)->dtim = 1;
2543                                 }
2544                                 else
2545                                 {
2546                                         adapter_to_pwrctl(padapter)->dtim = 3;
2547                                 }
2548 #endif //CONFIG_DYNAMIC_DTIM
2549                                 LPS_Enter(padapter, "TRAFFIC_IDLE");
2550                         }
2551                         else
2552                         {
2553                                 //do this at caller
2554                                 //rtw_lps_ctrl_wk_cmd(adapter, LPS_CTRL_ENTER, 1);
2555                                 //rtw_hal_dm_watchdog_in_lps(padapter);
2556                         }
2557 #ifdef CONFIG_DYNAMIC_DTIM
2558                         if (adapter_to_pwrctl(padapter)->bFwCurrentInPSMode ==_TRUE )
2559                                 pmlmepriv->LinkDetectInfo.LowPowerTransitionCount++;
2560 #endif //CONFIG_DYNAMIC_DTIM
2561                 }
2562                 else
2563                 {
2564 #ifdef CONFIG_DYNAMIC_DTIM
2565                         if(pmlmepriv->LinkDetectInfo.LowPowerTransitionCount != 8)
2566                                 pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
2567                         else
2568                                 pmlmepriv->LinkDetectInfo.LowPowerTransitionCount++;
2569 #endif //CONFIG_DYNAMIC_DTIM
2570                         if(!from_timer)
2571                         {
2572                                 LPS_Leave(padapter, "TRAFFIC_BUSY");
2573                         }
2574                         else
2575                         {
2576 #ifdef CONFIG_CONCURRENT_MODE
2577                                 if(padapter->iface_type == IFACE_PORT0)
2578 #endif
2579                                         rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_TRAFFIC_BUSY, 1);
2580                         }
2581                 }
2582
2583 #endif // CONFIG_LPS
2584         }
2585         else
2586         {
2587 #ifdef CONFIG_LPS
2588                 struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
2589                 int n_assoc_iface = 0;
2590                 int i;
2591
2592                 for (i = 0; i < dvobj->iface_nums; i++) {
2593                         if (check_fwstate(&(dvobj->padapters[i]->mlmepriv), WIFI_ASOC_STATE))
2594                                 n_assoc_iface++;
2595                 }
2596
2597                 if(!from_timer && n_assoc_iface == 0)
2598                         LPS_Leave(padapter, "NON_LINKED");
2599 #endif
2600         }
2601
2602         pmlmepriv->LinkDetectInfo.NumRxOkInPeriod = 0;
2603         pmlmepriv->LinkDetectInfo.NumTxOkInPeriod = 0;
2604         pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
2605         pmlmepriv->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
2606         pmlmepriv->LinkDetectInfo.bTxBusyTraffic = bTxBusyTraffic;
2607         pmlmepriv->LinkDetectInfo.bRxBusyTraffic = bRxBusyTraffic;
2608         pmlmepriv->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic;
2609         pmlmepriv->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic;
2610         pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
2611
2612         return bEnterPS;
2613
2614 }
2615
2616 void dynamic_chk_wk_hdl(_adapter *padapter)
2617 {
2618         struct mlme_priv *pmlmepriv;
2619         pmlmepriv = &(padapter->mlmepriv);
2620
2621 #ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
2622 #ifdef CONFIG_AP_MODE
2623         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) == _TRUE)
2624         {
2625                 expire_timeout_chk(padapter);
2626         }
2627 #endif
2628 #endif //CONFIG_ACTIVE_KEEP_ALIVE_CHECK
2629
2630 #ifdef DBG_CONFIG_ERROR_DETECT
2631         rtw_hal_sreset_xmit_status_check(padapter);
2632         rtw_hal_sreset_linked_status_check(padapter);
2633 #endif
2634
2635         //for debug purpose
2636         _linked_info_dump(padapter);
2637
2638
2639         //if(check_fwstate(pmlmepriv, _FW_UNDER_LINKING|_FW_UNDER_SURVEY)==_FALSE)
2640         {
2641                 linked_status_chk(padapter, 0);
2642                 traffic_status_watchdog(padapter, 0);
2643                 dm_DynamicUsbTxAgg(padapter, 0);
2644         }
2645
2646 #ifdef CONFIG_BEAMFORMING
2647         beamforming_watchdog(padapter);
2648 #endif
2649
2650         rtw_hal_dm_watchdog(padapter);
2651
2652         //check_hw_pbc(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type);
2653
2654 #ifdef CONFIG_BT_COEXIST
2655         //
2656         // BT-Coexist
2657         //
2658         rtw_btcoex_Handler(padapter);
2659 #endif
2660
2661
2662 #ifdef CONFIG_IPS_CHECK_IN_WD
2663         //always call rtw_ps_processor() at last one.
2664         if(is_primary_adapter(padapter))
2665                 rtw_ps_processor(padapter);
2666 #endif
2667 }
2668
2669 #ifdef CONFIG_LPS
2670
2671 void lps_ctrl_wk_hdl(_adapter *padapter, u8 lps_ctrl_type);
2672 void lps_ctrl_wk_hdl(_adapter *padapter, u8 lps_ctrl_type)
2673 {
2674         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
2675         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
2676         u8      mstatus;
2677
2678 _func_enter_;
2679
2680         if((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)
2681                 || (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE))
2682         {
2683                 return;
2684         }
2685
2686         switch(lps_ctrl_type)
2687         {
2688                 case LPS_CTRL_SCAN:
2689                         //DBG_871X("LPS_CTRL_SCAN \n");
2690 #ifdef CONFIG_BT_COEXIST
2691                         rtw_btcoex_ScanNotify(padapter, _TRUE);
2692 #endif // CONFIG_BT_COEXIST
2693                         if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
2694                         {
2695                                 // connect
2696                                 LPS_Leave(padapter, "LPS_CTRL_SCAN");
2697                         }
2698                         break;
2699                 case LPS_CTRL_JOINBSS:
2700                         //DBG_871X("LPS_CTRL_JOINBSS \n");
2701                         LPS_Leave(padapter, "LPS_CTRL_JOINBSS");
2702                         break;
2703                 case LPS_CTRL_CONNECT:
2704                         //DBG_871X("LPS_CTRL_CONNECT \n");
2705                         mstatus = 1;//connect
2706                         // Reset LPS Setting
2707                         pwrpriv->LpsIdleCount = 0;
2708                         rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
2709 #ifdef CONFIG_BT_COEXIST
2710                         rtw_btcoex_MediaStatusNotify(padapter, mstatus);
2711 #endif // CONFIG_BT_COEXIST
2712                         break;
2713                 case LPS_CTRL_DISCONNECT:
2714                         //DBG_871X("LPS_CTRL_DISCONNECT \n");
2715                         mstatus = 0;//disconnect
2716 #ifdef CONFIG_BT_COEXIST
2717                         rtw_btcoex_MediaStatusNotify(padapter, mstatus);
2718 #endif // CONFIG_BT_COEXIST
2719                         LPS_Leave(padapter, "LPS_CTRL_DISCONNECT");
2720                         rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
2721                         break;
2722                 case LPS_CTRL_SPECIAL_PACKET:
2723                         //DBG_871X("LPS_CTRL_SPECIAL_PACKET \n");
2724                         pwrpriv->DelayLPSLastTimeStamp = rtw_get_current_time();
2725 #ifdef CONFIG_BT_COEXIST
2726                         rtw_btcoex_SpecialPacketNotify(padapter, PACKET_DHCP);
2727 #endif // CONFIG_BT_COEXIST
2728                         LPS_Leave(padapter, "LPS_CTRL_SPECIAL_PACKET");
2729                         break;
2730                 case LPS_CTRL_LEAVE:
2731                         //DBG_871X("LPS_CTRL_LEAVE \n");
2732                         LPS_Leave(padapter, "LPS_CTRL_LEAVE");
2733                         break;
2734                 case LPS_CTRL_TRAFFIC_BUSY:
2735                         LPS_Leave(padapter, "LPS_CTRL_TRAFFIC_BUSY");
2736                         break;
2737                 case LPS_CTRL_TX_TRAFFIC_LEAVE:
2738                         LPS_Leave(padapter, "LPS_CTRL_TX_TRAFFIC_LEAVE");
2739                         break;
2740                 case LPS_CTRL_RX_TRAFFIC_LEAVE:
2741                         LPS_Leave(padapter, "LPS_CTRL_RX_TRAFFIC_LEAVE");
2742                         break;
2743                 case LPS_CTRL_ENTER:
2744                         LPS_Enter(padapter, "TRAFFIC_IDLE_1");
2745                         break;
2746                 default:
2747                         break;
2748         }
2749
2750 _func_exit_;
2751 }
2752
2753 u8 rtw_lps_ctrl_wk_cmd(_adapter*padapter, u8 lps_ctrl_type, u8 enqueue)
2754 {
2755         struct cmd_obj  *ph2c;
2756         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
2757         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
2758         //struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
2759         u8      res = _SUCCESS;
2760
2761 _func_enter_;
2762
2763         //if(!pwrctrlpriv->bLeisurePs)
2764         //      return res;
2765
2766         if(enqueue)
2767         {
2768                 ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
2769                 if(ph2c==NULL){
2770                         res= _FAIL;
2771                         goto exit;
2772                 }
2773
2774                 pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
2775                 if(pdrvextra_cmd_parm==NULL){
2776                         rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
2777                         res= _FAIL;
2778                         goto exit;
2779                 }
2780
2781                 pdrvextra_cmd_parm->ec_id = LPS_CTRL_WK_CID;
2782                 pdrvextra_cmd_parm->type = lps_ctrl_type;
2783                 pdrvextra_cmd_parm->size = 0;
2784                 pdrvextra_cmd_parm->pbuf = NULL;
2785
2786                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
2787
2788                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
2789         }
2790         else
2791         {
2792                 lps_ctrl_wk_hdl(padapter, lps_ctrl_type);
2793         }
2794
2795 exit:
2796
2797 _func_exit_;
2798
2799         return res;
2800
2801 }
2802
2803 void rtw_dm_in_lps_hdl(_adapter*padapter)
2804 {
2805         rtw_hal_set_hwreg(padapter, HW_VAR_DM_IN_LPS, NULL);
2806 }
2807
2808 u8 rtw_dm_in_lps_wk_cmd(_adapter*padapter)
2809 {
2810         struct cmd_obj  *ph2c;
2811         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
2812         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
2813         u8      res = _SUCCESS;
2814
2815
2816         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
2817         if(ph2c==NULL){
2818                 res= _FAIL;
2819                 goto exit;
2820         }
2821
2822         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
2823         if(pdrvextra_cmd_parm==NULL){
2824                 rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
2825                 res= _FAIL;
2826                 goto exit;
2827         }
2828
2829         pdrvextra_cmd_parm->ec_id = DM_IN_LPS_WK_CID;
2830         pdrvextra_cmd_parm->type = 0;
2831         pdrvextra_cmd_parm->size = 0;
2832         pdrvextra_cmd_parm->pbuf = NULL;
2833
2834         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
2835
2836         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
2837
2838 exit:
2839
2840         return res;
2841
2842 }
2843
2844 void rtw_lps_change_dtim_hdl(_adapter *padapter, u8 dtim)
2845 {
2846         struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
2847
2848         if(dtim <=0 || dtim > 16)
2849                 return;
2850
2851 #ifdef CONFIG_BT_COEXIST
2852         if (rtw_btcoex_IsBtControlLps(padapter) == _TRUE)
2853                 return;
2854 #endif
2855
2856 #ifdef CONFIG_LPS_LCLK
2857         _enter_pwrlock(&pwrpriv->lock);
2858 #endif
2859
2860         if(pwrpriv->dtim!=dtim)
2861         {
2862                 DBG_871X("change DTIM from %d to %d, bFwCurrentInPSMode=%d, ps_mode=%d\n", pwrpriv->dtim, dtim,
2863                         pwrpriv->bFwCurrentInPSMode, pwrpriv->pwr_mode);
2864
2865                 pwrpriv->dtim = dtim;
2866         }
2867
2868         if((pwrpriv->bFwCurrentInPSMode ==_TRUE) && (pwrpriv->pwr_mode > PS_MODE_ACTIVE))
2869         {
2870                 u8 ps_mode = pwrpriv->pwr_mode;
2871
2872                 //DBG_871X("change DTIM from %d to %d, ps_mode=%d\n", pwrpriv->dtim, dtim, ps_mode);
2873
2874                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
2875         }
2876
2877 #ifdef CONFIG_LPS_LCLK
2878         _exit_pwrlock(&pwrpriv->lock);
2879 #endif
2880
2881 }
2882
2883 #endif
2884
2885 u8 rtw_lps_change_dtim_cmd(_adapter*padapter, u8 dtim)
2886 {
2887         struct cmd_obj  *ph2c;
2888         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
2889         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
2890         u8      res = _SUCCESS;
2891 /*
2892 #ifdef CONFIG_CONCURRENT_MODE
2893         if (padapter->iface_type != IFACE_PORT0)
2894                 return res;
2895 #endif
2896 */
2897         {
2898                 ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
2899                 if(ph2c==NULL){
2900                         res= _FAIL;
2901                         goto exit;
2902                 }
2903
2904                 pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
2905                 if(pdrvextra_cmd_parm==NULL){
2906                         rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
2907                         res= _FAIL;
2908                         goto exit;
2909                 }
2910
2911                 pdrvextra_cmd_parm->ec_id = LPS_CHANGE_DTIM_CID;
2912                 pdrvextra_cmd_parm->type = dtim;
2913                 pdrvextra_cmd_parm->size = 0;
2914                 pdrvextra_cmd_parm->pbuf = NULL;
2915
2916                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
2917
2918                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
2919         }
2920
2921 exit:
2922
2923         return res;
2924
2925 }
2926
2927 #if (RATE_ADAPTIVE_SUPPORT==1)
2928 void rpt_timer_setting_wk_hdl(_adapter *padapter, u16 minRptTime)
2929 {
2930         rtw_hal_set_hwreg(padapter, HW_VAR_RPT_TIMER_SETTING, (u8 *)(&minRptTime));
2931 }
2932
2933 u8 rtw_rpt_timer_cfg_cmd(_adapter*padapter, u16 minRptTime)
2934 {
2935         struct cmd_obj          *ph2c;
2936         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
2937         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
2938
2939         u8      res = _SUCCESS;
2940
2941 _func_enter_;
2942         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
2943         if(ph2c==NULL){
2944                 res= _FAIL;
2945                 goto exit;
2946         }
2947
2948         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
2949         if(pdrvextra_cmd_parm==NULL){
2950                 rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
2951                 res= _FAIL;
2952                 goto exit;
2953         }
2954
2955         pdrvextra_cmd_parm->ec_id = RTP_TIMER_CFG_WK_CID;
2956         pdrvextra_cmd_parm->type = minRptTime;
2957         pdrvextra_cmd_parm->size = 0;
2958         pdrvextra_cmd_parm->pbuf = NULL;
2959         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
2960         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
2961 exit:
2962
2963 _func_exit_;
2964
2965         return res;
2966
2967 }
2968
2969 #endif
2970
2971 #ifdef CONFIG_ANTENNA_DIVERSITY
2972 void antenna_select_wk_hdl(_adapter *padapter, u8 antenna)
2973 {
2974         rtw_hal_set_hwreg(padapter, HW_VAR_ANTENNA_DIVERSITY_SELECT, (u8 *)(&antenna));
2975 }
2976
2977 u8 rtw_antenna_select_cmd(_adapter*padapter, u8 antenna,u8 enqueue)
2978 {
2979         struct cmd_obj          *ph2c;
2980         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
2981         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
2982         u8      bSupportAntDiv = _FALSE;
2983         u8      res = _SUCCESS;
2984
2985 _func_enter_;
2986         rtw_hal_get_def_var(padapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &(bSupportAntDiv));
2987         if(_FALSE == bSupportAntDiv )   return res;
2988
2989         if(_TRUE == enqueue)
2990         {
2991                 ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
2992                 if(ph2c==NULL){
2993                         res= _FAIL;
2994                         goto exit;
2995                 }
2996
2997                 pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
2998                 if(pdrvextra_cmd_parm==NULL){
2999                         rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
3000                         res= _FAIL;
3001                         goto exit;
3002                 }
3003
3004                 pdrvextra_cmd_parm->ec_id = ANT_SELECT_WK_CID;
3005                 pdrvextra_cmd_parm->type = antenna;
3006                 pdrvextra_cmd_parm->size = 0;
3007                 pdrvextra_cmd_parm->pbuf = NULL;
3008                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
3009
3010                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
3011         }
3012         else{
3013                 antenna_select_wk_hdl(padapter,antenna );
3014         }
3015 exit:
3016
3017 _func_exit_;
3018
3019         return res;
3020
3021 }
3022 #endif
3023
3024 void rtw_dm_ra_mask_hdl(_adapter *padapter, struct sta_info *psta)
3025 {
3026         if (psta) {
3027                 set_sta_rate(padapter, psta);
3028         }
3029 }
3030
3031 u8 rtw_dm_ra_mask_wk_cmd(_adapter*padapter, u8 *psta)
3032 {
3033         struct cmd_obj  *ph2c;
3034         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
3035         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
3036         u8      res = _SUCCESS;
3037
3038
3039         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
3040         if(ph2c==NULL){
3041                 res= _FAIL;
3042                 goto exit;
3043         }
3044
3045         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
3046         if(pdrvextra_cmd_parm==NULL){
3047                 rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
3048                 res= _FAIL;
3049                 goto exit;
3050         }
3051
3052         pdrvextra_cmd_parm->ec_id = DM_RA_MSK_WK_CID;
3053         pdrvextra_cmd_parm->type = 0;
3054         pdrvextra_cmd_parm->size = 0;
3055         pdrvextra_cmd_parm->pbuf = psta;
3056
3057         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
3058
3059         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
3060
3061 exit:
3062
3063         return res;
3064
3065 }
3066
3067 void power_saving_wk_hdl(_adapter *padapter)
3068 {
3069          rtw_ps_processor(padapter);
3070 }
3071
3072 //add for CONFIG_IEEE80211W, none 11w can use it
3073 void reset_securitypriv_hdl(_adapter *padapter)
3074 {
3075          rtw_reset_securitypriv(padapter);
3076 }
3077
3078 void free_assoc_resources_hdl(_adapter *padapter)
3079 {
3080          rtw_free_assoc_resources(padapter, 1);
3081 }
3082
3083 #ifdef CONFIG_P2P
3084 u8 p2p_protocol_wk_cmd(_adapter*padapter, int intCmdType )
3085 {
3086         struct cmd_obj  *ph2c;
3087         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
3088         struct wifidirect_info  *pwdinfo= &(padapter->wdinfo);
3089         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
3090         u8      res = _SUCCESS;
3091
3092 _func_enter_;
3093
3094         if(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
3095         {
3096                 return res;
3097         }
3098
3099         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
3100         if(ph2c==NULL){
3101                 res= _FAIL;
3102                 goto exit;
3103         }
3104
3105         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
3106         if(pdrvextra_cmd_parm==NULL){
3107                 rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
3108                 res= _FAIL;
3109                 goto exit;
3110         }
3111
3112         pdrvextra_cmd_parm->ec_id = P2P_PROTO_WK_CID;
3113         pdrvextra_cmd_parm->type = intCmdType;  //      As the command tppe.
3114         pdrvextra_cmd_parm->size = 0;
3115         pdrvextra_cmd_parm->pbuf = NULL;                //      Must be NULL here
3116
3117         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
3118
3119         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
3120
3121 exit:
3122
3123 _func_exit_;
3124
3125         return res;
3126
3127 }
3128 #endif //CONFIG_P2P
3129
3130 u8 rtw_ps_cmd(_adapter*padapter)
3131 {
3132         struct cmd_obj          *ppscmd;
3133         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
3134         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
3135
3136         u8      res = _SUCCESS;
3137 _func_enter_;
3138
3139 #ifdef CONFIG_CONCURRENT_MODE
3140         if (padapter->adapter_type != PRIMARY_ADAPTER)
3141                 goto exit;
3142 #endif
3143
3144         ppscmd = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
3145         if(ppscmd==NULL){
3146                 res= _FAIL;
3147                 goto exit;
3148         }
3149
3150         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
3151         if(pdrvextra_cmd_parm==NULL){
3152                 rtw_mfree((unsigned char *)ppscmd, sizeof(struct cmd_obj));
3153                 res= _FAIL;
3154                 goto exit;
3155         }
3156
3157         pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID;
3158         pdrvextra_cmd_parm->type = 0;
3159         pdrvextra_cmd_parm->size = 0;
3160         pdrvextra_cmd_parm->pbuf = NULL;
3161         init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
3162
3163         res = rtw_enqueue_cmd(pcmdpriv, ppscmd);
3164
3165 exit:
3166
3167 _func_exit_;
3168
3169         return res;
3170
3171 }
3172
3173 #ifdef CONFIG_AP_MODE
3174
3175 static void rtw_chk_hi_queue_hdl(_adapter *padapter)
3176 {
3177         struct sta_info *psta_bmc;
3178         struct sta_priv *pstapriv = &padapter->stapriv;
3179         u32 start = rtw_get_current_time();
3180         u8 empty = _FALSE;
3181
3182         psta_bmc = rtw_get_bcmc_stainfo(padapter);
3183         if(!psta_bmc)
3184                 return;
3185
3186         rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty);
3187
3188         while(_FALSE == empty && rtw_get_passing_time_ms(start) < rtw_get_wait_hiq_empty_ms())
3189         {
3190                 rtw_msleep_os(100);
3191                 rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &empty);
3192         }
3193
3194         if(psta_bmc->sleepq_len==0)
3195         {
3196                 if(empty == _SUCCESS)
3197                 {
3198                         bool update_tim = _FALSE;
3199
3200                         if (pstapriv->tim_bitmap & BIT(0))
3201                                 update_tim = _TRUE;
3202
3203                         pstapriv->tim_bitmap &= ~BIT(0);
3204                         pstapriv->sta_dz_bitmap &= ~BIT(0);
3205
3206                         if (update_tim == _TRUE)
3207                                 _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, "bmc sleepq and HIQ empty");
3208                 }
3209                 else //re check again
3210                 {
3211                         rtw_chk_hi_queue_cmd(padapter);
3212                 }
3213
3214         }
3215
3216 }
3217
3218 u8 rtw_chk_hi_queue_cmd(_adapter*padapter)
3219 {
3220         struct cmd_obj  *ph2c;
3221         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
3222         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
3223         u8      res = _SUCCESS;
3224
3225         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
3226         if(ph2c==NULL){
3227                 res= _FAIL;
3228                 goto exit;
3229         }
3230
3231         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
3232         if(pdrvextra_cmd_parm==NULL){
3233                 rtw_mfree((unsigned char *)ph2c, sizeof(struct cmd_obj));
3234                 res= _FAIL;
3235                 goto exit;
3236         }
3237
3238         pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID;
3239         pdrvextra_cmd_parm->type = 0;
3240         pdrvextra_cmd_parm->size = 0;
3241         pdrvextra_cmd_parm->pbuf = NULL;
3242
3243         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
3244
3245         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
3246
3247 exit:
3248
3249         return res;
3250
3251 }
3252 #endif
3253
3254 #ifdef CONFIG_BT_COEXIST
3255 struct btinfo {
3256         u8 cid;
3257         u8 len;
3258
3259         u8 bConnection:1;
3260         u8 bSCOeSCO:1;
3261         u8 bInQPage:1;
3262         u8 bACLBusy:1;
3263         u8 bSCOBusy:1;
3264         u8 bHID:1;
3265         u8 bA2DP:1;
3266         u8 bFTP:1;
3267
3268         u8 retry_cnt:4;
3269         u8 rsvd_34:1;
3270         u8 rsvd_35:1;
3271         u8 rsvd_36:1;
3272         u8 rsvd_37:1;
3273
3274         u8 rssi;
3275
3276         u8 rsvd_50:1;
3277         u8 rsvd_51:1;
3278         u8 rsvd_52:1;
3279         u8 rsvd_53:1;
3280         u8 rsvd_54:1;
3281         u8 rsvd_55:1;
3282         u8 eSCO_SCO:1;
3283         u8 Master_Slave:1;
3284
3285         u8 rsvd_6;
3286         u8 rsvd_7;
3287 };
3288
3289 void btinfo_evt_dump(void *sel, void *buf)
3290 {
3291         struct btinfo *info = (struct btinfo *)buf;
3292
3293         DBG_871X_SEL_NL(sel, "cid:0x%02x, len:%u\n", info->cid, info->len);
3294
3295         if (info->len > 2)
3296         DBG_871X_SEL_NL(sel, "byte2:%s%s%s%s%s%s%s%s\n"
3297                 , info->bConnection?"bConnection ":""
3298                 , info->bSCOeSCO?"bSCOeSCO ":""
3299                 , info->bInQPage?"bInQPage ":""
3300                 , info->bACLBusy?"bACLBusy ":""
3301                 , info->bSCOBusy?"bSCOBusy ":""
3302                 , info->bHID?"bHID ":""
3303                 , info->bA2DP?"bA2DP ":""
3304                 , info->bFTP?"bFTP":""
3305         );
3306
3307         if (info->len > 3)
3308         DBG_871X_SEL_NL(sel, "retry_cnt:%u\n", info->retry_cnt);
3309
3310         if (info->len > 4)
3311         DBG_871X_SEL_NL(sel, "rssi:%u\n", info->rssi);
3312
3313         if (info->len > 5)
3314         DBG_871X_SEL_NL(sel, "byte5:%s%s\n"
3315                 , info->eSCO_SCO?"eSCO_SCO ":""
3316                 , info->Master_Slave?"Master_Slave ":""
3317         );
3318 }
3319
3320 static void rtw_btinfo_hdl(_adapter *adapter, u8 *buf, u16 buf_len)
3321 {
3322         #define BTINFO_WIFI_FETCH 0x23
3323         #define BTINFO_BT_AUTO_RPT 0x27
3324         struct btinfo *info = (struct btinfo *)buf;
3325         u8 cmd_idx;
3326         u8 len;
3327
3328         cmd_idx = info->cid;
3329
3330         if (info->len > buf_len-2) {
3331                 rtw_warn_on(1);
3332                 len = buf_len-2;
3333         } else {
3334                 len = info->len;
3335         }
3336
3337 //#define DBG_PROC_SET_BTINFO_EVT
3338 #ifdef DBG_PROC_SET_BTINFO_EVT
3339         btinfo_evt_dump(RTW_DBGDUMP, info);
3340 #endif
3341
3342         /* transform BT-FW btinfo to WiFI-FW C2H format and notify */
3343         if (cmd_idx == BTINFO_WIFI_FETCH)
3344                 buf[1] = 0;
3345         else if (cmd_idx == BTINFO_BT_AUTO_RPT)
3346                 buf[1] = 2;
3347         rtw_btcoex_BtInfoNotify(adapter ,len+1, &buf[1]);
3348 }
3349
3350 u8 rtw_btinfo_cmd(_adapter *adapter, u8 *buf, u16 len)
3351 {
3352         struct cmd_obj *ph2c;
3353         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
3354         u8 *btinfo;
3355         struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
3356         u8      res = _SUCCESS;
3357
3358         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
3359         if (ph2c == NULL) {
3360                 res = _FAIL;
3361                 goto exit;
3362         }
3363
3364         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
3365         if (pdrvextra_cmd_parm == NULL) {
3366                 rtw_mfree((u8*)ph2c, sizeof(struct cmd_obj));
3367                 res = _FAIL;
3368                 goto exit;
3369         }
3370
3371         btinfo = rtw_zmalloc(len);
3372         if (btinfo == NULL) {
3373                 rtw_mfree((u8*)ph2c, sizeof(struct cmd_obj));
3374                 rtw_mfree((u8*)pdrvextra_cmd_parm, sizeof(struct drvextra_cmd_parm));
3375                 res = _FAIL;
3376                 goto exit;
3377         }
3378
3379         pdrvextra_cmd_parm->ec_id = BTINFO_WK_CID;
3380         pdrvextra_cmd_parm->type = 0;
3381         pdrvextra_cmd_parm->size = len;
3382         pdrvextra_cmd_parm->pbuf = btinfo;
3383
3384         _rtw_memcpy(btinfo, buf, len);
3385
3386         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
3387
3388         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
3389
3390 exit:
3391         return res;
3392 }
3393 #endif
3394
3395 //#ifdef CONFIG_C2H_PACKET_EN
3396 u8 rtw_c2h_packet_wk_cmd(PADAPTER padapter, u8 *pbuf, u16 length)
3397 {
3398         struct cmd_obj *ph2c;
3399         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
3400         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
3401         u8      res = _SUCCESS;
3402
3403         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
3404         if (ph2c == NULL) {
3405                 res = _FAIL;
3406                 goto exit;
3407         }
3408
3409         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
3410         if (pdrvextra_cmd_parm == NULL) {
3411                 rtw_mfree((u8*)ph2c, sizeof(struct cmd_obj));
3412                 res = _FAIL;
3413                 goto exit;
3414         }
3415
3416         pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
3417         pdrvextra_cmd_parm->type = 0;
3418         pdrvextra_cmd_parm->size = length;
3419         pdrvextra_cmd_parm->pbuf = pbuf;
3420
3421         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
3422
3423         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
3424
3425 exit:
3426         return res;
3427 }
3428
3429 //#else //CONFIG_C2H_PACKET_EN
3430 /* dont call R/W in this function, beucase SDIO interrupt have claim host */
3431 /* or deadlock will happen and cause special-systemserver-died in android */
3432
3433 u8 rtw_c2h_wk_cmd(PADAPTER padapter, u8 *c2h_evt)
3434 {
3435         struct cmd_obj *ph2c;
3436         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
3437         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
3438         u8      res = _SUCCESS;
3439
3440         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
3441         if (ph2c == NULL) {
3442                 res = _FAIL;
3443                 goto exit;
3444         }
3445
3446         pdrvextra_cmd_parm = (struct drvextra_cmd_parm*)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
3447         if (pdrvextra_cmd_parm == NULL) {
3448                 rtw_mfree((u8*)ph2c, sizeof(struct cmd_obj));
3449                 res = _FAIL;
3450                 goto exit;
3451         }
3452
3453         pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
3454         pdrvextra_cmd_parm->type = 0;
3455         pdrvextra_cmd_parm->size =  c2h_evt?16:0;
3456         pdrvextra_cmd_parm->pbuf = c2h_evt;
3457
3458         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
3459
3460         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
3461
3462 exit:
3463
3464         return res;
3465 }
3466 //#endif //CONFIG_C2H_PACKET_EN
3467
3468 u8 rtw_run_in_thread_cmd(PADAPTER padapter, void (*func)(void*), void* context)
3469 {
3470         struct cmd_priv *pcmdpriv;
3471         struct cmd_obj *ph2c;
3472         struct RunInThread_param *parm;
3473         s32 res = _SUCCESS;
3474
3475 _func_enter_;
3476
3477         pcmdpriv = &padapter->cmdpriv;
3478
3479         ph2c = (struct cmd_obj*)rtw_zmalloc(sizeof(struct cmd_obj));
3480         if (NULL == ph2c) {
3481                 res = _FAIL;
3482                 goto exit;
3483         }
3484
3485         parm = (struct RunInThread_param*)rtw_zmalloc(sizeof(struct RunInThread_param));
3486         if (NULL == parm) {
3487                 rtw_mfree((u8*)ph2c, sizeof(struct cmd_obj));
3488                 res = _FAIL;
3489                 goto exit;
3490         }
3491
3492         parm->func = func;
3493         parm->context = context;
3494         init_h2fwcmd_w_parm_no_rsp(ph2c, parm, GEN_CMD_CODE(_RunInThreadCMD));
3495
3496         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
3497 exit:
3498
3499 _func_exit_;
3500
3501         return res;
3502 }
3503
3504 s32 c2h_evt_hdl(_adapter *adapter, u8 *c2h_evt, c2h_id_filter filter)
3505 {
3506         s32 ret = _FAIL;
3507         u8 buf[16];
3508
3509         if (!c2h_evt) {
3510                 /* No c2h event in cmd_obj, read c2h event before handling*/
3511                 if (rtw_hal_c2h_evt_read(adapter, buf) == _SUCCESS) {
3512                         c2h_evt = buf;
3513
3514                         if (filter && filter(c2h_evt) == _FALSE)
3515                                 goto exit;
3516
3517                         ret = rtw_hal_c2h_handler(adapter, c2h_evt);
3518                 }
3519         } else {
3520
3521                 if (filter && filter(c2h_evt) == _FALSE)
3522                         goto exit;
3523
3524                 ret = rtw_hal_c2h_handler(adapter, c2h_evt);
3525         }
3526 exit:
3527         return ret;
3528 }
3529
3530 #ifdef CONFIG_C2H_WK
3531 static void c2h_wk_callback(_workitem *work)
3532 {
3533         struct evt_priv *evtpriv = container_of(work, struct evt_priv, c2h_wk);
3534         _adapter *adapter = container_of(evtpriv, _adapter, evtpriv);
3535         u8 *c2h_evt;
3536         c2h_id_filter ccx_id_filter = rtw_hal_c2h_id_filter_ccx(adapter);
3537
3538         evtpriv->c2h_wk_alive = _TRUE;
3539
3540         while (!rtw_cbuf_empty(evtpriv->c2h_queue)) {
3541                 if ((c2h_evt = (u8 *)rtw_cbuf_pop(evtpriv->c2h_queue)) != NULL) {
3542                         /* This C2H event is read, clear it */
3543                         c2h_evt_clear(adapter);
3544                 } else if ((c2h_evt = (u8 *)rtw_malloc(16)) != NULL) {
3545                         /* This C2H event is not read, read & clear now */
3546                         if (rtw_hal_c2h_evt_read(adapter, c2h_evt) != _SUCCESS) {
3547                                 rtw_mfree(c2h_evt, 16);
3548                                 continue;
3549                         }
3550                 }
3551
3552                 /* Special pointer to trigger c2h_evt_clear only */
3553                 if ((void *)c2h_evt == (void *)evtpriv)
3554                         continue;
3555
3556                 if (!rtw_hal_c2h_valid(adapter, c2h_evt)) {
3557                         rtw_mfree(c2h_evt, 16);
3558                         continue;
3559                 }
3560
3561                 if (ccx_id_filter(c2h_evt) == _TRUE) {
3562                         /* Handle CCX report here */
3563                         rtw_hal_c2h_handler(adapter, c2h_evt);
3564                         rtw_mfree(c2h_evt, 16);
3565                 } else {
3566                         /* Enqueue into cmd_thread for others */
3567                         rtw_c2h_wk_cmd(adapter, c2h_evt);
3568                 }
3569         }
3570
3571         evtpriv->c2h_wk_alive = _FALSE;
3572 }
3573 #endif
3574
3575 u8 rtw_drvextra_cmd_hdl(_adapter *padapter, unsigned char *pbuf)
3576 {
3577         struct drvextra_cmd_parm *pdrvextra_cmd;
3578
3579         if(!pbuf)
3580                 return H2C_PARAMETERS_ERROR;
3581
3582         pdrvextra_cmd = (struct drvextra_cmd_parm*)pbuf;
3583
3584         switch(pdrvextra_cmd->ec_id)
3585         {
3586                 case DYNAMIC_CHK_WK_CID://only  primary padapter go to this cmd, but execute dynamic_chk_wk_hdl() for two interfaces
3587 #ifdef CONFIG_CONCURRENT_MODE
3588                         if(padapter->pbuddy_adapter)
3589                         {
3590                                 dynamic_chk_wk_hdl(padapter->pbuddy_adapter);
3591                         }
3592 #endif //CONFIG_CONCURRENT_MODE
3593                         dynamic_chk_wk_hdl(padapter);
3594                         break;
3595                 case POWER_SAVING_CTRL_WK_CID:
3596                         power_saving_wk_hdl(padapter);
3597                         break;
3598 #ifdef CONFIG_LPS
3599                 case LPS_CTRL_WK_CID:
3600                         lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type);
3601                         break;
3602                 case DM_IN_LPS_WK_CID:
3603                         rtw_dm_in_lps_hdl(padapter);
3604                         break;
3605                 case LPS_CHANGE_DTIM_CID:
3606                         rtw_lps_change_dtim_hdl(padapter, (u8)pdrvextra_cmd->type);
3607                         break;
3608 #endif
3609 #if (RATE_ADAPTIVE_SUPPORT==1)
3610                 case RTP_TIMER_CFG_WK_CID:
3611                         rpt_timer_setting_wk_hdl(padapter, pdrvextra_cmd->type);
3612                         break;
3613 #endif
3614 #ifdef CONFIG_ANTENNA_DIVERSITY
3615                 case ANT_SELECT_WK_CID:
3616                         antenna_select_wk_hdl(padapter, pdrvextra_cmd->type);
3617                         break;
3618 #endif
3619 #ifdef CONFIG_P2P_PS
3620                 case P2P_PS_WK_CID:
3621                         p2p_ps_wk_hdl(padapter, pdrvextra_cmd->type);
3622                         break;
3623 #endif //CONFIG_P2P_PS
3624 #ifdef CONFIG_P2P
3625                 case P2P_PROTO_WK_CID:
3626                         //      Commented by Albert 2011/07/01
3627                         //      I used the type_size as the type command
3628                         p2p_protocol_wk_hdl( padapter, pdrvextra_cmd->type );
3629                         break;
3630 #endif //CONFIG_P2P
3631 #ifdef CONFIG_AP_MODE
3632                 case CHECK_HIQ_WK_CID:
3633                         rtw_chk_hi_queue_hdl(padapter);
3634                         break;
3635 #endif //CONFIG_AP_MODE
3636 #ifdef CONFIG_INTEL_WIDI
3637                 case INTEl_WIDI_WK_CID:
3638                         intel_widi_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf);
3639                         break;
3640 #endif //CONFIG_INTEL_WIDI
3641                 //add for CONFIG_IEEE80211W, none 11w can use it
3642                 case RESET_SECURITYPRIV:
3643                         reset_securitypriv_hdl(padapter);
3644                         break;
3645                 case FREE_ASSOC_RESOURCES:
3646                         free_assoc_resources_hdl(padapter);
3647                         break;
3648                 case C2H_WK_CID:
3649 #ifdef CONFIG_C2H_PACKET_EN
3650                         rtw_hal_set_hwreg_with_buf(padapter, HW_VAR_C2H_HANDLE, pdrvextra_cmd->pbuf, pdrvextra_cmd->size);
3651 #else
3652                         c2h_evt_hdl(padapter, pdrvextra_cmd->pbuf, NULL);
3653 #endif
3654                         break;
3655 #ifdef CONFIG_BEAMFORMING
3656                 case BEAMFORMING_WK_CID:
3657                         beamforming_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf);
3658                         break;
3659 #endif //CONFIG_BEAMFORMING
3660                 case DM_RA_MSK_WK_CID:
3661                         rtw_dm_ra_mask_hdl(padapter, (struct sta_info *)pdrvextra_cmd->pbuf);
3662                         break;
3663 #ifdef CONFIG_BT_COEXIST
3664                 case BTINFO_WK_CID:
3665                         rtw_btinfo_hdl(padapter ,pdrvextra_cmd->pbuf, pdrvextra_cmd->size);
3666                         break;
3667 #endif
3668                 default:
3669                         break;
3670         }
3671
3672         if (pdrvextra_cmd->pbuf && pdrvextra_cmd->size>0)
3673         {
3674                 rtw_mfree(pdrvextra_cmd->pbuf, pdrvextra_cmd->size);
3675         }
3676
3677         return H2C_SUCCESS;
3678 }
3679
3680 void rtw_survey_cmd_callback(_adapter*  padapter ,  struct cmd_obj *pcmd)
3681 {
3682         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
3683
3684 _func_enter_;
3685
3686         if(pcmd->res == H2C_DROPPED)
3687         {
3688                 //TODO: cancel timer and do timeout handler directly...
3689                 //need to make timeout handlerOS independent
3690                 _set_timer(&pmlmepriv->scan_to_timer, 1);
3691         }
3692         else if (pcmd->res != H2C_SUCCESS) {
3693                 _set_timer(&pmlmepriv->scan_to_timer, 1);
3694                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\n ********Error: MgntActrtw_set_802_11_bssid_LIST_SCAN Fail ************\n\n."));
3695         }
3696
3697         // free cmd
3698         rtw_free_cmd_obj(pcmd);
3699
3700 _func_exit_;
3701 }
3702 void rtw_disassoc_cmd_callback(_adapter*        padapter,  struct cmd_obj *pcmd)
3703 {
3704         _irqL   irqL;
3705         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
3706
3707 _func_enter_;
3708
3709         if (pcmd->res != H2C_SUCCESS)
3710         {
3711                 _enter_critical_bh(&pmlmepriv->lock, &irqL);
3712                 set_fwstate(pmlmepriv, _FW_LINKED);
3713                 _exit_critical_bh(&pmlmepriv->lock, &irqL);
3714
3715                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\n ***Error: disconnect_cmd_callback Fail ***\n."));
3716
3717                 goto exit;
3718         }
3719 #ifdef CONFIG_BR_EXT
3720         else //clear bridge database
3721                 nat25_db_cleanup(padapter);
3722 #endif //CONFIG_BR_EXT
3723
3724         // free cmd
3725         rtw_free_cmd_obj(pcmd);
3726
3727 exit:
3728
3729 _func_exit_;
3730 }
3731
3732
3733 void rtw_joinbss_cmd_callback(_adapter* padapter,  struct cmd_obj *pcmd)
3734 {
3735         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
3736         struct  mlme_ext_priv   *pmlmeext = &padapter->mlmeextpriv;
3737         struct  mlme_ext_info   *pmlmeinfo = &(pmlmeext->mlmext_info);
3738         u16     val;
3739
3740 _func_enter_;
3741
3742         if(pcmd->res == H2C_DROPPED)
3743         {
3744                 //TODO: cancel timer and do timeout handler directly...
3745                 //need to make timeout handlerOS independent
3746                 _set_timer(&pmlmepriv->assoc_timer, 1);
3747         }
3748         else if(pcmd->res != H2C_SUCCESS)
3749         {
3750                 _set_timer(&pmlmepriv->assoc_timer, 1);
3751         }
3752         else if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_TPLINK_745N)
3753         {
3754                 val = 0x0404;
3755                 rtw_hal_set_hwreg(padapter, HW_VAR_MAX_AGGR_NUM, (u8 *)&val);
3756         }
3757
3758         rtw_free_cmd_obj(pcmd);
3759
3760 _func_exit_;
3761 }
3762
3763 void rtw_createbss_cmd_callback(_adapter *padapter, struct cmd_obj *pcmd)
3764 {
3765         _irqL irqL;
3766         u8 timer_cancelled;
3767         struct sta_info *psta = NULL;
3768         struct wlan_network *pwlan = NULL;
3769         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
3770         WLAN_BSSID_EX *pnetwork = (WLAN_BSSID_EX *)pcmd->parmbuf;
3771         struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
3772
3773 _func_enter_;
3774
3775         if (pcmd->parmbuf == NULL)
3776                 goto exit;
3777
3778         if((pcmd->res != H2C_SUCCESS))
3779         {
3780                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\n ********Error: rtw_createbss_cmd_callback  Fail ************\n\n."));
3781                 _set_timer(&pmlmepriv->assoc_timer, 1 );
3782         }
3783
3784         _cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
3785
3786 #ifdef CONFIG_FW_MLMLE
3787        //endian_convert
3788         pnetwork->Length = le32_to_cpu(pnetwork->Length);
3789         pnetwork->Ssid.SsidLength = le32_to_cpu(pnetwork->Ssid.SsidLength);
3790         pnetwork->Privacy =le32_to_cpu(pnetwork->Privacy);
3791         pnetwork->Rssi = le32_to_cpu(pnetwork->Rssi);
3792         pnetwork->NetworkTypeInUse =le32_to_cpu(pnetwork->NetworkTypeInUse);
3793         pnetwork->Configuration.ATIMWindow = le32_to_cpu(pnetwork->Configuration.ATIMWindow);
3794         //pnetwork->Configuration.BeaconPeriod = le32_to_cpu(pnetwork->Configuration.BeaconPeriod);
3795         pnetwork->Configuration.DSConfig =le32_to_cpu(pnetwork->Configuration.DSConfig);
3796         pnetwork->Configuration.FHConfig.DwellTime=le32_to_cpu(pnetwork->Configuration.FHConfig.DwellTime);
3797         pnetwork->Configuration.FHConfig.HopPattern=le32_to_cpu(pnetwork->Configuration.FHConfig.HopPattern);
3798         pnetwork->Configuration.FHConfig.HopSet=le32_to_cpu(pnetwork->Configuration.FHConfig.HopSet);
3799         pnetwork->Configuration.FHConfig.Length=le32_to_cpu(pnetwork->Configuration.FHConfig.Length);
3800         pnetwork->Configuration.Length = le32_to_cpu(pnetwork->Configuration.Length);
3801         pnetwork->InfrastructureMode = le32_to_cpu(pnetwork->InfrastructureMode);
3802         pnetwork->IELength = le32_to_cpu(pnetwork->IELength);
3803 #endif
3804
3805         _enter_critical_bh(&pmlmepriv->lock, &irqL);
3806
3807
3808         if(check_fwstate(pmlmepriv, WIFI_AP_STATE) )
3809         {
3810                 psta = rtw_get_stainfo(&padapter->stapriv, pnetwork->MacAddress);
3811                 if(!psta)
3812                 {
3813                 psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress);
3814                 if (psta == NULL)
3815                 {
3816                         RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nCan't alloc sta_info when createbss_cmd_callback\n"));
3817                         goto createbss_cmd_fail ;
3818                 }
3819                 }
3820
3821                 rtw_indicate_connect( padapter);
3822         }
3823         else
3824         {
3825                 _irqL   irqL;
3826
3827                 pwlan = _rtw_alloc_network(pmlmepriv);
3828                 _enter_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
3829                 if ( pwlan == NULL)
3830                 {
3831                         pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
3832                         if( pwlan == NULL)
3833                         {
3834                                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\n Error:  can't get pwlan in rtw_joinbss_event_callback \n"));
3835                                 _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
3836                                 goto createbss_cmd_fail;
3837                         }
3838                         pwlan->last_scanned = rtw_get_current_time();
3839                 }
3840                 else
3841                 {
3842                         rtw_list_insert_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue);
3843                 }
3844
3845                 pnetwork->Length = get_WLAN_BSSID_EX_sz(pnetwork);
3846                 _rtw_memcpy(&(pwlan->network), pnetwork, pnetwork->Length);
3847                 //pwlan->fixed = _TRUE;
3848
3849                 //rtw_list_insert_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue);
3850
3851                 // copy pdev_network information to     pmlmepriv->cur_network
3852                 _rtw_memcpy(&tgt_network->network, pnetwork, (get_WLAN_BSSID_EX_sz(pnetwork)));
3853
3854                 // reset DSConfig
3855                 //tgt_network->network.Configuration.DSConfig = (u32)rtw_ch2freq(pnetwork->Configuration.DSConfig);
3856
3857                 _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
3858
3859 #if 0
3860                 if((pmlmepriv->fw_state) & WIFI_AP_STATE)
3861                 {
3862                         psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress);
3863
3864                         if (psta == NULL) { // for AP Mode & Adhoc Master Mode
3865                                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nCan't alloc sta_info when createbss_cmd_callback\n"));
3866                                 goto createbss_cmd_fail ;
3867                         }
3868
3869                         rtw_indicate_connect( padapter);
3870                 }
3871                 else {
3872
3873                         //rtw_indicate_disconnect(dev);
3874                 }
3875 #endif
3876                 _exit_critical_bh(&(pmlmepriv->scanned_queue.lock), &irqL);
3877                 // we will set _FW_LINKED when there is one more sat to join us (rtw_stassoc_event_callback)
3878
3879         }
3880
3881 createbss_cmd_fail:
3882
3883         _exit_critical_bh(&pmlmepriv->lock, &irqL);
3884 exit:
3885         rtw_free_cmd_obj(pcmd);
3886
3887 _func_exit_;
3888
3889 }
3890
3891
3892
3893 void rtw_setstaKey_cmdrsp_callback(_adapter*    padapter ,  struct cmd_obj *pcmd)
3894 {
3895
3896         struct sta_priv * pstapriv = &padapter->stapriv;
3897         struct set_stakey_rsp* psetstakey_rsp = (struct set_stakey_rsp*) (pcmd->rsp);
3898         struct sta_info*        psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr);
3899
3900 _func_enter_;
3901
3902         if(psta==NULL)
3903         {
3904                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nERROR: rtw_setstaKey_cmdrsp_callback => can't get sta_info \n\n"));
3905                 goto exit;
3906         }
3907
3908         //psta->aid = psta->mac_id = psetstakey_rsp->keyid; //CAM_ID(CAM_ENTRY)
3909
3910 exit:
3911
3912         rtw_free_cmd_obj(pcmd);
3913
3914 _func_exit_;
3915
3916 }
3917 void rtw_setassocsta_cmdrsp_callback(_adapter*  padapter,  struct cmd_obj *pcmd)
3918 {
3919         _irqL   irqL;
3920         struct sta_priv * pstapriv = &padapter->stapriv;
3921         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
3922         struct set_assocsta_parm* passocsta_parm = (struct set_assocsta_parm*)(pcmd->parmbuf);
3923         struct set_assocsta_rsp* passocsta_rsp = (struct set_assocsta_rsp*) (pcmd->rsp);
3924         struct sta_info*        psta = rtw_get_stainfo(pstapriv, passocsta_parm->addr);
3925
3926 _func_enter_;
3927
3928         if(psta==NULL)
3929         {
3930                 RT_TRACE(_module_rtl871x_cmd_c_,_drv_err_,("\nERROR: setassocsta_cmdrsp_callbac => can't get sta_info \n\n"));
3931                 goto exit;
3932         }
3933
3934         psta->aid = psta->mac_id = passocsta_rsp->cam_id;
3935
3936         _enter_critical_bh(&pmlmepriv->lock, &irqL);
3937
3938         if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == _TRUE) && (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == _TRUE))
3939                 _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
3940
3941         set_fwstate(pmlmepriv, _FW_LINKED);
3942         _exit_critical_bh(&pmlmepriv->lock, &irqL);
3943
3944 exit:
3945         rtw_free_cmd_obj(pcmd);
3946
3947 _func_exit_;
3948 }
3949
3950 void rtw_getrttbl_cmd_cmdrsp_callback(_adapter* padapter,  struct cmd_obj *pcmd);
3951 void rtw_getrttbl_cmd_cmdrsp_callback(_adapter* padapter,  struct cmd_obj *pcmd)
3952 {
3953 _func_enter_;
3954
3955         rtw_free_cmd_obj(pcmd);
3956 #ifdef CONFIG_MP_INCLUDED
3957         if (padapter->registrypriv.mp_mode == 1)
3958                 padapter->mppriv.workparam.bcompleted=_TRUE;
3959 #endif
3960
3961 _func_exit_;
3962
3963 }
3964