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