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