2 files changed, 35 insertions, 0 deletions

diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index 2dca17512e..87dacc3ed0 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -59,6 +59,7 @@
 #define CMD_SECURITY_FREEZE_LOCK   0xF5
 
 #define Q_SLEEP 0
+#define Q_CLOSE 1
 
 #define READ_TIMEOUT 5*HZ
 
@@ -66,6 +67,10 @@
 #define ATA_POWER_OFF_TIMEOUT 2*HZ
 #endif
 
+#ifdef ATA_DRIVER_CLOSE
+static struct thread_entry *ata_thread_p = NULL;
+#endif
+
 #if defined(MAX_PHYS_SECTOR_SIZE) && MEM == 64
 /* Hack - what's the deal with 5g? */
 struct ata_lock
@@ -941,6 +946,11 @@ static void ata_thread(void)
                 call_ata_idle_notifys(false);
                 last_disk_activity = current_tick - sleep_timeout + (HZ/2);
                 break;
+
+#ifdef ATA_DRIVER_CLOSE
+            case Q_CLOSE:
+                return;
+#endif
         }
     }
 }
@@ -1307,6 +1317,9 @@ int ata_init(void)
         mutex_lock(&ata_mtx); /* Balance unlock below */
 
         last_disk_activity = current_tick;
+#ifdef ATA_DRIVER_CLOSE
+        ata_thread_p =
+#endif
         create_thread(ata_thread, ata_stack,
                       sizeof(ata_stack), 0, ata_thread_name
                       IF_PRIO(, PRIORITY_USER_INTERFACE)
@@ -1322,6 +1335,21 @@ int ata_init(void)
     return rc;
 }
 
+#ifdef ATA_DRIVER_CLOSE
+void ata_close(void)
+{
+    struct thread_entry *thread = ata_thread_p;
+    
+    if (thread == NULL)
+        return;
+
+    ata_thread_p = NULL;
+
+    queue_post(&ata_queue, Q_CLOSE, 0);
+    thread_wait(thread);
+}
+#endif /* ATA_DRIVER_CLOSE */
+
 #if (CONFIG_LED == LED_REAL)
 void ata_set_led_enabled(bool enabled) 
 {
diff --git a/firmware/drivers/button.c b/firmware/drivers/button.c
index 90ff800c88..74c5a97167 100644
--- a/firmware/drivers/button.c
+++ b/firmware/drivers/button.c
@@ -410,6 +410,13 @@ void button_init(void)
     tick_add_task(button_tick);
 }
 
+#ifdef BUTTON_DRIVER_CLOSE
+void button_close(void)
+{
+    tick_remove_task(button_tick);
+}
+#endif /* BUTTON_DRIVER_CLOSE */
+
 #ifdef HAVE_LCD_BITMAP /* only bitmap displays can be flipped */
 /*
  * helper function to swap LEFT/RIGHT, UP/DOWN (if present), and F1/F3 (Recorder)