drivers/staging/rtl8723bs/core/rtw_cmd.c

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