marioparty4/src/dolphin/os/OSMutex.c

#include "dolphin/os.h"

#define PushTail(queue, mutex, link)                                                                                                                 \
    do {                                                                                                                                             \
        OSMutex *__prev;                                                                                                                             \
                                                                                                                                                     \
        __prev = (queue)->tail;                                                                                                                      \
        if (__prev == NULL)                                                                                                                          \
            (queue)->head = (mutex);                                                                                                                 \
        else                                                                                                                                         \
            __prev->link.next = (mutex);                                                                                                             \
        (mutex)->link.prev = __prev;                                                                                                                 \
        (mutex)->link.next = NULL;                                                                                                                   \
        (queue)->tail = (mutex);                                                                                                                     \
    } while (0)

#define PopHead(queue, mutex, link)                                                                                                                  \
    do {                                                                                                                                             \
        OSMutex *__next;                                                                                                                             \
                                                                                                                                                     \
        (mutex) = (queue)->head;                                                                                                                     \
        __next = (mutex)->link.next;                                                                                                                 \
        if (__next == NULL)                                                                                                                          \
            (queue)->tail = NULL;                                                                                                                    \
        else                                                                                                                                         \
            __next->link.prev = NULL;                                                                                                                \
        (queue)->head = __next;                                                                                                                      \
    } while (0)

#define PopItem(queue, mutex, link)                                                                                                                  \
    do {                                                                                                                                             \
        OSMutex *__next;                                                                                                                             \
        OSMutex *__prev;                                                                                                                             \
                                                                                                                                                     \
        __next = (mutex)->link.next;                                                                                                                 \
        __prev = (mutex)->link.prev;                                                                                                                 \
                                                                                                                                                     \
        if (__next == NULL)                                                                                                                          \
            (queue)->tail = __prev;                                                                                                                  \
        else                                                                                                                                         \
            __next->link.prev = __prev;                                                                                                              \
                                                                                                                                                     \
        if (__prev == NULL)                                                                                                                          \
            (queue)->head = __next;                                                                                                                  \
        else                                                                                                                                         \
            __prev->link.next = __next;                                                                                                              \
    } while (0)

void OSInitMutex(OSMutex *mutex)
{
    OSInitThreadQueue(&mutex->queue);
    mutex->thread = 0;
    mutex->count = 0;
}

void OSLockMutex(OSMutex *mutex)
{
    BOOL enabled = OSDisableInterrupts();
    OSThread *currentThread = OSGetCurrentThread();
    OSThread *ownerThread;

    while (TRUE) {
        ownerThread = ((OSMutex *)mutex)->thread;
        if (ownerThread == 0) {
            mutex->thread = currentThread;
            mutex->count++;
            PushTail(&currentThread->queueMutex, mutex, link);
            break;
        }
        else if (ownerThread == currentThread) {
            mutex->count++;
            break;
        }
        else {
            currentThread->mutex = mutex;
            __OSPromoteThread(mutex->thread, currentThread->priority);
            OSSleepThread(&mutex->queue);
            currentThread->mutex = 0;
        }
    }
    OSRestoreInterrupts(enabled);
}

void OSUnlockMutex(OSMutex *mutex)
{
    BOOL enabled = OSDisableInterrupts();
    OSThread *currentThread = OSGetCurrentThread();

    if (mutex->thread == currentThread && --mutex->count == 0) {
        PopItem(&currentThread->queueMutex, mutex, link);
        mutex->thread = NULL;
        if (currentThread->priority < currentThread->base) {
            currentThread->priority = __OSGetEffectivePriority(currentThread);
        }

        OSWakeupThread(&mutex->queue);
    }
    OSRestoreInterrupts(enabled);
}

void __OSUnlockAllMutex(OSThread *thread)
{
    OSMutex *mutex;

    while (thread->queueMutex.head) {
        PopHead(&thread->queueMutex, mutex, link);
        mutex->count = 0;
        mutex->thread = NULL;
        OSWakeupThread(&mutex->queue);
    }
}

BOOL OSTryLockMutex(OSMutex *mutex)
{
    BOOL enabled = OSDisableInterrupts();
    OSThread *currentThread = OSGetCurrentThread();
    BOOL locked;
    if (mutex->thread == 0) {
        mutex->thread = currentThread;
        mutex->count++;
        PushTail(&currentThread->queueMutex, mutex, link);
        locked = TRUE;
    }
    else if (mutex->thread == currentThread) {
        mutex->count++;
        locked = TRUE;
    }
    else {
        locked = FALSE;
    }
    OSRestoreInterrupts(enabled);
    return locked;
}

void OSInitCond(OSCond *cond)
{
    OSInitThreadQueue(&cond->queue);
}

void OSWaitCond(OSCond *cond, OSMutex *mutex)
{
    BOOL enabled = OSDisableInterrupts();
    OSThread *currentThread = OSGetCurrentThread();
    s32 count;

    if (mutex->thread == currentThread) {
        count = mutex->count;
        mutex->count = 0;
        PopItem(&currentThread->queueMutex, mutex, link);
        mutex->thread = NULL;

        if (currentThread->priority < currentThread->base) {
            currentThread->priority = __OSGetEffectivePriority(currentThread);
        }

        OSDisableScheduler();
        OSWakeupThread(&mutex->queue);
        OSEnableScheduler();
        OSSleepThread(&cond->queue);
        OSLockMutex(mutex);
        mutex->count = count;
    }

    OSRestoreInterrupts(enabled);
}

void OSSignalCond(OSCond *cond)
{
    OSWakeupThread(&cond->queue);
}

static BOOL IsMember(OSMutexQueue *queue, OSMutex *mutex)
{
    OSMutex *member;

    for (member = queue->head; member; member = member->link.next) {
        if (mutex == member)
            return TRUE;
    }
    return FALSE;
}

BOOL __OSCheckMutex(OSMutex *mutex)
{
    OSThread *thread;
    OSThreadQueue *queue;
    OSPriority priority = 0;

    queue = &mutex->queue;
    if (!(queue->head == NULL || queue->head->link.prev == NULL))
        return FALSE;
    if (!(queue->tail == NULL || queue->tail->link.next == NULL))
        return FALSE;
    for (thread = queue->head; thread; thread = thread->link.next) {
        if (!(thread->link.next == NULL || thread == thread->link.next->link.prev))
            return FALSE;
        if (!(thread->link.prev == NULL || thread == thread->link.prev->link.next))
            return FALSE;

        if (thread->state != OS_THREAD_STATE_WAITING)
            return FALSE;

        if (thread->priority < priority)
            return FALSE;
        priority = thread->priority;
    }

    if (mutex->thread) {
        if (mutex->count <= 0)
            return FALSE;
    }
    else {
        if (0 != mutex->count)
            return FALSE;
    }

    return TRUE;
}

BOOL __OSCheckDeadLock(OSThread *thread)
{
    OSMutex *mutex;

    mutex = thread->mutex;
    while (mutex && mutex->thread) {
        if (mutex->thread == thread)
            return TRUE;
        mutex = mutex->thread->mutex;
    }
    return FALSE;
}

BOOL __OSCheckMutexes(OSThread *thread)
{
    OSMutex *mutex;

    for (mutex = thread->queueMutex.head; mutex; mutex = mutex->link.next) {
        if (mutex->thread != thread)
            return FALSE;
        if (!__OSCheckMutex(mutex))
            return FALSE;
    }
    return TRUE;
}