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