/*---------------------------------------------------------------------------*
  Project:  CtrBrom - libraries - OS
  File:     os_mutex.c

  Copyright 2009 Nintendo.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Date::            $
  $Rev$
  $Author$
 *---------------------------------------------------------------------------*/
#include <brom/types.h>
#include <brom.h>

void i_osUnlockMutexCore(OSMutex *mutex, u32 type);

void    i_osEnqueueTail(OSThread *thread, OSMutex *mutex);
void    i_osDequeueItem(OSThread *thread, OSMutex *mutex);
OSMutex *i_osDequeueHead(OSThread *thread);

/*---------------------------------------------------------------------------*
  Name:         osInitMutex

  Description:  initialize mutex

  Arguments:    mutex       pointer to mutex structure
                            to be initialized

  Returns:      None
 *---------------------------------------------------------------------------*/
void osInitMutex(OSMutex *mutex)
{
    SDK_ASSERT(mutex);

    osInitThreadQueue(&mutex->queue);
    mutex->thread = NULL;
    osSetMutexCount( mutex, 0 );
    osSetMutexType( mutex, OS_MUTEX_TYPE_NONE );
}

/*---------------------------------------------------------------------------*
  Name:         osLockMutex

  Description:  lock mutex

  Arguments:    mutex       pointer to mutex structure

  Returns:      None
 *---------------------------------------------------------------------------*/
void osLockMutex(OSMutex *mutex)
{
    OSIntrMode e = osDisableInterrupts();
    OSThread *currentThread = osGetCurrentThread();

    while(1)
    {
        //---- try lock mutex
        if ( osTryLockMutex(mutex) )
        {
            break;
        }

        currentThread->mutex = mutex;
        osSleepThread(&mutex->queue);
        currentThread->mutex = NULL;
    }

    (void)osRestoreInterrupts(e);
}

/*---------------------------------------------------------------------------*
  Name:         osUnlockMutex

  Description:  unlock mutex

  Arguments:    mutex       pointer to mutex structure

  Returns:      None
 *---------------------------------------------------------------------------*/
void osUnlockMutex(OSMutex *mutex)
{
    i_osUnlockMutexCore(mutex, OS_MUTEX_TYPE_STD);
}

/*---------------------------------------------------------------------------*
  Name:         i_osUnlockAllMutex

  Description:  unlocks all the mutexes locked by the thread

  Arguments:    mutex       pointer to mutex structure

  Returns:      None.
 *---------------------------------------------------------------------------*/
void i_osUnlockAllMutex(OSThread *thread)
{
    OSMutex *mutex;

    SDK_ASSERT(thread);

#ifndef SDK_THREAD_INFINITY
    while (thread->mutexQueueHead)
    {
        mutex = i_osDequeueHead(thread);
        SDK_ASSERT(mutex->thread == thread);

        osSetMutexCount( mutex, 0 );
        mutex->thread = NULL;
        osSetMutexType( mutex, OS_MUTEX_TYPE_NONE );
        osWakeupThread(&(mutex->queue));
    }
#else
    while (thread->mutexQueue.head)
    {
        mutex = i_osRemoveMutexLinkFromQueue(&thread->mutexQueue);
        SDK_ASSERT(mutex->thread == thread);

        osSetMutexCount( mutex, 0 );
        mutex->thread = NULL;
        osSetMutexType( mutex, OS_MUTEX_TYPE_NONE );
        osWakeupThread(&mutex->queue);
    }
#endif
}

/*---------------------------------------------------------------------------*
  Name:         osTryLockMutex

  Description:  try to lock mutex

  Arguments:    mutex       pointer to mutex structure

  Returns:      True if lock
 *---------------------------------------------------------------------------*/
BOOL osTryLockMutex(OSMutex *mutex)
{
    OSIntrMode saved = osDisableInterrupts();
    OSThread *currentThread = osGetCurrentThread();
    BOOL    locked;

    SDK_ASSERT(mutex);

    // ---- able to lock mutex
    if (mutex->thread == NULL)
    {
        mutex->thread = currentThread;
        osSetMutexType( mutex, OS_MUTEX_TYPE_STD );
        osIncreaseMutexCount(mutex);
        i_osEnqueueTail(currentThread, mutex);
        locked = TRUE;
    }
    // ---- current thread is same with thread locking mutex
    else if (mutex->thread == currentThread)
    {
        osIncreaseMutexCount(mutex);
        locked = TRUE;
    }
    // ---- current thread is different from locking mutex
    else
    {
        locked = FALSE;
    }

    (void)osRestoreInterrupts(saved);
    return locked;
}

//================================================================================
// Read / Write Lock
//================================================================================
/*---------------------------------------------------------------------------*
  Name:         osLockMutexR

  Description:  lock RW mutex as READ access

  Arguments:    mutex       pointer to RW mutex structure

  Returns:      None
 *---------------------------------------------------------------------------*/
void osLockMutexR(OSMutex *mutex)
{
    OSIntrMode e = osDisableInterrupts();
    OSThread *currentThread = osGetCurrentThread();

    while(1)
    {
        //---- try lock by READ
        if ( osTryLockMutexR(mutex) )
        {
            break;
        }

        currentThread->mutex = mutex;
        osSleepThread(&mutex->queue);
        currentThread->mutex = NULL;
    }

    (void)osRestoreInterrupts(e);
}

/*---------------------------------------------------------------------------*
  Name:         osLockMutexW

  Description:  lock RW mutex as WRITE access

  Arguments:    mutex       pointer to RW mutex structure

  Returns:      None
 *---------------------------------------------------------------------------*/
void osLockMutexW(OSMutex *mutex)
{
    OSIntrMode e = osDisableInterrupts();
    OSThread *currentThread = osGetCurrentThread();

    while(1)
    {
        //---- try lock by WRITE
        if ( osTryLockMutexW(mutex) )
        {
            break;
        }

        currentThread->mutex = mutex;
        osSleepThread(&mutex->queue);
        currentThread->mutex = NULL;
    }

    (void)osRestoreInterrupts(e);
}

/*---------------------------------------------------------------------------*
  Name:         osTryLockMutexR

  Description:  try to lock RW mutex as READ access

  Arguments:    mutex       pointer to RW mutex structure

  Returns:      TRUE if locked
 *---------------------------------------------------------------------------*/
BOOL osTryLockMutexR(OSMutex *mutex)
{
    OSIntrMode e = osDisableInterrupts();
    BOOL locked = FALSE;
    OSThread * currentThread = osGetCurrentThread();

    switch( osGetMutexType(mutex) )
    {
    case OS_MUTEX_TYPE_NONE:
        mutex->thread = currentThread;
        osSetMutexType( mutex, OS_MUTEX_TYPE_R );
        osSetMutexCount( mutex, 1 );
        i_osEnqueueTail(currentThread, mutex);
        locked = TRUE;
        break;

    case OS_MUTEX_TYPE_R:
        osIncreaseMutexCount(mutex);
        locked = TRUE;
        break;

    case OS_MUTEX_TYPE_W:
    default:
        break;
    }

    (void)osRestoreInterrupts(e);
    return locked;
}

/*---------------------------------------------------------------------------*
  Name:         osTryLockMutexW

  Description:  try to lock RW mutex as WRITE access

  Arguments:    mutex       pointer to RW mutex structure

  Returns:      TRUE if locked
 *---------------------------------------------------------------------------*/
BOOL osTryLockMutexW(OSMutex *mutex)
{
    OSIntrMode e = osDisableInterrupts();
    BOOL locked = FALSE;
    OSThread * currentThread = osGetCurrentThread();

    switch( osGetMutexType(mutex) )
    {
    case OS_MUTEX_TYPE_NONE:
        mutex->thread = currentThread;
        osSetMutexType( mutex, OS_MUTEX_TYPE_W );
        osSetMutexCount( mutex, 1 );
        i_osEnqueueTail(currentThread, mutex);
        locked = TRUE;
        break;

    case OS_MUTEX_TYPE_W:
        if ( mutex->thread == currentThread )
        {
            osIncreaseMutexCount(mutex);
            locked = TRUE;
        }
        break;

    case OS_MUTEX_TYPE_R:
    default:
        break;
    }

    (void)osRestoreInterrupts(e);
    return locked;
}

/*---------------------------------------------------------------------------*
  Name:         i_osUnlockMutexCore

  Description:  core routine to unlock mutex

  Arguments:    mutex       pointer to RW mutex structure
                type        mutex type. if different from osGetMutexType(), error.
                              OS_MUTEX_TYPE_STD
                              OS_MUTEX_TYPE_R
                              OS_MUTEX_TYPE_W
                              OS_MUTEX_TYPE_NONE (no check)

  Returns:      None
 *---------------------------------------------------------------------------*/
void i_osUnlockMutexCore(OSMutex *mutex, u32 type )
{
    OSIntrMode e = osDisableInterrupts();
    OSThread *currentThread = osGetCurrentThread();
    BOOL unlocked = FALSE;

    SDK_ASSERT(mutex);

    //---- check unlock type
    if ( type != OS_MUTEX_TYPE_NONE && type != osGetMutexType(mutex) )
    {
        //osTPanic("Illegal unlock mutex\n");
        osTWarning("Illegal unlock mutex");
        (void)osRestoreInterrupts(e);
        return;
    }

    switch( osGetMutexType(mutex) )
    {
    case OS_MUTEX_TYPE_STD:
    case OS_MUTEX_TYPE_W:
        if ( mutex->thread == currentThread )
        {
            osDecreaseMutexCount(mutex);
            if ( osGetMutexCount(mutex) == 0 )
            {
                unlocked = TRUE;
            }
        }
        break;

    case OS_MUTEX_TYPE_R:
        osDecreaseMutexCount(mutex);
        if ( osGetMutexCount(mutex) == 0 )
        {
            unlocked = TRUE;
        }
        break;

    default:
        osTWarning("Illegal unlock mutex");
        (void)osRestoreInterrupts(e);
        return;
        //osTPanic("Illegal unlock mutex\n");
        //break;
    }

    //---- unlock mutex
    if ( unlocked )
    {
        i_osDequeueItem(currentThread, mutex);
        mutex->thread = NULL;
        osSetMutexType( mutex, OS_MUTEX_TYPE_NONE );
        osWakeupThread(&mutex->queue);
    }

    (void)osRestoreInterrupts(e);
}

/*---------------------------------------------------------------------------*
  Name:         osUnlockMutexR

  Description:  unlock mutex locked as READ access

  Arguments:    mutex       pointer to mutex structure

  Returns:      None
 *---------------------------------------------------------------------------*/
void osUnlockMutexR(OSMutex *mutex)
{
    i_osUnlockMutexCore( mutex, OS_MUTEX_TYPE_R );
}

/*---------------------------------------------------------------------------*
  Name:         osUnlockMutexW

  Description:  unlock mutex locked as WRITE access

  Arguments:    mutex       pointer to mutex structure

  Returns:      None
 *---------------------------------------------------------------------------*/
void osUnlockMutexW(OSMutex *mutex)
{
    i_osUnlockMutexCore( mutex, OS_MUTEX_TYPE_W );
}

/*---------------------------------------------------------------------------*
  Name:         osUnlockMutexRW

  Description:  unlock mutex locked as READ/WRITE access

  Arguments:    mutex       pointer to mutex structure

  Returns:      None
 *---------------------------------------------------------------------------*/
void osUnlockMutexRW(OSMutex *mutex)
{
    i_osUnlockMutexCore( mutex, OS_MUTEX_TYPE_NONE );
}

/*---------------------------------------------------------------------------*
  Name:         osLockMutexFromRToW

  Description:  Promote mutexR lock to mutexW lock without unlock.
                Wait till success.

  Arguments:    mutex       pointer to mutex structure

  Returns:      None
 *---------------------------------------------------------------------------*/
void osLockMutexFromRToW(OSMutex *mutex)
{
    OSIntrMode e = osDisableInterrupts();
    OSThread *currentThread = osGetCurrentThread();

    while(1)
    {
        if ( osTryLockMutexFromRToW(mutex) )
        {
            break;
        }

        currentThread->mutex = mutex;
        osSleepThread(&mutex->queue);
        currentThread->mutex = NULL;
    }

    (void)osRestoreInterrupts(e);
}

/*---------------------------------------------------------------------------*
  Name:         osTryLockMutexFromRToW

  Description:  Try to promote mutexR lock to mutexW lock without unlock.

  Arguments:    mutex       pointer to mutex structure

  Returns:      TRUE if success
 *---------------------------------------------------------------------------*/
BOOL osTryLockMutexFromRToW(OSMutex *mutex)
{
    OSIntrMode e = osDisableInterrupts();
    BOOL locked = FALSE;

    //---- change mutex type if no other thread is locked. and mutex is R
    if ( osGetMutexCount(mutex) == 1 && mutex->queue.head == NULL && osGetMutexType(mutex) == OS_MUTEX_TYPE_R )
    {
        osSetMutexType( mutex, OS_MUTEX_TYPE_W );
        locked = TRUE;
    }

    (void)osRestoreInterrupts(e);
    return locked;
}

/*---------------------------------------------------------------------------*
  Name:         osLockMutexFromWToR

  Description:  Demote mutexW lock to mutexR lock without unlock.
                Wait till success.

  Arguments:    mutex       pointer to mutex structure

  Returns:      None
 *---------------------------------------------------------------------------*/
void osLockMutexFromWToR(OSMutex *mutex)
{
    OSIntrMode e = osDisableInterrupts();
    OSThread *currentThread = osGetCurrentThread();

    while(1)
    {
        if ( osTryLockMutexFromWToR(mutex) )
        {
            break;
        }

        currentThread->mutex = mutex;
        osSleepThread(&mutex->queue);
        currentThread->mutex = NULL;
    }

    (void)osRestoreInterrupts(e);
}

/*---------------------------------------------------------------------------*
  Name:         osTryLockMutexFromWToR

  Description:  Try to demote mutexW lock to mutexR lock without unlock.

  Arguments:    mutex       pointer to mutex structure

  Returns:      TRUE if success
 *---------------------------------------------------------------------------*/
BOOL osTryLockMutexFromWToR(OSMutex *mutex)
{
    OSIntrMode e = osDisableInterrupts();
    BOOL locked = FALSE;

    //---- change mutex type if no other thread is locked. and mutex is W
    if ( osGetMutexCount(mutex) == 1 && mutex->queue.head == NULL && osGetMutexType(mutex) == OS_MUTEX_TYPE_W )
    {
        osSetMutexType( mutex, OS_MUTEX_TYPE_R );
        locked = TRUE;
    }

    (void)osRestoreInterrupts(e);
    return locked;
}


//===========================================================================
//     MUTEX QUEUE
//===========================================================================
/*---------------------------------------------------------------------------*
  Name:         i_osEnqueueTail

  Description:  internal function.
                add mutex to thread's mutex list

  Arguments:    thread      pointer to thread
                mutex       pointer to mutex to be add

  Returns:      None.
 *---------------------------------------------------------------------------*/
void i_osEnqueueTail(OSThread *thread, OSMutex *mutex)
{
#ifndef SDK_THREAD_INFINITY
    OSMutex *prev = thread->mutexQueueTail;

    SDK_ASSERT(thread && mutex);

    if (!prev)
    {
        thread->mutexQueueHead = mutex;
    }
    else
    {
        prev->next = mutex;
    }

    mutex->prev = prev;
    mutex->next = NULL;
    thread->mutexQueueTail = mutex;
#else
    OSMutex *prev = thread->mutexQueue.tail;

    SDK_ASSERT(thread && mutex);

    if (!prev)
    {
        thread->mutexQueue.head = mutex;
    }
    else
    {
        prev->link.next = mutex;
    }

    mutex->link.prev = prev;
    mutex->link.next = NULL;
    thread->mutexQueue.tail = mutex;
#endif
}

/*---------------------------------------------------------------------------*
  Name:         i_osDequeueItem

  Description:  internal function.
                remove specified mutex from thread's mutex list

  Arguments:    thread      pointer to thread
                mutex       pointer to mutex to be remove

  Returns:      None.
 *---------------------------------------------------------------------------*/
void i_osDequeueItem(OSThread *thread, OSMutex *mutex)
{
#ifndef SDK_THREAD_INFINITY
    OSMutex *next = mutex->next;
    OSMutex *prev = mutex->prev;

    SDK_ASSERT(thread && mutex);

    if (!next)
    {
        thread->mutexQueueTail = prev;
    }
    else
    {
        next->prev = prev;
    }

    if (!prev)
    {
        thread->mutexQueueHead = next;
    }
    else
    {
        prev->next = next;
    }
#else
    OSMutex *next = mutex->link.next;
    OSMutex *prev = mutex->link.prev;

    SDK_ASSERT(thread && mutex);

    if (!next)
    {
        thread->mutexQueue.tail = prev;
    }
    else
    {
        next->link.prev = prev;
    }

    if (!prev)
    {
        thread->mutexQueue.head = next;
    }
    else
    {
        prev->link.next = next;
    }
#endif
}

/*---------------------------------------------------------------------------*
  Name:         i_osDequeueHead

  Description:  remove top mutex from thread's list, and return mutex

  Arguments:    thread      pointer to thread

  Returns:      mutex which listed at top of thread
 *---------------------------------------------------------------------------*/
OSMutex *i_osDequeueHead(OSThread *thread)
{
#ifndef SDK_THREAD_INFINITY
    OSMutex *mutex = thread->mutexQueueHead;
    OSMutex *next = mutex->next;

    SDK_ASSERT(thread);

    if (!next)
    {
        thread->mutexQueueTail = NULL;
    }
    else
    {
        next->prev = NULL;
    }

    thread->mutexQueueHead = next;
#else
    OSMutex *mutex = thread->mutexQueue.head;
    OSMutex *next = mutex->link.next;

    SDK_ASSERT(thread);

    if (!next)
    {
        thread->mutexQueue.tail = NULL;
    }
    else
    {
        next->link.prev = NULL;
    }

    thread->mutexQueue.head = next;
#endif

    return mutex;
}