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;
}
