1 files changed, 15 insertions, 5 deletions

diff --git a/drivers/net/wireless/libertas/if_sdio.c b/drivers/net/wireless/libertas/if_sdio.c
index 4519d7314f47..76f4c653d641 100644
--- a/drivers/net/wireless/libertas/if_sdio.c
+++ b/drivers/net/wireless/libertas/if_sdio.c
@@ -95,6 +95,8 @@ struct if_sdio_card {
 
 	spinlock_t		lock;
 	struct if_sdio_packet	*packets;
+
+	struct workqueue_struct	*workqueue;
 	struct work_struct	packet_worker;
 };
 
@@ -209,6 +211,9 @@ static int if_sdio_handle_event(struct if_sdio_card *card,
 		event = sdio_readb(card->func, IF_SDIO_EVENT, &ret);
 		if (ret)
 			goto out;
+
+		/* right shift 3 bits to get the event id */
+		event >>= 3;
 	} else {
 		if (size < 4) {
 			lbs_deb_sdio("event packet too small (%d bytes)\n",
@@ -743,7 +748,7 @@ static int if_sdio_host_to_card(struct lbs_private *priv,
 
 	spin_unlock_irqrestore(&card->lock, flags);
 
-	schedule_work(&card->packet_worker);
+	queue_work(card->workqueue, &card->packet_worker);
 
 	ret = 0;
 
@@ -833,6 +838,7 @@ static int if_sdio_probe(struct sdio_func *func,
 	card->func = func;
 	card->model = model;
 	spin_lock_init(&card->lock);
+	card->workqueue = create_workqueue("libertas_sdio");
 	INIT_WORK(&card->packet_worker, if_sdio_host_to_card_worker);
 
 	for (i = 0;i < ARRAY_SIZE(if_sdio_models);i++) {
@@ -921,15 +927,17 @@ static int if_sdio_probe(struct sdio_func *func,
 	if (ret)
 		goto err_activate_card;
 
+	if (priv->fwcapinfo & FW_CAPINFO_PS)
+		priv->ps_supported = 1;
+
 out:
 	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
 
 	return ret;
 
 err_activate_card:
-	flush_scheduled_work();
-	free_netdev(priv->dev);
-	kfree(priv);
+	flush_workqueue(card->workqueue);
+	lbs_remove_card(priv);
 reclaim:
 	sdio_claim_host(func);
 release_int:
@@ -939,6 +947,7 @@ disable:
 release:
 	sdio_release_host(func);
 free:
+	destroy_workqueue(card->workqueue);
 	while (card->packets) {
 		packet = card->packets;
 		card->packets = card->packets->next;
@@ -965,7 +974,8 @@ static void if_sdio_remove(struct sdio_func *func)
 	lbs_stop_card(card->priv);
 	lbs_remove_card(card->priv);
 
-	flush_scheduled_work();
+	flush_workqueue(card->workqueue);
+	destroy_workqueue(card->workqueue);
 
 	sdio_claim_host(func);
 	sdio_release_irq(func);