rt2x00: Make sure device has reached requested state while suspend/resume

diff -uprN wireless-dev-rt2x00/drivers/net/wireless/d80211/rt2x00/rt2400pci.c wireless-dev-rt2x00-patch/drivers/net/wireless/d80211/rt2x00/rt2400pci.c
@@ -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 */
 

186 lines not on this page · raw message

168 lines not on this page · raw message