1 files changed, 68 insertions, 0 deletions

diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c
index 56b303da8d..681160cf01 100644
--- a/firmware/drivers/ata.c
+++ b/firmware/drivers/ata.c
@@ -66,6 +66,74 @@
 #define ATA_POWER_OFF_TIMEOUT 2*HZ
 #endif
 
+#ifdef MAX_PHYS_SECTOR_SIZE
+/* Hack - what's the deal with 5g? */
+struct ata_lock
+{
+    struct thread_entry *thread;
+    int count;
+    volatile unsigned char locked;
+    IF_COP( struct corelock cl; )
+};
+
+static void ata_lock_init(struct ata_lock *l)
+{
+    corelock_init(&l->cl);
+    l->locked = 0;
+    l->count = 0;
+    l->thread = NULL;
+}
+
+static void ata_lock_lock(struct ata_lock *l)
+{
+    struct thread_entry * const current = thread_get_current();
+
+    if (current == l->thread)
+    {
+        l->count++;
+        return;
+    }
+
+    corelock_lock(&l->cl);
+
+    IF_PRIO( current->skip_count = -1; )
+
+    while (l->locked != 0)
+    {
+        corelock_unlock(&l->cl);
+        switch_thread();
+        corelock_lock(&l->cl);
+    }
+
+    l->locked = 1;
+    l->thread = current;
+    corelock_unlock(&l->cl);
+}
+
+static void ata_lock_unlock(struct ata_lock *l)
+{
+    if (l->count > 0)
+    {
+        l->count--;
+        return;
+    }
+
+    corelock_lock(&l->cl);
+
+    IF_PRIO( l->thread->skip_count = 0; )
+
+    l->thread = NULL;
+    l->locked = 0;
+
+    corelock_unlock(&l->cl);
+}
+
+#define mutex           ata_lock
+#define mutex_init      ata_lock_init
+#define mutex_lock      ata_lock_lock
+#define mutex_unlock    ata_lock_unlock
+#endif /* MAX_PHYS_SECTOR_SIZE */
+
 static struct mutex ata_mtx NOCACHEBSS_ATTR;
 int ata_device; /* device 0 (master) or 1 (slave) */