3 files changed, 29 insertions, 0 deletions

diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index ec99bf799e..b4151d0352 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -503,6 +503,33 @@ static int ata_perform_sleep(void)
     return ret;
 }
 
+int ata_standby(int time)
+{
+    int ret = 0;
+
+    mutex_lock(&ata_mtx);
+
+    ATA_SELECT = ata_device;
+
+    if(!wait_for_rdy()) {
+        DEBUGF("ata_standby() - not RDY\n");
+        mutex_unlock(&ata_mtx);
+        return -1;
+    }
+
+    ATA_NSECTOR = ((time + 5) / 5) & 0xff; /* Round up to nearest 5 secs */
+    ATA_COMMAND = CMD_STANDBY;
+
+    if (!wait_for_rdy())
+    {
+        DEBUGF("ata_standby() - CMD failed\n");
+        ret = -2;
+    }
+
+    mutex_unlock(&ata_mtx);
+    return ret;
+}
+
 int ata_sleep(void)
 {
     queue_post(&ata_queue, Q_SLEEP, NULL);
diff --git a/firmware/export/ata.h b/firmware/export/ata.h
index 7a1fb4a952..213d3e5b8b 100644
--- a/firmware/export/ata.h
+++ b/firmware/export/ata.h
@@ -36,6 +36,7 @@ extern void ata_enable(bool on);
 extern void ata_spindown(int seconds);
 extern void ata_poweroff(bool enable);
 extern int ata_sleep(void);
+extern int ata_standby(int time);
 extern bool ata_disk_is_active(void);
 extern int ata_hard_reset(void);
 extern int ata_soft_reset(void);
diff --git a/firmware/usb.c b/firmware/usb.c
index fcc06c2f6e..1efecb8705 100644
--- a/firmware/usb.c
+++ b/firmware/usb.c
@@ -86,6 +86,7 @@ static void usb_slave_mode(bool on)
         DEBUGF("Entering USB slave mode\n");
         ata_soft_reset();
         ata_init();
+        ata_standby(15);
         ata_enable(false);
         usb_enable(true);
     }