drivers/staging/rtl8723bs/core/rtw_cmd.c

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