drivers/staging/rtl8723au/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 <rtl8723a_cmd.h>
  22 #include <rtw_sreset.h>
  23
  24 static struct cmd_hdl wlancmds[] = {
  25         GEN_DRV_CMD_HANDLER(0, NULL) /*0*/
  26         GEN_DRV_CMD_HANDLER(0, NULL)
  27         GEN_DRV_CMD_HANDLER(0, NULL)
  28         GEN_DRV_CMD_HANDLER(0, NULL)
  29         GEN_DRV_CMD_HANDLER(0, NULL)
  30         GEN_DRV_CMD_HANDLER(0, NULL)
  31         GEN_MLME_EXT_HANDLER(0, NULL)
  32         GEN_MLME_EXT_HANDLER(0, NULL)
  33         GEN_MLME_EXT_HANDLER(0, NULL)
  34         GEN_MLME_EXT_HANDLER(0, NULL)
  35         GEN_MLME_EXT_HANDLER(0, NULL) /*10*/
  36         GEN_MLME_EXT_HANDLER(0, NULL)
  37         GEN_MLME_EXT_HANDLER(0, NULL)
  38         GEN_MLME_EXT_HANDLER(0, NULL)
  39         GEN_MLME_EXT_HANDLER(sizeof(struct wlan_bssid_ex), join_cmd_hdl23a) /*14*/
  40         GEN_MLME_EXT_HANDLER(sizeof(struct disconnect_parm), disconnect_hdl23a)
  41         GEN_MLME_EXT_HANDLER(sizeof(struct wlan_bssid_ex), createbss_hdl23a)
  42         GEN_MLME_EXT_HANDLER(sizeof(struct setopmode_parm), setopmode_hdl23a)
  43         GEN_MLME_EXT_HANDLER(sizeof(struct sitesurvey_parm), sitesurvey_cmd_hdl23a) /*18*/
  44         GEN_MLME_EXT_HANDLER(sizeof(struct setauth_parm), setauth_hdl23a)
  45         GEN_MLME_EXT_HANDLER(sizeof(struct setkey_parm), setkey_hdl23a) /*20*/
  46         GEN_MLME_EXT_HANDLER(sizeof(struct set_stakey_parm), set_stakey_hdl23a)
  47         GEN_MLME_EXT_HANDLER(sizeof(struct set_assocsta_parm), NULL)
  48         GEN_MLME_EXT_HANDLER(sizeof(struct del_assocsta_parm), NULL)
  49         GEN_MLME_EXT_HANDLER(sizeof(struct setstapwrstate_parm), NULL)
  50         GEN_MLME_EXT_HANDLER(sizeof(struct setbasicrate_parm), NULL)
  51         GEN_MLME_EXT_HANDLER(sizeof(struct getbasicrate_parm), NULL)
  52         GEN_MLME_EXT_HANDLER(sizeof(struct setdatarate_parm), NULL)
  53         GEN_MLME_EXT_HANDLER(sizeof(struct getdatarate_parm), NULL)
  54         GEN_MLME_EXT_HANDLER(sizeof(struct setphyinfo_parm), NULL)
  55         GEN_MLME_EXT_HANDLER(sizeof(struct getphyinfo_parm), NULL)  /*30*/
  56         GEN_MLME_EXT_HANDLER(sizeof(struct setphy_parm), NULL)
  57         GEN_MLME_EXT_HANDLER(sizeof(struct getphy_parm), NULL)
  58         GEN_MLME_EXT_HANDLER(0, NULL)
  59         GEN_MLME_EXT_HANDLER(0, NULL)
  60         GEN_MLME_EXT_HANDLER(0, NULL)
  61         GEN_MLME_EXT_HANDLER(0, NULL)
  62         GEN_MLME_EXT_HANDLER(0, NULL)
  63         GEN_MLME_EXT_HANDLER(0, NULL)
  64         GEN_MLME_EXT_HANDLER(0, NULL)
  65         GEN_MLME_EXT_HANDLER(0, NULL)   /*40*/
  66         GEN_MLME_EXT_HANDLER(0, NULL)
  67         GEN_MLME_EXT_HANDLER(0, NULL)
  68         GEN_MLME_EXT_HANDLER(0, NULL)
  69         GEN_MLME_EXT_HANDLER(0, NULL)
  70         GEN_MLME_EXT_HANDLER(sizeof(struct addBaReq_parm), add_ba_hdl23a)
  71         GEN_MLME_EXT_HANDLER(sizeof(struct set_ch_parm), set_ch_hdl23a) /* 46 */
  72         GEN_MLME_EXT_HANDLER(0, NULL)
  73         GEN_MLME_EXT_HANDLER(0, NULL)
  74         GEN_MLME_EXT_HANDLER(0, NULL)
  75         GEN_MLME_EXT_HANDLER(0, NULL) /*50*/
  76         GEN_MLME_EXT_HANDLER(0, NULL)
  77         GEN_MLME_EXT_HANDLER(0, NULL)
  78         GEN_MLME_EXT_HANDLER(0, NULL)
  79         GEN_MLME_EXT_HANDLER(0, NULL)
  80         GEN_MLME_EXT_HANDLER(sizeof(struct Tx_Beacon_param), tx_beacon_hdl23a) /*55*/
  81
  82         GEN_MLME_EXT_HANDLER(0, mlme_evt_hdl23a) /*56*/
  83         GEN_MLME_EXT_HANDLER(0, rtw_drvextra_cmd_hdl23a) /*57*/
  84
  85         GEN_MLME_EXT_HANDLER(0, h2c_msg_hdl23a) /*58*/
  86         GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelPlan_param), set_chplan_hdl23a) /*59*/
  87         GEN_MLME_EXT_HANDLER(sizeof(struct LedBlink_param), led_blink_hdl23a) /*60*/
  88
  89         GEN_MLME_EXT_HANDLER(sizeof(struct SetChannelSwitch_param), set_csa_hdl23a) /*61*/
  90         GEN_MLME_EXT_HANDLER(sizeof(struct TDLSoption_param), tdls_hdl23a) /*62*/
  91 };
  92
  93 struct _cmd_callback    rtw_cmd_callback[] = {
  94         {GEN_CMD_CODE(_Read_MACREG), NULL}, /*0*/
  95         {GEN_CMD_CODE(_Write_MACREG), NULL},
  96         {GEN_CMD_CODE(_Read_BBREG), &rtw_getbbrfreg_cmdrsp_callback23a},
  97         {GEN_CMD_CODE(_Write_BBREG), NULL},
  98         {GEN_CMD_CODE(_Read_RFREG), &rtw_getbbrfreg_cmdrsp_callback23a},
  99         {GEN_CMD_CODE(_Write_RFREG), NULL}, /*5*/
 100         {GEN_CMD_CODE(_Read_EEPROM), NULL},
 101         {GEN_CMD_CODE(_Write_EEPROM), NULL},
 102         {GEN_CMD_CODE(_Read_EFUSE), NULL},
 103         {GEN_CMD_CODE(_Write_EFUSE), NULL},
 104
 105         {GEN_CMD_CODE(_Read_CAM),       NULL},  /*10*/
 106         {GEN_CMD_CODE(_Write_CAM),       NULL},
 107         {GEN_CMD_CODE(_setBCNITV), NULL},
 108         {GEN_CMD_CODE(_setMBIDCFG), NULL},
 109         {GEN_CMD_CODE(_JoinBss), &rtw_joinbss_cmd23a_callback},  /*14*/
 110         {GEN_CMD_CODE(_DisConnect), &rtw_disassoc_cmd23a_callback}, /*15*/
 111         {GEN_CMD_CODE(_CreateBss), &rtw_createbss_cmd23a_callback},
 112         {GEN_CMD_CODE(_SetOpMode), NULL},
 113         {GEN_CMD_CODE(_SiteSurvey), &rtw_survey_cmd_callback23a}, /*18*/
 114         {GEN_CMD_CODE(_SetAuth), NULL},
 115
 116         {GEN_CMD_CODE(_SetKey), NULL},  /*20*/
 117         {GEN_CMD_CODE(_SetStaKey), &rtw_setstaKey_cmdrsp_callback23a},
 118         {GEN_CMD_CODE(_SetAssocSta), &rtw_setassocsta_cmdrsp_callback23a},
 119         {GEN_CMD_CODE(_DelAssocSta), NULL},
 120         {GEN_CMD_CODE(_SetStaPwrState), NULL},
 121         {GEN_CMD_CODE(_SetBasicRate), NULL}, /*25*/
 122         {GEN_CMD_CODE(_GetBasicRate), NULL},
 123         {GEN_CMD_CODE(_SetDataRate), NULL},
 124         {GEN_CMD_CODE(_GetDataRate), NULL},
 125         {GEN_CMD_CODE(_SetPhyInfo), NULL},
 126
 127         {GEN_CMD_CODE(_GetPhyInfo), NULL}, /*30*/
 128         {GEN_CMD_CODE(_SetPhy), NULL},
 129         {GEN_CMD_CODE(_GetPhy), NULL},
 130         {GEN_CMD_CODE(_readRssi), NULL},
 131         {GEN_CMD_CODE(_readGain), NULL},
 132         {GEN_CMD_CODE(_SetAtim), NULL}, /*35*/
 133         {GEN_CMD_CODE(_SetPwrMode), NULL},
 134         {GEN_CMD_CODE(_JoinbssRpt), NULL},
 135         {GEN_CMD_CODE(_SetRaTable), NULL},
 136         {GEN_CMD_CODE(_GetRaTable), NULL},
 137
 138         {GEN_CMD_CODE(_GetCCXReport), NULL}, /*40*/
 139         {GEN_CMD_CODE(_GetDTMReport),   NULL},
 140         {GEN_CMD_CODE(_GetTXRateStatistics), NULL},
 141         {GEN_CMD_CODE(_SetUsbSuspend), NULL},
 142         {GEN_CMD_CODE(_SetH2cLbk), NULL},
 143         {GEN_CMD_CODE(_AddBAReq), NULL}, /*45*/
 144         {GEN_CMD_CODE(_SetChannel), NULL},              /*46*/
 145         {GEN_CMD_CODE(_SetTxPower), NULL},
 146         {GEN_CMD_CODE(_SwitchAntenna), NULL},
 147         {GEN_CMD_CODE(_SetCrystalCap), NULL},
 148         {GEN_CMD_CODE(_SetSingleCarrierTx), NULL},      /*50*/
 149
 150         {GEN_CMD_CODE(_SetSingleToneTx), NULL}, /*51*/
 151         {GEN_CMD_CODE(_SetCarrierSuppressionTx), NULL},
 152         {GEN_CMD_CODE(_SetContinuousTx), NULL},
 153         {GEN_CMD_CODE(_SwitchBandwidth), NULL},         /*54*/
 154         {GEN_CMD_CODE(_TX_Beacon), NULL},/*55*/
 155
 156         {GEN_CMD_CODE(_Set_MLME_EVT), NULL},/*56*/
 157         {GEN_CMD_CODE(_Set_Drv_Extra), NULL},/*57*/
 158         {GEN_CMD_CODE(_Set_H2C_MSG), NULL},/*58*/
 159         {GEN_CMD_CODE(_SetChannelPlan), NULL},/*59*/
 160         {GEN_CMD_CODE(_LedBlink), NULL},/*60*/
 161
 162         {GEN_CMD_CODE(_SetChannelSwitch), NULL},/*61*/
 163         {GEN_CMD_CODE(_TDLS), NULL},/*62*/
 164 };
 165
 166 /*
 167 Caller and the rtw_cmd_thread23a can protect cmd_q by spin_lock.
 168 No irqsave is necessary.
 169 */
 170
 171 int rtw_init_cmd_priv23a(struct cmd_priv *pcmdpriv)
 172 {
 173         int res = _SUCCESS;
 174
 175         pcmdpriv->cmd_issued_cnt = 0;
 176         pcmdpriv->cmd_done_cnt = 0;
 177         pcmdpriv->rsp_cnt = 0;
 178
 179         pcmdpriv->wq = alloc_workqueue("rtl8723au_cmd", 0, 1);
 180         if (!pcmdpriv->wq)
 181                 res = _FAIL;
 182
 183         return res;
 184 }
 185
 186 /* forward definition */
 187
 188 static void rtw_irq_work(struct work_struct *work);
 189
 190 u32 rtw_init_evt_priv23a(struct evt_priv *pevtpriv)
 191 {
 192         pevtpriv->wq = alloc_workqueue("rtl8723au_evt", 0, 1);
 193
 194         INIT_WORK(&pevtpriv->irq_wk, rtw_irq_work);
 195
 196         return _SUCCESS;
 197 }
 198
 199 void rtw_free_evt_priv23a(struct evt_priv *pevtpriv)
 200 {
 201         cancel_work_sync(&pevtpriv->irq_wk);
 202 }
 203
 204 static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 205 {
 206         /* set to true to allow enqueuing cmd when hw_init_completed is false */
 207         u8 bAllow = false;
 208
 209         if (cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
 210                 bAllow = true;
 211
 212         if (pcmdpriv->padapter->hw_init_completed == false && bAllow == false)
 213                 return _FAIL;
 214         return _SUCCESS;
 215 }
 216
 217 static void rtw_cmd_work(struct work_struct *work);
 218
 219 int rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 220 {
 221         int res = _FAIL;
 222
 223         if (!cmd_obj)
 224                 goto exit;
 225
 226         cmd_obj->padapter = pcmdpriv->padapter;
 227
 228         res = rtw_cmd_filter(pcmdpriv, cmd_obj);
 229         if (res == _FAIL) {
 230                 rtw_free_cmd_obj23a(cmd_obj);
 231                 goto exit;
 232         }
 233
 234         INIT_WORK(&cmd_obj->work, rtw_cmd_work);
 235
 236         res = queue_work(pcmdpriv->wq, &cmd_obj->work);
 237
 238         if (!res) {
 239                 printk(KERN_ERR "%s: Call to queue_work() failed\n", __func__);
 240                 res = _FAIL;
 241         } else
 242                 res = _SUCCESS;
 243 exit:
 244
 245         return res;
 246 }
 247
 248 void rtw_cmd_clr_isr23a(struct  cmd_priv *pcmdpriv)
 249 {
 250         pcmdpriv->cmd_done_cnt++;
 251 }
 252
 253 void rtw_free_cmd_obj23a(struct cmd_obj *pcmd)
 254 {
 255
 256         if (pcmd->cmdcode != _JoinBss_CMD_ &&
 257             pcmd->cmdcode != _CreateBss_CMD_) {
 258                 /* free parmbuf in cmd_obj */
 259                 kfree(pcmd->parmbuf);
 260         }
 261
 262         if (pcmd->rsp) {
 263                 if (pcmd->rspsz != 0) {
 264                         /* free rsp in cmd_obj */
 265                         kfree(pcmd->rsp);
 266                 }
 267         }
 268
 269         kfree(pcmd);
 270 }
 271
 272 static void rtw_cmd_work(struct work_struct *work)
 273 {
 274         int (*cmd_hdl)(struct rtw_adapter *padapter, const u8 *pbuf);
 275         void (*pcmd_callback)(struct rtw_adapter *dev, struct cmd_obj *pcmd);
 276         struct cmd_priv *pcmdpriv;
 277         struct cmd_obj *pcmd = container_of(work, struct cmd_obj, work);
 278
 279         pcmdpriv = &pcmd->padapter->cmdpriv;
 280
 281         if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) {
 282                 pcmd->res = H2C_DROPPED;
 283                 goto post_process;
 284         }
 285
 286         pcmdpriv->cmd_issued_cnt++;
 287
 288         pcmd->cmdsz = ALIGN(pcmd->cmdsz, 4);
 289
 290         if (pcmd->cmdcode < (sizeof(wlancmds)/sizeof(struct cmd_hdl))) {
 291                 cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
 292
 293                 if (cmd_hdl)
 294                         pcmd->res = cmd_hdl(pcmd->padapter, pcmd->parmbuf);
 295                 else
 296                         pcmd->res = H2C_DROPPED;
 297         } else
 298                 pcmd->res = H2C_PARAMETERS_ERROR;
 299
 300 post_process:
 301         /* call callback function for post-processed */
 302         if (pcmd->cmdcode < (sizeof(rtw_cmd_callback) /
 303                              sizeof(struct _cmd_callback))) {
 304                 pcmd_callback = rtw_cmd_callback[pcmd->cmdcode].callback;
 305                 if (!pcmd_callback) {
 306                         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
 307                                  ("mlme_cmd_hdl(): pcmd_callback = 0x%p, "
 308                                   "cmdcode = 0x%x\n",
 309                                   pcmd_callback, pcmd->cmdcode));
 310                         rtw_free_cmd_obj23a(pcmd);
 311                 } else {
 312                         /* need consider that free cmd_obj in
 313                            rtw_cmd_callback */
 314                         pcmd_callback(pcmd->padapter, pcmd);
 315                 }
 316         } else {
 317                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
 318                          ("%s: cmdcode = 0x%x callback not defined!\n",
 319                           __func__, pcmd->cmdcode));
 320                 rtw_free_cmd_obj23a(pcmd);
 321         }
 322 }
 323
 324
 325 int rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 326                           struct cfg80211_ssid *ssid, int ssid_num,
 327                           struct rtw_ieee80211_channel *ch, int ch_num)
 328 {
 329         int res = _FAIL;
 330         struct cmd_obj *ph2c;
 331         struct sitesurvey_parm *psurveyPara;
 332         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 333         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 334
 335         if (check_fwstate(pmlmepriv, _FW_LINKED))
 336                 rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_SCAN, 1);
 337
 338         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 339         if (!ph2c)
 340                 return _FAIL;
 341
 342         psurveyPara = kzalloc(sizeof(struct sitesurvey_parm), GFP_ATOMIC);
 343         if (!psurveyPara) {
 344                 kfree(ph2c);
 345                 return _FAIL;
 346         }
 347
 348         rtw_free_network_queue23a(padapter);
 349
 350         RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
 351                  ("%s: flush network queue\n", __func__));
 352
 353         init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara,
 354                                    GEN_CMD_CODE(_SiteSurvey));
 355
 356         /* psurveyPara->bsslimit = 48; */
 357         psurveyPara->scan_mode = pmlmepriv->scan_mode;
 358
 359         /* prepare ssid list */
 360         if (ssid) {
 361                 int i;
 362
 363                 for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) {
 364                         if (ssid[i].ssid_len) {
 365                                 memcpy(&psurveyPara->ssid[i], &ssid[i],
 366                                        sizeof(struct cfg80211_ssid));
 367                                 psurveyPara->ssid_num++;
 368                         }
 369                 }
 370         }
 371
 372         /* prepare channel list */
 373         if (ch) {
 374                 int i;
 375
 376                 for (i = 0; i < ch_num && i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
 377                         if (ch[i].hw_value &&
 378                             !(ch[i].flags & IEEE80211_CHAN_DISABLED)) {
 379                                 memcpy(&psurveyPara->ch[i], &ch[i],
 380                                        sizeof(struct rtw_ieee80211_channel));
 381                                 psurveyPara->ch_num++;
 382                         }
 383                 }
 384         }
 385
 386         set_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
 387
 388         res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 389
 390         if (res == _SUCCESS) {
 391                 mod_timer(&pmlmepriv->scan_to_timer, jiffies +
 392                           msecs_to_jiffies(SCANNING_TIMEOUT));
 393
 394                 pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
 395         } else
 396                 _clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 397
 398         return res;
 399 }
 400
 401 void rtw_getbbrfreg_cmdrsp_callback23a(struct rtw_adapter *padapter,
 402                                        struct cmd_obj *pcmd)
 403 {
 404         kfree(pcmd->parmbuf);
 405         kfree(pcmd);
 406 }
 407
 408 int rtw_createbss_cmd23a(struct rtw_adapter  *padapter)
 409 {
 410         struct cmd_obj *pcmd;
 411         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 412         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 413         struct wlan_bssid_ex *pdev_network;
 414         u8 res = _SUCCESS;
 415
 416         pdev_network = &padapter->registrypriv.dev_network;
 417
 418         if (pmlmepriv->assoc_ssid.ssid_len == 0) {
 419                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
 420                          (" createbss for Any SSid:%s\n",
 421                           pmlmepriv->assoc_ssid.ssid));
 422         } else {
 423                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
 424                          (" createbss for SSid:%s\n",
 425                           pmlmepriv->assoc_ssid.ssid));
 426         }
 427
 428         pcmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 429         if (!pcmd) {
 430                 res = _FAIL;
 431                 goto exit;
 432         }
 433
 434         pcmd->cmdcode = _CreateBss_CMD_;
 435         pcmd->parmbuf = (unsigned char *)pdev_network;
 436         pcmd->cmdsz = get_wlan_bssid_ex_sz(pdev_network);
 437         pcmd->rsp = NULL;
 438         pcmd->rspsz = 0;
 439
 440         pdev_network->Length = pcmd->cmdsz;
 441
 442         res = rtw_enqueue_cmd23a(pcmdpriv, pcmd);
 443
 444 exit:
 445
 446         return res;
 447 }
 448
 449 int rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 450                        struct wlan_network *pnetwork)
 451 {
 452         int res = _SUCCESS;
 453         struct wlan_bssid_ex *psecnetwork;
 454         struct cmd_obj *pcmd;
 455         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 456         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 457         struct security_priv *psecuritypriv = &padapter->securitypriv;
 458         struct registry_priv *pregistrypriv = &padapter->registrypriv;
 459         struct ht_priv *phtpriv = &pmlmepriv->htpriv;
 460         enum nl80211_iftype ifmode;
 461         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 462         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 463
 464         ifmode = pnetwork->network.ifmode;
 465
 466         if (pmlmepriv->assoc_ssid.ssid_len == 0) {
 467                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
 468                          ("+Join cmd: Any SSid\n"));
 469         } else {
 470                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_,
 471                          ("+Join cmd: SSid =[%s]\n",
 472                           pmlmepriv->assoc_ssid.ssid));
 473         }
 474
 475         pcmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 476         if (!pcmd) {
 477                 res = _FAIL;
 478                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
 479                          ("rtw_joinbss_cmd23a: memory allocate for cmd_obj "
 480                           "fail!!!\n"));
 481                 goto exit;
 482         }
 483
 484         /* for hidden ap to set fw_state here */
 485         if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE)) {
 486                 switch (ifmode) {
 487                 case NL80211_IFTYPE_ADHOC:
 488                         set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
 489                         break;
 490                 case NL80211_IFTYPE_P2P_CLIENT:
 491                 case NL80211_IFTYPE_STATION:
 492                         set_fwstate(pmlmepriv, WIFI_STATION_STATE);
 493                         break;
 494                 default:
 495                         break;
 496                 }
 497         }
 498
 499         psecnetwork = &psecuritypriv->sec_bss;
 500         if (!psecnetwork) {
 501                 kfree(pcmd);
 502                 res = _FAIL;
 503
 504                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
 505                          ("rtw_joinbss_cmd23a :psecnetwork == NULL!!!\n"));
 506
 507                 goto exit;
 508         }
 509
 510         memset(psecnetwork, 0, sizeof(struct wlan_bssid_ex));
 511
 512         memcpy(psecnetwork, &pnetwork->network,
 513                get_wlan_bssid_ex_sz(&pnetwork->network));
 514
 515         psecnetwork->IELength = 0;
 516         /*  Added by Albert 2009/02/18 */
 517         /*  If the the driver wants to use the bssid to create the
 518          *  connection. If not,  we have to copy the connecting AP's
 519          *  MAC address to it so that the driver just has the bssid
 520          *  information for PMKIDList searching. */
 521
 522         if (pmlmepriv->assoc_by_bssid == false)
 523                 ether_addr_copy(&pmlmepriv->assoc_bssid[0],
 524                                 &pnetwork->network.MacAddress[0]);
 525
 526         psecnetwork->IELength =
 527                 rtw_restruct_sec_ie23a(padapter, &pnetwork->network.IEs[0],
 528                                        &psecnetwork->IEs[0],
 529                                        pnetwork->network.IELength);
 530
 531         pmlmepriv->qos_option = 0;
 532
 533         if (pregistrypriv->wmm_enable) {
 534                 u32 tmp_len;
 535
 536                 tmp_len = rtw_restruct_wmm_ie23a(padapter,
 537                                                  &pnetwork->network.IEs[0],
 538                                                  &psecnetwork->IEs[0],
 539                                                  pnetwork->network.IELength,
 540                                                  psecnetwork->IELength);
 541
 542                 if (psecnetwork->IELength != tmp_len) {
 543                         psecnetwork->IELength = tmp_len;
 544                         /* There is WMM IE in this corresp. beacon */
 545                         pmlmepriv->qos_option = 1;
 546                 } else {
 547                         /* There is no WMM IE in this corresp. beacon */
 548                         pmlmepriv->qos_option = 0;
 549                 }
 550         }
 551
 552         phtpriv->ht_option = false;
 553         if (pregistrypriv->ht_enable) {
 554                 u32 algo = padapter->securitypriv.dot11PrivacyAlgrthm;
 555                 /*      Added by Albert 2010/06/23 */
 556                 /*      For the WEP mode, we will use the bg mode to do
 557                         the connection to avoid some IOT issue. */
 558                 /*      Especially for Realtek 8192u SoftAP. */
 559                 if (algo != WLAN_CIPHER_SUITE_WEP40 &&
 560                     algo != WLAN_CIPHER_SUITE_WEP104 &&
 561                     algo != WLAN_CIPHER_SUITE_TKIP) {
 562                         /* rtw_restructure_ht_ie23a */
 563                         rtw_restructure_ht_ie23a(padapter,
 564                                                  &pnetwork->network.IEs[0],
 565                                                  &psecnetwork->IEs[0],
 566                                                  pnetwork->network.IELength,
 567                                                  &psecnetwork->IELength);
 568                 }
 569         }
 570
 571         pmlmeinfo->assoc_AP_vendor =
 572                 check_assoc_AP23a(pnetwork->network.IEs,
 573                                   pnetwork->network.IELength);
 574
 575         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_TENDA)
 576                 padapter->pwrctrlpriv.smart_ps = 0;
 577         else
 578                 padapter->pwrctrlpriv.smart_ps =
 579                         padapter->registrypriv.smart_ps;
 580
 581         DBG_8723A("%s: smart_ps =%d\n", __func__,
 582                   padapter->pwrctrlpriv.smart_ps);
 583
 584         /* get cmdsz before endian conversion */
 585         pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork);
 586
 587         pcmd->cmdcode = _JoinBss_CMD_;/* GEN_CMD_CODE(_JoinBss) */
 588         pcmd->parmbuf = (unsigned char *)psecnetwork;
 589         pcmd->rsp = NULL;
 590         pcmd->rspsz = 0;
 591
 592         res = rtw_enqueue_cmd23a(pcmdpriv, pcmd);
 593 exit:
 594
 595         return res;
 596 }
 597
 598 int rtw_disassoc_cmd23a(struct rtw_adapter *padapter, u32 deauth_timeout_ms,
 599                         bool enqueue)
 600 {
 601         struct cmd_obj *cmdobj = NULL;
 602         struct disconnect_parm *param = NULL;
 603         struct cmd_priv *cmdpriv = &padapter->cmdpriv;
 604         int res = _SUCCESS;
 605
 606         RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_,
 607                  ("+rtw_disassoc_cmd23a\n"));
 608
 609         /* prepare cmd parameter */
 610         param = kzalloc(sizeof(*param), GFP_ATOMIC);
 611         if (param == NULL) {
 612                 res = _FAIL;
 613                 goto exit;
 614         }
 615         param->deauth_timeout_ms = deauth_timeout_ms;
 616
 617         if (enqueue) {
 618                 /* need enqueue, prepare cmd_obj and enqueue */
 619                 cmdobj = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 620                 if (!cmdobj) {
 621                         res = _FAIL;
 622                         kfree(param);
 623                         goto exit;
 624                 }
 625                 init_h2fwcmd_w_parm_no_rsp(cmdobj, param, _DisConnect_CMD_);
 626                 res = rtw_enqueue_cmd23a(cmdpriv, cmdobj);
 627         } else {
 628                 /* no need to enqueue, do the cmd hdl directly and
 629                    free cmd parameter */
 630                 if (H2C_SUCCESS != disconnect_hdl23a(padapter, (u8 *)param))
 631                         res = _FAIL;
 632                 kfree(param);
 633         }
 634
 635 exit:
 636         return res;
 637 }
 638
 639 int rtw_setopmode_cmd23a(struct rtw_adapter *padapter,
 640                          enum nl80211_iftype ifmode)
 641 {
 642         struct  cmd_obj *ph2c;
 643         struct  setopmode_parm *psetop;
 644         struct  cmd_priv   *pcmdpriv = &padapter->cmdpriv;
 645         int res = _SUCCESS;
 646
 647         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 648         if (!ph2c) {
 649                 res = false;
 650                 goto exit;
 651         }
 652         psetop = kzalloc(sizeof(struct setopmode_parm), GFP_KERNEL);
 653
 654         if (!psetop) {
 655                 kfree(ph2c);
 656                 res = false;
 657                 goto exit;
 658         }
 659
 660         init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
 661         psetop->mode = ifmode;
 662
 663         res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 664 exit:
 665         return res;
 666 }
 667
 668 int rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
 669 {
 670         struct cmd_obj *ph2c;
 671         struct set_stakey_parm *psetstakey_para;
 672         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 673         struct set_stakey_rsp *psetstakey_rsp = NULL;
 674         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 675         struct security_priv *psecuritypriv = &padapter->securitypriv;
 676         struct sta_info *sta = (struct sta_info *)psta;
 677         int res = _SUCCESS;
 678
 679         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 680         if (!ph2c) {
 681                 res = _FAIL;
 682                 goto exit;
 683         }
 684
 685         psetstakey_para = kzalloc(sizeof(struct set_stakey_parm), GFP_KERNEL);
 686         if (!psetstakey_para) {
 687                 kfree(ph2c);
 688                 res = _FAIL;
 689                 goto exit;
 690         }
 691
 692         psetstakey_rsp = kzalloc(sizeof(struct set_stakey_rsp), GFP_KERNEL);
 693         if (!psetstakey_rsp) {
 694                 kfree(ph2c);
 695                 kfree(psetstakey_para);
 696                 res = _FAIL;
 697                 goto exit;
 698         }
 699
 700         init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
 701         ph2c->rsp = (u8 *) psetstakey_rsp;
 702         ph2c->rspsz = sizeof(struct set_stakey_rsp);
 703
 704         ether_addr_copy(psetstakey_para->addr, sta->hwaddr);
 705
 706         if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
 707                 psetstakey_para->algorithm =
 708                         (unsigned char)psecuritypriv->dot11PrivacyAlgrthm;
 709         } else {
 710                 GET_ENCRY_ALGO(psecuritypriv, sta, psetstakey_para->algorithm,
 711                                false);
 712         }
 713
 714         if (unicast_key == true) {
 715                 memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
 716         } else {
 717                 int idx = psecuritypriv->dot118021XGrpKeyid;
 718
 719                 memcpy(&psetstakey_para->key,
 720                        &psecuritypriv->dot118021XGrpKey[idx].skey, 16);
 721         }
 722
 723         /* jeff: set this because at least sw key is ready */
 724         padapter->securitypriv.busetkipkey = 1;
 725
 726         res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 727
 728 exit:
 729
 730         return res;
 731 }
 732
 733 int rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry,
 734                            u8 enqueue)
 735 {
 736         struct cmd_obj *ph2c;
 737         struct set_stakey_parm *psetstakey_para;
 738         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 739         struct set_stakey_rsp *psetstakey_rsp = NULL;
 740         struct sta_info *sta = (struct sta_info *)psta;
 741         int res = _SUCCESS;
 742
 743         if (!enqueue) {
 744                 clear_cam_entry23a(padapter, entry);
 745         } else {
 746                 ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 747                 if (!ph2c) {
 748                         res = _FAIL;
 749                         goto exit;
 750                 }
 751
 752                 psetstakey_para = kzalloc(sizeof(struct set_stakey_parm),
 753                                           GFP_KERNEL);
 754                 if (!psetstakey_para) {
 755                         kfree(ph2c);
 756                         res = _FAIL;
 757                         goto exit;
 758                 }
 759
 760                 psetstakey_rsp = kzalloc(sizeof(struct set_stakey_rsp),
 761                                          GFP_KERNEL);
 762                 if (!psetstakey_rsp) {
 763                         kfree(ph2c);
 764                         kfree(psetstakey_para);
 765                         res = _FAIL;
 766                         goto exit;
 767                 }
 768
 769                 init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para,
 770                                            _SetStaKey_CMD_);
 771                 ph2c->rsp = (u8 *) psetstakey_rsp;
 772                 ph2c->rspsz = sizeof(struct set_stakey_rsp);
 773
 774                 ether_addr_copy(psetstakey_para->addr, sta->hwaddr);
 775
 776                 psetstakey_para->algorithm = 0;
 777
 778                 psetstakey_para->id = entry;
 779
 780                 res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 781         }
 782 exit:
 783         return res;
 784 }
 785
 786 int rtw_addbareq_cmd23a(struct rtw_adapter *padapter, u8 tid, u8 *addr)
 787 {
 788         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 789         struct cmd_obj *ph2c;
 790         struct addBaReq_parm *paddbareq_parm;
 791         int res = _SUCCESS;
 792
 793         if (tid >= MAXTID) {
 794                 res = _FAIL;
 795                 goto exit;
 796         }
 797
 798         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 799         if (!ph2c) {
 800                 res = _FAIL;
 801                 goto exit;
 802         }
 803
 804         paddbareq_parm = kzalloc(sizeof(struct addBaReq_parm), GFP_ATOMIC);
 805         if (!paddbareq_parm) {
 806                 kfree(ph2c);
 807                 res = _FAIL;
 808                 goto exit;
 809         }
 810
 811         paddbareq_parm->tid = tid;
 812         ether_addr_copy(paddbareq_parm->addr, addr);
 813
 814         init_h2fwcmd_w_parm_no_rsp(ph2c, paddbareq_parm,
 815                                    GEN_CMD_CODE(_AddBAReq));
 816
 817         res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 818 exit:
 819         return res;
 820 }
 821
 822 int rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter *padapter)
 823 {
 824         struct cmd_obj *ph2c;
 825         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 826         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 827         int res = _SUCCESS;
 828
 829         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 830         if (!ph2c) {
 831                 res = _FAIL;
 832                 goto exit;
 833         }
 834
 835         pdrvextra_cmd_parm = kzalloc(sizeof(*pdrvextra_cmd_parm), GFP_ATOMIC);
 836         if (!pdrvextra_cmd_parm) {
 837                 kfree(ph2c);
 838                 res = _FAIL;
 839                 goto exit;
 840         }
 841
 842         pdrvextra_cmd_parm->ec_id = DYNAMIC_CHK_WK_CID;
 843         pdrvextra_cmd_parm->type_size = 0;
 844         pdrvextra_cmd_parm->pbuf = (u8 *)padapter;
 845
 846         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm,
 847                                    GEN_CMD_CODE(_Set_Drv_Extra));
 848
 849         res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 850 exit:
 851
 852         return res;
 853 }
 854
 855 /*
 856  * This is only ever called from on_action_spct23a_ch_switch () which isn't
 857  * called from anywhere itself
 858  */
 859 int rtw_set_ch_cmd23a(struct rtw_adapter *padapter, u8 ch, u8 bw, u8 ch_offset,
 860                       u8 enqueue)
 861 {
 862         struct cmd_obj *pcmdobj;
 863         struct set_ch_parm *set_ch_parm;
 864         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 865         int res = _SUCCESS;
 866
 867         DBG_8723A("%s(%s): ch:%u, bw:%u, ch_offset:%u\n", __func__,
 868                   padapter->pnetdev->name, ch, bw, ch_offset);
 869
 870         /* check input parameter */
 871
 872         /* prepare cmd parameter */
 873         set_ch_parm = kzalloc(sizeof(*set_ch_parm), GFP_KERNEL);
 874         if (!set_ch_parm) {
 875                 res = _FAIL;
 876                 goto exit;
 877         }
 878         set_ch_parm->ch = ch;
 879         set_ch_parm->bw = bw;
 880         set_ch_parm->ch_offset = ch_offset;
 881
 882         if (enqueue) {
 883                 /* need enqueue, prepare cmd_obj and enqueue */
 884                 pcmdobj = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 885                 if (!pcmdobj) {
 886                         kfree(set_ch_parm);
 887                         res = _FAIL;
 888                         goto exit;
 889                 }
 890
 891                 init_h2fwcmd_w_parm_no_rsp(pcmdobj, set_ch_parm,
 892                                            GEN_CMD_CODE(_SetChannel));
 893                 res = rtw_enqueue_cmd23a(pcmdpriv, pcmdobj);
 894         } else {
 895                 /* no need to enqueue, do the cmd hdl directly and
 896                    free cmd parameter */
 897                 if (H2C_SUCCESS != set_ch_hdl23a(padapter, (u8 *)set_ch_parm))
 898                         res = _FAIL;
 899
 900                 kfree(set_ch_parm);
 901         }
 902
 903         /* do something based on res... */
 904 exit:
 905
 906         DBG_8723A("%s(%s): res:%u\n", __func__, padapter->pnetdev->name, res);
 907
 908         return res;
 909 }
 910
 911 static void traffic_status_watchdog(struct rtw_adapter *padapter)
 912 {
 913         u8 bEnterPS;
 914         u8 bBusyTraffic = false, bTxBusyTraffic = false, bRxBusyTraffic = false;
 915         u8 bHigherBusyTraffic = false, bHigherBusyRxTraffic = false;
 916         u8 bHigherBusyTxTraffic = false;
 917         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 918         int BusyThreshold = 100;
 919         struct rt_link_detect *ldi = &pmlmepriv->LinkDetectInfo;
 920
 921         /*  */
 922         /*  Determine if our traffic is busy now */
 923         /*  */
 924         if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 925                 if (rtl8723a_BT_coexist(padapter))
 926                         BusyThreshold = 50;
 927                 else if (ldi->bBusyTraffic)
 928                         BusyThreshold = 75;
 929                 /*  if we raise bBusyTraffic in last watchdog, using
 930                     lower threshold. */
 931                 if (ldi->NumRxOkInPeriod > BusyThreshold ||
 932                     ldi->NumTxOkInPeriod > BusyThreshold) {
 933                         bBusyTraffic = true;
 934
 935                         if (ldi->NumRxOkInPeriod > ldi->NumTxOkInPeriod)
 936                                 bRxBusyTraffic = true;
 937                         else
 938                                 bTxBusyTraffic = true;
 939                 }
 940
 941                 /*  Higher Tx/Rx data. */
 942                 if (ldi->NumRxOkInPeriod > 4000 ||
 943                     ldi->NumTxOkInPeriod > 4000) {
 944                         bHigherBusyTraffic = true;
 945
 946                         if (ldi->NumRxOkInPeriod > ldi->NumTxOkInPeriod)
 947                                 bHigherBusyRxTraffic = true;
 948                         else
 949                                 bHigherBusyTxTraffic = true;
 950                 }
 951
 952                 if (!rtl8723a_BT_coexist(padapter) ||
 953                     !rtl8723a_BT_using_antenna_1(padapter)) {
 954                 /*  check traffic for  powersaving. */
 955                         if (((ldi->NumRxUnicastOkInPeriod +
 956                               ldi->NumTxOkInPeriod) > 8) ||
 957                             ldi->NumRxUnicastOkInPeriod > 2)
 958                                 bEnterPS = false;
 959                         else
 960                                 bEnterPS = true;
 961
 962                         /*  LeisurePS only work in infra mode. */
 963                         if (bEnterPS)
 964                                 LPS_Enter23a(padapter);
 965                         else
 966                                 LPS_Leave23a(padapter);
 967                 }
 968         } else
 969                 LPS_Leave23a(padapter);
 970
 971         ldi->NumRxOkInPeriod = 0;
 972         ldi->NumTxOkInPeriod = 0;
 973         ldi->NumRxUnicastOkInPeriod = 0;
 974         ldi->bBusyTraffic = bBusyTraffic;
 975         ldi->bTxBusyTraffic = bTxBusyTraffic;
 976         ldi->bRxBusyTraffic = bRxBusyTraffic;
 977         ldi->bHigherBusyTraffic = bHigherBusyTraffic;
 978         ldi->bHigherBusyRxTraffic = bHigherBusyRxTraffic;
 979         ldi->bHigherBusyTxTraffic = bHigherBusyTxTraffic;
 980 }
 981
 982 static void dynamic_chk_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz)
 983 {
 984         struct mlme_priv *pmlmepriv;
 985
 986         padapter = (struct rtw_adapter *)pbuf;
 987         pmlmepriv = &padapter->mlmepriv;
 988
 989 #ifdef CONFIG_8723AU_AP_MODE
 990         if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 991                 expire_timeout_chk23a(padapter);
 992 #endif
 993
 994         rtl8723a_sreset_xmit_status_check(padapter);
 995
 996         linked_status_chk23a(padapter);
 997         traffic_status_watchdog(padapter);
 998
 999         rtl8723a_HalDmWatchDog(padapter);
1000
1001         /*  */
1002         /*  BT-Coexist */
1003         /*  */
1004         rtl8723a_BT_do_coexist(padapter);
1005 }
1006
1007 static void lps_ctrl_wk_hdl(struct rtw_adapter *padapter, u8 lps_ctrl_type)
1008 {
1009         struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
1010         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1011         u8 mstatus;
1012
1013         if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
1014             check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
1015                 return;
1016
1017         switch (lps_ctrl_type) {
1018         case LPS_CTRL_SCAN:
1019                 rtl8723a_BT_wifiscan_notify(padapter, true);
1020                 if (!rtl8723a_BT_using_antenna_1(padapter)) {
1021                         if (check_fwstate(pmlmepriv, _FW_LINKED))
1022                                 LPS_Leave23a(padapter);
1023                         }
1024                 break;
1025         case LPS_CTRL_JOINBSS:
1026                 LPS_Leave23a(padapter);
1027                 break;
1028         case LPS_CTRL_CONNECT:
1029                 mstatus = 1;/* connect */
1030                 /*  Reset LPS Setting */
1031                 padapter->pwrctrlpriv.LpsIdleCount = 0;
1032                 rtl8723a_set_FwJoinBssReport_cmd(padapter, 1);
1033                 rtl8723a_BT_mediastatus_notify(padapter, mstatus);
1034                 break;
1035         case LPS_CTRL_DISCONNECT:
1036                 mstatus = 0;/* disconnect */
1037                 rtl8723a_BT_mediastatus_notify(padapter, mstatus);
1038                 if (!rtl8723a_BT_using_antenna_1(padapter))
1039                         LPS_Leave23a(padapter);
1040                 rtl8723a_set_FwJoinBssReport_cmd(padapter, 0);
1041                 break;
1042         case LPS_CTRL_SPECIAL_PACKET:
1043                 pwrpriv->DelayLPSLastTimeStamp = jiffies;
1044                 rtl8723a_BT_specialpacket_notify(padapter);
1045                 if (!rtl8723a_BT_using_antenna_1(padapter))
1046                         LPS_Leave23a(padapter);
1047                 break;
1048         case LPS_CTRL_LEAVE:
1049                 rtl8723a_BT_lps_leave(padapter);
1050                 if (!rtl8723a_BT_using_antenna_1(padapter))
1051                         LPS_Leave23a(padapter);
1052                 break;
1053
1054         default:
1055                 break;
1056         }
1057 }
1058
1059 int rtw_lps_ctrl_wk_cmd23a(struct rtw_adapter *padapter,
1060                            u8 lps_ctrl_type, u8 enqueue)
1061 {
1062         struct cmd_obj *ph2c;
1063         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
1064         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1065         int res = _SUCCESS;
1066
1067         if (enqueue) {
1068                 ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
1069                 if (!ph2c) {
1070                         res = _FAIL;
1071                         goto exit;
1072                 }
1073
1074                 pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm),
1075                                              GFP_ATOMIC);
1076                 if (!pdrvextra_cmd_parm) {
1077                         kfree(ph2c);
1078                         res = _FAIL;
1079                         goto exit;
1080                 }
1081
1082                 pdrvextra_cmd_parm->ec_id = LPS_CTRL_WK_CID;
1083                 pdrvextra_cmd_parm->type_size = lps_ctrl_type;
1084                 pdrvextra_cmd_parm->pbuf = NULL;
1085
1086                 init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm,
1087                                            GEN_CMD_CODE(_Set_Drv_Extra));
1088
1089                 res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
1090         } else
1091                 lps_ctrl_wk_hdl(padapter, lps_ctrl_type);
1092 exit:
1093
1094         return res;
1095 }
1096
1097 int rtw_ps_cmd23a(struct rtw_adapter *padapter)
1098 {
1099         struct cmd_obj *ppscmd;
1100         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
1101         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1102         int res = _SUCCESS;
1103
1104         ppscmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
1105         if (!ppscmd) {
1106                 res = _FAIL;
1107                 goto exit;
1108         }
1109
1110         pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm),
1111                                      GFP_ATOMIC);
1112         if (!pdrvextra_cmd_parm) {
1113                 kfree(ppscmd);
1114                 res = _FAIL;
1115                 goto exit;
1116         }
1117
1118         pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID;
1119         pdrvextra_cmd_parm->pbuf = NULL;
1120         init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm,
1121                                    GEN_CMD_CODE(_Set_Drv_Extra));
1122
1123         res = rtw_enqueue_cmd23a(pcmdpriv, ppscmd);
1124 exit:
1125
1126         return res;
1127 }
1128
1129 #ifdef CONFIG_8723AU_AP_MODE
1130
1131 static void rtw_chk_hi_queue_hdl(struct rtw_adapter *padapter)
1132 {
1133         int cnt = 0;
1134         struct sta_info *psta_bmc;
1135         struct sta_priv *pstapriv = &padapter->stapriv;
1136
1137         psta_bmc = rtw_get_bcmc_stainfo23a(padapter);
1138         if (!psta_bmc)
1139                 return;
1140
1141         if (psta_bmc->sleepq_len == 0) {
1142                 bool val;
1143
1144                 val = rtl8723a_chk_hi_queue_empty(padapter);
1145
1146                 while (!val) {
1147                         msleep(100);
1148
1149                         cnt++;
1150
1151                         if (cnt > 10)
1152                                 break;
1153
1154                         val = rtl8723a_chk_hi_queue_empty(padapter);
1155                 }
1156
1157                 if (cnt <= 10) {
1158                         pstapriv->tim_bitmap &= ~BIT(0);
1159                         pstapriv->sta_dz_bitmap &= ~BIT(0);
1160
1161                         update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
1162                 } else /* re check again */
1163                         rtw_chk_hi_queue_cmd23a(padapter);
1164         }
1165 }
1166
1167 int rtw_chk_hi_queue_cmd23a(struct rtw_adapter *padapter)
1168 {
1169         struct cmd_obj *ph2c;
1170         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
1171         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1172         int res = _SUCCESS;
1173
1174         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
1175         if (!ph2c) {
1176                 res = _FAIL;
1177                 goto exit;
1178         }
1179
1180         pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm),
1181                                      GFP_ATOMIC);
1182         if (!pdrvextra_cmd_parm) {
1183                 kfree(ph2c);
1184                 res = _FAIL;
1185                 goto exit;
1186         }
1187
1188         pdrvextra_cmd_parm->ec_id = CHECK_HIQ_WK_CID;
1189         pdrvextra_cmd_parm->type_size = 0;
1190         pdrvextra_cmd_parm->pbuf = NULL;
1191
1192         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm,
1193                                    GEN_CMD_CODE(_Set_Drv_Extra));
1194
1195         res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
1196 exit:
1197
1198         return res;
1199 }
1200 #endif
1201
1202 int rtw_c2h_wk_cmd23a(struct rtw_adapter *padapter, u8 *c2h_evt)
1203 {
1204         struct cmd_obj *ph2c;
1205         struct drvextra_cmd_parm *pdrvextra_cmd_parm;
1206         struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
1207         int res = _SUCCESS;
1208
1209         ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
1210         if (!ph2c) {
1211                 res = _FAIL;
1212                 goto exit;
1213         }
1214
1215         pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm),
1216                                      GFP_ATOMIC);
1217         if (!pdrvextra_cmd_parm) {
1218                 kfree(ph2c);
1219                 res = _FAIL;
1220                 goto exit;
1221         }
1222
1223         pdrvextra_cmd_parm->ec_id = C2H_WK_CID;
1224         pdrvextra_cmd_parm->type_size = c2h_evt?16:0;
1225         pdrvextra_cmd_parm->pbuf = c2h_evt;
1226
1227         init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm,
1228                                    GEN_CMD_CODE(_Set_Drv_Extra));
1229
1230         res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
1231
1232 exit:
1233
1234         return res;
1235 }
1236
1237 static int c2h_evt_hdl(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt)
1238 {
1239         int ret = _FAIL;
1240         u8 buf[16];
1241
1242         if (!c2h_evt) {
1243                 /* No c2h event in cmd_obj, read c2h event before handling*/
1244                 if (c2h_evt_read23a(adapter, buf) == _SUCCESS) {
1245                         c2h_evt = (struct c2h_evt_hdr *)buf;
1246
1247                         ret = c2h_handler_8723a(adapter, c2h_evt);
1248                 }
1249         } else
1250                 ret = c2h_handler_8723a(adapter, c2h_evt);
1251
1252         return ret;
1253 }
1254
1255 static void rtw_irq_work(struct work_struct *work)
1256 {
1257         struct evt_priv *evtpriv;
1258         struct rtw_adapter *adapter;
1259
1260         evtpriv = container_of(work, struct evt_priv, irq_wk);
1261         adapter = container_of(evtpriv, struct rtw_adapter, evtpriv);
1262
1263         c2h_evt_clear23a(adapter);
1264 }
1265
1266 void rtw_evt_work(struct work_struct *work)
1267 {
1268         struct evt_work *ework;
1269         struct rtw_adapter *adapter;
1270
1271         ework = container_of(work, struct evt_work, work);
1272         adapter = ework->adapter;
1273
1274         c2h_evt_clear23a(adapter);
1275
1276         if (!c2h_evt_exist(&ework->u.c2h_evt)) {
1277                 kfree(ework);
1278                 return;
1279         }
1280
1281         if (c2h_id_filter_ccx_8723a(ework->u.c2h_evt.id) == true) {
1282                 /* Handle CCX report here */
1283                 c2h_handler_8723a(adapter, &ework->u.c2h_evt);
1284                 kfree(ework);
1285         } else {
1286                 /*
1287                  * Enqueue into cmd_thread for others.
1288                  * ework will be turned into a c2h_evt and freed once it
1289                  * has been consumed.
1290                  */
1291                 rtw_c2h_wk_cmd23a(adapter, (u8 *)&ework->u.c2h_evt);
1292         }
1293 }
1294
1295 int rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
1296 {
1297         const struct drvextra_cmd_parm *pdrvextra_cmd;
1298
1299         if (!pbuf)
1300                 return H2C_PARAMETERS_ERROR;
1301
1302         pdrvextra_cmd = (struct drvextra_cmd_parm *)pbuf;
1303
1304         switch (pdrvextra_cmd->ec_id) {
1305         case DYNAMIC_CHK_WK_CID:
1306                 dynamic_chk_wk_hdl(padapter, pdrvextra_cmd->pbuf,
1307                                    pdrvextra_cmd->type_size);
1308                 break;
1309         case POWER_SAVING_CTRL_WK_CID:
1310                 rtw_ps_processor23a(padapter);
1311                 break;
1312         case LPS_CTRL_WK_CID:
1313                 lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type_size);
1314                 break;
1315 #ifdef CONFIG_8723AU_AP_MODE
1316         case CHECK_HIQ_WK_CID:
1317                 rtw_chk_hi_queue_hdl(padapter);
1318                 break;
1319 #endif /* CONFIG_8723AU_AP_MODE */
1320         case C2H_WK_CID:
1321                 c2h_evt_hdl(padapter,
1322                             (struct c2h_evt_hdr *)pdrvextra_cmd->pbuf);
1323                 break;
1324
1325         default:
1326                 break;
1327         }
1328
1329         if (pdrvextra_cmd->pbuf && (pdrvextra_cmd->type_size > 0)) {
1330                 kfree(pdrvextra_cmd->pbuf);
1331                 /*
1332                  * No need to set pdrvextra_cmd->pbuf = NULL as we were
1333                  * operating on a copy of the original pcmd->parmbuf
1334                  * created in rtw_cmd_work().
1335                  */
1336         }
1337
1338         return H2C_SUCCESS;
1339 }
1340
1341 void rtw_survey_cmd_callback23a(struct rtw_adapter *padapter,
1342                                 struct cmd_obj *pcmd)
1343 {
1344         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
1345
1346         if (pcmd->res == H2C_DROPPED) {
1347                 /* TODO: cancel timer and do timeout handler directly... */
1348                 /* need to make timeout handlerOS independent */
1349                 mod_timer(&pmlmepriv->scan_to_timer,
1350                           jiffies + msecs_to_jiffies(1));
1351         } else if (pcmd->res != H2C_SUCCESS) {
1352                 mod_timer(&pmlmepriv->scan_to_timer,
1353                           jiffies + msecs_to_jiffies(1));
1354                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
1355                          ("\n ********Error: MgntActrtw_set_802_11_bssid23a_"
1356                           "LIST_SCAN Fail ************\n\n."));
1357         }
1358
1359         /*  free cmd */
1360         rtw_free_cmd_obj23a(pcmd);
1361 }
1362
1363 void rtw_disassoc_cmd23a_callback(struct rtw_adapter *padapter,
1364                                   struct cmd_obj *pcmd)
1365 {
1366         struct  mlme_priv *pmlmepriv = &padapter->mlmepriv;
1367
1368         if (pcmd->res != H2C_SUCCESS) {
1369                 spin_lock_bh(&pmlmepriv->lock);
1370                 set_fwstate(pmlmepriv, _FW_LINKED);
1371                 spin_unlock_bh(&pmlmepriv->lock);
1372                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
1373                          ("\n ***Error: disconnect_cmd_callback Fail ***\n."));
1374                 return;
1375         }
1376
1377         /*  free cmd */
1378         rtw_free_cmd_obj23a(pcmd);
1379 }
1380
1381 void rtw_joinbss_cmd23a_callback(struct rtw_adapter *padapter,
1382                                  struct cmd_obj *pcmd)
1383 {
1384         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1385
1386         if (pcmd->res == H2C_DROPPED) {
1387                 /* TODO: cancel timer and do timeout handler directly... */
1388                 /* need to make timeout handlerOS independent */
1389                 mod_timer(&pmlmepriv->assoc_timer,
1390                           jiffies + msecs_to_jiffies(1));
1391         } else if (pcmd->res != H2C_SUCCESS) {
1392                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
1393                          ("********Error:rtw_select_and_join_from_scanned_"
1394                           "queue Wait Sema  Fail ************\n"));
1395                 mod_timer(&pmlmepriv->assoc_timer,
1396                           jiffies + msecs_to_jiffies(1));
1397         }
1398
1399         rtw_free_cmd_obj23a(pcmd);
1400 }
1401
1402 void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
1403                                    struct cmd_obj *pcmd)
1404 {
1405         struct sta_info *psta;
1406         struct wlan_network *pwlan;
1407         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1408         struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)pcmd->parmbuf;
1409         struct wlan_network *tgt_network = &pmlmepriv->cur_network;
1410
1411         if (pcmd->res != H2C_SUCCESS) {
1412                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
1413                          ("\n ********Error: rtw_createbss_cmd23a_callback  "
1414                           "Fail ************\n\n."));
1415                 mod_timer(&pmlmepriv->assoc_timer,
1416                           jiffies + msecs_to_jiffies(1));
1417         }
1418
1419         del_timer_sync(&pmlmepriv->assoc_timer);
1420
1421         if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
1422                 psta = rtw_get_stainfo23a(&padapter->stapriv,
1423                                           pnetwork->MacAddress);
1424                 if (!psta) {
1425                         psta = rtw_alloc_stainfo23a(&padapter->stapriv,
1426                                                     pnetwork->MacAddress,
1427                                                     GFP_KERNEL);
1428                         if (!psta) {
1429                                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
1430                                          ("\nCan't alloc sta_info when "
1431                                           "createbss_cmd_callback\n"));
1432                                 goto createbss_cmd_fail ;
1433                         }
1434                 }
1435
1436                 spin_lock_bh(&pmlmepriv->lock);
1437                 rtw_indicate_connect23a(padapter);
1438                 spin_unlock_bh(&pmlmepriv->lock);
1439         } else {
1440                 pwlan = rtw_alloc_network(pmlmepriv, GFP_KERNEL);
1441                 spin_lock_bh(&pmlmepriv->scanned_queue.lock);
1442                 if (!pwlan) {
1443                         pwlan = rtw_get_oldest_wlan_network23a(&pmlmepriv->scanned_queue);
1444                         if (!pwlan) {
1445                                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
1446                                          ("\n Error:  can't get pwlan in "
1447                                           "rtw23a_joinbss_event_cb\n"));
1448                                 spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
1449                                 goto createbss_cmd_fail;
1450                         }
1451                         pwlan->last_scanned = jiffies;
1452                 } else {
1453                         list_add_tail(&pwlan->list,
1454                                       &pmlmepriv->scanned_queue.queue);
1455                 }
1456
1457                 pnetwork->Length = get_wlan_bssid_ex_sz(pnetwork);
1458                 memcpy(&pwlan->network, pnetwork, pnetwork->Length);
1459                 /* pwlan->fixed = true; */
1460
1461                 /* list_add_tail(&pwlan->list,
1462                    &pmlmepriv->scanned_queue.queue); */
1463
1464                 /*  copy pdev_network information to
1465                     pmlmepriv->cur_network */
1466                 memcpy(&tgt_network->network, pnetwork,
1467                        get_wlan_bssid_ex_sz(pnetwork));
1468
1469                 /*  reset DSConfig */
1470
1471                 clr_fwstate(pmlmepriv, _FW_UNDER_LINKING);
1472
1473                 spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
1474                 /*  we will set _FW_LINKED when there is one more sat to
1475                     join us (rtw_stassoc_event_callback23a) */
1476         }
1477
1478 createbss_cmd_fail:
1479
1480         rtw_free_cmd_obj23a(pcmd);
1481 }
1482
1483 void rtw_setstaKey_cmdrsp_callback23a(struct rtw_adapter *padapter,
1484                                       struct cmd_obj *pcmd)
1485 {
1486         struct sta_priv *pstapriv;
1487         struct set_stakey_rsp *psetstakey_rsp;
1488         struct sta_info *psta;
1489
1490         pstapriv = &padapter->stapriv;
1491         psetstakey_rsp = (struct set_stakey_rsp *) (pcmd->rsp);
1492         psta = rtw_get_stainfo23a(pstapriv, psetstakey_rsp->addr);
1493
1494         if (!psta) {
1495                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
1496                          ("\nERROR: rtw_setstaKey_cmdrsp_callback23a => "
1497                           "can't get sta_info\n\n"));
1498                 goto exit;
1499         }
1500
1501 exit:
1502
1503         rtw_free_cmd_obj23a(pcmd);
1504 }
1505
1506 void rtw_setassocsta_cmdrsp_callback23a(struct rtw_adapter *padapter,
1507                                         struct cmd_obj *pcmd)
1508 {
1509         struct sta_priv *pstapriv = &padapter->stapriv;
1510         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1511         struct set_assocsta_parm *passocsta_parm;
1512         struct set_assocsta_rsp *passocsta_rsp;
1513         struct sta_info *psta;
1514
1515         passocsta_parm = (struct set_assocsta_parm *)(pcmd->parmbuf);
1516         passocsta_rsp = (struct set_assocsta_rsp *) (pcmd->rsp);
1517         psta = rtw_get_stainfo23a(pstapriv, passocsta_parm->addr);
1518
1519         if (psta == NULL) {
1520                 RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
1521                          ("\nERROR: setassocsta_cmdrsp_callbac => can't "
1522                           "get sta_info\n\n"));
1523                 goto exit;
1524         }
1525
1526         psta->aid = psta->mac_id = passocsta_rsp->cam_id;
1527
1528         spin_lock_bh(&pmlmepriv->lock);
1529
1530         if (check_fwstate(pmlmepriv, WIFI_MP_STATE) &&
1531             check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
1532                 _clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
1533
1534         set_fwstate(pmlmepriv, _FW_LINKED);
1535         spin_unlock_bh(&pmlmepriv->lock);
1536
1537 exit:
1538         rtw_free_cmd_obj23a(pcmd);
1539 }