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