int lbs_process_command_response(struct lbs_private *priv, u8 *data, u32 len) { uint16_t respcmd, curcmd; struct cmd_header *resp; int ret = 0; unsigned long flags; uint16_t result; lbs_deb_enter(LBS_DEB_HOST); mutex_lock(&priv->lock); spin_lock_irqsave(&priv->driver_lock, flags); if (!priv->cur_cmd) { lbs_deb_host("CMD_RESP: cur_cmd is NULL\n"); ret = -1; spin_unlock_irqrestore(&priv->driver_lock, flags); goto done; } resp = (void *)data; curcmd = le16_to_cpu(priv->cur_cmd->cmdbuf->command); respcmd = le16_to_cpu(resp->command); result = le16_to_cpu(resp->result); lbs_deb_cmd("CMD_RESP: response 0x%04x, seq %d, size %d\n", respcmd, le16_to_cpu(resp->seqnum), len); lbs_deb_hex(LBS_DEB_CMD, "CMD_RESP", (void *) resp, len); if (resp->seqnum != priv->cur_cmd->cmdbuf->seqnum) { lbs_pr_info("Received CMD_RESP with invalid sequence %d (expected %d)\n", le16_to_cpu(resp->seqnum), le16_to_cpu(priv->cur_cmd->cmdbuf->seqnum)); spin_unlock_irqrestore(&priv->driver_lock, flags); ret = -1; goto done; } if (respcmd != CMD_RET(curcmd) && respcmd != CMD_RET_802_11_ASSOCIATE && curcmd != CMD_802_11_ASSOCIATE) { lbs_pr_info("Invalid CMD_RESP %x to command %x!\n", respcmd, curcmd); spin_unlock_irqrestore(&priv->driver_lock, flags); ret = -1; goto done; } if (resp->result == cpu_to_le16(0x0004)) { /* 0x0004 means -EAGAIN. Drop the response, let it time out and be resubmitted */ lbs_pr_info("Firmware returns DEFER to command %x. Will let it time out...\n", le16_to_cpu(resp->command)); spin_unlock_irqrestore(&priv->driver_lock, flags); ret = -1; goto done; } /* Now we got response from FW, cancel the command timer */ del_timer(&priv->command_timer); priv->cmd_timed_out = 0; if (respcmd == CMD_RET(CMD_802_11_PS_MODE)) { struct cmd_ds_802_11_ps_mode *psmode = (void *) &resp[1]; u16 action = le16_to_cpu(psmode->action); lbs_deb_host( "CMD_RESP: PS_MODE cmd reply result 0x%x, action 0x%x\n", result, action); if (result) { lbs_deb_host("CMD_RESP: PS command failed with 0x%x\n", result); /* * We should not re-try enter-ps command in * ad-hoc mode. It takes place in * lbs_execute_next_command(). */ if (priv->wdev->iftype == NL80211_IFTYPE_MONITOR && action == PS_MODE_ACTION_ENTER_PS) priv->psmode = LBS802_11POWERMODECAM; } else if (action == PS_MODE_ACTION_ENTER_PS) { priv->needtowakeup = 0; priv->psstate = PS_STATE_AWAKE; lbs_deb_host("CMD_RESP: ENTER_PS command response\n"); if (priv->connect_status != LBS_CONNECTED) { /* * When Deauth Event received before Enter_PS command * response, We need to wake up the firmware. */ lbs_deb_host( "disconnected, invoking lbs_ps_wakeup\n"); spin_unlock_irqrestore(&priv->driver_lock, flags); mutex_unlock(&priv->lock); lbs_set_ps_mode(priv, PS_MODE_ACTION_EXIT_PS, false); mutex_lock(&priv->lock); spin_lock_irqsave(&priv->driver_lock, flags); } } else if (action == PS_MODE_ACTION_EXIT_PS) { priv->needtowakeup = 0; priv->psstate = PS_STATE_FULL_POWER; lbs_deb_host("CMD_RESP: EXIT_PS command response\n"); } else { lbs_deb_host("CMD_RESP: PS action 0x%X\n", action); } lbs_complete_command(priv, priv->cur_cmd, result); spin_unlock_irqrestore(&priv->driver_lock, flags); ret = 0; goto done; } /* If the command is not successful, cleanup and return failure */ if ((result != 0 || !(respcmd & 0x8000))) { lbs_deb_host("CMD_RESP: error 0x%04x in command reply 0x%04x\n", result, respcmd); /* * Handling errors here */ switch (respcmd) { case CMD_RET(CMD_GET_HW_SPEC): case CMD_RET(CMD_802_11_RESET): lbs_deb_host("CMD_RESP: reset failed\n"); break; } lbs_complete_command(priv, priv->cur_cmd, result); spin_unlock_irqrestore(&priv->driver_lock, flags); ret = -1; goto done; } spin_unlock_irqrestore(&priv->driver_lock, flags); if (priv->cur_cmd && priv->cur_cmd->callback) { ret = priv->cur_cmd->callback(priv, priv->cur_cmd->callback_arg, resp); } spin_lock_irqsave(&priv->driver_lock, flags); if (priv->cur_cmd) { /* Clean up and Put current command back to cmdfreeq */ lbs_complete_command(priv, priv->cur_cmd, result); } spin_unlock_irqrestore(&priv->driver_lock, flags); done: mutex_unlock(&priv->lock); lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret); return ret; }
/** * lbs_thread - handles the major jobs in the LBS driver. * It handles all events generated by firmware, RX data received * from firmware and TX data sent from kernel. * * @data: A pointer to &lbs_thread structure * returns: 0 */ static int lbs_thread(void *data) { struct net_device *dev = data; struct lbs_private *priv = dev->ml_priv; wait_queue_t wait; lbs_deb_enter(LBS_DEB_THREAD); init_waitqueue_entry(&wait, current); for (;;) { int shouldsleep; u8 resp_idx; lbs_deb_thread("1: currenttxskb %p, dnld_sent %d\n", priv->currenttxskb, priv->dnld_sent); add_wait_queue(&priv->waitq, &wait); set_current_state(TASK_INTERRUPTIBLE); spin_lock_irq(&priv->driver_lock); if (kthread_should_stop()) shouldsleep = 0; /* Bye */ else if (priv->surpriseremoved) shouldsleep = 1; /* We need to wait until we're _told_ to die */ else if (priv->psstate == PS_STATE_SLEEP) shouldsleep = 1; /* Sleep mode. Nothing we can do till it wakes */ else if (priv->cmd_timed_out) shouldsleep = 0; /* Command timed out. Recover */ else if (!priv->fw_ready) shouldsleep = 1; /* Firmware not ready. We're waiting for it */ else if (priv->dnld_sent) shouldsleep = 1; /* Something is en route to the device already */ else if (priv->tx_pending_len > 0) shouldsleep = 0; /* We've a packet to send */ else if (priv->resp_len[priv->resp_idx]) shouldsleep = 0; /* We have a command response */ else if (priv->cur_cmd) shouldsleep = 1; /* Can't send a command; one already running */ else if (!list_empty(&priv->cmdpendingq) && !(priv->wakeup_dev_required)) shouldsleep = 0; /* We have a command to send */ else if (kfifo_len(&priv->event_fifo)) shouldsleep = 0; /* We have an event to process */ else shouldsleep = 1; /* No command */ if (shouldsleep) { lbs_deb_thread("sleeping, connect_status %d, " "psmode %d, psstate %d\n", priv->connect_status, priv->psmode, priv->psstate); spin_unlock_irq(&priv->driver_lock); schedule(); } else spin_unlock_irq(&priv->driver_lock); lbs_deb_thread("2: currenttxskb %p, dnld_send %d\n", priv->currenttxskb, priv->dnld_sent); set_current_state(TASK_RUNNING); remove_wait_queue(&priv->waitq, &wait); lbs_deb_thread("3: currenttxskb %p, dnld_sent %d\n", priv->currenttxskb, priv->dnld_sent); if (kthread_should_stop()) { lbs_deb_thread("break from main thread\n"); break; } if (priv->surpriseremoved) { lbs_deb_thread("adapter removed; waiting to die...\n"); continue; } lbs_deb_thread("4: currenttxskb %p, dnld_sent %d\n", priv->currenttxskb, priv->dnld_sent); /* Process any pending command response */ spin_lock_irq(&priv->driver_lock); resp_idx = priv->resp_idx; if (priv->resp_len[resp_idx]) { spin_unlock_irq(&priv->driver_lock); lbs_process_command_response(priv, priv->resp_buf[resp_idx], priv->resp_len[resp_idx]); spin_lock_irq(&priv->driver_lock); priv->resp_len[resp_idx] = 0; } spin_unlock_irq(&priv->driver_lock); /* Process hardware events, e.g. card removed, link lost */ spin_lock_irq(&priv->driver_lock); while (kfifo_len(&priv->event_fifo)) { u32 event; if (kfifo_out(&priv->event_fifo, (unsigned char *) &event, sizeof(event)) != sizeof(event)) break; spin_unlock_irq(&priv->driver_lock); lbs_process_event(priv, event); spin_lock_irq(&priv->driver_lock); } spin_unlock_irq(&priv->driver_lock); if (priv->wakeup_dev_required) { lbs_deb_thread("Waking up device...\n"); /* Wake up device */ if (priv->exit_deep_sleep(priv)) lbs_deb_thread("Wakeup device failed\n"); continue; } /* command timeout stuff */ if (priv->cmd_timed_out && priv->cur_cmd) { struct cmd_ctrl_node *cmdnode = priv->cur_cmd; netdev_info(dev, "Timeout submitting command 0x%04x\n", le16_to_cpu(cmdnode->cmdbuf->command)); lbs_complete_command(priv, cmdnode, -ETIMEDOUT); if (priv->reset_card) priv->reset_card(priv); } priv->cmd_timed_out = 0; if (!priv->fw_ready) continue; /* Check if we need to confirm Sleep Request received previously */ if (priv->psstate == PS_STATE_PRE_SLEEP && !priv->dnld_sent && !priv->cur_cmd) { if (priv->connect_status == LBS_CONNECTED) { lbs_deb_thread("pre-sleep, currenttxskb %p, " "dnld_sent %d, cur_cmd %p\n", priv->currenttxskb, priv->dnld_sent, priv->cur_cmd); lbs_ps_confirm_sleep(priv); } else { /* workaround for firmware sending * deauth/linkloss event immediately * after sleep request; remove this * after firmware fixes it */ priv->psstate = PS_STATE_AWAKE; netdev_alert(dev, "ignore PS_SleepConfirm in non-connected state\n"); } } /* The PS state is changed during processing of Sleep Request * event above */ if ((priv->psstate == PS_STATE_SLEEP) || (priv->psstate == PS_STATE_PRE_SLEEP)) continue; if (priv->is_deep_sleep) continue; /* Execute the next command */ if (!priv->dnld_sent && !priv->cur_cmd) lbs_execute_next_command(priv); spin_lock_irq(&priv->driver_lock); if (!priv->dnld_sent && priv->tx_pending_len > 0) { int ret = priv->hw_host_to_card(priv, MVMS_DAT, priv->tx_pending_buf, priv->tx_pending_len); if (ret) { lbs_deb_tx("host_to_card failed %d\n", ret); priv->dnld_sent = DNLD_RES_RECEIVED; } else { mod_timer(&priv->tx_lockup_timer, jiffies + (HZ * 5)); } priv->tx_pending_len = 0; if (!priv->currenttxskb) { /* We can wake the queues immediately if we aren't waiting for TX feedback */ if (priv->connect_status == LBS_CONNECTED) netif_wake_queue(priv->dev); if (priv->mesh_dev && netif_running(priv->mesh_dev)) netif_wake_queue(priv->mesh_dev); } } spin_unlock_irq(&priv->driver_lock); } del_timer(&priv->command_timer); del_timer(&priv->tx_lockup_timer); del_timer(&priv->auto_deepsleep_timer); lbs_deb_leave(LBS_DEB_THREAD); return 0; }
//HelloX: This thread will be waken up by other routines through the calling of //wake_up_interruptible routine,in HelloX's implementation,this mechanism should be //replaced by event,a dedicated event object is defined under lbs_private structure, //the lbs_thread should wait on this event object when shouldsleep = 1. //Other routines who want to process lbs command,should set the event object status //to signal,thus lead the wake up of lbs_thread. int lbs_thread(struct lbs_private *priv) { //struct net_device *dev = data; //struct lbs_private *priv = dev->ml_priv; //wait_queue_t wait; int shouldsleep; u8 resp_idx; //lbs_deb_enter(LBS_DEB_THREAD); //init_waitqueue_entry(&wait, current); do{ /*lbs_deb_thread("1: currenttxskb %p, dnld_sent %d\n", priv->currenttxskb, priv->dnld_sent); */ /*add_wait_queue(&priv->waitq, &wait); set_current_state(TASK_INTERRUPTIBLE); spin_lock_irq(&priv->driver_lock); if (kthread_should_stop()) shouldsleep = 0; */ if (priv->surpriseremoved) shouldsleep = 1; /* We need to wait until we're _told_ to die */ else if (priv->psstate == PS_STATE_SLEEP) shouldsleep = 1; /* Sleep mode. Nothing we can do till it wakes */ else if (priv->cmd_timed_out) shouldsleep = 0; /* Command timed out. Recover */ else if (!priv->fw_ready) shouldsleep = 1; /* Firmware not ready. We're waiting for it */ else if (priv->dnld_sent) shouldsleep = 1; /* Something is en route to the device already */ //else if (priv->tx_pending_len > 0) // shouldsleep = 0; /* We've a packet to send */ else if (priv->resp_len[priv->resp_idx]) shouldsleep = 0; /* We have a command response */ else if (priv->cur_cmd) shouldsleep = 1; /* Can't send a command; one already running */ else if (!list_empty(&priv->cmdpendingq)) shouldsleep = 0; /* We have a command to send */ /* else if (__kfifo_len(priv->event_fifo)) shouldsleep = 0; */ /* We have an event to process */ else shouldsleep = 1; /* No command */ if (shouldsleep) { /*lbs_deb_thread("sleeping, connect_status %d, " "psmode %d, psstate %d\n", priv->connect_status, priv->psmode, priv->psstate);*/ //spin_unlock_irq(&priv->driver_lock); //schedule(); break; //HelloX: This break clause might be added by the modifier,to giveup execute. //Should remove this clause when implement this thread under HelloX. } /*else spin_unlock_irq(&priv->driver_lock);*/ /*lbs_deb_thread("2: currenttxskb %p, dnld_send %d\n", priv->currenttxskb, priv->dnld_sent);*/ /*set_current_state(TASK_RUNNING); remove_wait_queue(&priv->waitq, &wait);*/ //HelloX: Should reset the event status to non-signaling here,to achieve sychronization //purpose. /*lbs_deb_thread("3: currenttxskb %p, dnld_sent %d\n", priv->currenttxskb, priv->dnld_sent); if (kthread_should_stop()) { lbs_deb_thread("break from main thread\n"); break; }*/ if (priv->surpriseremoved) { lbs_deb_thread("adapter removed; waiting to die...\n"); continue; } /*lbs_deb_thread("4: currenttxskb %p, dnld_sent %d\n", priv->currenttxskb, priv->dnld_sent);*/ /* Process any pending command response */ //spin_lock_irq(&priv->driver_lock); resp_idx = priv->resp_idx; if (priv->resp_len[resp_idx]) { // spin_unlock_irq(&priv->driver_lock); lbs_process_command_response(priv, priv->resp_buf[resp_idx], priv->resp_len[resp_idx]); //spin_lock_irq(&priv->driver_lock); priv->resp_len[resp_idx] = 0; } //spin_unlock_irq(&priv->driver_lock); /* command timeout stuff */ if (priv->cmd_timed_out && priv->cur_cmd) { struct cmd_ctrl_node *cmdnode = priv->cur_cmd; if (++priv->nr_retries > 0) { lbs_pr_info("Excessive timeouts submitting " "command 0x%04x\n", le16_to_cpu(cmdnode->cmdbuf->command)); lbs_complete_command(priv, cmdnode, -ETIMEDOUT); priv->nr_retries = 0; /*if (priv->reset_card) priv->reset_card(priv);*/ } else { priv->cur_cmd = NULL; priv->dnld_sent = DNLD_RES_RECEIVED; lbs_pr_info("requeueing command 0x%04x due " "to timeout (#%d)\n", le16_to_cpu(cmdnode->cmdbuf->command), priv->nr_retries); /* Stick it back at the _top_ of the pending queue for immediate resubmission */ list_add(&cmdnode->list, &priv->cmdpendingq); } } priv->cmd_timed_out = 0; /* Process hardware events, e.g. card removed, link lost */ //spin_lock_irq(&priv->driver_lock); /* while (__kfifo_len(priv->event_fifo)) { u32 event; __kfifo_get(priv->event_fifo,(unsigned char *)&event,sizeof(event)); //spin_unlock_irq(&priv->driver_lock); lbs_process_event(priv, event); //spin_lock_irq(&priv->driver_lock); } */ //spin_unlock_irq(&priv->driver_lock); if (!priv->fw_ready) continue; /* Check if we need to confirm Sleep Request received previously */ if (priv->psstate == PS_STATE_PRE_SLEEP && !priv->dnld_sent && !priv->cur_cmd) { if (priv->connect_status == LBS_CONNECTED) { /*lbs_deb_thread("pre-sleep, currenttxskb %p, " "dnld_sent %d, cur_cmd %p\n", priv->currenttxskb, priv->dnld_sent, priv->cur_cmd);*/ //lbs_ps_confirm_sleep(priv); lbs_pr_alert("ignore PS_SleepConfirm in " "non-connected state\n"); while(1){ if(priv->psstate != PS_STATE_PRE_SLEEP)//bug break; } } else { /* workaround for firmware sending * deauth/linkloss event immediately * after sleep request; remove this * after firmware fixes it */ priv->psstate = PS_STATE_AWAKE; lbs_pr_alert("ignore PS_SleepConfirm in " "non-connected state\n"); } } /* The PS state is changed during processing of Sleep Request * event above */ if ((priv->psstate == PS_STATE_SLEEP) || (priv->psstate == PS_STATE_PRE_SLEEP)) continue; /* Execute the next command */ if (!priv->dnld_sent && !priv->cur_cmd) lbs_execute_next_command(priv); /* Wake-up command waiters which can't sleep in * lbs_prepare_and_send_command */ /*if (!list_empty(&priv->cmdpendingq)) wake_up_all(&priv->cmd_pending);*/ #ifdef MASK_DEBUG //spin_lock_irq(&priv->driver_lock); if (!priv->dnld_sent && priv->tx_pending_len > 0) { int ret = priv->hw_host_to_card(priv, MVMS_DAT, priv->tx_pending_buf, priv->tx_pending_len); if (ret) { lbs_deb_tx("host_to_card failed %d\n", ret); priv->dnld_sent = DNLD_RES_RECEIVED; } priv->tx_pending_len = 0; /*if (!priv->currenttxskb) { //We can wake the queues immediately if we aren't // waiting for TX feedback if (priv->connect_status == LBS_CONNECTED) netif_wake_queue(priv->dev); if (priv->mesh_dev && priv->mesh_connect_status == LBS_CONNECTED) netif_wake_queue(priv->mesh_dev); }*/ } #endif //spin_unlock_irq(&priv->driver_lock); }while(0); //HelloX: Maybe modified by the previous author,should change to while(1) clause when //migrate to HelloX. //del_timer(&priv->command_timer); //wake_up_all(&priv->cmd_pending); //lbs_deb_leave("leave main thread!\n"); return 0; }