diff --git a/drivers/net/wireless/rt2x00/rt2800.h b/drivers/net/wireless/rt2x00/rt2800.h
index 2571a2f..822f9e5 100644
--- a/drivers/net/wireless/rt2x00/rt2800.h
+++ b/drivers/net/wireless/rt2x00/rt2800.h
@@ -1627,6 +1627,7 @@ struct mac_iveiv_entry {
 
 /*
  * H2M_MAILBOX_CSR: Host-to-MCU Mailbox.
+ * CMD_TOKEN: Command id, 0xff disable status reporting
  */
 #define H2M_MAILBOX_CSR			0x7010
 #define H2M_MAILBOX_CSR_ARG0		FIELD32(0x000000ff)
@@ -1636,6 +1637,8 @@ struct mac_iveiv_entry {
 
 /*
  * H2M_MAILBOX_CID:
+ * Free slots contain 0xff. MCU will store command's token to lowest free slot.
+ * If all slots are occupied status will be dropped.
  */
 #define H2M_MAILBOX_CID			0x7014
 #define H2M_MAILBOX_CID_CMD0		FIELD32(0x000000ff)
@@ -1645,6 +1648,7 @@ struct mac_iveiv_entry {
 
 /*
  * H2M_MAILBOX_STATUS:
+ * Command status will be saved to same slot as command id.
  */
 #define H2M_MAILBOX_STATUS		0x701c
 
@@ -2259,6 +2263,12 @@ struct mac_iveiv_entry {
 
 /*
  * MCU mailbox commands.
+ * MCU_SLEEP - go to power-save mode.
+ *             arg1: 1: save as much power as possible, 0: save less power
+ *             status: 1: success, 2: already asleep,
+ *                     3: maybe MAC is busy so can't finish this task
+ * MCU_RADIO_OFF
+ *             arg0: 0: do power-saving, NOT turn off radio
  */
 #define MCU_SLEEP			0x30
 #define MCU_WAKEUP			0x31
@@ -2279,7 +2289,9 @@ struct mac_iveiv_entry {
 /*
  * MCU mailbox tokens
  */
-#define TOKEN_WAKUP			3
+#define TOKEN_SLEEP			1
+#define TOKEN_RADIO_OFF			2
+#define TOKEN_WAKEUP			3
 
 /*
  * DMA descriptor defines.
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index dc88bae..9ac3017 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -517,23 +517,6 @@ static void rt2800pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 	}
 }
 
-static int rt2800pci_set_state(struct rt2x00_dev *rt2x00dev,
-			       enum dev_state state)
-{
-	if (state == STATE_AWAKE) {
-		rt2800_mcu_request(rt2x00dev, MCU_WAKEUP, TOKEN_WAKUP, 0, 0x02);
-		rt2800pci_mcu_status(rt2x00dev, TOKEN_WAKUP);
-	} else if (state == STATE_SLEEP) {
-		rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_STATUS,
-					 0xffffffff);
-		rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CID,
-					 0xffffffff);
-		rt2800_mcu_request(rt2x00dev, MCU_SLEEP, 0x01, 0xff, 0x01);
-	}
-
-	return 0;
-}
-
 static int rt2800pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 				      enum dev_state state)
 {
@@ -541,14 +524,20 @@ static int rt2800pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 
 	switch (state) {
 	case STATE_RADIO_ON:
-		/*
-		 * Before the radio can be enabled, the device first has
-		 * to be woken up. After that it needs a bit of time
-		 * to be fully awake and then the radio can be enabled.
-		 */
-		rt2800pci_set_state(rt2x00dev, STATE_AWAKE);
-		msleep(1);
+		/* Initialise all registers and send MCU_BOOT_SIGNAL. */
 		retval = rt2800pci_enable_radio(rt2x00dev);
+
+		/* After resume MCU_BOOT_SIGNAL will trash those. */
+		rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_STATUS, ~0);
+		rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CID, ~0);
+
+		/* Finish initialization procedure. */
+		rt2800_mcu_request(rt2x00dev, MCU_SLEEP, TOKEN_RADIO_OFF,
+				   0xff, 0x02);
+		rt2800pci_mcu_status(rt2x00dev, TOKEN_RADIO_OFF);
+
+		rt2800_mcu_request(rt2x00dev, MCU_WAKEUP, TOKEN_WAKEUP, 0, 0);
+		rt2800pci_mcu_status(rt2x00dev, TOKEN_WAKEUP);
 		break;
 	case STATE_RADIO_OFF:
 		/*
@@ -556,7 +545,7 @@ static int rt2800pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		 * be put to sleep for powersaving.
 		 */
 		rt2800pci_disable_radio(rt2x00dev);
-		rt2800pci_set_state(rt2x00dev, STATE_SLEEP);
+		rt2800pci_set_device_state(rt2x00dev, STATE_SLEEP);
 		break;
 	case STATE_RADIO_IRQ_ON:
 	case STATE_RADIO_IRQ_OFF:
@@ -565,8 +554,16 @@ static int rt2800pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 	case STATE_DEEP_SLEEP:
 	case STATE_SLEEP:
 	case STATE_STANDBY:
+		/* PCIe devices won't report status after SLEEP request. */
+		rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_STATUS, ~0);
+		rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CID, ~0);
+		rt2800_mcu_request(rt2x00dev, MCU_SLEEP, TOKEN_SLEEP,
+				   0xff, 0x01);
+		break;
 	case STATE_AWAKE:
-		retval = rt2800pci_set_state(rt2x00dev, state);
+		rt2800_mcu_request(rt2x00dev, MCU_WAKEUP, TOKEN_WAKEUP,
+				   0, 0x02);
+		rt2800pci_mcu_status(rt2x00dev, TOKEN_WAKEUP);
 		break;
 	default:
 		retval = -ENOTSUPP;