diff --git a/firmware/drivers/ata.c b/firmware/drivers/ata.c index 6a1db22919..87dacc3ed0 100644 --- a/firmware/drivers/ata.c +++ b/firmware/drivers/ata.c @@ -71,6 +71,74 @@ 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 +{ + 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 SHAREDBSS_ATTR; int ata_device; /* device 0 (master) or 1 (slave) */ diff --git a/firmware/thread.c b/firmware/thread.c index 71656e1711..0f4273107f 100644 --- a/firmware/thread.c +++ b/firmware/thread.c @@ -161,6 +161,13 @@ void switch_thread(void) * Processor-specific section */ +#if defined(MAX_PHYS_SECTOR_SIZE) && MEM == 64 +/* Support a special workaround object for large-sector disks */ +#define IF_NO_SKIP_YIELD(...) __VA_ARGS__ +#else +#define IF_NO_SKIP_YIELD(...) +#endif + #if defined(CPU_ARM) /*--------------------------------------------------------------------------- * Start the thread running and terminate it if it returns @@ -1945,8 +1952,9 @@ void switch_thread(void) #endif #ifdef HAVE_PRIORITY_SCHEDULING + IF_NO_SKIP_YIELD( if (thread->skip_count != -1) ) /* Reset the value of thread's skip count */ - thread->skip_count = 0; + thread->skip_count = 0; #endif for (;;) @@ -2002,6 +2010,7 @@ void switch_thread(void) * priority threads are runnable. The highest priority runnable * thread(s) are never skipped. */ if (priority <= max || + IF_NO_SKIP_YIELD( thread->skip_count == -1 || ) (diff = priority - max, ++thread->skip_count > diff*diff)) { cores[core].running = thread; @@ -2146,7 +2155,8 @@ unsigned int wakeup_thread(struct thread_entry **list) if (bl == NULL) { /* No inheritance - just boost the thread by aging */ - thread->skip_count = thread->priority; + IF_NO_SKIP_YIELD( if (thread->skip_count != -1) ) + thread->skip_count = thread->priority; current = cores[CURRENT_CORE].running; } else