drivers/staging/rtl8188eu/core/rtw_cmd.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  ******************************************************************************/
  15 #define _RTW_CMD_C_
  16
  17 #include <osdep_service.h>
  18 #include <drv_types.h>
  19 #include <recv_osdep.h>
  20 #include <mlme_osdep.h>
  21 #include <rtw_mlme_ext.h>
  22
  23 /*
  24 Caller and the rtw_cmd_thread can protect cmd_q by spin_lock.
  25 No irqsave is necessary.
  26 */
  27
  28 int rtw_init_cmd_priv(struct cmd_priv *pcmdpriv)
  29 {
  30         init_completion(&pcmdpriv->cmd_queue_comp);
  31         init_completion(&pcmdpriv->terminate_cmdthread_comp);
  32
  33         _rtw_init_queue(&(pcmdpriv->cmd_queue));
  34         return _SUCCESS;
  35 }
  36
  37 /*
  38 Calling Context:
  39
  40 rtw_enqueue_cmd can only be called between kernel thread,
  41 since only spin_lock is used.
  42
  43 ISR/Call-Back functions can't call this sub-function.
  44
  45 */
  46
  47 static int _rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj)
  48 {
  49         unsigned long irqL;
  50
  51
  52         if (obj == NULL)
  53                 goto exit;
  54
  55         spin_lock_irqsave(&queue->lock, irqL);
  56
  57         list_add_tail(&obj->list, &queue->queue);
  58
  59         spin_unlock_irqrestore(&queue->lock, irqL);
  60
  61 exit:
  62
  63
  64         return _SUCCESS;
  65 }
  66
  67 struct cmd_obj *rtw_dequeue_cmd(struct __queue *queue)
  68 {
  69         unsigned long irqL;
  70         struct cmd_obj *obj;
  71
  72         spin_lock_irqsave(&queue->lock, irqL);
  73         obj = list_first_entry_or_null(&queue->queue, struct cmd_obj, list);
  74         if (obj)
  75                 list_del_init(&obj->list);
  76         spin_unlock_irqrestore(&queue->lock, irqL);
  77
  78         return obj;
  79 }
  80
  81 static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
  82 {
  83         u8 bAllow = false; /* set to true to allow enqueuing cmd when hw_init_completed is false */
  84
  85         /* To decide allow or not */
  86         if ((pcmdpriv->padapter->pwrctrlpriv.bHWPwrPindetect) &&
  87             (!pcmdpriv->padapter->registrypriv.usbss_enable)) {
  88                 if (cmd_obj->cmdcode == _Set_Drv_Extra_CMD_) {
  89                         struct drvextra_cmd_parm        *pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)cmd_obj->parmbuf;
  90
  91                         if (pdrvextra_cmd_parm->ec_id == POWER_SAVING_CTRL_WK_CID)
  92                                 bAllow = true;
  93                 }
  94         }
  95
  96         if (cmd_obj->cmdcode == _SetChannelPlan_CMD_)
  97                 bAllow = true;
  98
  99         if ((!pcmdpriv->padapter->hw_init_completed && !bAllow) ||
 100             !pcmdpriv->cmdthd_running)  /* com_thread not running */
 101                 return _FAIL;
 102         return _SUCCESS;
 103 }
 104
 105 u32 rtw_enqueue_cmd(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 106 {
 107         int res = _FAIL;
 108         struct adapter *padapter = pcmdpriv->padapter;
 109
 110
 111         if (cmd_obj == NULL)
 112                 goto exit;
 113
 114         cmd_obj->padapter = padapter;
 115
 116         res = rtw_cmd_filter(pcmdpriv, cmd_obj);
 117         if (res == _FAIL) {
 118                 rtw_free_cmd_obj(cmd_obj);
 119                 goto exit;
 120         }
 121
 122         res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj);
 123
 124         if (res == _SUCCESS)
 125                 complete(&pcmdpriv->cmd_queue_comp);
 126
 127 exit:
 128
 129
 130         return res;
 131 }
 132
 133 void rtw_free_cmd_obj(struct cmd_obj *pcmd)
 134 {
 135
 136         if ((pcmd->cmdcode != _JoinBss_CMD_) && (pcmd->cmdcode != _CreateBss_CMD_)) {
 137                 /* free parmbuf in cmd_obj */
 138                 kfree(pcmd->parmbuf);
 139         }
 140
 141         if (pcmd->rsp != NULL) {
 142                 if (pcmd->rspsz != 0) {
 143                         /* free rsp in cmd_obj */
 144                         kfree(pcmd->rsp);
 145                 }
 146         }
 147
 148         /* free cmd_obj */
 149         kfree(pcmd);
 150
 151 }
 152
 153 int rtw_cmd_thread(void *context)
 154 {
 155         u8 ret;
 156         struct cmd_obj *pcmd;
 157         u8 (*cmd_hdl)(struct adapter *padapter, u8 *pbuf);
 158         void (*pcmd_callback)(struct adapter *dev, struct cmd_obj *pcmd);
 159         struct adapter *padapter = context;
 160         struct cmd_priv *pcmdpriv = &(padapter->cmdpriv);
 161
 162         allow_signal(SIGTERM);
 163
 164         pcmdpriv->cmdthd_running = true;
 165         complete(&pcmdpriv->terminate_cmdthread_comp);
 166
 167         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("start r871x rtw_cmd_thread !!!!\n"));
 168
 169         while (1) {
 170                 if (wait_for_completion_interruptible(&pcmdpriv->cmd_queue_comp))
 171                         break;
 172
 173                 if (padapter->bDriverStopped ||
 174                     padapter->bSurpriseRemoved) {
 175                         DBG_88E("%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
 176                                 __func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
 177                         break;
 178                 }
 179 _next:
 180                 if (padapter->bDriverStopped ||
 181                     padapter->bSurpriseRemoved) {
 182                         DBG_88E("%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
 183                                 __func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
 184                         break;
 185                 }
 186
 187                 pcmd = rtw_dequeue_cmd(&pcmdpriv->cmd_queue);
 188                 if (!pcmd)
 189                         continue;
 190
 191                 if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) {
 192                         pcmd->res = H2C_DROPPED;
 193                 } else {
 194                         if (pcmd->cmdcode < ARRAY_SIZE(wlancmds)) {
 195                             cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
 196
 197                                 if (cmd_hdl) {
 198                                         ret = cmd_hdl(pcmd->padapter, pcmd->parmbuf);
 199                                         pcmd->res = ret;
 200                                 }
 201                         } else {
 202                                 pcmd->res = H2C_PARAMETERS_ERROR;
 203                         }
 204
 205                         cmd_hdl = NULL;
 206                 }
 207
 208                 /* call callback function for post-processed */
 209                 if (pcmd->cmdcode < ARRAY_SIZE(rtw_cmd_callback)) {
 210                         pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
 211                         if (pcmd_callback == NULL) {
 212                                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("mlme_cmd_hdl(): pcmd_callback = 0x%p, cmdcode = 0x%x\n", pcmd_callback, pcmd->cmdcode));
 213                                 rtw_free_cmd_obj(pcmd);
 214                         } else {
 215                                 /* todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!= NULL) */
 216                                 pcmd_callback(pcmd->padapter, pcmd);/* need conider that free cmd_obj in rtw_cmd_callback */
 217                         }
 218                 } else {
 219                         RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("%s: cmdcode = 0x%x callback not defined!\n", __func__, pcmd->cmdcode));
 220                         rtw_free_cmd_obj(pcmd);
 221                 }
 222
 223                 if (signal_pending(current))
 224                         flush_signals(current);
 225
 226                 goto _next;
 227         }
 228         pcmdpriv->cmdthd_running = false;
 229
 230         /*  free all cmd_obj resources */
 231         while ((pcmd = rtw_dequeue_cmd(&pcmdpriv->cmd_queue))) {
 232                 /* DBG_88E("%s: leaving... drop cmdcode:%u\n", __func__, pcmd->cmdcode); */
 233
 234                 rtw_free_cmd_obj(pcmd);
 235         }
 236
 237         complete(&pcmdpriv->terminate_cmdthread_comp);
 238
 239
 240         complete_and_exit(NULL, 0);
 241 }
 242
 243 /*
 244 rtw_sitesurvey_cmd(~)
 245         ### NOTE:#### (!!!!)
 246         MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
 247 */
 248 u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid, int ssid_num,
 249         struct rtw_ieee80211_channel *ch, int ch_num)
 250 {
 251         u8 res = _FAIL;
 252         struct cmd_obj          *ph2c;
 253         struct sitesurvey_parm  *psurveyPara;
 254         struct cmd_priv         *pcmdpriv = &padapter->cmdpriv;
 255         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
 256
 257         if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
 258                 rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1);
 259
 260         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 261         if (!ph2c)
 262                 return _FAIL;
 263
 264         psurveyPara = kzalloc(sizeof(struct sitesurvey_parm), GFP_ATOMIC);
 265         if (!psurveyPara) {
 266                 kfree(ph2c);
 267                 return _FAIL;
 268         }
 269
 270         rtw_free_network_queue(padapter, false);
 271
 272         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("%s: flush network queue\n", __func__));
 273
 274         init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, _SiteSurvey_CMD_);
 275
 276         /* psurveyPara->bsslimit = 48; */
 277         psurveyPara->scan_mode = pmlmepriv->scan_mode;
 278
 279         /* prepare ssid list */
 280         if (ssid) {
 281                 int i;
 282
 283                 for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) {
 284                         if (ssid[i].SsidLength) {
 285                                 memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(struct ndis_802_11_ssid));
 286                                 psurveyPara->ssid_num++;
 287                         }
 288                 }
 289         }
 290
 291         /* prepare channel list */
 292         if (ch) {
 293                 int i;
 294
 295                 for (i = 0; i < ch_num && i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
 296                         if (ch[i].hw_value && !(ch[i].flags & RTW_IEEE80211_CHAN_DISABLED)) {
 297                                 memcpy(&psurveyPara->ch[i], &ch[i], sizeof(struct rtw_ieee80211_channel));
 298                                 psurveyPara->ch_num++;
 299                         }
 300                 }
 301         }
 302
 303         set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
 304
 305         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 306
 307         if (res == _SUCCESS) {
 308                 mod_timer(&pmlmepriv->scan_to_timer,
 309                           jiffies + msecs_to_jiffies(SCANNING_TIMEOUT));
 310
 311                 rtw_led_control(padapter, LED_CTL_SITE_SURVEY);
 312
 313                 pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
 314         } else {
 315                 _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 316         }
 317
 318
 319         return res;
 320 }
 321
 322 void rtw_readtssi_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
 323 {
 324
 325         kfree(pcmd->parmbuf);
 326         kfree(pcmd);
 327 }
 328
 329 u8 rtw_createbss_cmd(struct adapter  *padapter)
 330 {
 331         struct cmd_obj *pcmd;
 332         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 333         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 334         struct wlan_bssid_ex *pdev_network = &padapter->registrypriv.dev_network;
 335         u8      res = _SUCCESS;
 336
 337
 338         rtw_led_control(padapter, LED_CTL_START_TO_LINK);
 339
 340         if (pmlmepriv->assoc_ssid.SsidLength == 0)
 341                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for Any SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
 342         else
 343                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
 344
 345         pcmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 346         if (!pcmd) {
 347                 res = _FAIL;
 348                 goto exit;
 349         }
 350
 351         INIT_LIST_HEAD(&pcmd->list);
 352         pcmd->cmdcode = _CreateBss_CMD_;
 353         pcmd->parmbuf = (unsigned char *)pdev_network;
 354         pcmd->cmdsz = get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network);
 355         pcmd->rsp = NULL;
 356         pcmd->rspsz = 0;
 357         pdev_network->Length = pcmd->cmdsz;
 358         res = rtw_enqueue_cmd(pcmdpriv, pcmd);
 359 exit:
 360
 361
 362         return res;
 363 }
 364
 365 u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
 366 {
 367         u8      res = _SUCCESS;
 368         uint    t_len = 0;
 369         struct wlan_bssid_ex            *psecnetwork;
 370         struct cmd_obj          *pcmd;
 371         struct cmd_priv         *pcmdpriv = &padapter->cmdpriv;
 372         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
 373         struct qos_priv         *pqospriv = &pmlmepriv->qospriv;
 374         struct security_priv    *psecuritypriv = &padapter->securitypriv;
 375         struct registry_priv    *pregistrypriv = &padapter->registrypriv;
 376         struct ht_priv          *phtpriv = &pmlmepriv->htpriv;
 377         enum ndis_802_11_network_infra ndis_network_mode = pnetwork->network.InfrastructureMode;
 378         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
 379         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 380
 381
 382         rtw_led_control(padapter, LED_CTL_START_TO_LINK);
 383
 384         if (pmlmepriv->assoc_ssid.SsidLength == 0)
 385                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+Join cmd: Any SSid\n"));
 386         else
 387                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+Join cmd: SSid =[%s]\n", pmlmepriv->assoc_ssid.Ssid));
 388
 389         pcmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 390         if (!pcmd) {
 391                 res = _FAIL;
 392                 goto exit;
 393         }
 394         /* for IEs is fix buf size */
 395         t_len = sizeof(struct wlan_bssid_ex);
 396
 397
 398         /* for hidden ap to set fw_state here */
 399         if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE)) {
 400                 switch (ndis_network_mode) {
 401                 case Ndis802_11IBSS:
 402                         set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
 403                         break;
 404                 case Ndis802_11Infrastructure:
 405                         set_fwstate(pmlmepriv, WIFI_STATION_STATE);
 406                         break;
 407                 case Ndis802_11APMode:
 408                 case Ndis802_11AutoUnknown:
 409                 case Ndis802_11InfrastructureMax:
 410                         break;
 411                 }
 412         }
 413
 414         psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss;
 415         if (psecnetwork == NULL) {
 416                 kfree(pcmd);
 417
 418                 res = _FAIL;
 419
 420                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd :psecnetwork == NULL!!!\n"));
 421
 422                 goto exit;
 423         }
 424
 425         memset(psecnetwork, 0, t_len);
 426
 427         memcpy(psecnetwork, &pnetwork->network, get_wlan_bssid_ex_sz(&pnetwork->network));
 428
 429         psecuritypriv->authenticator_ie[0] = (unsigned char)psecnetwork->IELength;
 430
 431         if ((psecnetwork->IELength-12) < (256-1))
 432                 memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], psecnetwork->IELength-12);
 433         else
 434                 memcpy(&psecuritypriv->authenticator_ie[1], &psecnetwork->IEs[12], (256-1));
 435
 436         psecnetwork->IELength = 0;
 437         /*  Added by Albert 2009/02/18 */
 438         /*  If the driver wants to use the bssid to create the connection. */
 439         /*  If not,  we have to copy the connecting AP's MAC address to it so that */
 440         /*  the driver just has the bssid information for PMKIDList searching. */
 441
 442         if (!pmlmepriv->assoc_by_bssid)
 443                 memcpy(&pmlmepriv->assoc_bssid[0], &pnetwork->network.MacAddress[0], ETH_ALEN);
 444
 445         psecnetwork->IELength = rtw_restruct_sec_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength);
 446
 447
 448         pqospriv->qos_option = 0;
 449
 450         if (pregistrypriv->wmm_enable) {
 451                 u32 tmp_len;
 452
 453                 tmp_len = rtw_restruct_wmm_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0], pnetwork->network.IELength, psecnetwork->IELength);
 454
 455                 if (psecnetwork->IELength != tmp_len) {
 456                         psecnetwork->IELength = tmp_len;
 457                         pqospriv->qos_option = 1; /* There is WMM IE in this corresp. beacon */
 458                 } else {
 459                         pqospriv->qos_option = 0;/* There is no WMM IE in this corresp. beacon */
 460                 }
 461         }
 462
 463         phtpriv->ht_option = false;
 464         if (pregistrypriv->ht_enable) {
 465                 /*
 466                  * Added by Albert 2010/06/23
 467                  * For the WEP mode, we will use the bg mode to do
 468                  * the connection to avoid some IOT issue.
 469                  * Especially for Realtek 8192u SoftAP.
 470                  */
 471                 if ((padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_) &&
 472                     (padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_) &&
 473                     (padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_)) {
 474                         /* rtw_restructure_ht_ie */
 475                         rtw_restructure_ht_ie(padapter, &pnetwork->network.IEs[0], &psecnetwork->IEs[0],
 476                                                                         pnetwork->network.IELength, &psecnetwork->IELength);
 477                 }
 478         }
 479
 480         pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength);
 481
 482         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_TENDA)
 483                 padapter->pwrctrlpriv.smart_ps = 0;
 484         else
 485                 padapter->pwrctrlpriv.smart_ps = padapter->registrypriv.smart_ps;
 486
 487         DBG_88E("%s: smart_ps =%d\n", __func__, padapter->pwrctrlpriv.smart_ps);
 488
 489         pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork);/* get cmdsz before endian conversion */
 490
 491         INIT_LIST_HEAD(&pcmd->list);
 492         pcmd->cmdcode = _JoinBss_CMD_;
 493         pcmd->parmbuf = (unsigned char *)psecnetwork;
 494         pcmd->rsp = NULL;
 495         pcmd->rspsz = 0;
 496
 497         res = rtw_enqueue_cmd(pcmdpriv, pcmd);
 498
 499 exit:
 500
 501
 502         return res;
 503 }
 504
 505 u8 rtw_disassoc_cmd(struct adapter *padapter, u32 deauth_timeout_ms, bool enqueue) /* for sta_mode */
 506 {
 507         struct cmd_obj *cmdobj = NULL;
 508         struct disconnect_parm *param = NULL;
 509         struct cmd_priv *cmdpriv = &padapter->cmdpriv;
 510         u8 res = _SUCCESS;
 511
 512
 513         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_disassoc_cmd\n"));
 514
 515         /* prepare cmd parameter */
 516         param = kzalloc(sizeof(*param), GFP_KERNEL);
 517         if (!param) {
 518                 res = _FAIL;
 519                 goto exit;
 520         }
 521         param->deauth_timeout_ms = deauth_timeout_ms;
 522
 523         if (enqueue) {
 524                 /* need enqueue, prepare cmd_obj and enqueue */
 525                 cmdobj = kzalloc(sizeof(*cmdobj), GFP_ATOMIC);
 526                 if (!cmdobj) {
 527                         res = _FAIL;
 528                         kfree(param);
 529                         goto exit;
 530                 }
 531                 init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_);
 532                 res = rtw_enqueue_cmd(cmdpriv, cmdobj);
 533         } else {
 534                 /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
 535                 if (disconnect_hdl(padapter, (u8 *)param) != H2C_SUCCESS)
 536                         res = _FAIL;
 537                 kfree(param);
 538         }
 539
 540 exit:
 541
 542
 543         return res;
 544 }
 545
 546 u8 rtw_setopmode_cmd(struct adapter  *padapter, enum ndis_802_11_network_infra networktype)
 547 {
 548         struct  cmd_obj *ph2c;
 549         struct  setopmode_parm *psetop;
 550
 551         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
 552
 553         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 554         psetop = kzalloc(sizeof(struct setopmode_parm), GFP_KERNEL);
 555         if (!ph2c || !psetop) {
 556                 kfree(ph2c);
 557                 kfree(psetop);
 558                 return false;
 559         }
 560
 561         init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
 562         psetop->mode = (u8)networktype;
 563
 564         return rtw_enqueue_cmd(pcmdpriv, ph2c);
 565 }
 566
 567 u8 rtw_setstakey_cmd(struct adapter *padapter, u8 *psta, u8 unicast_key)
 568 {
 569         struct cmd_obj *ph2c;
 570         struct set_stakey_parm *psetstakey_para;
 571         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 572         struct set_stakey_rsp *psetstakey_rsp = NULL;
 573
 574         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 575         struct security_priv *psecuritypriv = &padapter->securitypriv;
 576         struct sta_info *sta = (struct sta_info *)psta;
 577
 578         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 579         psetstakey_para = kzalloc(sizeof(struct set_stakey_parm), GFP_KERNEL);
 580         psetstakey_rsp = kzalloc(sizeof(struct set_stakey_rsp), GFP_KERNEL);
 581
 582         if (!ph2c || !psetstakey_para || !psetstakey_rsp) {
 583                 kfree(ph2c);
 584                 kfree(psetstakey_para);
 585                 kfree(psetstakey_rsp);
 586                 return _FAIL;
 587         }
 588
 589         init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
 590         ph2c->rsp = (u8 *)psetstakey_rsp;
 591         ph2c->rspsz = sizeof(struct set_stakey_rsp);
 592
 593         ether_addr_copy(psetstakey_para->addr, sta->hwaddr);
 594
 595         if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 596                 psetstakey_para->algorithm = (unsigned char)psecuritypriv->dot11PrivacyAlgrthm;
 597         else
 598                 GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm, false);
 599
 600         if (unicast_key)
 601                 memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
 602         else
 603                 memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16);
 604
 605         /* jeff: set this because at least sw key is ready */
 606         padapter->securitypriv.busetkipkey = true;
 607
 608         return rtw_enqueue_cmd(pcmdpriv, ph2c);
 609 }
 610
 611 u8 rtw_clearstakey_cmd(struct adapter *padapter, u8 *psta, u8 entry, u8 enqueue)
 612 {
 613         struct cmd_obj *ph2c;
 614         struct set_stakey_parm  *psetstakey_para;
 615         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 616         struct set_stakey_rsp *psetstakey_rsp = NULL;
 617         struct sta_info *sta = (struct sta_info *)psta;
 618         u8      res = _SUCCESS;
 619
 620
 621         if (!enqueue) {
 622                 clear_cam_entry(padapter, entry);
 623         } else {
 624                 ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 625                 if (!ph2c) {
 626                         res = _FAIL;
 627                         goto exit;
 628                 }
 629
 630                 psetstakey_para = kzalloc(sizeof(struct set_stakey_parm), GFP_ATOMIC);
 631                 if (!psetstakey_para) {
 632                         kfree(ph2c);
 633                         res = _FAIL;
 634                         goto exit;
 635                 }
 636
 637                 psetstakey_rsp = kzalloc(sizeof(struct set_stakey_rsp), GFP_ATOMIC);
 638                 if (!psetstakey_rsp) {
 639                         kfree(ph2c);
 640                         kfree(psetstakey_para);
 641                         res = _FAIL;
 642                         goto exit;
 643                 }
 644
 645                 init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
 646                 ph2c->rsp = (u8 *)psetstakey_rsp;
 647                 ph2c->rspsz = sizeof(struct set_stakey_rsp);
 648
 649                 ether_addr_copy(psetstakey_para->addr, sta->hwaddr);
 650
 651                 psetstakey_para->algorithm = _NO_PRIVACY_;
 652
 653                 psetstakey_para->id = entry;
 654
 655                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 656         }
 657 exit:
 658
 659
 660         return res;
 661 }
 662
 663 u8 rtw_addbareq_cmd(struct adapter *padapter, u8 tid, u8 *addr)
 664 {
 665         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 666         struct cmd_obj *ph2c;
 667         struct addBaReq_parm *paddbareq_parm;
 668         u8      res = _SUCCESS;
 669
 670
 671         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 672         if (!ph2c) {
 673                 res = _FAIL;
 674                 goto exit;
 675         }
 676
 677         paddbareq_parm = kzalloc(sizeof(struct addBaReq_parm), GFP_ATOMIC);
 678         if (!paddbareq_parm) {
 679                 kfree(ph2c);
 680                 res = _FAIL;
 681                 goto exit;
 682         }
 683
 684         paddbareq_parm->tid = tid;
 685         memcpy(paddbareq_parm->addr, addr, ETH_ALEN);
 686
 687         init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm, _AddBAReq_CMD_);
 688
 689         /* DBG_88E("rtw_addbareq_cmd, tid =%d\n", tid); */
 690
 691         /* rtw_enqueue_cmd(pcmdpriv, ph2c); */
 692         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 693
 694 exit:
 695
 696
 697         return res;
 698 }
 699
 700 u8 rtw_dynamic_chk_wk_cmd(struct adapter *padapter)
 701 {
 702         struct cmd_obj *ph2c;
 703         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 704         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 705         u8      res = _SUCCESS;
 706
 707
 708         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 709         if (!ph2c) {
 710                 res = _FAIL;
 711                 goto exit;
 712         }
 713
 714         pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
 715         if (!pdrvextra_cmd_parm) {
 716                 kfree(ph2c);
 717                 res = _FAIL;
 718                 goto exit;
 719         }
 720
 721         pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID;
 722         pdrvextra_cmd_parm->type_size = 0;
 723         pdrvextra_cmd_parm->pbuf = (u8 *)padapter;
 724
 725         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, _Set_Drv_Extra_CMD_);
 726
 727
 728         /* rtw_enqueue_cmd(pcmdpriv, ph2c); */
 729         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 730 exit:
 731         return res;
 732 }
 733
 734 u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue)
 735 {
 736         struct  cmd_obj *pcmdobj;
 737         struct  SetChannelPlan_param *setChannelPlan_param;
 738         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
 739
 740         u8      res = _SUCCESS;
 741
 742
 743         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_chplan_cmd\n"));
 744
 745         /* check input parameter */
 746         if (!rtw_is_channel_plan_valid(chplan)) {
 747                 res = _FAIL;
 748                 goto exit;
 749         }
 750
 751         /* prepare cmd parameter */
 752         setChannelPlan_param = kzalloc(sizeof(struct SetChannelPlan_param), GFP_KERNEL);
 753         if (!setChannelPlan_param) {
 754                 res = _FAIL;
 755                 goto exit;
 756         }
 757         setChannelPlan_param->channel_plan = chplan;
 758
 759         if (enqueue) {
 760                 /* need enqueue, prepare cmd_obj and enqueue */
 761                 pcmdobj = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 762                 if (!pcmdobj) {
 763                         kfree(setChannelPlan_param);
 764                         res = _FAIL;
 765                         goto exit;
 766                 }
 767
 768                 init_h2fwcmd_w_parm_no_rsp(pcmdobj, setChannelPlan_param, _SetChannelPlan_CMD_);
 769                 res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
 770         } else {
 771                 /* no need to enqueue, do the cmd hdl directly and free cmd parameter */
 772                 if (set_chplan_hdl(padapter, (unsigned char *)setChannelPlan_param) != H2C_SUCCESS)
 773                         res = _FAIL;
 774
 775                 kfree(setChannelPlan_param);
 776         }
 777
 778         /* do something based on res... */
 779         if (res == _SUCCESS)
 780                 padapter->mlmepriv.ChannelPlan = chplan;
 781
 782 exit:
 783
 784
 785         return res;
 786 }
 787
 788 static void traffic_status_watchdog(struct adapter *padapter)
 789 {
 790         u8      bEnterPS;
 791         u8      bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false;
 792         u8      bHigherBusyTraffic = false, bHigherBusyRxTraffic = false, bHigherBusyTxTraffic = false;
 793         struct mlme_priv                *pmlmepriv = &(padapter->mlmepriv);
 794
 795         /*  */
 796         /*  Determine if our traffic is busy now */
 797         /*  */
 798         if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 799                 if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 100 ||
 800                     pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 100) {
 801                         bBusyTraffic = true;
 802
 803                         if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
 804                                 bRxBusyTraffic = true;
 805                         else
 806                                 bTxBusyTraffic = true;
 807                 }
 808
 809                 /*  Higher Tx/Rx data. */
 810                 if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 4000 ||
 811                     pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 4000) {
 812                         bHigherBusyTraffic = true;
 813
 814                         if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > pmlmepriv->LinkDetectInfo.NumTxOkInPeriod)
 815                                 bHigherBusyRxTraffic = true;
 816                         else
 817                                 bHigherBusyTxTraffic = true;
 818                 }
 819
 820                 /*  check traffic for  powersaving. */
 821                 if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod + pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
 822                     (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2))
 823                         bEnterPS = false;
 824                 else
 825                         bEnterPS = true;
 826
 827                 /*  LeisurePS only work in infra mode. */
 828                 if (bEnterPS)
 829                         LPS_Enter(padapter);
 830                 else
 831                         LPS_Leave(padapter);
 832         } else {
 833                 LPS_Leave(padapter);
 834         }
 835
 836         pmlmepriv->LinkDetectInfo.NumRxOkInPeriod = 0;
 837         pmlmepriv->LinkDetectInfo.NumTxOkInPeriod = 0;
 838         pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod = 0;
 839         pmlmepriv->LinkDetectInfo.bBusyTraffic = bBusyTraffic;
 840         pmlmepriv->LinkDetectInfo.bTxBusyTraffic = bTxBusyTraffic;
 841         pmlmepriv->LinkDetectInfo.bRxBusyTraffic = bRxBusyTraffic;
 842         pmlmepriv->LinkDetectInfo.bHigherBusyTraffic = bHigherBusyTraffic;
 843         pmlmepriv->LinkDetectInfo.bHigherBusyRxTraffic = bHigherBusyRxTraffic;
 844         pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
 845 }
 846
 847 static void dynamic_chk_wk_hdl(struct adapter *padapter, u8 *pbuf, int sz)
 848 {
 849         struct mlme_priv *pmlmepriv;
 850
 851         padapter = (struct adapter *)pbuf;
 852         pmlmepriv = &(padapter->mlmepriv);
 853
 854 #ifdef CONFIG_88EU_AP_MODE
 855         if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
 856                 expire_timeout_chk(padapter);
 857 #endif
 858
 859         linked_status_chk(padapter);
 860         traffic_status_watchdog(padapter);
 861
 862         rtw_hal_dm_watchdog(padapter);
 863 }
 864
 865 static void lps_ctrl_wk_hdl(struct adapter *padapter, u8 lps_ctrl_type)
 866 {
 867         struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 868         struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
 869         u8      mstatus;
 870
 871
 872         if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
 873             (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
 874                 return;
 875
 876         switch (lps_ctrl_type) {
 877         case LPS_CTRL_SCAN:
 878                 if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
 879                         /* connect */
 880                         LPS_Leave(padapter);
 881                 }
 882                 break;
 883         case LPS_CTRL_JOINBSS:
 884                 LPS_Leave(padapter);
 885                 break;
 886         case LPS_CTRL_CONNECT:
 887                 mstatus = 1;/* connect */
 888                 /*  Reset LPS Setting */
 889                 padapter->pwrctrlpriv.LpsIdleCount = 0;
 890                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
 891                 break;
 892         case LPS_CTRL_DISCONNECT:
 893                 mstatus = 0;/* disconnect */
 894                 LPS_Leave(padapter);
 895                 rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_JOINBSSRPT, (u8 *)(&mstatus));
 896                 break;
 897         case LPS_CTRL_SPECIAL_PACKET:
 898                 /* DBG_88E("LPS_CTRL_SPECIAL_PACKET\n"); */
 899                 pwrpriv->DelayLPSLastTimeStamp = jiffies;
 900                 LPS_Leave(padapter);
 901                 break;
 902         case LPS_CTRL_LEAVE:
 903                 LPS_Leave(padapter);
 904                 break;
 905         default:
 906                 break;
 907         }
 908
 909 }
 910
 911 u8 rtw_lps_ctrl_wk_cmd(struct adapter *padapter, u8 lps_ctrl_type, u8 enqueue)
 912 {
 913         struct cmd_obj  *ph2c;
 914         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
 915         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 916         /* struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv; */
 917         u8      res = _SUCCESS;
 918
 919         if (enqueue) {
 920                 ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 921                 if (!ph2c) {
 922                         res = _FAIL;
 923                         goto exit;
 924                 }
 925
 926                 pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
 927                 if (!pdrvextra_cmd_parm) {
 928                         kfree(ph2c);
 929                         res = _FAIL;
 930                         goto exit;
 931                 }
 932
 933                 pdrvextra_cmd_parm->ec_id = LPS_CTRL_WK_CID;
 934                 pdrvextra_cmd_parm->type_size = lps_ctrl_type;
 935                 pdrvextra_cmd_parm->pbuf = NULL;
 936
 937                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, _Set_Drv_Extra_CMD_);
 938
 939                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 940         } else {
 941                 lps_ctrl_wk_hdl(padapter, lps_ctrl_type);
 942         }
 943
 944 exit:
 945
 946
 947         return res;
 948 }
 949
 950 static void rpt_timer_setting_wk_hdl(struct adapter *padapter, u16 min_time)
 951 {
 952         rtw_hal_set_hwreg(padapter, HW_VAR_RPT_TIMER_SETTING, (u8 *)(&min_time));
 953 }
 954
 955 u8 rtw_rpt_timer_cfg_cmd(struct adapter *padapter, u16 min_time)
 956 {
 957         struct cmd_obj          *ph2c;
 958         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
 959         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 960
 961         u8      res = _SUCCESS;
 962
 963         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 964         if (!ph2c) {
 965                 res = _FAIL;
 966                 goto exit;
 967         }
 968
 969         pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
 970         if (!pdrvextra_cmd_parm) {
 971                 kfree(ph2c);
 972                 res = _FAIL;
 973                 goto exit;
 974         }
 975
 976         pdrvextra_cmd_parm->ec_id = RTP_TIMER_CFG_WK_CID;
 977         pdrvextra_cmd_parm->type_size = min_time;
 978         pdrvextra_cmd_parm->pbuf = NULL;
 979         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, _Set_Drv_Extra_CMD_);
 980         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
 981 exit:
 982
 983
 984         return res;
 985 }
 986
 987 static void antenna_select_wk_hdl(struct adapter *padapter, u8 antenna)
 988 {
 989         rtw_hal_set_hwreg(padapter, HW_VAR_ANTENNA_DIVERSITY_SELECT, (u8 *)(&antenna));
 990 }
 991
 992 u8 rtw_antenna_select_cmd(struct adapter *padapter, u8 antenna, u8 enqueue)
 993 {
 994         struct cmd_obj          *ph2c;
 995         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
 996         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 997         u8      support_ant_div;
 998         u8      res = _SUCCESS;
 999
1000         rtw_hal_get_def_var(padapter, HAL_DEF_IS_SUPPORT_ANT_DIV, &support_ant_div);
1001         if (!support_ant_div)
1002                 return res;
1003
1004         if (enqueue) {
1005                 ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
1006                 if (!ph2c) {
1007                         res = _FAIL;
1008                         goto exit;
1009                 }
1010
1011                 pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_KERNEL);
1012                 if (!pdrvextra_cmd_parm) {
1013                         kfree(ph2c);
1014                         res = _FAIL;
1015                         goto exit;
1016                 }
1017
1018                 pdrvextra_cmd_parm->ec_id = ANT_SELECT_WK_CID;
1019                 pdrvextra_cmd_parm->type_size = antenna;
1020                 pdrvextra_cmd_parm->pbuf = NULL;
1021                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, _Set_Drv_Extra_CMD_);
1022
1023                 res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1024         } else {
1025                 antenna_select_wk_hdl(padapter, antenna);
1026         }
1027 exit:
1028
1029
1030         return res;
1031 }
1032
1033 u8 rtw_ps_cmd(struct adapter *padapter)
1034 {
1035         struct cmd_obj          *ppscmd;
1036         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
1037         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1038
1039         ppscmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
1040         pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
1041         if (!ppscmd || !pdrvextra_cmd_parm) {
1042                 kfree(ppscmd);
1043                 kfree(pdrvextra_cmd_parm);
1044                 return _FAIL;
1045         }
1046
1047         pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID;
1048         pdrvextra_cmd_parm->pbuf = NULL;
1049         init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, _Set_Drv_Extra_CMD_);
1050
1051         return rtw_enqueue_cmd(pcmdpriv, ppscmd);
1052 }
1053
1054 #ifdef CONFIG_88EU_AP_MODE
1055
1056 static void rtw_chk_hi_queue_hdl(struct adapter *padapter)
1057 {
1058         int cnt = 0;
1059         struct sta_info *psta_bmc;
1060         struct sta_priv *pstapriv = &padapter->stapriv;
1061
1062         psta_bmc = rtw_get_bcmc_stainfo(padapter);
1063         if (!psta_bmc)
1064                 return;
1065
1066         if (psta_bmc->sleepq_len == 0) {
1067                 u8 val = 0;
1068
1069                 /* while ((rtw_read32(padapter, 0x414)&0x00ffff00)!= 0) */
1070                 /* while ((rtw_read32(padapter, 0x414)&0x0000ff00)!= 0) */
1071
1072                 rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
1073
1074                 while (!val) {
1075                         msleep(100);
1076
1077                         cnt++;
1078
1079                         if (cnt > 10)
1080                                 break;
1081
1082                         rtw_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
1083                 }
1084
1085                 if (cnt <= 10) {
1086                         pstapriv->tim_bitmap &= ~BIT(0);
1087                         pstapriv->sta_dz_bitmap &= ~BIT(0);
1088
1089                         update_beacon(padapter, _TIM_IE_, NULL, false);
1090                 } else { /* re check again */
1091                         rtw_chk_hi_queue_cmd(padapter);
1092                 }
1093         }
1094 }
1095
1096 u8 rtw_chk_hi_queue_cmd(struct adapter *padapter)
1097 {
1098         struct cmd_obj  *ph2c;
1099         struct drvextra_cmd_parm        *pdrvextra_cmd_parm;
1100         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1101         u8      res = _SUCCESS;
1102
1103         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
1104         if (!ph2c) {
1105                 res = _FAIL;
1106                 goto exit;
1107         }
1108
1109         pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_KERNEL);
1110         if (!pdrvextra_cmd_parm) {
1111                 kfree(ph2c);
1112                 res = _FAIL;
1113                 goto exit;
1114         }
1115
1116         pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID;
1117         pdrvextra_cmd_parm->type_size = 0;
1118         pdrvextra_cmd_parm->pbuf = NULL;
1119
1120         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, _Set_Drv_Extra_CMD_);
1121
1122         res = rtw_enqueue_cmd(pcmdpriv, ph2c);
1123 exit:
1124         return res;
1125 }
1126 #endif
1127
1128 u8 rtw_drvextra_cmd_hdl(struct adapter *padapter, unsigned char *pbuf)
1129 {
1130         struct drvextra_cmd_parm *pdrvextra_cmd;
1131
1132         if (!pbuf)
1133                 return H2C_PARAMETERS_ERROR;
1134
1135         pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf;
1136
1137         switch (pdrvextra_cmd->ec_id) {
1138         case DYNAMIC_CHK_WK_CID:
1139                 dynamic_chk_wk_hdl(padapter, pdrvextra_cmd->pbuf, pdrvextra_cmd->type_size);
1140                 break;
1141         case POWER_SAVING_CTRL_WK_CID:
1142                 rtw_ps_processor(padapter);
1143                 break;
1144         case LPS_CTRL_WK_CID:
1145                 lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type_size);
1146                 break;
1147         case RTP_TIMER_CFG_WK_CID:
1148                 rpt_timer_setting_wk_hdl(padapter, pdrvextra_cmd->type_size);
1149                 break;
1150         case ANT_SELECT_WK_CID:
1151                 antenna_select_wk_hdl(padapter, pdrvextra_cmd->type_size);
1152                 break;
1153 #ifdef CONFIG_88EU_AP_MODE
1154         case CHECK_HIQ_WK_CID:
1155                 rtw_chk_hi_queue_hdl(padapter);
1156                 break;
1157 #endif /* CONFIG_88EU_AP_MODE */
1158         default:
1159                 break;
1160         }
1161
1162         if (pdrvextra_cmd->pbuf && pdrvextra_cmd->type_size > 0)
1163                 kfree(pdrvextra_cmd->pbuf);
1164
1165         return H2C_SUCCESS;
1166 }
1167
1168 void rtw_survey_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
1169 {
1170         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
1171
1172
1173         if (pcmd->res == H2C_DROPPED) {
1174                 /* TODO: cancel timer and do timeout handler directly... */
1175                 /* need to make timeout handlerOS independent */
1176                 mod_timer(&pmlmepriv->scan_to_timer,
1177                           jiffies + msecs_to_jiffies(1));
1178         } else if (pcmd->res != H2C_SUCCESS) {
1179                 mod_timer(&pmlmepriv->scan_to_timer,
1180                           jiffies + msecs_to_jiffies(1));
1181                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: MgntActrtw_set_802_11_bssid_LIST_SCAN Fail ************\n\n."));
1182         }
1183
1184         /*  free cmd */
1185         rtw_free_cmd_obj(pcmd);
1186
1187 }
1188 void rtw_disassoc_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
1189 {
1190         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
1191
1192
1193         if (pcmd->res != H2C_SUCCESS) {
1194                 spin_lock_bh(&pmlmepriv->lock);
1195                 set_fwstate(pmlmepriv, _FW_LINKED);
1196                 spin_unlock_bh(&pmlmepriv->lock);
1197
1198                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ***Error: disconnect_cmd_callback Fail ***\n."));
1199                 return;
1200         }
1201
1202         /*  free cmd */
1203         rtw_free_cmd_obj(pcmd);
1204 }
1205
1206 void rtw_joinbss_cmd_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
1207 {
1208         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
1209
1210
1211         if (pcmd->res == H2C_DROPPED) {
1212                 /* TODO: cancel timer and do timeout handler directly... */
1213                 /* need to make timeout handlerOS independent */
1214                 mod_timer(&pmlmepriv->assoc_timer,
1215                           jiffies + msecs_to_jiffies(1));
1216         } else if (pcmd->res != H2C_SUCCESS) {
1217                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("********Error:rtw_select_and_join_from_scanned_queue Wait Sema  Fail ************\n"));
1218                 mod_timer(&pmlmepriv->assoc_timer,
1219                           jiffies + msecs_to_jiffies(1));
1220         }
1221
1222         rtw_free_cmd_obj(pcmd);
1223
1224 }
1225
1226 void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
1227 {
1228         struct sta_info *psta = NULL;
1229         struct wlan_network *pwlan = NULL;
1230         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
1231         struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)pcmd->parmbuf;
1232         struct wlan_network *tgt_network = &(pmlmepriv->cur_network);
1233
1234
1235         if (pcmd->res != H2C_SUCCESS) {
1236                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: rtw_createbss_cmd_callback  Fail ************\n\n."));
1237                 mod_timer(&pmlmepriv->assoc_timer,
1238                           jiffies + msecs_to_jiffies(1));
1239         }
1240
1241         del_timer_sync(&pmlmepriv->assoc_timer);
1242
1243         spin_lock_bh(&pmlmepriv->lock);
1244
1245         if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
1246                 psta = rtw_get_stainfo(&padapter->stapriv, pnetwork->MacAddress);
1247                 if (!psta) {
1248                         psta = rtw_alloc_stainfo(&padapter->stapriv, pnetwork->MacAddress);
1249                         if (psta == NULL) {
1250                                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nCan't alloc sta_info when createbss_cmd_callback\n"));
1251                                 goto createbss_cmd_fail;
1252                         }
1253                 }
1254
1255                 rtw_indicate_connect(padapter);
1256         } else {
1257                 pwlan = _rtw_alloc_network(pmlmepriv);
1258                 spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
1259                 if (pwlan == NULL) {
1260                         pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
1261                         if (pwlan == NULL) {
1262                                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n Error:  can't get pwlan in rtw_joinbss_event_callback\n"));
1263                                 spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
1264                                 goto createbss_cmd_fail;
1265                         }
1266                         pwlan->last_scanned = jiffies;
1267                 } else {
1268                         list_add_tail(&(pwlan->list), &pmlmepriv->scanned_queue.queue);
1269                 }
1270
1271                 pnetwork->Length = get_wlan_bssid_ex_sz(pnetwork);
1272                 memcpy(&(pwlan->network), pnetwork, pnetwork->Length);
1273
1274                 memcpy(&tgt_network->network, pnetwork, (get_wlan_bssid_ex_sz(pnetwork)));
1275
1276                 _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
1277
1278                 spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
1279                 /*  we will set _FW_LINKED when there is one more sat to join us (rtw_stassoc_event_callback) */
1280         }
1281
1282 createbss_cmd_fail:
1283
1284         spin_unlock_bh(&pmlmepriv->lock);
1285
1286         rtw_free_cmd_obj(pcmd);
1287
1288 }
1289
1290 void rtw_setstaKey_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
1291 {
1292         struct sta_priv *pstapriv = &padapter->stapriv;
1293         struct set_stakey_rsp *psetstakey_rsp = (struct set_stakey_rsp *)(pcmd->rsp);
1294         struct sta_info *psta = rtw_get_stainfo(pstapriv, psetstakey_rsp->addr);
1295
1296
1297         if (psta == NULL) {
1298                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: rtw_setstaKey_cmdrsp_callback => can't get sta_info\n\n"));
1299                 goto exit;
1300         }
1301 exit:
1302         rtw_free_cmd_obj(pcmd);
1303 }
1304
1305 void rtw_setassocsta_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pcmd)
1306 {
1307         struct sta_priv *pstapriv = &padapter->stapriv;
1308         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1309         struct set_assocsta_parm *passocsta_parm = (struct set_assocsta_parm *)(pcmd->parmbuf);
1310         struct set_assocsta_rsp *passocsta_rsp = (struct set_assocsta_rsp *)(pcmd->rsp);
1311         struct sta_info *psta = rtw_get_stainfo(pstapriv, passocsta_parm->addr);
1312
1313
1314         if (psta == NULL) {
1315                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: setassocsta_cmdrsp_callbac => can't get sta_info\n\n"));
1316                 goto exit;
1317         }
1318
1319         psta->aid = passocsta_rsp->cam_id;
1320         psta->mac_id = passocsta_rsp->cam_id;
1321
1322         spin_lock_bh(&pmlmepriv->lock);
1323
1324         if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) && (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true))
1325                 _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
1326
1327         set_fwstate(pmlmepriv, _FW_LINKED);
1328         spin_unlock_bh(&pmlmepriv->lock);
1329
1330 exit:
1331         rtw_free_cmd_obj(pcmd);
1332
1333 }