2 files changed, 23 insertions, 14 deletions

diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index 67aab8a9ba..1b917686c5 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -1128,12 +1128,18 @@ static int init_and_check(bool hard_reset)
 
 int ata_init(void)
 {
-    int rc;
-    bool coldstart = ata_is_coldstart();
-         /* must be called before ata_device_init() */          
+    int rc = 0;
+    bool coldstart;
 
-    spinlock_init(&ata_spinlock IF_COP(, SPINLOCK_TASK_SWITCH));
+    if ( !initialized ) {
+        spinlock_init(&ata_spinlock IF_COP(, SPINLOCK_TASK_SWITCH));
+        queue_init(&ata_queue, true);
+    }
+
+    spinlock_lock(&ata_spinlock);
 
+    /* must be called before ata_device_init() */
+    coldstart = ata_is_coldstart();
     ata_led(false);
     ata_device_init();
     sleeping = false;
@@ -1143,6 +1149,9 @@ int ata_init(void)
 #endif
 
     if ( !initialized ) {
+        /* First call won't have multiple thread contention */
+        spinlock_unlock(&ata_spinlock);
+
         if (!ide_powered()) /* somebody has switched it off */
         {
             ide_power_enable(true);
@@ -1202,8 +1211,6 @@ int ata_init(void)
         if (rc)
             return -60 + rc;
 
-        queue_init(&ata_queue, true);
-
         last_disk_activity = current_tick;
         create_thread(ata_thread, ata_stack,
                       sizeof(ata_stack), 0, ata_thread_name
@@ -1214,9 +1221,10 @@ int ata_init(void)
     }
     rc = set_multiple_mode(multisectors);
     if (rc)
-        return -70 + rc;
+        rc = -70 + rc;
 
-    return 0;
+    spinlock_unlock(&ata_spinlock);
+    return rc;
 }
 
 #if (CONFIG_LED == LED_REAL)
diff --git a/firmware/target/arm/sandisk/ata-c200_e200.c b/firmware/target/arm/sandisk/ata-c200_e200.c
index 5a7577f08d..ef2bad387b 100644
--- a/firmware/target/arm/sandisk/ata-c200_e200.c
+++ b/firmware/target/arm/sandisk/ata-c200_e200.c
@@ -1149,17 +1149,17 @@ int ata_init(void)
 {
     int ret = 0;
 
+    if (!initialized)
+        spinlock_init(&sd_spin IF_COP(, SPINLOCK_TASK_SWITCH));
+
+    spinlock_lock(&sd_spin);
+
     ata_led(false);
 
-    /* NOTE: This init isn't dual core safe */
     if (!initialized)
     {
         initialized = true;
 
-        spinlock_init(&sd_spin IF_COP(, SPINLOCK_TASK_SWITCH));
-
-        spinlock_lock(&sd_spin);
-
         /* init controller */
         outl(inl(0x70000088) & ~(0x4), 0x70000088);
         outl(inl(0x7000008c) & ~(0x4), 0x7000008c);
@@ -1213,9 +1213,10 @@ int ata_init(void)
         GPIOL_INT_EN |= 0x08;
 #endif
 #endif
-        spinlock_unlock(&sd_spin);
     }
 
+    spinlock_unlock(&sd_spin);
+
     return ret;
 }