Subject: [PATCH 18/32] rt2x00: Make sure device has reached requested state while suspend/resume

From: Ivo van Doorn <[email-masked]>
Date: Fri, 28 Apr 2006 00:03:10 +0200

From: Ivo van Doorn <[email-masked]>

Add the *_set_state functions which makes sure
the device is switching state to awake or sleep.
Fix bad behaviour in the suspend routine,
and disable the radio before suspending.

Signed-off-by: Ivo van Doorn <[email-masked]>

diff -uprN wireless-dev-rt2x00/drivers/net/wireless/d80211/rt2x00/rt2400pci.c wireless-dev-rt2x00-patch/drivers/net/wireless/d80211/rt2x00/rt2400pci.c
--- wireless-dev-rt2x00/drivers/net/wireless/d80211/rt2x00/rt2400pci.c	2006-04-27 21:49:08.000000000 +0200
+++ wireless-dev-rt2x00-patch/drivers/net/wireless/d80211/rt2x00/rt2400pci.c	2006-04-27 21:49:59.000000000 +0200
@@ -653,6 +653,44 @@ rt2400pci_link_tuner(struct rt2x00_pci *
 }
 
 /*
+ * Device state switch.
+ * This will put the device to sleep, or awake it.
+ */
+static int
+rt2400pci_set_state(struct rt2x00_pci *rt2x00pci, enum dev_state state)
+{
+	u32			reg;
+	int			counter;
+	char			put_to_sleep;
+
+	put_to_sleep = (state != STATE_AWAKE);
+
+	rt2x00_register_read(rt2x00pci, PWRCSR1, &reg);
+	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
+	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, state);
+	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, state);
+	rt2x00_set_field32(&reg, PWRCSR1_PUT_TO_SLEEP, put_to_sleep);
+	rt2x00_register_write(rt2x00pci, PWRCSR1, reg);
+
+	/*
+	 * Device is not guarenteed to be in the requested state yet.
+	 * We must wait untill the register indicates that the
+	 * device has entered the correct state.
+	 */
+	for (counter = 0; counter < REGISTER_BUSY_COUNT; counter++) {
+		rt2x00_register_read(rt2x00pci, PWRCSR1, &reg);
+		if ((rt2x00_get_field32(reg, PWRCSR1_BBP_CURR_STATE) == state)
+		&& (rt2x00_get_field32(reg, PWRCSR1_RF_CURR_STATE) == state))
+			return 0;
+		msleep(10);
+	}
+
+	NOTICE("Device failed to %s.\n" , put_to_sleep ? "suspend" : "resume");
+
+	return -EBUSY;
+}
+
+/*
  * TX descriptor initialization
  */
 static void
@@ -1190,6 +1228,9 @@ rt2400pci_init_registers(struct rt2x00_p
 {
 	u32		reg;
 
+	if (rt2400pci_set_state(rt2x00pci, STATE_AWAKE))
+		return -EBUSY;
+
 	rt2x00_register_write(rt2x00pci, PWRCSR0, cpu_to_le32(0x3f3b3100));
 
 	rt2x00_register_write(rt2x00pci, PSCSR0, cpu_to_le32(0x00020002));
@@ -2342,57 +2383,42 @@ rt2400pci_remove(struct pci_dev *pci_dev
 }
 
 #ifdef CONFIG_PM
-static int rt2400pci_resume(struct pci_dev *pci_dev);
-
 static int
 rt2400pci_suspend(struct pci_dev *pci_dev, pm_message_t state)
 {
 	struct net_device	*net_dev = pci_get_drvdata(pci_dev);
 	struct rt2x00_pci	*rt2x00pci = ieee80211_dev_hw_data(net_dev);
-	int			counter;
-	u32			reg;
 
 	NOTICE("Going to sleep.\n");
 
-	if (ieee80211_netif_oper(net_dev, NETIF_DETACH))
-		return -EBUSY;
-
-	rt2x00_register_read(rt2x00pci, PWRCSR1, &reg);
-	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
-	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, 1);
-	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, 1);
-	rt2x00_set_field32(&reg, PWRCSR1_PUT_TO_SLEEP, 1);
-	rt2x00_register_write(rt2x00pci, PWRCSR1, reg);
-
 	/*
-	 * Device is not guarenteed to be asleep yet.
-	 * We must wait untill the register indicates
-	 * device has been correctly put to sleep.
+	 * If radio was enabled, stop radio and
+	 * set the resume flag to the radio will be enabled
+	 * when resuming.
 	 */
-	for (counter = 0; counter < REGISTER_BUSY_COUNT; counter++) {
-		rt2x00_register_read(rt2x00pci, PWRCSR1, &reg);
-		if ((rt2x00_get_field32(reg, PWRCSR1_BBP_CURR_STATE) == 1)
-		&& (rt2x00_get_field32(reg, PWRCSR1_RF_CURR_STATE) == 1))
-			break;
-		NOTICE("Waiting for device to sleep.\n");
-		msleep(10);
+	if (GET_FLAG(rt2x00pci, RADIO_ENABLED)) {
+		if (net_dev->stop(net_dev))
+			return -EBUSY;
+		SET_FLAG(rt2x00pci, RADIO_RESUME);
 	}
 
-	if (counter == REGISTER_BUSY_COUNT)
-		goto exit;
-
-	if (pci_enable_wake(pci_dev, pci_choose_state(pci_dev, state), 1)
-	|| pci_save_state(pci_dev)
-	|| pci_set_power_state(pci_dev, pci_choose_state(pci_dev, state)))
-		goto exit;
-
-	return 0;
+	/*
+	 * Set device mode to sleep for power management.
+	 */
+	if (rt2400pci_set_state(rt2x00pci, STATE_SLEEP))
+		return -EBUSY;
 
-exit:
-	ERROR("Failed to suspend device.\n");
+	/*
+	 * Uninitialize hardware.
+	 */
+	rt2400pci_uninitialize(net_dev);
 
-	rt2400pci_resume(pci_dev);
-	return -EIO;
+	/*
+	 * Disable PCI.
+	 */
+	pci_save_state(pci_dev);
+	pci_disable_device(pci_dev);
+	return pci_set_power_state(pci_dev, pci_choose_state(pci_dev, state));
 }
 
 static int
@@ -2400,25 +2426,44 @@ rt2400pci_resume(struct pci_dev *pci_dev
 {
 	struct net_device	*net_dev = pci_get_drvdata(pci_dev);
 	struct rt2x00_pci	*rt2x00pci = ieee80211_dev_hw_data(net_dev);
-	u32			reg;
 
 	NOTICE("Waking up.\n");
 
+	/*
+	 * Enable PCI.
+	 */
 	if (pci_set_power_state(pci_dev, PCI_D0)
-	|| pci_restore_state(pci_dev)
-	|| pci_enable_wake(pci_dev, PCI_D0, 0)) {
+	|| pci_enable_device(pci_dev)
+	|| pci_restore_state(pci_dev)) {
 		ERROR("Failed to resume device.\n");
 		return -EIO;
 	}
 
-	rt2x00_register_read(rt2x00pci, PWRCSR1, &reg);
-	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
-	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, 3);
-	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, 3);
-	rt2x00_set_field32(&reg, PWRCSR1_PUT_TO_SLEEP, 0);
-	rt2x00_register_write(rt2x00pci, PWRCSR1, reg);
+	/*
+	 * Initialize hardware.
+	 */
+	if (rt2400pci_initialize(pci_dev, net_dev)) {
+		ERROR("Failed to initialize device.\n");
+		return -ENOMEM;
+	}
 
-	return ieee80211_netif_oper(net_dev, NETIF_ATTACH);
+	/*
+	 * Set device mode to awake.
+	 */
+	if (rt2400pci_set_state(rt2x00pci, STATE_AWAKE))
+		return -EBUSY;
+
+	/*
+	 * Only enable radio when it was enabled
+	 * when we suspended.
+	 */
+	if (GET_FLAG(rt2x00pci, RADIO_RESUME)) {
+		if (net_dev->open(net_dev))
+			return -EBUSY;
+		CLEAR_FLAG(rt2x00pci, RADIO_RESUME);
+	}
+
+	return 0;
 }
 #endif /* CONFIG_PM */
 
diff -uprN wireless-dev-rt2x00/drivers/net/wireless/d80211/rt2x00/rt2500pci.c wireless-dev-rt2x00-patch/drivers/net/wireless/d80211/rt2x00/rt2500pci.c
--- wireless-dev-rt2x00/drivers/net/wireless/d80211/rt2x00/rt2500pci.c	2006-04-27 21:49:08.000000000 +0200
+++ wireless-dev-rt2x00-patch/drivers/net/wireless/d80211/rt2x00/rt2500pci.c	2006-04-27 21:49:59.000000000 +0200
@@ -706,6 +706,44 @@ dynamic_cca_tune:
 }
 
 /*
+ * Device state switch.
+ * This will put the device to sleep, or awake it.
+ */
+static int
+rt2500pci_set_state(struct rt2x00_pci *rt2x00pci, enum dev_state state)
+{
+	u32			reg;
+	int			counter;
+	char			put_to_sleep;
+
+	put_to_sleep = (state != STATE_AWAKE);
+
+	rt2x00_register_read(rt2x00pci, PWRCSR1, &reg);
+	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
+	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, state);
+	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, state);
+	rt2x00_set_field32(&reg, PWRCSR1_PUT_TO_SLEEP, put_to_sleep);
+	rt2x00_register_write(rt2x00pci, PWRCSR1, reg);
+
+	/*
+	 * Device is not guarenteed to be in the requested state yet.
+	 * We must wait untill the register indicates that the
+	 * device has entered the correct state.
+	 */
+	for (counter = 0; counter < REGISTER_BUSY_COUNT; counter++) {
+		rt2x00_register_read(rt2x00pci, PWRCSR1, &reg);
+		if ((rt2x00_get_field32(reg, PWRCSR1_BBP_CURR_STATE) == state)
+		&& (rt2x00_get_field32(reg, PWRCSR1_RF_CURR_STATE) == state))
+			return 0;
+		msleep(10);
+	}
+
+	NOTICE("Device failed to %s.\n" , put_to_sleep ? "suspend" : "resume");
+
+	return -EBUSY;
+}
+
+/*
  * TX descriptor initialization
  */
 static void
@@ -1272,6 +1310,9 @@ rt2500pci_init_registers(struct rt2x00_p
 {
 	u32		reg;
 
+	if (rt2500pci_set_state(rt2x00pci, STATE_AWAKE))
+		return -EBUSY;
+
 	rt2x00_register_write(rt2x00pci, PWRCSR0, cpu_to_le32(0x3f3b3100));
 
 	rt2x00_register_write(rt2x00pci, PSCSR0, cpu_to_le32(0x00020002));
@@ -2636,57 +2677,42 @@ rt2500pci_remove(struct pci_dev *pci_dev
 }
 
 #ifdef CONFIG_PM
-static int rt2500pci_resume(struct pci_dev *pci_dev);
-
 static int
 rt2500pci_suspend(struct pci_dev *pci_dev, pm_message_t state)
 {
 	struct net_device	*net_dev = pci_get_drvdata(pci_dev);
 	struct rt2x00_pci	*rt2x00pci = ieee80211_dev_hw_data(net_dev);
-	int			counter;
-	u32			reg;
 
 	NOTICE("Going to sleep.\n");
 
-	if (ieee80211_netif_oper(net_dev, NETIF_DETACH))
-		return -EBUSY;
-
-	rt2x00_register_read(rt2x00pci, PWRCSR1, &reg);
-	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
-	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, 1);
-	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, 1);
-	rt2x00_set_field32(&reg, PWRCSR1_PUT_TO_SLEEP, 1);
-	rt2x00_register_write(rt2x00pci, PWRCSR1, reg);
-
 	/*
-	 * Device is not guarenteed to be asleep yet.
-	 * We must wait untill the register indicates
-	 * device has been correctly put to sleep.
+	 * If radio was enabled, stop radio and
+	 * set the resume flag to the radio will be enabled
+	 * when resuming.
 	 */
-	for (counter = 0; counter < REGISTER_BUSY_COUNT; counter++) {
-		rt2x00_register_read(rt2x00pci, PWRCSR1, &reg);
-		if ((rt2x00_get_field32(reg, PWRCSR1_BBP_CURR_STATE) == 1)
-		&& (rt2x00_get_field32(reg, PWRCSR1_RF_CURR_STATE) == 1))
-			break;
-		NOTICE("Waiting for device to sleep.\n");
-		msleep(10);
+	if (GET_FLAG(rt2x00pci, RADIO_ENABLED)) {
+		if (net_dev->stop(net_dev))
+			return -EBUSY;
+		SET_FLAG(rt2x00pci, RADIO_RESUME);
 	}
 
-	if (counter == REGISTER_BUSY_COUNT)
-		goto exit;
-
-	if (pci_enable_wake(pci_dev, pci_choose_state(pci_dev, state), 1)
-	|| pci_save_state(pci_dev)
-	|| pci_set_power_state(pci_dev, pci_choose_state(pci_dev, state)))
-		goto exit;
-
-	return 0;
+	/*
+	 * Set device mode to sleep for power management.
+	 */
+	if (rt2500pci_set_state(rt2x00pci, STATE_SLEEP))
+		return -EBUSY;
 
-exit:
-	ERROR("Failed to suspend device.\n");
+	/*
+	 * Uninitialize hardware.
+	 */
+	rt2500pci_uninitialize(net_dev);
 
-	rt2500pci_resume(pci_dev);
-	return -EIO;
+	/*
+	 * Disable PCI.
+	 */
+	pci_save_state(pci_dev);
+	pci_disable_device(pci_dev);
+	return pci_set_power_state(pci_dev, pci_choose_state(pci_dev, state));
 }
 
 static int
@@ -2694,25 +2720,44 @@ rt2500pci_resume(struct pci_dev *pci_dev
 {
 	struct net_device	*net_dev = pci_get_drvdata(pci_dev);
 	struct rt2x00_pci	*rt2x00pci = ieee80211_dev_hw_data(net_dev);
-	u32			reg;
 
 	NOTICE("Waking up.\n");
 
+	/*
+	 * Enable PCI.
+	 */
 	if (pci_set_power_state(pci_dev, PCI_D0)
-	|| pci_restore_state(pci_dev)
-	|| pci_enable_wake(pci_dev, PCI_D0, 0)) {
+	|| pci_enable_device(pci_dev)
+	|| pci_restore_state(pci_dev)) {
 		ERROR("Failed to resume device.\n");
 		return -EIO;
 	}
 
-	rt2x00_register_read(rt2x00pci, PWRCSR1, &reg);
-	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
-	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, 3);
-	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, 3);
-	rt2x00_set_field32(&reg, PWRCSR1_PUT_TO_SLEEP, 0);
-	rt2x00_register_write(rt2x00pci, PWRCSR1, reg);
+	/*
+	 * Initialize hardware.
+	 */
+	if (rt2500pci_initialize(pci_dev, net_dev)) {
+		ERROR("Failed to initialize device.\n");
+		return -ENOMEM;
+	}
 
-	return ieee80211_netif_oper(net_dev, NETIF_ATTACH);
+	/*
+	 * Set device mode to awake.
+	 */
+	if (rt2500pci_set_state(rt2x00pci, STATE_AWAKE))
+		return -EBUSY;
+
+	/*
+	 * Only enable radio when it was enabled
+	 * when we suspended.
+	 */
+	if (GET_FLAG(rt2x00pci, RADIO_RESUME)) {
+		if (net_dev->open(net_dev))
+			return -EBUSY;
+		CLEAR_FLAG(rt2x00pci, RADIO_RESUME);
+	}
+
+	return 0;
 }
 #endif /* CONFIG_PM */
 
diff -uprN wireless-dev-rt2x00/drivers/net/wireless/d80211/rt2x00/rt2500usb.c wireless-dev-rt2x00-patch/drivers/net/wireless/d80211/rt2x00/rt2500usb.c
--- wireless-dev-rt2x00/drivers/net/wireless/d80211/rt2x00/rt2500usb.c	2006-04-27 21:49:08.000000000 +0200
+++ wireless-dev-rt2x00-patch/drivers/net/wireless/d80211/rt2x00/rt2500usb.c	2006-04-27 21:49:59.000000000 +0200
@@ -547,6 +547,46 @@ rt2500usb_link_tuner(struct rt2x00_usb *
 }
 
 /*
+ * Device state switch.
+ * This will put the device to sleep, or awake it.
+ */
+static int
+rt2500usb_set_state(struct rt2x00_usb *rt2x00usb, enum dev_state state)
+{
+	u16			reg;
+	int			counter;
+	char			put_to_sleep;
+
+	put_to_sleep = (state != STATE_AWAKE);
+
+	rt2x00_register_read(rt2x00usb, MAC_CSR17, &reg);
+	rt2x00_set_field16_nb(&reg, MAC_CSR17_SET_STATE, 1);
+	rt2x00_set_field16_nb(&reg, MAC_CSR17_BBP_DESIRE_STATE, state);
+	rt2x00_set_field16_nb(&reg, MAC_CSR17_RF_DESIRE_STATE, state);
+	rt2x00_set_field16_nb(&reg, MAC_CSR17_PUT_TO_SLEEP, put_to_sleep);
+	rt2x00_register_write(rt2x00usb, MAC_CSR17, reg);
+
+	/*
+	 * Device is not guarenteed to be in the requested state yet.
+	 * We must wait untill the register indicates that the
+	 * device has entered the correct state.
+	 */
+	for (counter = 0; counter < REGISTER_BUSY_COUNT; counter++) {
+		rt2x00_register_read(rt2x00usb, MAC_CSR17, &reg);
+		if ((rt2x00_get_field16_nb(
+			reg, MAC_CSR17_BBP_CURR_STATE) == state)
+		&& (rt2x00_get_field16_nb(
+			reg, MAC_CSR17_RF_CURR_STATE) == state))
+			return 0;
+		msleep(10);
+	}
+
+	NOTICE("Device failed to %s.\n" , put_to_sleep ? "suspend" : "resume");
+
+	return -EBUSY;
+}
+
+/*
  * TX descriptor initialization
  */
 static void
@@ -1082,6 +1122,9 @@ rt2500usb_init_registers(struct rt2x00_u
 {
 	u16			reg;
 
+	if (rt2500usb_set_state(rt2x00usb, STATE_AWAKE))
+		return -EBUSY;
+
 	rt2x00_vendor_request(rt2x00usb, USB_DEVICE_MODE,
 		USB_VENDOR_REQUEST_OUT, USB_MODE_TEST, 0x00, NULL, 0);
 	rt2x00_vendor_request(rt2x00usb, USB_SINGLE_WRITE,
@@ -2221,44 +2264,42 @@ rt2500usb_disconnect(struct usb_interfac
 }
 
 #ifdef CONFIG_PM
-static int rt2500usb_resume(struct usb_interface *usb_intf);
-
 static int
 rt2500usb_suspend(struct usb_interface *usb_intf, pm_message_t state)
 {
 	struct net_device	*net_dev = usb_get_intfdata(usb_intf);
 	struct rt2x00_usb	*rt2x00usb = ieee80211_dev_hw_data(net_dev);
-	int			counter;
-	u16			reg;
 
 	NOTICE("Going to sleep.\n");
 
-	if (ieee80211_netif_oper(net_dev, NETIF_DETACH))
+	/*
+	 * If radio was enabled, stop radio and
+	 * set the resume flag to the radio will be enabled
+	 * when resuming.
+	 */
+	if (GET_FLAG(rt2x00usb, RADIO_ENABLED)) {
+		if (net_dev->stop(net_dev))
+			return -EBUSY;
+		SET_FLAG(rt2x00usb, RADIO_RESUME);
+	}
+
+	/*
+	 * Set device mode to sleep for power management.
+	 */
+	if (rt2500usb_set_state(rt2x00usb, STATE_SLEEP))
 		return -EBUSY;
 
-	rt2x00_register_read(rt2x00usb, MAC_CSR17, &reg);
-	rt2x00_set_field16_nb(&reg, MAC_CSR17_SET_STATE, 1);
-	rt2x00_set_field16_nb(&reg, MAC_CSR17_BBP_DESIRE_STATE, 1);
-	rt2x00_set_field16_nb(&reg, MAC_CSR17_RF_DESIRE_STATE, 1);
-	rt2x00_set_field16_nb(&reg, MAC_CSR17_PUT_TO_SLEEP, 1);
-	rt2x00_register_write(rt2x00usb, MAC_CSR17, reg);
+	/*
+	 * Uninitialize hardware.
+	 */
+	rt2500usb_uninitialize(net_dev);
 
 	/*
-	 * Device is not guarenteed to be asleep yet.
-	 * We must wait untill the register indicates
-	 * device has been correctly put to sleep.
+	 * Decrease usbdev refcount.
 	 */
-	for (counter = 0; counter < REGISTER_BUSY_COUNT; counter++) {
-		rt2x00_register_read(rt2x00usb, MAC_CSR17, &reg);
-		if ((rt2x00_get_field16_nb(reg, MAC_CSR17_BBP_CURR_STATE) == 1)
-		&& (rt2x00_get_field16_nb(reg, MAC_CSR17_RF_CURR_STATE) == 1))
-	return 0;
-		NOTICE("Waiting for device to sleep.\n");
-		msleep(10);
-	}
+	usb_put_dev(interface_to_usbdev(usb_intf));
 
-	rt2500usb_resume(usb_intf);
-	return -EIO;
+	return 0;
 }
 
 static int
@@ -2266,18 +2307,39 @@ rt2500usb_resume(struct usb_interface *u
 {
 	struct net_device	*net_dev = usb_get_intfdata(usb_intf);
 	struct rt2x00_usb	*rt2x00usb = ieee80211_dev_hw_data(net_dev);
-	u16			reg;
 
 	NOTICE("Waking up.\n");
 
-	rt2x00_register_read(rt2x00usb, MAC_CSR17, &reg);
-	rt2x00_set_field16_nb(&reg, MAC_CSR17_SET_STATE, 1);
-	rt2x00_set_field16_nb(&reg, MAC_CSR17_BBP_DESIRE_STATE, 3);
-	rt2x00_set_field16_nb(&reg, MAC_CSR17_RF_DESIRE_STATE, 3);
-	rt2x00_set_field16_nb(&reg, MAC_CSR17_PUT_TO_SLEEP, 0);
-	rt2x00_register_write(rt2x00usb, MAC_CSR17, reg);
+	/*
+	 * Increase usbdev refcount.
+	 */
+	usb_get_dev(interface_to_usbdev(usb_intf));
 
-	return ieee80211_netif_oper(net_dev, NETIF_ATTACH);
+	/*
+	 * Initialize hardware.
+	 */
+	if (rt2500usb_initialize(usb_intf, net_dev)) {
+		ERROR("Failed to initialize device.\n");
+		return -ENOMEM;
+	}
+
+	/*
+	 * Set device mode to awake.
+	 */
+	if (rt2500usb_set_state(rt2x00usb, STATE_AWAKE))
+		return -EBUSY;
+
+	/*
+	 * Only enable radio when it was enabled
+	 * when we suspended.
+	 */
+	if (GET_FLAG(rt2x00usb, RADIO_RESUME)) {
+		if (net_dev->open(net_dev))
+			return -EBUSY;
+		CLEAR_FLAG(rt2x00usb, RADIO_RESUME);
+	}
+
+	return 0;
 }
 #endif /* CONFIG_PM */