drivers/net/wireless/marvell/libertas_tf/cmd.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  *  Copyright (C) 2008, cozybit Inc.
   4  *  Copyright (C) 2003-2006, Marvell International Ltd.
   5  */
   6 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   7
   8 #include <linux/hardirq.h>
   9 #include <linux/slab.h>
  10 #include <linux/export.h>
  11
  12 #include "libertas_tf.h"
  13
  14 static const struct channel_range channel_ranges[] = {
  15         { LBTF_REGDOMAIN_US,            1, 12 },
  16         { LBTF_REGDOMAIN_CA,            1, 12 },
  17         { LBTF_REGDOMAIN_EU,            1, 14 },
  18         { LBTF_REGDOMAIN_JP,            1, 14 },
  19         { LBTF_REGDOMAIN_SP,            1, 14 },
  20         { LBTF_REGDOMAIN_FR,            1, 14 },
  21 };
  22
  23 static u16 lbtf_region_code_to_index[MRVDRV_MAX_REGION_CODE] =
  24 {
  25         LBTF_REGDOMAIN_US, LBTF_REGDOMAIN_CA, LBTF_REGDOMAIN_EU,
  26         LBTF_REGDOMAIN_SP, LBTF_REGDOMAIN_FR, LBTF_REGDOMAIN_JP,
  27 };
  28
  29 static struct cmd_ctrl_node *lbtf_get_cmd_ctrl_node(struct lbtf_private *priv);
  30
  31
  32 /**
  33  *  lbtf_cmd_copyback - Simple callback that copies response back into command
  34  *
  35  *  @priv       A pointer to struct lbtf_private structure
  36  *  @extra      A pointer to the original command structure for which
  37  *              'resp' is a response
  38  *  @resp       A pointer to the command response
  39  *
  40  *  Returns: 0 on success, error on failure
  41  */
  42 int lbtf_cmd_copyback(struct lbtf_private *priv, unsigned long extra,
  43                      struct cmd_header *resp)
  44 {
  45         struct cmd_header *buf = (void *)extra;
  46         uint16_t copy_len;
  47
  48         copy_len = min(le16_to_cpu(buf->size), le16_to_cpu(resp->size));
  49         memcpy(buf, resp, copy_len);
  50         return 0;
  51 }
  52 EXPORT_SYMBOL_GPL(lbtf_cmd_copyback);
  53
  54 #define CHAN_TO_IDX(chan) ((chan) - 1)
  55
  56 static void lbtf_geo_init(struct lbtf_private *priv)
  57 {
  58         const struct channel_range *range = channel_ranges;
  59         u8 ch;
  60         int i;
  61
  62         for (i = 0; i < ARRAY_SIZE(channel_ranges); i++)
  63                 if (channel_ranges[i].regdomain == priv->regioncode) {
  64                         range = &channel_ranges[i];
  65                         break;
  66                 }
  67
  68         for (ch = range->start; ch < range->end; ch++)
  69                 priv->channels[CHAN_TO_IDX(ch)].flags = 0;
  70 }
  71
  72 /**
  73  *  lbtf_update_hw_spec: Updates the hardware details.
  74  *
  75  *  @priv       A pointer to struct lbtf_private structure
  76  *
  77  *  Returns: 0 on success, error on failure
  78  */
  79 int lbtf_update_hw_spec(struct lbtf_private *priv)
  80 {
  81         struct cmd_ds_get_hw_spec cmd;
  82         int ret = -1;
  83         u32 i;
  84
  85         lbtf_deb_enter(LBTF_DEB_CMD);
  86
  87         memset(&cmd, 0, sizeof(cmd));
  88         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
  89         memcpy(cmd.permanentaddr, priv->current_addr, ETH_ALEN);
  90         ret = lbtf_cmd_with_response(priv, CMD_GET_HW_SPEC, &cmd);
  91         if (ret)
  92                 goto out;
  93
  94         priv->fwcapinfo = le32_to_cpu(cmd.fwcapinfo);
  95
  96         /* The firmware release is in an interesting format: the patch
  97          * level is in the most significant nibble ... so fix that: */
  98         priv->fwrelease = le32_to_cpu(cmd.fwrelease);
  99         priv->fwrelease = (priv->fwrelease << 8) |
 100                 (priv->fwrelease >> 24 & 0xff);
 101
 102         printk(KERN_INFO "libertastf: %pM, fw %u.%u.%up%u, cap 0x%08x\n",
 103                 cmd.permanentaddr,
 104                 priv->fwrelease >> 24 & 0xff,
 105                 priv->fwrelease >> 16 & 0xff,
 106                 priv->fwrelease >>  8 & 0xff,
 107                 priv->fwrelease       & 0xff,
 108                 priv->fwcapinfo);
 109         lbtf_deb_cmd("GET_HW_SPEC: hardware interface 0x%x, hardware spec 0x%04x\n",
 110                     cmd.hwifversion, cmd.version);
 111
 112         /* Clamp region code to 8-bit since FW spec indicates that it should
 113          * only ever be 8-bit, even though the field size is 16-bit.  Some
 114          * firmware returns non-zero high 8 bits here.
 115          */
 116         priv->regioncode = le16_to_cpu(cmd.regioncode) & 0xFF;
 117
 118         for (i = 0; i < MRVDRV_MAX_REGION_CODE; i++) {
 119                 /* use the region code to search for the index */
 120                 if (priv->regioncode == lbtf_region_code_to_index[i])
 121                         break;
 122         }
 123
 124         /* if it's unidentified region code, use the default (USA) */
 125         if (i >= MRVDRV_MAX_REGION_CODE) {
 126                 priv->regioncode = 0x10;
 127                 pr_info("unidentified region code; using the default (USA)\n");
 128         }
 129
 130         if (priv->current_addr[0] == 0xff)
 131                 memmove(priv->current_addr, cmd.permanentaddr, ETH_ALEN);
 132
 133         SET_IEEE80211_PERM_ADDR(priv->hw, priv->current_addr);
 134
 135         lbtf_geo_init(priv);
 136 out:
 137         lbtf_deb_leave(LBTF_DEB_CMD);
 138         return ret;
 139 }
 140
 141 /**
 142  *  lbtf_set_channel: Set the radio channel
 143  *
 144  *  @priv       A pointer to struct lbtf_private structure
 145  *  @channel    The desired channel, or 0 to clear a locked channel
 146  *
 147  *  Returns: 0 on success, error on failure
 148  */
 149 int lbtf_set_channel(struct lbtf_private *priv, u8 channel)
 150 {
 151         int ret = 0;
 152         struct cmd_ds_802_11_rf_channel cmd;
 153
 154         lbtf_deb_enter(LBTF_DEB_CMD);
 155
 156         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 157         cmd.action = cpu_to_le16(CMD_OPT_802_11_RF_CHANNEL_SET);
 158         cmd.channel = cpu_to_le16(channel);
 159
 160         ret = lbtf_cmd_with_response(priv, CMD_802_11_RF_CHANNEL, &cmd);
 161         lbtf_deb_leave_args(LBTF_DEB_CMD, "ret %d", ret);
 162         return ret;
 163 }
 164
 165 int lbtf_beacon_set(struct lbtf_private *priv, struct sk_buff *beacon)
 166 {
 167         struct cmd_ds_802_11_beacon_set cmd;
 168         int size;
 169
 170         lbtf_deb_enter(LBTF_DEB_CMD);
 171
 172         if (beacon->len > MRVL_MAX_BCN_SIZE) {
 173                 lbtf_deb_leave_args(LBTF_DEB_CMD, "ret %d", -1);
 174                 return -1;
 175         }
 176         size =  sizeof(cmd) - sizeof(cmd.beacon) + beacon->len;
 177         cmd.hdr.size = cpu_to_le16(size);
 178         cmd.len = cpu_to_le16(beacon->len);
 179         memcpy(cmd.beacon, (u8 *) beacon->data, beacon->len);
 180
 181         lbtf_cmd_async(priv, CMD_802_11_BEACON_SET, &cmd.hdr, size);
 182
 183         lbtf_deb_leave_args(LBTF_DEB_CMD, "ret %d", 0);
 184         return 0;
 185 }
 186
 187 int lbtf_beacon_ctrl(struct lbtf_private *priv, bool beacon_enable,
 188                      int beacon_int)
 189 {
 190         struct cmd_ds_802_11_beacon_control cmd;
 191         lbtf_deb_enter(LBTF_DEB_CMD);
 192
 193         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 194         cmd.action = cpu_to_le16(CMD_ACT_SET);
 195         cmd.beacon_enable = cpu_to_le16(beacon_enable);
 196         cmd.beacon_period = cpu_to_le16(beacon_int);
 197
 198         lbtf_cmd_async(priv, CMD_802_11_BEACON_CTRL, &cmd.hdr, sizeof(cmd));
 199
 200         lbtf_deb_leave(LBTF_DEB_CMD);
 201         return 0;
 202 }
 203
 204 static void lbtf_queue_cmd(struct lbtf_private *priv,
 205                           struct cmd_ctrl_node *cmdnode)
 206 {
 207         unsigned long flags;
 208         lbtf_deb_enter(LBTF_DEB_HOST);
 209
 210         if (!cmdnode) {
 211                 lbtf_deb_host("QUEUE_CMD: cmdnode is NULL\n");
 212                 goto qcmd_done;
 213         }
 214
 215         if (!cmdnode->cmdbuf->size) {
 216                 lbtf_deb_host("DNLD_CMD: cmd size is zero\n");
 217                 goto qcmd_done;
 218         }
 219
 220         cmdnode->result = 0;
 221         spin_lock_irqsave(&priv->driver_lock, flags);
 222         list_add_tail(&cmdnode->list, &priv->cmdpendingq);
 223         spin_unlock_irqrestore(&priv->driver_lock, flags);
 224
 225         lbtf_deb_host("QUEUE_CMD: inserted command 0x%04x into cmdpendingq\n",
 226                      le16_to_cpu(cmdnode->cmdbuf->command));
 227
 228 qcmd_done:
 229         lbtf_deb_leave(LBTF_DEB_HOST);
 230 }
 231
 232 static void lbtf_submit_command(struct lbtf_private *priv,
 233                                struct cmd_ctrl_node *cmdnode)
 234 {
 235         unsigned long flags;
 236         struct cmd_header *cmd;
 237         uint16_t cmdsize;
 238         uint16_t command;
 239         int timeo = 5 * HZ;
 240         int ret;
 241
 242         lbtf_deb_enter(LBTF_DEB_HOST);
 243
 244         cmd = cmdnode->cmdbuf;
 245
 246         spin_lock_irqsave(&priv->driver_lock, flags);
 247         priv->cur_cmd = cmdnode;
 248         cmdsize = le16_to_cpu(cmd->size);
 249         command = le16_to_cpu(cmd->command);
 250
 251         lbtf_deb_cmd("DNLD_CMD: command 0x%04x, seq %d, size %d\n",
 252                      command, le16_to_cpu(cmd->seqnum), cmdsize);
 253         lbtf_deb_hex(LBTF_DEB_CMD, "DNLD_CMD", (void *) cmdnode->cmdbuf, cmdsize);
 254
 255         ret = priv->ops->hw_host_to_card(priv, MVMS_CMD, (u8 *)cmd, cmdsize);
 256         spin_unlock_irqrestore(&priv->driver_lock, flags);
 257
 258         if (ret) {
 259                 pr_info("DNLD_CMD: hw_host_to_card failed: %d\n", ret);
 260                 /* Let the timer kick in and retry, and potentially reset
 261                    the whole thing if the condition persists */
 262                 timeo = HZ;
 263         }
 264
 265         /* Setup the timer after transmit command */
 266         mod_timer(&priv->command_timer, jiffies + timeo);
 267
 268         lbtf_deb_leave(LBTF_DEB_HOST);
 269 }
 270
 271 /**
 272  *  This function inserts command node to cmdfreeq
 273  *  after cleans it. Requires priv->driver_lock held.
 274  */
 275 static void __lbtf_cleanup_and_insert_cmd(struct lbtf_private *priv,
 276                                          struct cmd_ctrl_node *cmdnode)
 277 {
 278         lbtf_deb_enter(LBTF_DEB_HOST);
 279
 280         if (!cmdnode)
 281                 goto cl_ins_out;
 282
 283         cmdnode->callback = NULL;
 284         cmdnode->callback_arg = 0;
 285
 286         memset(cmdnode->cmdbuf, 0, LBS_CMD_BUFFER_SIZE);
 287
 288         list_add_tail(&cmdnode->list, &priv->cmdfreeq);
 289
 290 cl_ins_out:
 291         lbtf_deb_leave(LBTF_DEB_HOST);
 292 }
 293
 294 static void lbtf_cleanup_and_insert_cmd(struct lbtf_private *priv,
 295         struct cmd_ctrl_node *ptempcmd)
 296 {
 297         unsigned long flags;
 298
 299         spin_lock_irqsave(&priv->driver_lock, flags);
 300         __lbtf_cleanup_and_insert_cmd(priv, ptempcmd);
 301         spin_unlock_irqrestore(&priv->driver_lock, flags);
 302 }
 303
 304 void lbtf_complete_command(struct lbtf_private *priv, struct cmd_ctrl_node *cmd,
 305                           int result)
 306 {
 307         cmd->result = result;
 308         cmd->cmdwaitqwoken = 1;
 309         wake_up_interruptible(&cmd->cmdwait_q);
 310
 311         if (!cmd->callback)
 312                 __lbtf_cleanup_and_insert_cmd(priv, cmd);
 313         priv->cur_cmd = NULL;
 314 }
 315
 316 int lbtf_cmd_set_mac_multicast_addr(struct lbtf_private *priv)
 317 {
 318         struct cmd_ds_mac_multicast_addr cmd;
 319
 320         lbtf_deb_enter(LBTF_DEB_CMD);
 321
 322         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 323         cmd.action = cpu_to_le16(CMD_ACT_SET);
 324
 325         cmd.nr_of_adrs = cpu_to_le16((u16) priv->nr_of_multicastmacaddr);
 326
 327         lbtf_deb_cmd("MULTICAST_ADR: setting %d addresses\n", cmd.nr_of_adrs);
 328
 329         memcpy(cmd.maclist, priv->multicastlist,
 330                priv->nr_of_multicastmacaddr * ETH_ALEN);
 331
 332         lbtf_cmd_async(priv, CMD_MAC_MULTICAST_ADR, &cmd.hdr, sizeof(cmd));
 333
 334         lbtf_deb_leave(LBTF_DEB_CMD);
 335         return 0;
 336 }
 337
 338 void lbtf_set_mode(struct lbtf_private *priv, enum lbtf_mode mode)
 339 {
 340         struct cmd_ds_set_mode cmd;
 341         lbtf_deb_enter(LBTF_DEB_WEXT);
 342
 343         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 344         cmd.mode = cpu_to_le16(mode);
 345         lbtf_deb_wext("Switching to mode: 0x%x\n", mode);
 346         lbtf_cmd_async(priv, CMD_802_11_SET_MODE, &cmd.hdr, sizeof(cmd));
 347
 348         lbtf_deb_leave(LBTF_DEB_WEXT);
 349 }
 350
 351 void lbtf_set_bssid(struct lbtf_private *priv, bool activate, const u8 *bssid)
 352 {
 353         struct cmd_ds_set_bssid cmd;
 354         lbtf_deb_enter(LBTF_DEB_CMD);
 355
 356         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 357         cmd.activate = activate ? 1 : 0;
 358         if (activate)
 359                 memcpy(cmd.bssid, bssid, ETH_ALEN);
 360
 361         lbtf_cmd_async(priv, CMD_802_11_SET_BSSID, &cmd.hdr, sizeof(cmd));
 362         lbtf_deb_leave(LBTF_DEB_CMD);
 363 }
 364
 365 int lbtf_set_mac_address(struct lbtf_private *priv, uint8_t *mac_addr)
 366 {
 367         struct cmd_ds_802_11_mac_address cmd;
 368         lbtf_deb_enter(LBTF_DEB_CMD);
 369
 370         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 371         cmd.action = cpu_to_le16(CMD_ACT_SET);
 372
 373         memcpy(cmd.macadd, mac_addr, ETH_ALEN);
 374
 375         lbtf_cmd_async(priv, CMD_802_11_MAC_ADDRESS, &cmd.hdr, sizeof(cmd));
 376         lbtf_deb_leave(LBTF_DEB_CMD);
 377         return 0;
 378 }
 379
 380 int lbtf_set_radio_control(struct lbtf_private *priv)
 381 {
 382         int ret = 0;
 383         struct cmd_ds_802_11_radio_control cmd;
 384
 385         lbtf_deb_enter(LBTF_DEB_CMD);
 386
 387         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 388         cmd.action = cpu_to_le16(CMD_ACT_SET);
 389
 390         switch (priv->preamble) {
 391         case CMD_TYPE_SHORT_PREAMBLE:
 392                 cmd.control = cpu_to_le16(SET_SHORT_PREAMBLE);
 393                 break;
 394
 395         case CMD_TYPE_LONG_PREAMBLE:
 396                 cmd.control = cpu_to_le16(SET_LONG_PREAMBLE);
 397                 break;
 398
 399         case CMD_TYPE_AUTO_PREAMBLE:
 400         default:
 401                 cmd.control = cpu_to_le16(SET_AUTO_PREAMBLE);
 402                 break;
 403         }
 404
 405         if (priv->radioon)
 406                 cmd.control |= cpu_to_le16(TURN_ON_RF);
 407         else
 408                 cmd.control &= cpu_to_le16(~TURN_ON_RF);
 409
 410         lbtf_deb_cmd("RADIO_SET: radio %d, preamble %d\n", priv->radioon,
 411                     priv->preamble);
 412
 413         ret = lbtf_cmd_with_response(priv, CMD_802_11_RADIO_CONTROL, &cmd);
 414
 415         lbtf_deb_leave_args(LBTF_DEB_CMD, "ret %d", ret);
 416         return ret;
 417 }
 418
 419 void lbtf_set_mac_control(struct lbtf_private *priv)
 420 {
 421         struct cmd_ds_mac_control cmd;
 422         lbtf_deb_enter(LBTF_DEB_CMD);
 423
 424         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 425         cmd.action = cpu_to_le16(priv->mac_control);
 426         cmd.reserved = 0;
 427
 428         lbtf_cmd_async(priv, CMD_MAC_CONTROL,
 429                 &cmd.hdr, sizeof(cmd));
 430
 431         lbtf_deb_leave(LBTF_DEB_CMD);
 432 }
 433
 434 /**
 435  *  lbtf_allocate_cmd_buffer - Allocates cmd buffer, links it to free cmd queue
 436  *
 437  *  @priv       A pointer to struct lbtf_private structure
 438  *
 439  *  Returns: 0 on success.
 440  */
 441 int lbtf_allocate_cmd_buffer(struct lbtf_private *priv)
 442 {
 443         int ret = 0;
 444         u32 bufsize;
 445         u32 i;
 446         struct cmd_ctrl_node *cmdarray;
 447
 448         lbtf_deb_enter(LBTF_DEB_HOST);
 449
 450         /* Allocate and initialize the command array */
 451         bufsize = sizeof(struct cmd_ctrl_node) * LBS_NUM_CMD_BUFFERS;
 452         cmdarray = kzalloc(bufsize, GFP_KERNEL);
 453         if (!cmdarray) {
 454                 lbtf_deb_host("ALLOC_CMD_BUF: tempcmd_array is NULL\n");
 455                 ret = -1;
 456                 goto done;
 457         }
 458         priv->cmd_array = cmdarray;
 459
 460         /* Allocate and initialize each command buffer in the command array */
 461         for (i = 0; i < LBS_NUM_CMD_BUFFERS; i++) {
 462                 cmdarray[i].cmdbuf = kzalloc(LBS_CMD_BUFFER_SIZE, GFP_KERNEL);
 463                 if (!cmdarray[i].cmdbuf) {
 464                         lbtf_deb_host("ALLOC_CMD_BUF: ptempvirtualaddr is NULL\n");
 465                         ret = -1;
 466                         goto done;
 467                 }
 468         }
 469
 470         for (i = 0; i < LBS_NUM_CMD_BUFFERS; i++) {
 471                 init_waitqueue_head(&cmdarray[i].cmdwait_q);
 472                 lbtf_cleanup_and_insert_cmd(priv, &cmdarray[i]);
 473         }
 474
 475         ret = 0;
 476
 477 done:
 478         lbtf_deb_leave_args(LBTF_DEB_HOST, "ret %d", ret);
 479         return ret;
 480 }
 481
 482 /**
 483  *  lbtf_free_cmd_buffer - Frees the cmd buffer.
 484  *
 485  *  @priv       A pointer to struct lbtf_private structure
 486  *
 487  *  Returns: 0
 488  */
 489 int lbtf_free_cmd_buffer(struct lbtf_private *priv)
 490 {
 491         struct cmd_ctrl_node *cmdarray;
 492         unsigned int i;
 493
 494         lbtf_deb_enter(LBTF_DEB_HOST);
 495
 496         /* need to check if cmd array is allocated or not */
 497         if (priv->cmd_array == NULL) {
 498                 lbtf_deb_host("FREE_CMD_BUF: cmd_array is NULL\n");
 499                 goto done;
 500         }
 501
 502         cmdarray = priv->cmd_array;
 503
 504         /* Release shared memory buffers */
 505         for (i = 0; i < LBS_NUM_CMD_BUFFERS; i++) {
 506                 kfree(cmdarray[i].cmdbuf);
 507                 cmdarray[i].cmdbuf = NULL;
 508         }
 509
 510         /* Release cmd_ctrl_node */
 511         kfree(priv->cmd_array);
 512         priv->cmd_array = NULL;
 513
 514 done:
 515         lbtf_deb_leave(LBTF_DEB_HOST);
 516         return 0;
 517 }
 518
 519 /**
 520  *  lbtf_get_cmd_ctrl_node - Gets free cmd node from free cmd queue.
 521  *
 522  *  @priv               A pointer to struct lbtf_private structure
 523  *
 524  *  Returns: pointer to a struct cmd_ctrl_node or NULL if none available.
 525  */
 526 static struct cmd_ctrl_node *lbtf_get_cmd_ctrl_node(struct lbtf_private *priv)
 527 {
 528         struct cmd_ctrl_node *tempnode;
 529         unsigned long flags;
 530
 531         lbtf_deb_enter(LBTF_DEB_HOST);
 532
 533         if (!priv)
 534                 return NULL;
 535
 536         spin_lock_irqsave(&priv->driver_lock, flags);
 537
 538         if (!list_empty(&priv->cmdfreeq)) {
 539                 tempnode = list_first_entry(&priv->cmdfreeq,
 540                                             struct cmd_ctrl_node, list);
 541                 list_del(&tempnode->list);
 542         } else {
 543                 lbtf_deb_host("GET_CMD_NODE: cmd_ctrl_node is not available\n");
 544                 tempnode = NULL;
 545         }
 546
 547         spin_unlock_irqrestore(&priv->driver_lock, flags);
 548
 549         lbtf_deb_leave(LBTF_DEB_HOST);
 550         return tempnode;
 551 }
 552
 553 /**
 554  *  lbtf_execute_next_command: execute next command in cmd pending queue.
 555  *
 556  *  @priv     A pointer to struct lbtf_private structure
 557  *
 558  *  Returns: 0 on success.
 559  */
 560 int lbtf_execute_next_command(struct lbtf_private *priv)
 561 {
 562         struct cmd_ctrl_node *cmdnode = NULL;
 563         struct cmd_header *cmd;
 564         unsigned long flags;
 565         int ret = 0;
 566
 567         /* Debug group is lbtf_deb_THREAD and not lbtf_deb_HOST, because the
 568          * only caller to us is lbtf_thread() and we get even when a
 569          * data packet is received */
 570         lbtf_deb_enter(LBTF_DEB_THREAD);
 571
 572         spin_lock_irqsave(&priv->driver_lock, flags);
 573
 574         if (priv->cur_cmd) {
 575                 pr_alert("EXEC_NEXT_CMD: already processing command!\n");
 576                 spin_unlock_irqrestore(&priv->driver_lock, flags);
 577                 ret = -1;
 578                 goto done;
 579         }
 580
 581         if (!list_empty(&priv->cmdpendingq)) {
 582                 cmdnode = list_first_entry(&priv->cmdpendingq,
 583                                            struct cmd_ctrl_node, list);
 584         }
 585
 586         if (cmdnode) {
 587                 cmd = cmdnode->cmdbuf;
 588
 589                 list_del(&cmdnode->list);
 590                 lbtf_deb_host("EXEC_NEXT_CMD: sending command 0x%04x\n",
 591                             le16_to_cpu(cmd->command));
 592                 spin_unlock_irqrestore(&priv->driver_lock, flags);
 593                 lbtf_submit_command(priv, cmdnode);
 594         } else
 595                 spin_unlock_irqrestore(&priv->driver_lock, flags);
 596
 597         ret = 0;
 598 done:
 599         lbtf_deb_leave(LBTF_DEB_THREAD);
 600         return ret;
 601 }
 602
 603 static struct cmd_ctrl_node *__lbtf_cmd_async(struct lbtf_private *priv,
 604         uint16_t command, struct cmd_header *in_cmd, int in_cmd_size,
 605         int (*callback)(struct lbtf_private *, unsigned long,
 606                         struct cmd_header *),
 607         unsigned long callback_arg)
 608 {
 609         struct cmd_ctrl_node *cmdnode;
 610
 611         lbtf_deb_enter(LBTF_DEB_HOST);
 612
 613         if (priv->surpriseremoved) {
 614                 lbtf_deb_host("PREP_CMD: card removed\n");
 615                 cmdnode = ERR_PTR(-ENOENT);
 616                 goto done;
 617         }
 618
 619         cmdnode = lbtf_get_cmd_ctrl_node(priv);
 620         if (cmdnode == NULL) {
 621                 lbtf_deb_host("PREP_CMD: cmdnode is NULL\n");
 622
 623                 /* Wake up main thread to execute next command */
 624                 queue_work(lbtf_wq, &priv->cmd_work);
 625                 cmdnode = ERR_PTR(-ENOBUFS);
 626                 goto done;
 627         }
 628
 629         cmdnode->callback = callback;
 630         cmdnode->callback_arg = callback_arg;
 631
 632         /* Copy the incoming command to the buffer */
 633         memcpy(cmdnode->cmdbuf, in_cmd, in_cmd_size);
 634
 635         /* Set sequence number, clean result, move to buffer */
 636         priv->seqnum++;
 637         cmdnode->cmdbuf->command = cpu_to_le16(command);
 638         cmdnode->cmdbuf->size    = cpu_to_le16(in_cmd_size);
 639         cmdnode->cmdbuf->seqnum  = cpu_to_le16(priv->seqnum);
 640         cmdnode->cmdbuf->result  = 0;
 641
 642         lbtf_deb_host("PREP_CMD: command 0x%04x\n", command);
 643
 644         cmdnode->cmdwaitqwoken = 0;
 645         lbtf_queue_cmd(priv, cmdnode);
 646         queue_work(lbtf_wq, &priv->cmd_work);
 647
 648  done:
 649         lbtf_deb_leave_args(LBTF_DEB_HOST, "ret %p", cmdnode);
 650         return cmdnode;
 651 }
 652
 653 void lbtf_cmd_async(struct lbtf_private *priv, uint16_t command,
 654         struct cmd_header *in_cmd, int in_cmd_size)
 655 {
 656         lbtf_deb_enter(LBTF_DEB_CMD);
 657         __lbtf_cmd_async(priv, command, in_cmd, in_cmd_size, NULL, 0);
 658         lbtf_deb_leave(LBTF_DEB_CMD);
 659 }
 660
 661 int __lbtf_cmd(struct lbtf_private *priv, uint16_t command,
 662               struct cmd_header *in_cmd, int in_cmd_size,
 663               int (*callback)(struct lbtf_private *,
 664                               unsigned long, struct cmd_header *),
 665               unsigned long callback_arg)
 666 {
 667         struct cmd_ctrl_node *cmdnode;
 668         unsigned long flags;
 669         int ret = 0;
 670
 671         lbtf_deb_enter(LBTF_DEB_HOST);
 672
 673         cmdnode = __lbtf_cmd_async(priv, command, in_cmd, in_cmd_size,
 674                                   callback, callback_arg);
 675         if (IS_ERR(cmdnode)) {
 676                 ret = PTR_ERR(cmdnode);
 677                 goto done;
 678         }
 679
 680         might_sleep();
 681         ret = wait_event_interruptible(cmdnode->cmdwait_q,
 682                                        cmdnode->cmdwaitqwoken);
 683         if (ret) {
 684                 pr_info("PREP_CMD: command 0x%04x interrupted by signal: %d\n",
 685                             command, ret);
 686                 goto done;
 687         }
 688
 689         spin_lock_irqsave(&priv->driver_lock, flags);
 690         ret = cmdnode->result;
 691         if (ret)
 692                 pr_info("PREP_CMD: command 0x%04x failed: %d\n",
 693                             command, ret);
 694
 695         __lbtf_cleanup_and_insert_cmd(priv, cmdnode);
 696         spin_unlock_irqrestore(&priv->driver_lock, flags);
 697
 698 done:
 699         lbtf_deb_leave_args(LBTF_DEB_HOST, "ret %d", ret);
 700         return ret;
 701 }
 702 EXPORT_SYMBOL_GPL(__lbtf_cmd);
 703
 704 /* Call holding driver_lock */
 705 void lbtf_cmd_response_rx(struct lbtf_private *priv)
 706 {
 707         priv->cmd_response_rxed = 1;
 708         queue_work(lbtf_wq, &priv->cmd_work);
 709 }
 710 EXPORT_SYMBOL_GPL(lbtf_cmd_response_rx);
 711
 712 int lbtf_process_rx_command(struct lbtf_private *priv)
 713 {
 714         uint16_t respcmd, curcmd;
 715         struct cmd_header *resp;
 716         int ret = 0;
 717         unsigned long flags;
 718         uint16_t result;
 719
 720         lbtf_deb_enter(LBTF_DEB_CMD);
 721
 722         mutex_lock(&priv->lock);
 723         spin_lock_irqsave(&priv->driver_lock, flags);
 724
 725         if (!priv->cur_cmd) {
 726                 ret = -1;
 727                 spin_unlock_irqrestore(&priv->driver_lock, flags);
 728                 goto done;
 729         }
 730
 731         resp = (void *)priv->cmd_resp_buff;
 732         curcmd = le16_to_cpu(priv->cur_cmd->cmdbuf->command);
 733         respcmd = le16_to_cpu(resp->command);
 734         result = le16_to_cpu(resp->result);
 735
 736         lbtf_deb_cmd("libertastf: cmd response 0x%04x, seq %d, size %d\n",
 737                      respcmd, le16_to_cpu(resp->seqnum),
 738                      le16_to_cpu(resp->size));
 739
 740         if (resp->seqnum != priv->cur_cmd->cmdbuf->seqnum) {
 741                 spin_unlock_irqrestore(&priv->driver_lock, flags);
 742                 ret = -1;
 743                 goto done;
 744         }
 745         if (respcmd != CMD_RET(curcmd)) {
 746                 spin_unlock_irqrestore(&priv->driver_lock, flags);
 747                 ret = -1;
 748                 goto done;
 749         }
 750
 751         if (resp->result == cpu_to_le16(0x0004)) {
 752                 /* 0x0004 means -EAGAIN. Drop the response, let it time out
 753                    and be resubmitted */
 754                 spin_unlock_irqrestore(&priv->driver_lock, flags);
 755                 ret = -1;
 756                 goto done;
 757         }
 758
 759         /* Now we got response from FW, cancel the command timer */
 760         del_timer(&priv->command_timer);
 761         priv->cmd_timed_out = 0;
 762         if (priv->nr_retries)
 763                 priv->nr_retries = 0;
 764
 765         /* If the command is not successful, cleanup and return failure */
 766         if ((result != 0 || !(respcmd & 0x8000))) {
 767                 /*
 768                  * Handling errors here
 769                  */
 770                 switch (respcmd) {
 771                 case CMD_RET(CMD_GET_HW_SPEC):
 772                 case CMD_RET(CMD_802_11_RESET):
 773                         pr_info("libertastf: reset failed\n");
 774                         break;
 775
 776                 }
 777                 lbtf_complete_command(priv, priv->cur_cmd, result);
 778                 spin_unlock_irqrestore(&priv->driver_lock, flags);
 779
 780                 ret = -1;
 781                 goto done;
 782         }
 783
 784         spin_unlock_irqrestore(&priv->driver_lock, flags);
 785
 786         if (priv->cur_cmd && priv->cur_cmd->callback) {
 787                 ret = priv->cur_cmd->callback(priv, priv->cur_cmd->callback_arg,
 788                                 resp);
 789         }
 790         spin_lock_irqsave(&priv->driver_lock, flags);
 791
 792         if (priv->cur_cmd) {
 793                 /* Clean up and Put current command back to cmdfreeq */
 794                 lbtf_complete_command(priv, priv->cur_cmd, result);
 795         }
 796         spin_unlock_irqrestore(&priv->driver_lock, flags);
 797
 798 done:
 799         mutex_unlock(&priv->lock);
 800         lbtf_deb_leave_args(LBTF_DEB_CMD, "ret %d", ret);
 801         return ret;
 802 }