drivers/net/wireless/libertas/cmd.c

   1 /**
   2   * This file contains the handling of command.
   3   * It prepares command and sends it to firmware when it is ready.
   4   */
   5
   6 #include <net/iw_handler.h>
   7 #include <linux/kfifo.h>
   8 #include "host.h"
   9 #include "hostcmd.h"
  10 #include "decl.h"
  11 #include "defs.h"
  12 #include "dev.h"
  13 #include "assoc.h"
  14 #include "wext.h"
  15 #include "cmd.h"
  16
  17 static struct cmd_ctrl_node *lbs_get_cmd_ctrl_node(struct lbs_private *priv);
  18
  19
  20 /**
  21  *  @brief Simple callback that copies response back into command
  22  *
  23  *  @param priv         A pointer to struct lbs_private structure
  24  *  @param extra        A pointer to the original command structure for which
  25  *                      'resp' is a response
  26  *  @param resp         A pointer to the command response
  27  *
  28  *  @return             0 on success, error on failure
  29  */
  30 int lbs_cmd_copyback(struct lbs_private *priv, unsigned long extra,
  31                      struct cmd_header *resp)
  32 {
  33         struct cmd_header *buf = (void *)extra;
  34         uint16_t copy_len;
  35
  36         copy_len = min(le16_to_cpu(buf->size), le16_to_cpu(resp->size));
  37         memcpy(buf, resp, copy_len);
  38         return 0;
  39 }
  40 EXPORT_SYMBOL_GPL(lbs_cmd_copyback);
  41
  42 /**
  43  *  @brief Simple callback that ignores the result. Use this if
  44  *  you just want to send a command to the hardware, but don't
  45  *  care for the result.
  46  *
  47  *  @param priv         ignored
  48  *  @param extra        ignored
  49  *  @param resp         ignored
  50  *
  51  *  @return             0 for success
  52  */
  53 static int lbs_cmd_async_callback(struct lbs_private *priv, unsigned long extra,
  54                      struct cmd_header *resp)
  55 {
  56         return 0;
  57 }
  58
  59
  60 /**
  61  *  @brief Checks whether a command is allowed in Power Save mode
  62  *
  63  *  @param command the command ID
  64  *  @return        1 if allowed, 0 if not allowed
  65  */
  66 static u8 is_command_allowed_in_ps(u16 cmd)
  67 {
  68         switch (cmd) {
  69         case CMD_802_11_RSSI:
  70                 return 1;
  71         default:
  72                 break;
  73         }
  74         return 0;
  75 }
  76
  77 /**
  78  *  @brief Updates the hardware details like MAC address and regulatory region
  79  *
  80  *  @param priv         A pointer to struct lbs_private structure
  81  *
  82  *  @return             0 on success, error on failure
  83  */
  84 int lbs_update_hw_spec(struct lbs_private *priv)
  85 {
  86         struct cmd_ds_get_hw_spec cmd;
  87         int ret = -1;
  88         u32 i;
  89         DECLARE_MAC_BUF(mac);
  90
  91         lbs_deb_enter(LBS_DEB_CMD);
  92
  93         memset(&cmd, 0, sizeof(cmd));
  94         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
  95         memcpy(cmd.permanentaddr, priv->current_addr, ETH_ALEN);
  96         ret = lbs_cmd_with_response(priv, CMD_GET_HW_SPEC, &cmd);
  97         if (ret)
  98                 goto out;
  99
 100         priv->fwcapinfo = le32_to_cpu(cmd.fwcapinfo);
 101
 102         /* The firmware release is in an interesting format: the patch
 103          * level is in the most significant nibble ... so fix that: */
 104         priv->fwrelease = le32_to_cpu(cmd.fwrelease);
 105         priv->fwrelease = (priv->fwrelease << 8) |
 106                 (priv->fwrelease >> 24 & 0xff);
 107
 108         /* Some firmware capabilities:
 109          * CF card    firmware 5.0.16p0:   cap 0x00000303
 110          * USB dongle firmware 5.110.17p2: cap 0x00000303
 111          */
 112         printk("libertas: %s, fw %u.%u.%up%u, cap 0x%08x\n",
 113                 print_mac(mac, cmd.permanentaddr),
 114                 priv->fwrelease >> 24 & 0xff,
 115                 priv->fwrelease >> 16 & 0xff,
 116                 priv->fwrelease >>  8 & 0xff,
 117                 priv->fwrelease       & 0xff,
 118                 priv->fwcapinfo);
 119         lbs_deb_cmd("GET_HW_SPEC: hardware interface 0x%x, hardware spec 0x%04x\n",
 120                     cmd.hwifversion, cmd.version);
 121
 122         /* Clamp region code to 8-bit since FW spec indicates that it should
 123          * only ever be 8-bit, even though the field size is 16-bit.  Some firmware
 124          * returns non-zero high 8 bits here.
 125          */
 126         priv->regioncode = le16_to_cpu(cmd.regioncode) & 0xFF;
 127
 128         for (i = 0; i < MRVDRV_MAX_REGION_CODE; i++) {
 129                 /* use the region code to search for the index */
 130                 if (priv->regioncode == lbs_region_code_to_index[i])
 131                         break;
 132         }
 133
 134         /* if it's unidentified region code, use the default (USA) */
 135         if (i >= MRVDRV_MAX_REGION_CODE) {
 136                 priv->regioncode = 0x10;
 137                 lbs_pr_info("unidentified region code; using the default (USA)\n");
 138         }
 139
 140         if (priv->current_addr[0] == 0xff)
 141                 memmove(priv->current_addr, cmd.permanentaddr, ETH_ALEN);
 142
 143         memcpy(priv->dev->dev_addr, priv->current_addr, ETH_ALEN);
 144         if (priv->mesh_dev)
 145                 memcpy(priv->mesh_dev->dev_addr, priv->current_addr, ETH_ALEN);
 146
 147         if (lbs_set_regiontable(priv, priv->regioncode, 0)) {
 148                 ret = -1;
 149                 goto out;
 150         }
 151
 152         if (lbs_set_universaltable(priv, 0)) {
 153                 ret = -1;
 154                 goto out;
 155         }
 156
 157 out:
 158         lbs_deb_leave(LBS_DEB_CMD);
 159         return ret;
 160 }
 161
 162 int lbs_host_sleep_cfg(struct lbs_private *priv, uint32_t criteria)
 163 {
 164         struct cmd_ds_host_sleep cmd_config;
 165         int ret;
 166
 167         cmd_config.hdr.size = cpu_to_le16(sizeof(cmd_config));
 168         cmd_config.criteria = cpu_to_le32(criteria);
 169         cmd_config.gpio = priv->wol_gpio;
 170         cmd_config.gap = priv->wol_gap;
 171
 172         ret = lbs_cmd_with_response(priv, CMD_802_11_HOST_SLEEP_CFG, &cmd_config);
 173         if (!ret) {
 174                 lbs_deb_cmd("Set WOL criteria to %x\n", criteria);
 175                 priv->wol_criteria = criteria;
 176         } else {
 177                 lbs_pr_info("HOST_SLEEP_CFG failed %d\n", ret);
 178         }
 179
 180         return ret;
 181 }
 182 EXPORT_SYMBOL_GPL(lbs_host_sleep_cfg);
 183
 184 static int lbs_cmd_802_11_ps_mode(struct cmd_ds_command *cmd,
 185                                    u16 cmd_action)
 186 {
 187         struct cmd_ds_802_11_ps_mode *psm = &cmd->params.psmode;
 188
 189         lbs_deb_enter(LBS_DEB_CMD);
 190
 191         cmd->command = cpu_to_le16(CMD_802_11_PS_MODE);
 192         cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_ps_mode) +
 193                                 S_DS_GEN);
 194         psm->action = cpu_to_le16(cmd_action);
 195         psm->multipledtim = 0;
 196         switch (cmd_action) {
 197         case CMD_SUBCMD_ENTER_PS:
 198                 lbs_deb_cmd("PS command:" "SubCode- Enter PS\n");
 199
 200                 psm->locallisteninterval = 0;
 201                 psm->nullpktinterval = 0;
 202                 psm->multipledtim =
 203                     cpu_to_le16(MRVDRV_DEFAULT_MULTIPLE_DTIM);
 204                 break;
 205
 206         case CMD_SUBCMD_EXIT_PS:
 207                 lbs_deb_cmd("PS command:" "SubCode- Exit PS\n");
 208                 break;
 209
 210         case CMD_SUBCMD_SLEEP_CONFIRMED:
 211                 lbs_deb_cmd("PS command: SubCode- sleep confirm\n");
 212                 break;
 213
 214         default:
 215                 break;
 216         }
 217
 218         lbs_deb_leave(LBS_DEB_CMD);
 219         return 0;
 220 }
 221
 222 int lbs_cmd_802_11_inactivity_timeout(struct lbs_private *priv,
 223                                       uint16_t cmd_action, uint16_t *timeout)
 224 {
 225         struct cmd_ds_802_11_inactivity_timeout cmd;
 226         int ret;
 227
 228         lbs_deb_enter(LBS_DEB_CMD);
 229
 230         cmd.hdr.command = cpu_to_le16(CMD_802_11_INACTIVITY_TIMEOUT);
 231         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 232
 233         cmd.action = cpu_to_le16(cmd_action);
 234
 235         if (cmd_action == CMD_ACT_SET)
 236                 cmd.timeout = cpu_to_le16(*timeout);
 237         else
 238                 cmd.timeout = 0;
 239
 240         ret = lbs_cmd_with_response(priv, CMD_802_11_INACTIVITY_TIMEOUT, &cmd);
 241
 242         if (!ret)
 243                 *timeout = le16_to_cpu(cmd.timeout);
 244
 245         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 246         return 0;
 247 }
 248
 249 int lbs_cmd_802_11_sleep_params(struct lbs_private *priv, uint16_t cmd_action,
 250                                 struct sleep_params *sp)
 251 {
 252         struct cmd_ds_802_11_sleep_params cmd;
 253         int ret;
 254
 255         lbs_deb_enter(LBS_DEB_CMD);
 256
 257         if (cmd_action == CMD_ACT_GET) {
 258                 memset(&cmd, 0, sizeof(cmd));
 259         } else {
 260                 cmd.error = cpu_to_le16(sp->sp_error);
 261                 cmd.offset = cpu_to_le16(sp->sp_offset);
 262                 cmd.stabletime = cpu_to_le16(sp->sp_stabletime);
 263                 cmd.calcontrol = sp->sp_calcontrol;
 264                 cmd.externalsleepclk = sp->sp_extsleepclk;
 265                 cmd.reserved = cpu_to_le16(sp->sp_reserved);
 266         }
 267         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 268         cmd.action = cpu_to_le16(cmd_action);
 269
 270         ret = lbs_cmd_with_response(priv, CMD_802_11_SLEEP_PARAMS, &cmd);
 271
 272         if (!ret) {
 273                 lbs_deb_cmd("error 0x%x, offset 0x%x, stabletime 0x%x, "
 274                             "calcontrol 0x%x extsleepclk 0x%x\n",
 275                             le16_to_cpu(cmd.error), le16_to_cpu(cmd.offset),
 276                             le16_to_cpu(cmd.stabletime), cmd.calcontrol,
 277                             cmd.externalsleepclk);
 278
 279                 sp->sp_error = le16_to_cpu(cmd.error);
 280                 sp->sp_offset = le16_to_cpu(cmd.offset);
 281                 sp->sp_stabletime = le16_to_cpu(cmd.stabletime);
 282                 sp->sp_calcontrol = cmd.calcontrol;
 283                 sp->sp_extsleepclk = cmd.externalsleepclk;
 284                 sp->sp_reserved = le16_to_cpu(cmd.reserved);
 285         }
 286
 287         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 288         return 0;
 289 }
 290
 291 int lbs_cmd_802_11_set_wep(struct lbs_private *priv, uint16_t cmd_action,
 292                            struct assoc_request *assoc)
 293 {
 294         struct cmd_ds_802_11_set_wep cmd;
 295         int ret = 0;
 296
 297         lbs_deb_enter(LBS_DEB_CMD);
 298
 299         cmd.hdr.command = cpu_to_le16(CMD_802_11_SET_WEP);
 300         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 301
 302         cmd.action = cpu_to_le16(cmd_action);
 303
 304         if (cmd_action == CMD_ACT_ADD) {
 305                 int i;
 306
 307                 /* default tx key index */
 308                 cmd.keyindex = cpu_to_le16(assoc->wep_tx_keyidx &
 309                                            CMD_WEP_KEY_INDEX_MASK);
 310
 311                 /* Copy key types and material to host command structure */
 312                 for (i = 0; i < 4; i++) {
 313                         struct enc_key *pkey = &assoc->wep_keys[i];
 314
 315                         switch (pkey->len) {
 316                         case KEY_LEN_WEP_40:
 317                                 cmd.keytype[i] = CMD_TYPE_WEP_40_BIT;
 318                                 memmove(cmd.keymaterial[i], pkey->key, pkey->len);
 319                                 lbs_deb_cmd("SET_WEP: add key %d (40 bit)\n", i);
 320                                 break;
 321                         case KEY_LEN_WEP_104:
 322                                 cmd.keytype[i] = CMD_TYPE_WEP_104_BIT;
 323                                 memmove(cmd.keymaterial[i], pkey->key, pkey->len);
 324                                 lbs_deb_cmd("SET_WEP: add key %d (104 bit)\n", i);
 325                                 break;
 326                         case 0:
 327                                 break;
 328                         default:
 329                                 lbs_deb_cmd("SET_WEP: invalid key %d, length %d\n",
 330                                             i, pkey->len);
 331                                 ret = -1;
 332                                 goto done;
 333                                 break;
 334                         }
 335                 }
 336         } else if (cmd_action == CMD_ACT_REMOVE) {
 337                 /* ACT_REMOVE clears _all_ WEP keys */
 338
 339                 /* default tx key index */
 340                 cmd.keyindex = cpu_to_le16(priv->wep_tx_keyidx &
 341                                            CMD_WEP_KEY_INDEX_MASK);
 342                 lbs_deb_cmd("SET_WEP: remove key %d\n", priv->wep_tx_keyidx);
 343         }
 344
 345         ret = lbs_cmd_with_response(priv, CMD_802_11_SET_WEP, &cmd);
 346 done:
 347         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 348         return ret;
 349 }
 350
 351 int lbs_cmd_802_11_enable_rsn(struct lbs_private *priv, uint16_t cmd_action,
 352                               uint16_t *enable)
 353 {
 354         struct cmd_ds_802_11_enable_rsn cmd;
 355         int ret;
 356
 357         lbs_deb_enter(LBS_DEB_CMD);
 358
 359         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 360         cmd.action = cpu_to_le16(cmd_action);
 361
 362         if (cmd_action == CMD_ACT_SET) {
 363                 if (*enable)
 364                         cmd.enable = cpu_to_le16(CMD_ENABLE_RSN);
 365                 else
 366                         cmd.enable = cpu_to_le16(CMD_DISABLE_RSN);
 367                 lbs_deb_cmd("ENABLE_RSN: %d\n", *enable);
 368         }
 369
 370         ret = lbs_cmd_with_response(priv, CMD_802_11_ENABLE_RSN, &cmd);
 371         if (!ret && cmd_action == CMD_ACT_GET)
 372                 *enable = le16_to_cpu(cmd.enable);
 373
 374         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 375         return ret;
 376 }
 377
 378 static void set_one_wpa_key(struct MrvlIEtype_keyParamSet *keyparam,
 379                             struct enc_key *key)
 380 {
 381         lbs_deb_enter(LBS_DEB_CMD);
 382
 383         if (key->flags & KEY_INFO_WPA_ENABLED)
 384                 keyparam->keyinfo |= cpu_to_le16(KEY_INFO_WPA_ENABLED);
 385         if (key->flags & KEY_INFO_WPA_UNICAST)
 386                 keyparam->keyinfo |= cpu_to_le16(KEY_INFO_WPA_UNICAST);
 387         if (key->flags & KEY_INFO_WPA_MCAST)
 388                 keyparam->keyinfo |= cpu_to_le16(KEY_INFO_WPA_MCAST);
 389
 390         keyparam->type = cpu_to_le16(TLV_TYPE_KEY_MATERIAL);
 391         keyparam->keytypeid = cpu_to_le16(key->type);
 392         keyparam->keylen = cpu_to_le16(key->len);
 393         memcpy(keyparam->key, key->key, key->len);
 394
 395         /* Length field doesn't include the {type,length} header */
 396         keyparam->length = cpu_to_le16(sizeof(*keyparam) - 4);
 397         lbs_deb_leave(LBS_DEB_CMD);
 398 }
 399
 400 int lbs_cmd_802_11_key_material(struct lbs_private *priv, uint16_t cmd_action,
 401                                 struct assoc_request *assoc)
 402 {
 403         struct cmd_ds_802_11_key_material cmd;
 404         int ret = 0;
 405         int index = 0;
 406
 407         lbs_deb_enter(LBS_DEB_CMD);
 408
 409         cmd.action = cpu_to_le16(cmd_action);
 410         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 411
 412         if (cmd_action == CMD_ACT_GET) {
 413                 cmd.hdr.size = cpu_to_le16(S_DS_GEN + 2);
 414         } else {
 415                 memset(cmd.keyParamSet, 0, sizeof(cmd.keyParamSet));
 416
 417                 if (test_bit(ASSOC_FLAG_WPA_UCAST_KEY, &assoc->flags)) {
 418                         set_one_wpa_key(&cmd.keyParamSet[index],
 419                                         &assoc->wpa_unicast_key);
 420                         index++;
 421                 }
 422
 423                 if (test_bit(ASSOC_FLAG_WPA_MCAST_KEY, &assoc->flags)) {
 424                         set_one_wpa_key(&cmd.keyParamSet[index],
 425                                         &assoc->wpa_mcast_key);
 426                         index++;
 427                 }
 428
 429                 /* The common header and as many keys as we included */
 430                 cmd.hdr.size = cpu_to_le16(offsetof(typeof(cmd),
 431                                                     keyParamSet[index]));
 432         }
 433         ret = lbs_cmd_with_response(priv, CMD_802_11_KEY_MATERIAL, &cmd);
 434         /* Copy the returned key to driver private data */
 435         if (!ret && cmd_action == CMD_ACT_GET) {
 436                 void *buf_ptr = cmd.keyParamSet;
 437                 void *resp_end = &(&cmd)[1];
 438
 439                 while (buf_ptr < resp_end) {
 440                         struct MrvlIEtype_keyParamSet *keyparam = buf_ptr;
 441                         struct enc_key *key;
 442                         uint16_t param_set_len = le16_to_cpu(keyparam->length);
 443                         uint16_t key_len = le16_to_cpu(keyparam->keylen);
 444                         uint16_t key_flags = le16_to_cpu(keyparam->keyinfo);
 445                         uint16_t key_type = le16_to_cpu(keyparam->keytypeid);
 446                         void *end;
 447
 448                         end = (void *)keyparam + sizeof(keyparam->type)
 449                                 + sizeof(keyparam->length) + param_set_len;
 450
 451                         /* Make sure we don't access past the end of the IEs */
 452                         if (end > resp_end)
 453                                 break;
 454
 455                         if (key_flags & KEY_INFO_WPA_UNICAST)
 456                                 key = &priv->wpa_unicast_key;
 457                         else if (key_flags & KEY_INFO_WPA_MCAST)
 458                                 key = &priv->wpa_mcast_key;
 459                         else
 460                                 break;
 461
 462                         /* Copy returned key into driver */
 463                         memset(key, 0, sizeof(struct enc_key));
 464                         if (key_len > sizeof(key->key))
 465                                 break;
 466                         key->type = key_type;
 467                         key->flags = key_flags;
 468                         key->len = key_len;
 469                         memcpy(key->key, keyparam->key, key->len);
 470
 471                         buf_ptr = end + 1;
 472                 }
 473         }
 474
 475         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 476         return ret;
 477 }
 478
 479 static int lbs_cmd_802_11_reset(struct cmd_ds_command *cmd, int cmd_action)
 480 {
 481         struct cmd_ds_802_11_reset *reset = &cmd->params.reset;
 482
 483         lbs_deb_enter(LBS_DEB_CMD);
 484
 485         cmd->command = cpu_to_le16(CMD_802_11_RESET);
 486         cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_reset) + S_DS_GEN);
 487         reset->action = cpu_to_le16(cmd_action);
 488
 489         lbs_deb_leave(LBS_DEB_CMD);
 490         return 0;
 491 }
 492
 493 static int lbs_cmd_802_11_snmp_mib(struct lbs_private *priv,
 494                                     struct cmd_ds_command *cmd,
 495                                     int cmd_action,
 496                                     int cmd_oid, void *pdata_buf)
 497 {
 498         struct cmd_ds_802_11_snmp_mib *pSNMPMIB = &cmd->params.smib;
 499         u8 ucTemp;
 500
 501         lbs_deb_enter(LBS_DEB_CMD);
 502
 503         lbs_deb_cmd("SNMP_CMD: cmd_oid = 0x%x\n", cmd_oid);
 504
 505         cmd->command = cpu_to_le16(CMD_802_11_SNMP_MIB);
 506         cmd->size = cpu_to_le16(sizeof(*pSNMPMIB) + S_DS_GEN);
 507
 508         switch (cmd_oid) {
 509         case OID_802_11_INFRASTRUCTURE_MODE:
 510         {
 511                 u8 mode = (u8) (size_t) pdata_buf;
 512                 pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_SET);
 513                 pSNMPMIB->oid = cpu_to_le16((u16) DESIRED_BSSTYPE_I);
 514                 pSNMPMIB->bufsize = cpu_to_le16(sizeof(u8));
 515                 if (mode == IW_MODE_ADHOC) {
 516                         ucTemp = SNMP_MIB_VALUE_ADHOC;
 517                 } else {
 518                         /* Infra and Auto modes */
 519                         ucTemp = SNMP_MIB_VALUE_INFRA;
 520                 }
 521
 522                 memmove(pSNMPMIB->value, &ucTemp, sizeof(u8));
 523
 524                 break;
 525         }
 526
 527         case OID_802_11D_ENABLE:
 528                 {
 529                         u32 ulTemp;
 530
 531                         pSNMPMIB->oid = cpu_to_le16((u16) DOT11D_I);
 532
 533                         if (cmd_action == CMD_ACT_SET) {
 534                                 pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_SET);
 535                                 pSNMPMIB->bufsize = cpu_to_le16(sizeof(u16));
 536                                 ulTemp = *(u32 *)pdata_buf;
 537                                 *((__le16 *)(pSNMPMIB->value)) =
 538                                     cpu_to_le16((u16) ulTemp);
 539                         }
 540                         break;
 541                 }
 542
 543         case OID_802_11_FRAGMENTATION_THRESHOLD:
 544                 {
 545                         u32 ulTemp;
 546
 547                         pSNMPMIB->oid = cpu_to_le16((u16) FRAGTHRESH_I);
 548
 549                         if (cmd_action == CMD_ACT_GET) {
 550                                 pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_GET);
 551                         } else if (cmd_action == CMD_ACT_SET) {
 552                                 pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_SET);
 553                                 pSNMPMIB->bufsize = cpu_to_le16(sizeof(u16));
 554                                 ulTemp = *((u32 *) pdata_buf);
 555                                 *((__le16 *)(pSNMPMIB->value)) =
 556                                     cpu_to_le16((u16) ulTemp);
 557
 558                         }
 559
 560                         break;
 561                 }
 562
 563         case OID_802_11_RTS_THRESHOLD:
 564                 {
 565
 566                         u32 ulTemp;
 567                         pSNMPMIB->oid = cpu_to_le16(RTSTHRESH_I);
 568
 569                         if (cmd_action == CMD_ACT_GET) {
 570                                 pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_GET);
 571                         } else if (cmd_action == CMD_ACT_SET) {
 572                                 pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_SET);
 573                                 pSNMPMIB->bufsize = cpu_to_le16(sizeof(u16));
 574                                 ulTemp = *((u32 *)pdata_buf);
 575                                 *(__le16 *)(pSNMPMIB->value) =
 576                                     cpu_to_le16((u16) ulTemp);
 577
 578                         }
 579                         break;
 580                 }
 581         case OID_802_11_TX_RETRYCOUNT:
 582                 pSNMPMIB->oid = cpu_to_le16((u16) SHORT_RETRYLIM_I);
 583
 584                 if (cmd_action == CMD_ACT_GET) {
 585                         pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_GET);
 586                 } else if (cmd_action == CMD_ACT_SET) {
 587                         pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_SET);
 588                         pSNMPMIB->bufsize = cpu_to_le16(sizeof(u16));
 589                         *((__le16 *)(pSNMPMIB->value)) =
 590                             cpu_to_le16((u16) priv->txretrycount);
 591                 }
 592
 593                 break;
 594         default:
 595                 break;
 596         }
 597
 598         lbs_deb_cmd(
 599                "SNMP_CMD: command=0x%x, size=0x%x, seqnum=0x%x, result=0x%x\n",
 600                le16_to_cpu(cmd->command), le16_to_cpu(cmd->size),
 601                le16_to_cpu(cmd->seqnum), le16_to_cpu(cmd->result));
 602
 603         lbs_deb_cmd(
 604                "SNMP_CMD: action 0x%x, oid 0x%x, oidsize 0x%x, value 0x%x\n",
 605                le16_to_cpu(pSNMPMIB->querytype), le16_to_cpu(pSNMPMIB->oid),
 606                le16_to_cpu(pSNMPMIB->bufsize),
 607                le16_to_cpu(*(__le16 *) pSNMPMIB->value));
 608
 609         lbs_deb_leave(LBS_DEB_CMD);
 610         return 0;
 611 }
 612
 613 static int lbs_cmd_802_11_rf_tx_power(struct cmd_ds_command *cmd,
 614                                        u16 cmd_action, void *pdata_buf)
 615 {
 616
 617         struct cmd_ds_802_11_rf_tx_power *prtp = &cmd->params.txp;
 618
 619         lbs_deb_enter(LBS_DEB_CMD);
 620
 621         cmd->size =
 622             cpu_to_le16((sizeof(struct cmd_ds_802_11_rf_tx_power)) + S_DS_GEN);
 623         cmd->command = cpu_to_le16(CMD_802_11_RF_TX_POWER);
 624         prtp->action = cpu_to_le16(cmd_action);
 625
 626         lbs_deb_cmd("RF_TX_POWER_CMD: size:%d cmd:0x%x Act:%d\n",
 627                     le16_to_cpu(cmd->size), le16_to_cpu(cmd->command),
 628                     le16_to_cpu(prtp->action));
 629
 630         switch (cmd_action) {
 631         case CMD_ACT_TX_POWER_OPT_GET:
 632                 prtp->action = cpu_to_le16(CMD_ACT_GET);
 633                 prtp->currentlevel = 0;
 634                 break;
 635
 636         case CMD_ACT_TX_POWER_OPT_SET_HIGH:
 637                 prtp->action = cpu_to_le16(CMD_ACT_SET);
 638                 prtp->currentlevel = cpu_to_le16(CMD_ACT_TX_POWER_INDEX_HIGH);
 639                 break;
 640
 641         case CMD_ACT_TX_POWER_OPT_SET_MID:
 642                 prtp->action = cpu_to_le16(CMD_ACT_SET);
 643                 prtp->currentlevel = cpu_to_le16(CMD_ACT_TX_POWER_INDEX_MID);
 644                 break;
 645
 646         case CMD_ACT_TX_POWER_OPT_SET_LOW:
 647                 prtp->action = cpu_to_le16(CMD_ACT_SET);
 648                 prtp->currentlevel = cpu_to_le16(*((u16 *) pdata_buf));
 649                 break;
 650         }
 651
 652         lbs_deb_leave(LBS_DEB_CMD);
 653         return 0;
 654 }
 655
 656 static int lbs_cmd_802_11_monitor_mode(struct cmd_ds_command *cmd,
 657                                       u16 cmd_action, void *pdata_buf)
 658 {
 659         struct cmd_ds_802_11_monitor_mode *monitor = &cmd->params.monitor;
 660
 661         cmd->command = cpu_to_le16(CMD_802_11_MONITOR_MODE);
 662         cmd->size =
 663             cpu_to_le16(sizeof(struct cmd_ds_802_11_monitor_mode) +
 664                              S_DS_GEN);
 665
 666         monitor->action = cpu_to_le16(cmd_action);
 667         if (cmd_action == CMD_ACT_SET) {
 668                 monitor->mode =
 669                     cpu_to_le16((u16) (*(u32 *) pdata_buf));
 670         }
 671
 672         return 0;
 673 }
 674
 675 static int lbs_cmd_802_11_rate_adapt_rateset(struct lbs_private *priv,
 676                                               struct cmd_ds_command *cmd,
 677                                               u16 cmd_action)
 678 {
 679         struct cmd_ds_802_11_rate_adapt_rateset
 680         *rateadapt = &cmd->params.rateset;
 681
 682         lbs_deb_enter(LBS_DEB_CMD);
 683         cmd->size =
 684             cpu_to_le16(sizeof(struct cmd_ds_802_11_rate_adapt_rateset)
 685                              + S_DS_GEN);
 686         cmd->command = cpu_to_le16(CMD_802_11_RATE_ADAPT_RATESET);
 687
 688         rateadapt->action = cpu_to_le16(cmd_action);
 689         rateadapt->enablehwauto = cpu_to_le16(priv->enablehwauto);
 690         rateadapt->bitmap = cpu_to_le16(priv->ratebitmap);
 691
 692         lbs_deb_leave(LBS_DEB_CMD);
 693         return 0;
 694 }
 695
 696 /**
 697  *  @brief Get the current data rate
 698  *
 699  *  @param priv         A pointer to struct lbs_private structure
 700  *
 701  *  @return             The data rate on success, error on failure
 702  */
 703 int lbs_get_data_rate(struct lbs_private *priv)
 704 {
 705         struct cmd_ds_802_11_data_rate cmd;
 706         int ret = -1;
 707
 708         lbs_deb_enter(LBS_DEB_CMD);
 709
 710         memset(&cmd, 0, sizeof(cmd));
 711         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 712         cmd.action = cpu_to_le16(CMD_ACT_GET_TX_RATE);
 713
 714         ret = lbs_cmd_with_response(priv, CMD_802_11_DATA_RATE, &cmd);
 715         if (ret)
 716                 goto out;
 717
 718         lbs_deb_hex(LBS_DEB_CMD, "DATA_RATE_RESP", (u8 *) &cmd, sizeof (cmd));
 719
 720         ret = (int) lbs_fw_index_to_data_rate(cmd.rates[0]);
 721         lbs_deb_cmd("DATA_RATE: current rate 0x%02x\n", ret);
 722
 723 out:
 724         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 725         return ret;
 726 }
 727
 728 /**
 729  *  @brief Set the data rate
 730  *
 731  *  @param priv         A pointer to struct lbs_private structure
 732  *  @param rate         The desired data rate, or 0 to clear a locked rate
 733  *
 734  *  @return             0 on success, error on failure
 735  */
 736 int lbs_set_data_rate(struct lbs_private *priv, u8 rate)
 737 {
 738         struct cmd_ds_802_11_data_rate cmd;
 739         int ret = 0;
 740
 741         lbs_deb_enter(LBS_DEB_CMD);
 742
 743         memset(&cmd, 0, sizeof(cmd));
 744         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 745
 746         if (rate > 0) {
 747                 cmd.action = cpu_to_le16(CMD_ACT_SET_TX_FIX_RATE);
 748                 cmd.rates[0] = lbs_data_rate_to_fw_index(rate);
 749                 if (cmd.rates[0] == 0) {
 750                         lbs_deb_cmd("DATA_RATE: invalid requested rate of"
 751                                     " 0x%02X\n", rate);
 752                         ret = 0;
 753                         goto out;
 754                 }
 755                 lbs_deb_cmd("DATA_RATE: set fixed 0x%02X\n", cmd.rates[0]);
 756         } else {
 757                 cmd.action = cpu_to_le16(CMD_ACT_SET_TX_AUTO);
 758                 lbs_deb_cmd("DATA_RATE: setting auto\n");
 759         }
 760
 761         ret = lbs_cmd_with_response(priv, CMD_802_11_DATA_RATE, &cmd);
 762         if (ret)
 763                 goto out;
 764
 765         lbs_deb_hex(LBS_DEB_CMD, "DATA_RATE_RESP", (u8 *) &cmd, sizeof (cmd));
 766
 767         /* FIXME: get actual rates FW can do if this command actually returns
 768          * all data rates supported.
 769          */
 770         priv->cur_rate = lbs_fw_index_to_data_rate(cmd.rates[0]);
 771         lbs_deb_cmd("DATA_RATE: current rate is 0x%02x\n", priv->cur_rate);
 772
 773 out:
 774         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 775         return ret;
 776 }
 777
 778 static int lbs_cmd_mac_multicast_adr(struct lbs_private *priv,
 779                                       struct cmd_ds_command *cmd,
 780                                       u16 cmd_action)
 781 {
 782         struct cmd_ds_mac_multicast_adr *pMCastAdr = &cmd->params.madr;
 783
 784         lbs_deb_enter(LBS_DEB_CMD);
 785         cmd->size = cpu_to_le16(sizeof(struct cmd_ds_mac_multicast_adr) +
 786                              S_DS_GEN);
 787         cmd->command = cpu_to_le16(CMD_MAC_MULTICAST_ADR);
 788
 789         lbs_deb_cmd("MULTICAST_ADR: setting %d addresses\n", pMCastAdr->nr_of_adrs);
 790         pMCastAdr->action = cpu_to_le16(cmd_action);
 791         pMCastAdr->nr_of_adrs =
 792             cpu_to_le16((u16) priv->nr_of_multicastmacaddr);
 793         memcpy(pMCastAdr->maclist, priv->multicastlist,
 794                priv->nr_of_multicastmacaddr * ETH_ALEN);
 795
 796         lbs_deb_leave(LBS_DEB_CMD);
 797         return 0;
 798 }
 799
 800 /**
 801  *  @brief Get the radio channel
 802  *
 803  *  @param priv         A pointer to struct lbs_private structure
 804  *
 805  *  @return             The channel on success, error on failure
 806  */
 807 int lbs_get_channel(struct lbs_private *priv)
 808 {
 809         struct cmd_ds_802_11_rf_channel cmd;
 810         int ret = 0;
 811
 812         lbs_deb_enter(LBS_DEB_CMD);
 813
 814         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 815         cmd.action = cpu_to_le16(CMD_OPT_802_11_RF_CHANNEL_GET);
 816
 817         ret = lbs_cmd_with_response(priv, CMD_802_11_RF_CHANNEL, &cmd);
 818         if (ret)
 819                 goto out;
 820
 821         ret = le16_to_cpu(cmd.channel);
 822         lbs_deb_cmd("current radio channel is %d\n", ret);
 823
 824 out:
 825         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 826         return ret;
 827 }
 828
 829 int lbs_update_channel(struct lbs_private *priv)
 830 {
 831         int ret;
 832
 833         /* the channel in f/w could be out of sync; get the current channel */
 834         lbs_deb_enter(LBS_DEB_ASSOC);
 835
 836         ret = lbs_get_channel(priv);
 837         if (ret > 0) {
 838                 priv->curbssparams.channel = ret;
 839                 ret = 0;
 840         }
 841         lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
 842         return ret;
 843 }
 844
 845 /**
 846  *  @brief Set the radio channel
 847  *
 848  *  @param priv         A pointer to struct lbs_private structure
 849  *  @param channel      The desired channel, or 0 to clear a locked channel
 850  *
 851  *  @return             0 on success, error on failure
 852  */
 853 int lbs_set_channel(struct lbs_private *priv, u8 channel)
 854 {
 855         struct cmd_ds_802_11_rf_channel cmd;
 856         u8 old_channel = priv->curbssparams.channel;
 857         int ret = 0;
 858
 859         lbs_deb_enter(LBS_DEB_CMD);
 860
 861         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 862         cmd.action = cpu_to_le16(CMD_OPT_802_11_RF_CHANNEL_SET);
 863         cmd.channel = cpu_to_le16(channel);
 864
 865         ret = lbs_cmd_with_response(priv, CMD_802_11_RF_CHANNEL, &cmd);
 866         if (ret)
 867                 goto out;
 868
 869         priv->curbssparams.channel = (uint8_t) le16_to_cpu(cmd.channel);
 870         lbs_deb_cmd("channel switch from %d to %d\n", old_channel,
 871                 priv->curbssparams.channel);
 872
 873 out:
 874         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 875         return ret;
 876 }
 877
 878 static int lbs_cmd_802_11_rssi(struct lbs_private *priv,
 879                                 struct cmd_ds_command *cmd)
 880 {
 881
 882         lbs_deb_enter(LBS_DEB_CMD);
 883         cmd->command = cpu_to_le16(CMD_802_11_RSSI);
 884         cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_rssi) + S_DS_GEN);
 885         cmd->params.rssi.N = cpu_to_le16(DEFAULT_BCN_AVG_FACTOR);
 886
 887         /* reset Beacon SNR/NF/RSSI values */
 888         priv->SNR[TYPE_BEACON][TYPE_NOAVG] = 0;
 889         priv->SNR[TYPE_BEACON][TYPE_AVG] = 0;
 890         priv->NF[TYPE_BEACON][TYPE_NOAVG] = 0;
 891         priv->NF[TYPE_BEACON][TYPE_AVG] = 0;
 892         priv->RSSI[TYPE_BEACON][TYPE_NOAVG] = 0;
 893         priv->RSSI[TYPE_BEACON][TYPE_AVG] = 0;
 894
 895         lbs_deb_leave(LBS_DEB_CMD);
 896         return 0;
 897 }
 898
 899 static int lbs_cmd_reg_access(struct cmd_ds_command *cmdptr,
 900                                u8 cmd_action, void *pdata_buf)
 901 {
 902         struct lbs_offset_value *offval;
 903
 904         lbs_deb_enter(LBS_DEB_CMD);
 905
 906         offval = (struct lbs_offset_value *)pdata_buf;
 907
 908         switch (le16_to_cpu(cmdptr->command)) {
 909         case CMD_MAC_REG_ACCESS:
 910                 {
 911                         struct cmd_ds_mac_reg_access *macreg;
 912
 913                         cmdptr->size =
 914                             cpu_to_le16(sizeof (struct cmd_ds_mac_reg_access)
 915                                         + S_DS_GEN);
 916                         macreg =
 917                             (struct cmd_ds_mac_reg_access *)&cmdptr->params.
 918                             macreg;
 919
 920                         macreg->action = cpu_to_le16(cmd_action);
 921                         macreg->offset = cpu_to_le16((u16) offval->offset);
 922                         macreg->value = cpu_to_le32(offval->value);
 923
 924                         break;
 925                 }
 926
 927         case CMD_BBP_REG_ACCESS:
 928                 {
 929                         struct cmd_ds_bbp_reg_access *bbpreg;
 930
 931                         cmdptr->size =
 932                             cpu_to_le16(sizeof
 933                                              (struct cmd_ds_bbp_reg_access)
 934                                              + S_DS_GEN);
 935                         bbpreg =
 936                             (struct cmd_ds_bbp_reg_access *)&cmdptr->params.
 937                             bbpreg;
 938
 939                         bbpreg->action = cpu_to_le16(cmd_action);
 940                         bbpreg->offset = cpu_to_le16((u16) offval->offset);
 941                         bbpreg->value = (u8) offval->value;
 942
 943                         break;
 944                 }
 945
 946         case CMD_RF_REG_ACCESS:
 947                 {
 948                         struct cmd_ds_rf_reg_access *rfreg;
 949
 950                         cmdptr->size =
 951                             cpu_to_le16(sizeof
 952                                              (struct cmd_ds_rf_reg_access) +
 953                                              S_DS_GEN);
 954                         rfreg =
 955                             (struct cmd_ds_rf_reg_access *)&cmdptr->params.
 956                             rfreg;
 957
 958                         rfreg->action = cpu_to_le16(cmd_action);
 959                         rfreg->offset = cpu_to_le16((u16) offval->offset);
 960                         rfreg->value = (u8) offval->value;
 961
 962                         break;
 963                 }
 964
 965         default:
 966                 break;
 967         }
 968
 969         lbs_deb_leave(LBS_DEB_CMD);
 970         return 0;
 971 }
 972
 973 static int lbs_cmd_bt_access(struct cmd_ds_command *cmd,
 974                                u16 cmd_action, void *pdata_buf)
 975 {
 976         struct cmd_ds_bt_access *bt_access = &cmd->params.bt;
 977         lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
 978
 979         cmd->command = cpu_to_le16(CMD_BT_ACCESS);
 980         cmd->size = cpu_to_le16(sizeof(struct cmd_ds_bt_access) + S_DS_GEN);
 981         cmd->result = 0;
 982         bt_access->action = cpu_to_le16(cmd_action);
 983
 984         switch (cmd_action) {
 985         case CMD_ACT_BT_ACCESS_ADD:
 986                 memcpy(bt_access->addr1, pdata_buf, 2 * ETH_ALEN);
 987                 lbs_deb_hex(LBS_DEB_MESH, "BT_ADD: blinded MAC addr", bt_access->addr1, 6);
 988                 break;
 989         case CMD_ACT_BT_ACCESS_DEL:
 990                 memcpy(bt_access->addr1, pdata_buf, 1 * ETH_ALEN);
 991                 lbs_deb_hex(LBS_DEB_MESH, "BT_DEL: blinded MAC addr", bt_access->addr1, 6);
 992                 break;
 993         case CMD_ACT_BT_ACCESS_LIST:
 994                 bt_access->id = cpu_to_le32(*(u32 *) pdata_buf);
 995                 break;
 996         case CMD_ACT_BT_ACCESS_RESET:
 997                 break;
 998         case CMD_ACT_BT_ACCESS_SET_INVERT:
 999                 bt_access->id = cpu_to_le32(*(u32 *) pdata_buf);
1000                 break;
1001         case CMD_ACT_BT_ACCESS_GET_INVERT:
1002                 break;
1003         default:
1004                 break;
1005         }
1006         lbs_deb_leave(LBS_DEB_CMD);
1007         return 0;
1008 }
1009
1010 static int lbs_cmd_fwt_access(struct cmd_ds_command *cmd,
1011                                u16 cmd_action, void *pdata_buf)
1012 {
1013         struct cmd_ds_fwt_access *fwt_access = &cmd->params.fwt;
1014         lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
1015
1016         cmd->command = cpu_to_le16(CMD_FWT_ACCESS);
1017         cmd->size = cpu_to_le16(sizeof(struct cmd_ds_fwt_access) + S_DS_GEN);
1018         cmd->result = 0;
1019
1020         if (pdata_buf)
1021                 memcpy(fwt_access, pdata_buf, sizeof(*fwt_access));
1022         else
1023                 memset(fwt_access, 0, sizeof(*fwt_access));
1024
1025         fwt_access->action = cpu_to_le16(cmd_action);
1026
1027         lbs_deb_leave(LBS_DEB_CMD);
1028         return 0;
1029 }
1030
1031 int lbs_mesh_access(struct lbs_private *priv, uint16_t cmd_action,
1032                     struct cmd_ds_mesh_access *cmd)
1033 {
1034         int ret;
1035
1036         lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
1037
1038         cmd->hdr.command = cpu_to_le16(CMD_MESH_ACCESS);
1039         cmd->hdr.size = cpu_to_le16(sizeof(*cmd));
1040         cmd->hdr.result = 0;
1041
1042         cmd->action = cpu_to_le16(cmd_action);
1043
1044         ret = lbs_cmd_with_response(priv, CMD_MESH_ACCESS, cmd);
1045
1046         lbs_deb_leave(LBS_DEB_CMD);
1047         return ret;
1048 }
1049
1050 int lbs_mesh_config(struct lbs_private *priv, uint16_t enable, uint16_t chan)
1051 {
1052         struct cmd_ds_mesh_config cmd;
1053
1054         memset(&cmd, 0, sizeof(cmd));
1055         cmd.action = cpu_to_le16(enable);
1056         cmd.channel = cpu_to_le16(chan);
1057         cmd.type = cpu_to_le16(priv->mesh_tlv);
1058         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
1059
1060         if (enable) {
1061                 cmd.length = cpu_to_le16(priv->mesh_ssid_len);
1062                 memcpy(cmd.data, priv->mesh_ssid, priv->mesh_ssid_len);
1063         }
1064         lbs_deb_cmd("mesh config enable %d TLV %x channel %d SSID %s\n",
1065                     enable, priv->mesh_tlv, chan,
1066                     escape_essid(priv->mesh_ssid, priv->mesh_ssid_len));
1067         return lbs_cmd_with_response(priv, CMD_MESH_CONFIG, &cmd);
1068 }
1069
1070 static int lbs_cmd_bcn_ctrl(struct lbs_private * priv,
1071                                 struct cmd_ds_command *cmd,
1072                                 u16 cmd_action)
1073 {
1074         struct cmd_ds_802_11_beacon_control
1075                 *bcn_ctrl = &cmd->params.bcn_ctrl;
1076
1077         lbs_deb_enter(LBS_DEB_CMD);
1078         cmd->size =
1079             cpu_to_le16(sizeof(struct cmd_ds_802_11_beacon_control)
1080                              + S_DS_GEN);
1081         cmd->command = cpu_to_le16(CMD_802_11_BEACON_CTRL);
1082
1083         bcn_ctrl->action = cpu_to_le16(cmd_action);
1084         bcn_ctrl->beacon_enable = cpu_to_le16(priv->beacon_enable);
1085         bcn_ctrl->beacon_period = cpu_to_le16(priv->beacon_period);
1086
1087         lbs_deb_leave(LBS_DEB_CMD);
1088         return 0;
1089 }
1090
1091 static void lbs_queue_cmd(struct lbs_private *priv,
1092                           struct cmd_ctrl_node *cmdnode)
1093 {
1094         unsigned long flags;
1095         int addtail = 1;
1096
1097         lbs_deb_enter(LBS_DEB_HOST);
1098
1099         if (!cmdnode) {
1100                 lbs_deb_host("QUEUE_CMD: cmdnode is NULL\n");
1101                 goto done;
1102         }
1103         if (!cmdnode->cmdbuf->size) {
1104                 lbs_deb_host("DNLD_CMD: cmd size is zero\n");
1105                 goto done;
1106         }
1107         cmdnode->result = 0;
1108
1109         /* Exit_PS command needs to be queued in the header always. */
1110         if (le16_to_cpu(cmdnode->cmdbuf->command) == CMD_802_11_PS_MODE) {
1111                 struct cmd_ds_802_11_ps_mode *psm = (void *) &cmdnode->cmdbuf[1];
1112
1113                 if (psm->action == cpu_to_le16(CMD_SUBCMD_EXIT_PS)) {
1114                         if (priv->psstate != PS_STATE_FULL_POWER)
1115                                 addtail = 0;
1116                 }
1117         }
1118
1119         spin_lock_irqsave(&priv->driver_lock, flags);
1120
1121         if (addtail)
1122                 list_add_tail(&cmdnode->list, &priv->cmdpendingq);
1123         else
1124                 list_add(&cmdnode->list, &priv->cmdpendingq);
1125
1126         spin_unlock_irqrestore(&priv->driver_lock, flags);
1127
1128         lbs_deb_host("QUEUE_CMD: inserted command 0x%04x into cmdpendingq\n",
1129                      le16_to_cpu(cmdnode->cmdbuf->command));
1130
1131 done:
1132         lbs_deb_leave(LBS_DEB_HOST);
1133 }
1134
1135 static void lbs_submit_command(struct lbs_private *priv,
1136                                struct cmd_ctrl_node *cmdnode)
1137 {
1138         unsigned long flags;
1139         struct cmd_header *cmd;
1140         uint16_t cmdsize;
1141         uint16_t command;
1142         int timeo = 5 * HZ;
1143         int ret;
1144
1145         lbs_deb_enter(LBS_DEB_HOST);
1146
1147         cmd = cmdnode->cmdbuf;
1148
1149         spin_lock_irqsave(&priv->driver_lock, flags);
1150         priv->cur_cmd = cmdnode;
1151         priv->cur_cmd_retcode = 0;
1152         spin_unlock_irqrestore(&priv->driver_lock, flags);
1153
1154         cmdsize = le16_to_cpu(cmd->size);
1155         command = le16_to_cpu(cmd->command);
1156
1157         /* These commands take longer */
1158         if (command == CMD_802_11_SCAN || command == CMD_802_11_ASSOCIATE ||
1159             command == CMD_802_11_AUTHENTICATE)
1160                 timeo = 10 * HZ;
1161
1162         lbs_deb_cmd("DNLD_CMD: command 0x%04x, seq %d, size %d\n",
1163                      command, le16_to_cpu(cmd->seqnum), cmdsize);
1164         lbs_deb_hex(LBS_DEB_CMD, "DNLD_CMD", (void *) cmdnode->cmdbuf, cmdsize);
1165
1166         ret = priv->hw_host_to_card(priv, MVMS_CMD, (u8 *) cmd, cmdsize);
1167
1168         if (ret) {
1169                 lbs_pr_info("DNLD_CMD: hw_host_to_card failed: %d\n", ret);
1170                 /* Let the timer kick in and retry, and potentially reset
1171                    the whole thing if the condition persists */
1172                 timeo = HZ;
1173         }
1174
1175         /* Setup the timer after transmit command */
1176         mod_timer(&priv->command_timer, jiffies + timeo);
1177
1178         lbs_deb_leave(LBS_DEB_HOST);
1179 }
1180
1181 /**
1182  *  This function inserts command node to cmdfreeq
1183  *  after cleans it. Requires priv->driver_lock held.
1184  */
1185 static void __lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
1186                                          struct cmd_ctrl_node *cmdnode)
1187 {
1188         lbs_deb_enter(LBS_DEB_HOST);
1189
1190         if (!cmdnode)
1191                 goto out;
1192
1193         cmdnode->callback = NULL;
1194         cmdnode->callback_arg = 0;
1195
1196         memset(cmdnode->cmdbuf, 0, LBS_CMD_BUFFER_SIZE);
1197
1198         list_add_tail(&cmdnode->list, &priv->cmdfreeq);
1199  out:
1200         lbs_deb_leave(LBS_DEB_HOST);
1201 }
1202
1203 static void lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
1204         struct cmd_ctrl_node *ptempcmd)
1205 {
1206         unsigned long flags;
1207
1208         spin_lock_irqsave(&priv->driver_lock, flags);
1209         __lbs_cleanup_and_insert_cmd(priv, ptempcmd);
1210         spin_unlock_irqrestore(&priv->driver_lock, flags);
1211 }
1212
1213 void lbs_complete_command(struct lbs_private *priv, struct cmd_ctrl_node *cmd,
1214                           int result)
1215 {
1216         if (cmd == priv->cur_cmd)
1217                 priv->cur_cmd_retcode = result;
1218
1219         cmd->result = result;
1220         cmd->cmdwaitqwoken = 1;
1221         wake_up_interruptible(&cmd->cmdwait_q);
1222
1223         if (!cmd->callback || cmd->callback == lbs_cmd_async_callback)
1224                 __lbs_cleanup_and_insert_cmd(priv, cmd);
1225         priv->cur_cmd = NULL;
1226 }
1227
1228 int lbs_set_radio_control(struct lbs_private *priv)
1229 {
1230         int ret = 0;
1231         struct cmd_ds_802_11_radio_control cmd;
1232
1233         lbs_deb_enter(LBS_DEB_CMD);
1234
1235         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
1236         cmd.action = cpu_to_le16(CMD_ACT_SET);
1237
1238         switch (priv->preamble) {
1239         case CMD_TYPE_SHORT_PREAMBLE:
1240                 cmd.control = cpu_to_le16(SET_SHORT_PREAMBLE);
1241                 break;
1242
1243         case CMD_TYPE_LONG_PREAMBLE:
1244                 cmd.control = cpu_to_le16(SET_LONG_PREAMBLE);
1245                 break;
1246
1247         case CMD_TYPE_AUTO_PREAMBLE:
1248         default:
1249                 cmd.control = cpu_to_le16(SET_AUTO_PREAMBLE);
1250                 break;
1251         }
1252
1253         if (priv->radioon)
1254                 cmd.control |= cpu_to_le16(TURN_ON_RF);
1255         else
1256                 cmd.control &= cpu_to_le16(~TURN_ON_RF);
1257
1258         lbs_deb_cmd("RADIO_SET: radio %d, preamble %d\n", priv->radioon,
1259                     priv->preamble);
1260
1261         ret = lbs_cmd_with_response(priv, CMD_802_11_RADIO_CONTROL, &cmd);
1262
1263         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
1264         return ret;
1265 }
1266
1267 void lbs_set_mac_control(struct lbs_private *priv)
1268 {
1269         struct cmd_ds_mac_control cmd;
1270
1271         lbs_deb_enter(LBS_DEB_CMD);
1272
1273         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
1274         cmd.action = cpu_to_le16(priv->mac_control);
1275         cmd.reserved = 0;
1276
1277         lbs_cmd_async(priv, CMD_MAC_CONTROL,
1278                 &cmd.hdr, sizeof(cmd));
1279
1280         lbs_deb_leave(LBS_DEB_CMD);
1281 }
1282
1283 /**
1284  *  @brief This function prepare the command before send to firmware.
1285  *
1286  *  @param priv         A pointer to struct lbs_private structure
1287  *  @param cmd_no       command number
1288  *  @param cmd_action   command action: GET or SET
1289  *  @param wait_option  wait option: wait response or not
1290  *  @param cmd_oid      cmd oid: treated as sub command
1291  *  @param pdata_buf    A pointer to informaion buffer
1292  *  @return             0 or -1
1293  */
1294 int lbs_prepare_and_send_command(struct lbs_private *priv,
1295                           u16 cmd_no,
1296                           u16 cmd_action,
1297                           u16 wait_option, u32 cmd_oid, void *pdata_buf)
1298 {
1299         int ret = 0;
1300         struct cmd_ctrl_node *cmdnode;
1301         struct cmd_ds_command *cmdptr;
1302         unsigned long flags;
1303
1304         lbs_deb_enter(LBS_DEB_HOST);
1305
1306         if (!priv) {
1307                 lbs_deb_host("PREP_CMD: priv is NULL\n");
1308                 ret = -1;
1309                 goto done;
1310         }
1311
1312         if (priv->surpriseremoved) {
1313                 lbs_deb_host("PREP_CMD: card removed\n");
1314                 ret = -1;
1315                 goto done;
1316         }
1317
1318         cmdnode = lbs_get_cmd_ctrl_node(priv);
1319
1320         if (cmdnode == NULL) {
1321                 lbs_deb_host("PREP_CMD: cmdnode is NULL\n");
1322
1323                 /* Wake up main thread to execute next command */
1324                 wake_up_interruptible(&priv->waitq);
1325                 ret = -1;
1326                 goto done;
1327         }
1328
1329         cmdnode->callback = NULL;
1330         cmdnode->callback_arg = (unsigned long)pdata_buf;
1331
1332         cmdptr = (struct cmd_ds_command *)cmdnode->cmdbuf;
1333
1334         lbs_deb_host("PREP_CMD: command 0x%04x\n", cmd_no);
1335
1336         /* Set sequence number, command and INT option */
1337         priv->seqnum++;
1338         cmdptr->seqnum = cpu_to_le16(priv->seqnum);
1339
1340         cmdptr->command = cpu_to_le16(cmd_no);
1341         cmdptr->result = 0;
1342
1343         switch (cmd_no) {
1344         case CMD_802_11_PS_MODE:
1345                 ret = lbs_cmd_802_11_ps_mode(cmdptr, cmd_action);
1346                 break;
1347
1348         case CMD_802_11_ASSOCIATE:
1349         case CMD_802_11_REASSOCIATE:
1350                 ret = lbs_cmd_80211_associate(priv, cmdptr, pdata_buf);
1351                 break;
1352
1353         case CMD_802_11_DEAUTHENTICATE:
1354                 ret = lbs_cmd_80211_deauthenticate(priv, cmdptr);
1355                 break;
1356
1357         case CMD_802_11_AD_HOC_START:
1358                 ret = lbs_cmd_80211_ad_hoc_start(priv, cmdptr, pdata_buf);
1359                 break;
1360
1361         case CMD_802_11_RESET:
1362                 ret = lbs_cmd_802_11_reset(cmdptr, cmd_action);
1363                 break;
1364
1365         case CMD_802_11_AUTHENTICATE:
1366                 ret = lbs_cmd_80211_authenticate(priv, cmdptr, pdata_buf);
1367                 break;
1368
1369         case CMD_802_11_SNMP_MIB:
1370                 ret = lbs_cmd_802_11_snmp_mib(priv, cmdptr,
1371                                                cmd_action, cmd_oid, pdata_buf);
1372                 break;
1373
1374         case CMD_MAC_REG_ACCESS:
1375         case CMD_BBP_REG_ACCESS:
1376         case CMD_RF_REG_ACCESS:
1377                 ret = lbs_cmd_reg_access(cmdptr, cmd_action, pdata_buf);
1378                 break;
1379
1380         case CMD_802_11_RF_TX_POWER:
1381                 ret = lbs_cmd_802_11_rf_tx_power(cmdptr,
1382                                                  cmd_action, pdata_buf);
1383                 break;
1384
1385         case CMD_802_11_RATE_ADAPT_RATESET:
1386                 ret = lbs_cmd_802_11_rate_adapt_rateset(priv,
1387                                                          cmdptr, cmd_action);
1388                 break;
1389
1390         case CMD_MAC_MULTICAST_ADR:
1391                 ret = lbs_cmd_mac_multicast_adr(priv, cmdptr, cmd_action);
1392                 break;
1393
1394         case CMD_802_11_MONITOR_MODE:
1395                 ret = lbs_cmd_802_11_monitor_mode(cmdptr,
1396                                           cmd_action, pdata_buf);
1397                 break;
1398
1399         case CMD_802_11_AD_HOC_JOIN:
1400                 ret = lbs_cmd_80211_ad_hoc_join(priv, cmdptr, pdata_buf);
1401                 break;
1402
1403         case CMD_802_11_RSSI:
1404                 ret = lbs_cmd_802_11_rssi(priv, cmdptr);
1405                 break;
1406
1407         case CMD_802_11_AD_HOC_STOP:
1408                 ret = lbs_cmd_80211_ad_hoc_stop(cmdptr);
1409                 break;
1410
1411         case CMD_802_11_SET_AFC:
1412         case CMD_802_11_GET_AFC:
1413
1414                 cmdptr->command = cpu_to_le16(cmd_no);
1415                 cmdptr->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_afc) +
1416                                            S_DS_GEN);
1417
1418                 memmove(&cmdptr->params.afc,
1419                         pdata_buf, sizeof(struct cmd_ds_802_11_afc));
1420
1421                 ret = 0;
1422                 goto done;
1423
1424         case CMD_802_11D_DOMAIN_INFO:
1425                 ret = lbs_cmd_802_11d_domain_info(priv, cmdptr,
1426                                                    cmd_no, cmd_action);
1427                 break;
1428
1429         case CMD_802_11_TPC_CFG:
1430                 cmdptr->command = cpu_to_le16(CMD_802_11_TPC_CFG);
1431                 cmdptr->size =
1432                     cpu_to_le16(sizeof(struct cmd_ds_802_11_tpc_cfg) +
1433                                      S_DS_GEN);
1434
1435                 memmove(&cmdptr->params.tpccfg,
1436                         pdata_buf, sizeof(struct cmd_ds_802_11_tpc_cfg));
1437
1438                 ret = 0;
1439                 break;
1440         case CMD_802_11_LED_GPIO_CTRL:
1441                 {
1442                         struct mrvlietypes_ledgpio *gpio =
1443                             (struct mrvlietypes_ledgpio*)
1444                             cmdptr->params.ledgpio.data;
1445
1446                         memmove(&cmdptr->params.ledgpio,
1447                                 pdata_buf,
1448                                 sizeof(struct cmd_ds_802_11_led_ctrl));
1449
1450                         cmdptr->command =
1451                             cpu_to_le16(CMD_802_11_LED_GPIO_CTRL);
1452
1453 #define ACTION_NUMLED_TLVTYPE_LEN_FIELDS_LEN 8
1454                         cmdptr->size =
1455                             cpu_to_le16(le16_to_cpu(gpio->header.len)
1456                                 + S_DS_GEN
1457                                 + ACTION_NUMLED_TLVTYPE_LEN_FIELDS_LEN);
1458                         gpio->header.len = gpio->header.len;
1459
1460                         ret = 0;
1461                         break;
1462                 }
1463
1464         case CMD_BT_ACCESS:
1465                 ret = lbs_cmd_bt_access(cmdptr, cmd_action, pdata_buf);
1466                 break;
1467
1468         case CMD_FWT_ACCESS:
1469                 ret = lbs_cmd_fwt_access(cmdptr, cmd_action, pdata_buf);
1470                 break;
1471
1472         case CMD_GET_TSF:
1473                 cmdptr->command = cpu_to_le16(CMD_GET_TSF);
1474                 cmdptr->size = cpu_to_le16(sizeof(struct cmd_ds_get_tsf) +
1475                                            S_DS_GEN);
1476                 ret = 0;
1477                 break;
1478         case CMD_802_11_BEACON_CTRL:
1479                 ret = lbs_cmd_bcn_ctrl(priv, cmdptr, cmd_action);
1480                 break;
1481         default:
1482                 lbs_deb_host("PREP_CMD: unknown command 0x%04x\n", cmd_no);
1483                 ret = -1;
1484                 break;
1485         }
1486
1487         /* return error, since the command preparation failed */
1488         if (ret != 0) {
1489                 lbs_deb_host("PREP_CMD: command preparation failed\n");
1490                 lbs_cleanup_and_insert_cmd(priv, cmdnode);
1491                 ret = -1;
1492                 goto done;
1493         }
1494
1495         cmdnode->cmdwaitqwoken = 0;
1496
1497         lbs_queue_cmd(priv, cmdnode);
1498         wake_up_interruptible(&priv->waitq);
1499
1500         if (wait_option & CMD_OPTION_WAITFORRSP) {
1501                 lbs_deb_host("PREP_CMD: wait for response\n");
1502                 might_sleep();
1503                 wait_event_interruptible(cmdnode->cmdwait_q,
1504                                          cmdnode->cmdwaitqwoken);
1505         }
1506
1507         spin_lock_irqsave(&priv->driver_lock, flags);
1508         if (priv->cur_cmd_retcode) {
1509                 lbs_deb_host("PREP_CMD: command failed with return code %d\n",
1510                        priv->cur_cmd_retcode);
1511                 priv->cur_cmd_retcode = 0;
1512                 ret = -1;
1513         }
1514         spin_unlock_irqrestore(&priv->driver_lock, flags);
1515
1516 done:
1517         lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
1518         return ret;
1519 }
1520
1521 /**
1522  *  @brief This function allocates the command buffer and link
1523  *  it to command free queue.
1524  *
1525  *  @param priv         A pointer to struct lbs_private structure
1526  *  @return             0 or -1
1527  */
1528 int lbs_allocate_cmd_buffer(struct lbs_private *priv)
1529 {
1530         int ret = 0;
1531         u32 bufsize;
1532         u32 i;
1533         struct cmd_ctrl_node *cmdarray;
1534
1535         lbs_deb_enter(LBS_DEB_HOST);
1536
1537         /* Allocate and initialize the command array */
1538         bufsize = sizeof(struct cmd_ctrl_node) * LBS_NUM_CMD_BUFFERS;
1539         if (!(cmdarray = kzalloc(bufsize, GFP_KERNEL))) {
1540                 lbs_deb_host("ALLOC_CMD_BUF: tempcmd_array is NULL\n");
1541                 ret = -1;
1542                 goto done;
1543         }
1544         priv->cmd_array = cmdarray;
1545
1546         /* Allocate and initialize each command buffer in the command array */
1547         for (i = 0; i < LBS_NUM_CMD_BUFFERS; i++) {
1548                 cmdarray[i].cmdbuf = kzalloc(LBS_CMD_BUFFER_SIZE, GFP_KERNEL);
1549                 if (!cmdarray[i].cmdbuf) {
1550                         lbs_deb_host("ALLOC_CMD_BUF: ptempvirtualaddr is NULL\n");
1551                         ret = -1;
1552                         goto done;
1553                 }
1554         }
1555
1556         for (i = 0; i < LBS_NUM_CMD_BUFFERS; i++) {
1557                 init_waitqueue_head(&cmdarray[i].cmdwait_q);
1558                 lbs_cleanup_and_insert_cmd(priv, &cmdarray[i]);
1559         }
1560         ret = 0;
1561
1562 done:
1563         lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
1564         return ret;
1565 }
1566
1567 /**
1568  *  @brief This function frees the command buffer.
1569  *
1570  *  @param priv         A pointer to struct lbs_private structure
1571  *  @return             0 or -1
1572  */
1573 int lbs_free_cmd_buffer(struct lbs_private *priv)
1574 {
1575         struct cmd_ctrl_node *cmdarray;
1576         unsigned int i;
1577
1578         lbs_deb_enter(LBS_DEB_HOST);
1579
1580         /* need to check if cmd array is allocated or not */
1581         if (priv->cmd_array == NULL) {
1582                 lbs_deb_host("FREE_CMD_BUF: cmd_array is NULL\n");
1583                 goto done;
1584         }
1585
1586         cmdarray = priv->cmd_array;
1587
1588         /* Release shared memory buffers */
1589         for (i = 0; i < LBS_NUM_CMD_BUFFERS; i++) {
1590                 if (cmdarray[i].cmdbuf) {
1591                         kfree(cmdarray[i].cmdbuf);
1592                         cmdarray[i].cmdbuf = NULL;
1593                 }
1594         }
1595
1596         /* Release cmd_ctrl_node */
1597         if (priv->cmd_array) {
1598                 kfree(priv->cmd_array);
1599                 priv->cmd_array = NULL;
1600         }
1601
1602 done:
1603         lbs_deb_leave(LBS_DEB_HOST);
1604         return 0;
1605 }
1606
1607 /**
1608  *  @brief This function gets a free command node if available in
1609  *  command free queue.
1610  *
1611  *  @param priv         A pointer to struct lbs_private structure
1612  *  @return cmd_ctrl_node A pointer to cmd_ctrl_node structure or NULL
1613  */
1614 static struct cmd_ctrl_node *lbs_get_cmd_ctrl_node(struct lbs_private *priv)
1615 {
1616         struct cmd_ctrl_node *tempnode;
1617         unsigned long flags;
1618
1619         lbs_deb_enter(LBS_DEB_HOST);
1620
1621         if (!priv)
1622                 return NULL;
1623
1624         spin_lock_irqsave(&priv->driver_lock, flags);
1625
1626         if (!list_empty(&priv->cmdfreeq)) {
1627                 tempnode = list_first_entry(&priv->cmdfreeq,
1628                                             struct cmd_ctrl_node, list);
1629                 list_del(&tempnode->list);
1630         } else {
1631                 lbs_deb_host("GET_CMD_NODE: cmd_ctrl_node is not available\n");
1632                 tempnode = NULL;
1633         }
1634
1635         spin_unlock_irqrestore(&priv->driver_lock, flags);
1636
1637         lbs_deb_leave(LBS_DEB_HOST);
1638         return tempnode;
1639 }
1640
1641 /**
1642  *  @brief This function executes next command in command
1643  *  pending queue. It will put fimware back to PS mode
1644  *  if applicable.
1645  *
1646  *  @param priv     A pointer to struct lbs_private structure
1647  *  @return        0 or -1
1648  */
1649 int lbs_execute_next_command(struct lbs_private *priv)
1650 {
1651         struct cmd_ctrl_node *cmdnode = NULL;
1652         struct cmd_header *cmd;
1653         unsigned long flags;
1654         int ret = 0;
1655
1656         /* Debug group is LBS_DEB_THREAD and not LBS_DEB_HOST, because the
1657          * only caller to us is lbs_thread() and we get even when a
1658          * data packet is received */
1659         lbs_deb_enter(LBS_DEB_THREAD);
1660
1661         spin_lock_irqsave(&priv->driver_lock, flags);
1662
1663         if (priv->cur_cmd) {
1664                 lbs_pr_alert( "EXEC_NEXT_CMD: already processing command!\n");
1665                 spin_unlock_irqrestore(&priv->driver_lock, flags);
1666                 ret = -1;
1667                 goto done;
1668         }
1669
1670         if (!list_empty(&priv->cmdpendingq)) {
1671                 cmdnode = list_first_entry(&priv->cmdpendingq,
1672                                            struct cmd_ctrl_node, list);
1673         }
1674
1675         spin_unlock_irqrestore(&priv->driver_lock, flags);
1676
1677         if (cmdnode) {
1678                 cmd = cmdnode->cmdbuf;
1679
1680                 if (is_command_allowed_in_ps(le16_to_cpu(cmd->command))) {
1681                         if ((priv->psstate == PS_STATE_SLEEP) ||
1682                             (priv->psstate == PS_STATE_PRE_SLEEP)) {
1683                                 lbs_deb_host(
1684                                        "EXEC_NEXT_CMD: cannot send cmd 0x%04x in psstate %d\n",
1685                                        le16_to_cpu(cmd->command),
1686                                        priv->psstate);
1687                                 ret = -1;
1688                                 goto done;
1689                         }
1690                         lbs_deb_host("EXEC_NEXT_CMD: OK to send command "
1691                                      "0x%04x in psstate %d\n",
1692                                      le16_to_cpu(cmd->command), priv->psstate);
1693                 } else if (priv->psstate != PS_STATE_FULL_POWER) {
1694                         /*
1695                          * 1. Non-PS command:
1696                          * Queue it. set needtowakeup to TRUE if current state
1697                          * is SLEEP, otherwise call lbs_ps_wakeup to send Exit_PS.
1698                          * 2. PS command but not Exit_PS:
1699                          * Ignore it.
1700                          * 3. PS command Exit_PS:
1701                          * Set needtowakeup to TRUE if current state is SLEEP,
1702                          * otherwise send this command down to firmware
1703                          * immediately.
1704                          */
1705                         if (cmd->command != cpu_to_le16(CMD_802_11_PS_MODE)) {
1706                                 /*  Prepare to send Exit PS,
1707                                  *  this non PS command will be sent later */
1708                                 if ((priv->psstate == PS_STATE_SLEEP)
1709                                     || (priv->psstate == PS_STATE_PRE_SLEEP)
1710                                     ) {
1711                                         /* w/ new scheme, it will not reach here.
1712                                            since it is blocked in main_thread. */
1713                                         priv->needtowakeup = 1;
1714                                 } else
1715                                         lbs_ps_wakeup(priv, 0);
1716
1717                                 ret = 0;
1718                                 goto done;
1719                         } else {
1720                                 /*
1721                                  * PS command. Ignore it if it is not Exit_PS.
1722                                  * otherwise send it down immediately.
1723                                  */
1724                                 struct cmd_ds_802_11_ps_mode *psm = (void *)&cmd[1];
1725
1726                                 lbs_deb_host(
1727                                        "EXEC_NEXT_CMD: PS cmd, action 0x%02x\n",
1728                                        psm->action);
1729                                 if (psm->action !=
1730                                     cpu_to_le16(CMD_SUBCMD_EXIT_PS)) {
1731                                         lbs_deb_host(
1732                                                "EXEC_NEXT_CMD: ignore ENTER_PS cmd\n");
1733                                         list_del(&cmdnode->list);
1734                                         spin_lock_irqsave(&priv->driver_lock, flags);
1735                                         lbs_complete_command(priv, cmdnode, 0);
1736                                         spin_unlock_irqrestore(&priv->driver_lock, flags);
1737
1738                                         ret = 0;
1739                                         goto done;
1740                                 }
1741
1742                                 if ((priv->psstate == PS_STATE_SLEEP) ||
1743                                     (priv->psstate == PS_STATE_PRE_SLEEP)) {
1744                                         lbs_deb_host(
1745                                                "EXEC_NEXT_CMD: ignore EXIT_PS cmd in sleep\n");
1746                                         list_del(&cmdnode->list);
1747                                         spin_lock_irqsave(&priv->driver_lock, flags);
1748                                         lbs_complete_command(priv, cmdnode, 0);
1749                                         spin_unlock_irqrestore(&priv->driver_lock, flags);
1750                                         priv->needtowakeup = 1;
1751
1752                                         ret = 0;
1753                                         goto done;
1754                                 }
1755
1756                                 lbs_deb_host(
1757                                        "EXEC_NEXT_CMD: sending EXIT_PS\n");
1758                         }
1759                 }
1760                 list_del(&cmdnode->list);
1761                 lbs_deb_host("EXEC_NEXT_CMD: sending command 0x%04x\n",
1762                             le16_to_cpu(cmd->command));
1763                 lbs_submit_command(priv, cmdnode);
1764         } else {
1765                 /*
1766                  * check if in power save mode, if yes, put the device back
1767                  * to PS mode
1768                  */
1769                 if ((priv->psmode != LBS802_11POWERMODECAM) &&
1770                     (priv->psstate == PS_STATE_FULL_POWER) &&
1771                     ((priv->connect_status == LBS_CONNECTED) ||
1772                     (priv->mesh_connect_status == LBS_CONNECTED))) {
1773                         if (priv->secinfo.WPAenabled ||
1774                             priv->secinfo.WPA2enabled) {
1775                                 /* check for valid WPA group keys */
1776                                 if (priv->wpa_mcast_key.len ||
1777                                     priv->wpa_unicast_key.len) {
1778                                         lbs_deb_host(
1779                                                "EXEC_NEXT_CMD: WPA enabled and GTK_SET"
1780                                                " go back to PS_SLEEP");
1781                                         lbs_ps_sleep(priv, 0);
1782                                 }
1783                         } else {
1784                                 lbs_deb_host(
1785                                        "EXEC_NEXT_CMD: cmdpendingq empty, "
1786                                        "go back to PS_SLEEP");
1787                                 lbs_ps_sleep(priv, 0);
1788                         }
1789                 }
1790         }
1791
1792         ret = 0;
1793 done:
1794         lbs_deb_leave(LBS_DEB_THREAD);
1795         return ret;
1796 }
1797
1798 void lbs_send_iwevcustom_event(struct lbs_private *priv, s8 *str)
1799 {
1800         union iwreq_data iwrq;
1801         u8 buf[50];
1802
1803         lbs_deb_enter(LBS_DEB_WEXT);
1804
1805         memset(&iwrq, 0, sizeof(union iwreq_data));
1806         memset(buf, 0, sizeof(buf));
1807
1808         snprintf(buf, sizeof(buf) - 1, "%s", str);
1809
1810         iwrq.data.length = strlen(buf) + 1 + IW_EV_LCP_LEN;
1811
1812         /* Send Event to upper layer */
1813         lbs_deb_wext("event indication string %s\n", (char *)buf);
1814         lbs_deb_wext("event indication length %d\n", iwrq.data.length);
1815         lbs_deb_wext("sending wireless event IWEVCUSTOM for %s\n", str);
1816
1817         wireless_send_event(priv->dev, IWEVCUSTOM, &iwrq, buf);
1818
1819         lbs_deb_leave(LBS_DEB_WEXT);
1820 }
1821
1822 static void lbs_send_confirmsleep(struct lbs_private *priv)
1823 {
1824         unsigned long flags;
1825         int ret;
1826
1827         lbs_deb_enter(LBS_DEB_HOST);
1828         lbs_deb_hex(LBS_DEB_HOST, "sleep confirm", (u8 *) &confirm_sleep,
1829                 sizeof(confirm_sleep));
1830
1831         ret = priv->hw_host_to_card(priv, MVMS_CMD, (u8 *) &confirm_sleep,
1832                 sizeof(confirm_sleep));
1833         if (ret) {
1834                 lbs_pr_alert("confirm_sleep failed\n");
1835                 goto out;
1836         }
1837
1838         spin_lock_irqsave(&priv->driver_lock, flags);
1839
1840         /* If nothing to do, go back to sleep (?) */
1841         if (!__kfifo_len(priv->event_fifo) && !priv->resp_len[priv->resp_idx])
1842                 priv->psstate = PS_STATE_SLEEP;
1843
1844         spin_unlock_irqrestore(&priv->driver_lock, flags);
1845
1846 out:
1847         lbs_deb_leave(LBS_DEB_HOST);
1848 }
1849
1850 void lbs_ps_sleep(struct lbs_private *priv, int wait_option)
1851 {
1852         lbs_deb_enter(LBS_DEB_HOST);
1853
1854         /*
1855          * PS is currently supported only in Infrastructure mode
1856          * Remove this check if it is to be supported in IBSS mode also
1857          */
1858
1859         lbs_prepare_and_send_command(priv, CMD_802_11_PS_MODE,
1860                               CMD_SUBCMD_ENTER_PS, wait_option, 0, NULL);
1861
1862         lbs_deb_leave(LBS_DEB_HOST);
1863 }
1864
1865 /**
1866  *  @brief This function sends Exit_PS command to firmware.
1867  *
1868  *  @param priv         A pointer to struct lbs_private structure
1869  *  @param wait_option  wait response or not
1870  *  @return             n/a
1871  */
1872 void lbs_ps_wakeup(struct lbs_private *priv, int wait_option)
1873 {
1874         __le32 Localpsmode;
1875
1876         lbs_deb_enter(LBS_DEB_HOST);
1877
1878         Localpsmode = cpu_to_le32(LBS802_11POWERMODECAM);
1879
1880         lbs_prepare_and_send_command(priv, CMD_802_11_PS_MODE,
1881                               CMD_SUBCMD_EXIT_PS,
1882                               wait_option, 0, &Localpsmode);
1883
1884         lbs_deb_leave(LBS_DEB_HOST);
1885 }
1886
1887 /**
1888  *  @brief This function checks condition and prepares to
1889  *  send sleep confirm command to firmware if ok.
1890  *
1891  *  @param priv         A pointer to struct lbs_private structure
1892  *  @param psmode       Power Saving mode
1893  *  @return             n/a
1894  */
1895 void lbs_ps_confirm_sleep(struct lbs_private *priv)
1896 {
1897         unsigned long flags =0;
1898         int allowed = 1;
1899
1900         lbs_deb_enter(LBS_DEB_HOST);
1901
1902         if (priv->dnld_sent) {
1903                 allowed = 0;
1904                 lbs_deb_host("dnld_sent was set\n");
1905         }
1906
1907         spin_lock_irqsave(&priv->driver_lock, flags);
1908         /* In-progress command? */
1909         if (priv->cur_cmd) {
1910                 allowed = 0;
1911                 lbs_deb_host("cur_cmd was set\n");
1912         }
1913
1914         /* Pending events or command responses? */
1915         if (__kfifo_len(priv->event_fifo) || priv->resp_len[priv->resp_idx]) {
1916                 allowed = 0;
1917                 lbs_deb_host("pending events or command responses\n");
1918         }
1919         spin_unlock_irqrestore(&priv->driver_lock, flags);
1920
1921         if (allowed) {
1922                 lbs_deb_host("sending lbs_ps_confirm_sleep\n");
1923                 lbs_send_confirmsleep(priv);
1924         } else {
1925                 lbs_deb_host("sleep confirm has been delayed\n");
1926         }
1927
1928         lbs_deb_leave(LBS_DEB_HOST);
1929 }
1930
1931
1932 static struct cmd_ctrl_node *__lbs_cmd_async(struct lbs_private *priv,
1933         uint16_t command, struct cmd_header *in_cmd, int in_cmd_size,
1934         int (*callback)(struct lbs_private *, unsigned long, struct cmd_header *),
1935         unsigned long callback_arg)
1936 {
1937         struct cmd_ctrl_node *cmdnode;
1938
1939         lbs_deb_enter(LBS_DEB_HOST);
1940
1941         if (priv->surpriseremoved) {
1942                 lbs_deb_host("PREP_CMD: card removed\n");
1943                 cmdnode = ERR_PTR(-ENOENT);
1944                 goto done;
1945         }
1946
1947         cmdnode = lbs_get_cmd_ctrl_node(priv);
1948         if (cmdnode == NULL) {
1949                 lbs_deb_host("PREP_CMD: cmdnode is NULL\n");
1950
1951                 /* Wake up main thread to execute next command */
1952                 wake_up_interruptible(&priv->waitq);
1953                 cmdnode = ERR_PTR(-ENOBUFS);
1954                 goto done;
1955         }
1956
1957         cmdnode->callback = callback;
1958         cmdnode->callback_arg = callback_arg;
1959
1960         /* Copy the incoming command to the buffer */
1961         memcpy(cmdnode->cmdbuf, in_cmd, in_cmd_size);
1962
1963         /* Set sequence number, clean result, move to buffer */
1964         priv->seqnum++;
1965         cmdnode->cmdbuf->command = cpu_to_le16(command);
1966         cmdnode->cmdbuf->size    = cpu_to_le16(in_cmd_size);
1967         cmdnode->cmdbuf->seqnum  = cpu_to_le16(priv->seqnum);
1968         cmdnode->cmdbuf->result  = 0;
1969
1970         lbs_deb_host("PREP_CMD: command 0x%04x\n", command);
1971
1972         cmdnode->cmdwaitqwoken = 0;
1973         lbs_queue_cmd(priv, cmdnode);
1974         wake_up_interruptible(&priv->waitq);
1975
1976  done:
1977         lbs_deb_leave_args(LBS_DEB_HOST, "ret %p", cmdnode);
1978         return cmdnode;
1979 }
1980
1981 void lbs_cmd_async(struct lbs_private *priv, uint16_t command,
1982         struct cmd_header *in_cmd, int in_cmd_size)
1983 {
1984         lbs_deb_enter(LBS_DEB_CMD);
1985         __lbs_cmd_async(priv, command, in_cmd, in_cmd_size,
1986                 lbs_cmd_async_callback, 0);
1987         lbs_deb_leave(LBS_DEB_CMD);
1988 }
1989
1990 int __lbs_cmd(struct lbs_private *priv, uint16_t command,
1991               struct cmd_header *in_cmd, int in_cmd_size,
1992               int (*callback)(struct lbs_private *, unsigned long, struct cmd_header *),
1993               unsigned long callback_arg)
1994 {
1995         struct cmd_ctrl_node *cmdnode;
1996         unsigned long flags;
1997         int ret = 0;
1998
1999         lbs_deb_enter(LBS_DEB_HOST);
2000
2001         cmdnode = __lbs_cmd_async(priv, command, in_cmd, in_cmd_size,
2002                                   callback, callback_arg);
2003         if (IS_ERR(cmdnode)) {
2004                 ret = PTR_ERR(cmdnode);
2005                 goto done;
2006         }
2007
2008         might_sleep();
2009         wait_event_interruptible(cmdnode->cmdwait_q, cmdnode->cmdwaitqwoken);
2010
2011         spin_lock_irqsave(&priv->driver_lock, flags);
2012         ret = cmdnode->result;
2013         if (ret)
2014                 lbs_pr_info("PREP_CMD: command 0x%04x failed: %d\n",
2015                             command, ret);
2016
2017         __lbs_cleanup_and_insert_cmd(priv, cmdnode);
2018         spin_unlock_irqrestore(&priv->driver_lock, flags);
2019
2020 done:
2021         lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
2022         return ret;
2023 }
2024 EXPORT_SYMBOL_GPL(__lbs_cmd);
2025
2026