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

  Copyright 2008 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/os.h>

#define osPrintf(...) ((void)0)
#define osTPrintf(...) ((void)0)
#define i_osPanic(...) ((void)0)

//---------------------------------------------------------------------------

extern unsigned long SDK_SYS_STACKSIZE[];
extern unsigned long SDK_IRQ_STACKSIZE[];
#define OSi_SYS_STACKSIZE             ((s32)SDK_SYS_STACKSIZE)
#define OSi_IRQ_STACKSIZE             ((s32)SDK_IRQ_STACKSIZE)

//---- Stack CheckNumber
#ifdef SDK_ARM9
#  define  OSi_STACK_CHECKNUM_BOTTOM     0xfddb597dUL
#  define  OSi_STACK_CHECKNUM_TOP        0x7bf9dd5bUL
#  define  OSi_STACK_CHECKNUM_WARN       0x597dfbd9UL
#else // SDK_ARM11
#  define  OSi_STACK_CHECKNUM_BOTTOM     0xd73bfdf7UL
#  define  OSi_STACK_CHECKNUM_TOP        0xfbdd37bbUL
#  define  OSi_STACK_CHECKNUM_WARN       0xbdf7db3dUL
#endif

//---- defs for launcher thread stack
#ifdef SDK_ARM9
extern void *Load$$DTCM$$Base;
#  define  OSi_LAUNCHER_STACK_LO_DEFAULT    (Load$$DTCM$$Base)
#else // SDK_ARM11
#  define  OSi_LAUNCHER_STACK_LO_DEFAULT    HW_AXI_WRAM
#endif
#  define  OSi_LAUNCHER_STACK_HI_MAX        (HW_BROM_SVC_STACK - OSi_IRQ_STACKSIZE)
#  define  OSi_LAUNCHER_STACK_BOTTOM        (HW_BROM_SVC_STACK - OSi_IRQ_STACKSIZE)

//---- defs for idle thread
// for checkNumber and SVC stack (if defined)
#ifdef SDK_CONTEXT_HAS_SP_SVC
#define OSi_IDLE_CHECKNUM_SIZE  ( sizeof(u32)*2 + HW_BROM_SVC_STACK_SIZE )
#else
#define OSi_IDLE_CHECKNUM_SIZE  ( sizeof(u32)*2 )
#endif
// for SVC to stack out registers
#ifdef SDK_ARM9
#define OSi_IDLE_SVC_SIZE  ( sizeof(u32)*32 )   // arm9 svc stacks 14 words and makes in 8byte-alignment
#else // SDK_ARM11
#define OSi_IDLE_SVC_SIZE  ( sizeof(u32)*16 )   // arm7 svc stacks 14 words
#endif
// stack size of idle thread
#define OSi_IDLE_THREAD_STACK_SIZE    ( OSi_IDLE_CHECKNUM_SIZE + OSi_IDLE_SVC_SIZE )

#if defined(SDK_TCM_APPLY) && defined(SDK_ARM9)
#include    <brom/itcm_begin.h>
#endif
//---- threads
OSThread i_osLauncherThread;
OSThread i_osIdleThread;

//---- thread information
OSThreadInfo i_osThreadInfo;

//---- current thread pointer (for quick reference)
OSThread **i_osCurrentThreadPtr;
#define       i_osGetCurrentThread()    (*i_osCurrentThreadPtr)

//---- thread initialization flag
BOOL    i_osIsThreadInitialized = FALSE;

//---- idle thread stack
u32     i_osIdleThreadStack[OSi_IDLE_THREAD_STACK_SIZE / sizeof(u32)];

//---- system callback in switch thread ( maybe for profile )
void   *i_osSystemCallbackInSwitchThread = NULL;

//---- reschedule counter. if value>=0, do not reschedule.
u32     i_osRescheduleCount = 0;
#if defined(SDK_TCM_APPLY) && defined(SDK_ARM9)
#include    <brom/itcm_end.h>
#endif

#ifdef SDK_THREAD_INFINITY
//---- thread id count
static int i_osThreadIdCount = 0;
#endif

void   *i_osStackForDestructor = NULL;


#ifndef SDK_THREAD_INFINITY
static int i_osSearchFreeEntry(void);
#endif
static void i_osCancelThreadAlarmForSleep(OSThread *thread);
static void i_osInsertThreadToList(OSThread *thread);
static void i_osRemoveThreadFromList(OSThread *thread);
static void i_osSleepAlarmCallback(void *arg);
static void i_osIdleThreadProc(void *);
void    i_osSetSystemCallbackInSwitchThread(void *callback);

static void i_osExitThread_ArgSpecified(OSThread *thread, void *arg);
static void i_osExitThread(void *arg);
static void i_osExitThread_Destroy(void);

#if defined(SDK_TCM_APPLY) && defined(SDK_ARM9)
#include    <brom/itcm_begin.h>
#endif
static void i_osRescheduleThread(void);

#ifdef SDK_THREAD_INFINITY
/*---------------------------------------------------------------------------*
  Name:         i_osGetUnusedThreadId

  Description:  get unused thread id

  Arguments:    None

  Returns:      thread id (0-0x7fffffff) which is never used.
 *---------------------------------------------------------------------------*/
static int i_osGetUnusedThreadId(void)
{
    ++i_osThreadIdCount;
    SDK_ASSERT(i_osThreadIdCount > 0); // overflow check
    return i_osThreadIdCount;
}
#endif /* ; */

/*---------------------------------------------------------------------------*
  Name:         i_osInsertLinkToQueue

  Description:  insert thread to thread queue

  Arguments:    queue  : thread queue
  				thread : thread to insert

  Returns:      None
 *---------------------------------------------------------------------------*/
static void i_osInsertLinkToQueue(OSThreadQueue *queue, OSThread *thread)
{
    OSThread *next = queue->head;

    while (next && next->priority <= thread->priority)
    {
        if (next == thread)
        {
            return;
        }
        next = next->link.next;
    }

    if (!next)
    {
        OSThread *prev = queue->tail;

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

        thread->link.prev = prev;
        thread->link.next = NULL;
        queue->tail = thread;
    }
    else
    {
        OSThread *prev = next->link.prev;

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

        thread->link.prev = prev;
        thread->link.next = next;
        next->link.prev = thread;
    }
}

/*---------------------------------------------------------------------------*
  Name:         i_osRemoveLinkFromQueue

  Description:  remove head thread from thread queue

  Arguments:    queue  : thread queue

  Returns:      thread pointer which is removed
 *---------------------------------------------------------------------------*/
static OSThread *i_osRemoveLinkFromQueue(OSThreadQueue *queue)
{
    OSThread *t = queue->head;

    if (t)
    {
        OSThread *next = t->link.next;

        queue->head = next;

        if (next)
        {
            next->link.prev = NULL;
        }
        else
        {
            queue->tail = NULL;
            t->queue = NULL;
        }
    }

    return t;
}

/*---------------------------------------------------------------------------*
  Name:         i_osRemoveSpecifiedLinkFromQueue

  Description:  remove specified thread from thread queue

  Arguments:    queue  : thread queue

  Returns:      thread pointer which is removed
 *---------------------------------------------------------------------------*/
static OSThread *i_osRemoveSpecifiedLinkFromQueue(OSThreadQueue *queue, OSThread *thread)
{
    OSThread *t = queue->head;
    OSThread *next;
    OSThread *prev;

    while (t)
    {
        next = t->link.next;

        if (t == thread)
        {
            prev = t->link.prev;

            //---- whether if head link
            if (queue->head == t)
            {
                queue->head = next;
            }
            else
            {
                prev->link.next = next;
            }

            //---- whether if tail link
            if (queue->tail == t)
            {
                queue->tail = prev;
            }
            else
            {
                next->link.prev = prev;
            }

            break;
        }

        t = next;
    }

    return t;
}

/*---------------------------------------------------------------------------*
  Name:         i_osRemoveMutexLinkFromQueue

  Description:  remove mutex from mutex queue

  Arguments:    queue  : mutex queue

  Returns:      mutex pointer which is removed
 *---------------------------------------------------------------------------*/
OSMutex *i_osRemoveMutexLinkFromQueue(OSMutexQueue * queue)
{
    OSMutex *t = queue->head;

    if (t)
    {
        OSMutex *next = t->link.next;

        queue->head = next;

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

    return t;
}

/*---------------------------------------------------------------------------*
  Name:         i_osSetSystemCallbackInSwitchThread

  Description:  set system callback in switching thread

  Arguments:    callback

  Returns:      None
 *---------------------------------------------------------------------------*/
void i_osSetSystemCallbackInSwitchThread(void *callback)
{
    i_osSystemCallbackInSwitchThread = callback;
}

#ifndef SDK_THREAD_INFINITY
/*---------------------------------------------------------------------------*
  Name:         i_osSearchFreeEntry

  Description:  search free thread entry area

  Arguments:    None

  Returns:      0 - OS_THREAD_MAX_NUM-1  ... entry index
                -1                       ... not found
 *---------------------------------------------------------------------------*/
static int i_osSearchFreeEntry(void)
{
    int     i;

    for (i = 0; i < OS_THREAD_MAX_NUM; i++)
    {
        if (!i_osThreadInfo.entry[i])
        {
            return i;
        }
    }
    return -1;                         // not found
}
#endif

/*---------------------------------------------------------------------------*
  Name:         i_osInsertThreadToList

  Description:  insert thread to thread list which is arranged by priority

  Arguments:    pointer of thread

  Returns:      None
 *---------------------------------------------------------------------------*/
static void i_osInsertThreadToList(OSThread *thread)
{
    OSThread *t = i_osThreadInfo.list;
    OSThread *pre = NULL;

    while (t && t->priority < thread->priority)
    {
        pre = t;
        t = t->next;
    }

    if (!pre)
    {
        thread->next = i_osThreadInfo.list;
        i_osThreadInfo.list = thread;
    }
    else
    {
        thread->next = pre->next;
        pre->next = thread;
    }
}

/*---------------------------------------------------------------------------*
  Name:         i_osRemoveThreadFromList

  Description:  remove thread from thread list

  Arguments:    pointer of thread

  Returns:      None
 *---------------------------------------------------------------------------*/
static void i_osRemoveThreadFromList(OSThread *thread)
{
    OSThread *t = i_osThreadInfo.list;
    OSThread *pre = NULL;

    while (t && t != thread)
    {
        pre = t;
        t = t->next;
    }

    SDK_ASSERTMSG(t, "Cannot remove thread from list.");

    if (!pre)
    {
        i_osThreadInfo.list = thread->next;
    }
    else
    {
        pre->next = thread->next;
    }
}

/*---------------------------------------------------------------------------*
  Name:         i_osRescheduleThread

  Description:  Switch to the runnable thread highest priority
                without interrupts disabled.

  Arguments:    None

  Returns:      None or Never return
 *---------------------------------------------------------------------------*/
static void i_osRescheduleThread(void)
{
    //---- if scheduler is set to be disabled, do nothing.
    if (i_osRescheduleCount <= 0)
    {
        OSThreadInfo *info = &i_osThreadInfo;
        if (info->irqDepth > 0 || osGetProcMode() == OS_PROCMODE_IRQ)
        {
            // If in IRQ, do rescheduling at end of IRQ handler
            info->isNeedRescheduling = TRUE;
        }
        else
        {
            OSThread *currentThread, *nextThread;
            currentThread = i_osGetCurrentThread();
            nextThread = osSelectThread();

            if (currentThread == nextThread || !nextThread)     // maybe nextThread != NULL
            {
                return;                // Don't have to switch the current context
            }

            if (currentThread->state != OS_THREAD_STATE_TERMINATED
                && osSaveContext(&currentThread->context))
            {
                return;                // Return if go back via osLoadContext
            }

            //---- call thread switch callback for system
            if (i_osSystemCallbackInSwitchThread)
            {
                ((OSSwitchThreadCallback)i_osSystemCallbackInSwitchThread) (currentThread,
                                                                            nextThread);
            }

            //---- call thread switch callback for user
            if (info->switchCallback)
            {
                ((OSSwitchThreadCallback)info->switchCallback) (currentThread, nextThread);
            }

            osSetCurrentThread(nextThread);

            osLoadContext(&nextThread->context);
            // Never reached
        }
    }
}

/*---------------------------------------------------------------------------*
  Name:         osInitThread

  Description:  Initialize thread system

  Arguments:    None

  Returns:      None
 *---------------------------------------------------------------------------*/
void osInitThread(void)
{
    void   *stackLo;
#ifndef SDK_THREAD_INFINITY
    int     i;
#endif

    if (i_osIsThreadInitialized)
    {
        return;
    }
    i_osIsThreadInitialized = TRUE;

#ifndef SDK_THREAD_INFINITY
    //---- Set thread info
    for (i = 0; i < OS_THREAD_MAX_NUM; i++)
    {
        i_osThreadInfo.entry[i] = NULL;
    }
#endif

    //---- set pointer to current thread buffer
    //     (for quick reference)
    i_osCurrentThreadPtr = &(i_osThreadInfo.current);

    //---- Setup launcher thread
    i_osLauncherThread.priority = OS_THREAD_LAUNCHER_PRIORITY;
    i_osLauncherThread.id = 0;
    i_osLauncherThread.state = OS_THREAD_STATE_READY;
    i_osLauncherThread.next = NULL;

    //---- clear profile pointer
    i_osLauncherThread.profiler = NULL;

    //---- clear thread entry and listPtr
#ifndef SDK_THREAD_INFINITY
    i_osThreadInfo.entry[0] = &i_osLauncherThread;
#endif
    i_osThreadInfo.list = &i_osLauncherThread;

    //---- let launch thread be current
    osSetCurrentThread(&i_osLauncherThread);

    //---- StackLo
    stackLo = (OSi_SYS_STACKSIZE <= 0) ?
        (void *)((u32)OSi_LAUNCHER_STACK_LO_DEFAULT - OSi_SYS_STACKSIZE) :
        (void *)((u32)OSi_LAUNCHER_STACK_HI_MAX - OSi_SYS_STACKSIZE);
    SDK_ASSERT((u32)OSi_LAUNCHER_STACK_LO_DEFAULT <= (u32)stackLo
               && (u32)stackLo <= (u32)OSi_LAUNCHER_STACK_HI_MAX);

    //---- set Stack Bottom & Top
    i_osLauncherThread.stackBottom = (u32)OSi_LAUNCHER_STACK_BOTTOM;
    i_osLauncherThread.stackTop = (u32)stackLo;
    i_osLauncherThread.stackWarningOffset = 0;

    //---- Set Stack CheckNumber
    *(u32 *)(i_osLauncherThread.stackBottom - sizeof(u32)*2) = OSi_STACK_CHECKNUM_BOTTOM;
    *(u32 *)i_osLauncherThread.stackTop = OSi_STACK_CHECKNUM_TOP;

    //---- clear join queue
    osInitThreadQueue(&i_osLauncherThread.joinQueue);

#ifndef SDK_THREAD_INFINITY
    //---- max number of thread
    i_osThreadInfo.max_entry = OS_THREAD_MAX_NUM;
#endif

    //---- around IRQ
    i_osThreadInfo.isNeedRescheduling = FALSE;
    i_osThreadInfo.irqDepth = 0;
    SDK_ASSERTMSG(OSi_IRQ_STACKSIZE > 0, "IRQ STACKSIZE must be >0");

    //---- store thread info pointer
#if 0 // for ISD
#ifdef SDK_ARM9
    osGetSystemWork()->threadinfo_mainp = &i_osThreadInfo;
#else
    osGetSystemWork()->threadinfo_subp = &i_osThreadInfo;
#endif
#endif

    //---- set thread switch callback
    (void)osSetSwitchThreadCallback(NULL);

    //---- idle thread
    osCreateThread(&i_osIdleThread,
                    i_osIdleThreadProc,
                    (void *)NULL,
                    i_osIdleThreadStack + OSi_IDLE_THREAD_STACK_SIZE / sizeof(u32),
                    OSi_IDLE_THREAD_STACK_SIZE,
                    OS_THREAD_PRIORITY_MAX /*pseudo. change at next line. */ );
    i_osIdleThread.priority = OS_THREAD_PRIORITY_MAX + 1;       // lower priority than the lowest (=OS_THREAD_PRIORITY_MAX)
    i_osIdleThread.state = OS_THREAD_STATE_READY;
}

/*---------------------------------------------------------------------------*
  Name:         osIsThreadAvailable

  Description:  check if thread system is available

  Arguments:    None

  Returns:      TRUE if available, FALSE if not
 *---------------------------------------------------------------------------*/
#include <brom/code32.h>
asm BOOL osIsThreadAvailable( void )
{
    INASM_EXTERN( i_osIsThreadInitialized )

    ldr      r0, =i_osIsThreadInitialized
    ldr      r0, [r0, #0]
    bx       lr
}
#include <brom/codereset.h>

/*---------------------------------------------------------------------------*
  Name:         osCreateThread

  Description:  Creates a new thread

  Arguments:    thread      pointer of thread structure
                func        function to start thread
                arg         argument for func
                stack       stack bottom address
                stackSize   stack size (byte. must be aligned by 4)
                prio        thread priority

  Returns:      None
 *---------------------------------------------------------------------------*/
void osCreateThread(OSThread *thread,
                     void (*func) (void *), void *arg, void *stack, u32 stackSize, u32 prio)
{
#define STACK_ALIGN 4

    OSIntrMode enable;
    int     index;

    SDK_ASSERTMSG(i_osGetCurrentThread(), "thread system were not initialized");
    SDK_ASSERTMSG(OS_THREAD_PRIORITY_MIN <= prio
                  && prio <= OS_THREAD_PRIORITY_MAX, "invalid priority");
    SDK_ASSERTMSG(stackSize % STACK_ALIGN == 0, "stack size must be aligned by %d", STACK_ALIGN);
    SDK_ASSERTMSG((u32)stack % STACK_ALIGN == 0, "stack must be aligned by %d", STACK_ALIGN);

    enable = osDisableInterrupts();

#ifndef SDK_THREAD_INFINITY
    //---- search free entry
    if ((index = i_osSearchFreeEntry()) < 0)
    {
        SDK_ASSERTMSG(index >= 0, "osCreateThread: thread entry not allocated");
        (void)osRestoreInterrupts(enable);
        return;
    }
#else
    index = i_osGetUnusedThreadId();
#endif

    //---- setup thread
    thread->priority = prio;
    thread->id = (u32)index;
    thread->state = OS_THREAD_STATE_WAITING;

    //---- clear profile pointer
    thread->profiler = NULL;

    //---- set thread entry and listPtr
#ifndef SDK_THREAD_INFINITY
    i_osThreadInfo.entry[index] = thread;
#endif
    i_osInsertThreadToList(thread);

    //---- set Stack Bottom & Top
    thread->stackBottom = (u32)stack;
    thread->stackTop = (u32)stack - stackSize;
    thread->stackWarningOffset = 0;

    //---- Set Stack CheckNumber
    *(u32 *)(thread->stackBottom - sizeof(u32)*2) = OSi_STACK_CHECKNUM_BOTTOM; // minus for stack CheckNumber and padding
    *(u32 *)thread->stackTop = OSi_STACK_CHECKNUM_TOP;

    //---- clear join queue
    osInitThreadQueue(&thread->joinQueue);

    //---- Init context
    osInitContext(&thread->context, (u32)func, (u32)stack - sizeof(u32)*2); // minus for stack CheckNumber and padding

    thread->context.r[0] = (u32)arg;   // argument for func
    thread->context.lr = (u32)osExitThread;

    //---- clear Stack (except check code (=sizeof(u32)*2) and padding(sizeof(u32))
    miCpuClear32((void *)((u32)stack - stackSize + sizeof(u32)), stackSize - sizeof(u32) * 2 - sizeof(u32) );

    //---- clear mutex
    thread->mutex = NULL;
#ifndef SDK_THREAD_INFINITY
    thread->mutexQueueHead = NULL;
    thread->mutexQueueTail = NULL;
#else
    thread->mutexQueue.head = NULL;
    thread->mutexQueue.tail = NULL;
#endif

    //---- clear destructor
#ifdef SDK_THREAD_INFINITY
    osSetThreadDestructor(thread, NULL);
#endif

    //---- clear queue
#ifdef SDK_THREAD_INFINITY
    thread->queue = NULL;
    thread->link.prev = thread->link.next = NULL;

    //---- clear specific member
    miCpuClear32(&thread->specific[0], sizeof(void *) * OS_THREAD_SPECIFIC_MAX);
#endif

    //---- clear alarm pointer for sleep
    thread->alarmForSleep = NULL;

    (void)osRestoreInterrupts(enable);
}


/*---------------------------------------------------------------------------*
  Name:         osExitThread

  Description:  Terminates the current thread

  Arguments:    none

  Returns:      none
 *---------------------------------------------------------------------------*/
void osExitThread(void)
{
    (void)osDisableInterrupts();

#ifdef  SDK_THREAD_INFINITY
    i_osExitThread_ArgSpecified(osGetCurrentThread(), 0);
#else
    i_osExitThread_Destroy();
#endif
}


#ifdef  SDK_THREAD_INFINITY
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// デストラクタスタックを考慮する i_osExitThread
// 割込み禁止状態で呼び出すこと
static void i_osExitThread_ArgSpecified(OSThread *thread, void *arg)
{
    if (i_osStackForDestructor)
    {
        osInitContext(&thread->context, (u32)i_osExitThread, (u32)i_osStackForDestructor);
        thread->context.r[0] = (u32)arg;
        thread->context.cpsr |= HW_PSR_IRQ_DISABLE;
        thread->state = OS_THREAD_STATE_READY;
        osLoadContext(&thread->context);
        // Never Returns
    }
    else
    {
        i_osExitThread(arg);
        // Never Returns
    }
}
#endif


#ifdef	SDK_THREAD_INFINITY
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// osKillThread において参照されるコンテキストスイッチ先
// 割り込み禁止状態で呼び出すこと
static void i_osExitThread(void *arg)
{
    OSThread *currentThread = i_osGetCurrentThread();
    OSThreadDestructor destructor;

    SDK_ASSERT(currentThread);

    // デストラクタの処理
    destructor = currentThread->destructor;
    if (destructor)
    {
        currentThread->destructor = NULL;
        destructor(arg);
        (void)osDisableInterrupts();  // 再び割り込み禁止へ
    }

    i_osExitThread_Destroy();          // Never return
}
#endif

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// osDestroyThread からカレントスレッドを destroy するときに呼び出される
// 割り込み禁止状態で呼び出すこと
static void i_osExitThread_Destroy(void)
{
    OSThread *currentThread = i_osGetCurrentThread();
    SDK_ASSERT(currentThread);

#ifdef SDK_THREAD_INFINITY
    (void)osDisableScheduler();
#endif

#ifndef SDK_THREAD_INFINITY
    //---- current thread check
    SDK_ASSERT(i_osThreadInfo.entry[currentThread->id] == currentThread);
#endif

    //---- Release all the locked mutexes by current thread
    i_osUnlockAllMutex(currentThread);


    //---- remove from thread queue list
    if (currentThread->queue)
    {
        (void)i_osRemoveSpecifiedLinkFromQueue(currentThread->queue, currentThread);
    }

    //---- remove from thread list
    i_osRemoveThreadFromList(currentThread);

    //---- delete this thread
#ifndef SDK_THREAD_INFINITY
    i_osThreadInfo.entry[currentThread->id] = NULL;
#endif
    currentThread->state = OS_THREAD_STATE_TERMINATED;

    //---- wakeup thread waiting currentThread terminated
#ifndef SDK_THREAD_INFINITY
    osWakeupThread(&currentThread->joinQueue); // possible to never return
#else
    osWakeupThread(&currentThread->joinQueue);
#endif

#ifdef SDK_THREAD_INFINITY
    (void)osEnableScheduler();
#endif

    osRescheduleThread();             // never return

    osTerminate();
}

// TEST ---------------------------------------------------------------------
#ifdef NITRO_UTEST_H__
static vu32 exitThreadStatus = 0;

static void exitThreadFunc(void *arg)
{
    exitThreadStatus = 1;
    (void)arg;
}

void    UTEST_osExitThread(void);
void UTEST_osExitThread(void)
{
    OSThread thread;
    u32     stack[1024];

    osInit();
    osInitThread();

    osCreateThread(&thread,
                    exitThreadFunc,
                    NULL,
                    stack + 1024, sizeof(stack), osGetThreadPriority(osGetCurrentThread()) - 1);
    osWakeupThreadDirect(&thread);
    osJoinThread(&thread);
    UT_AssertEq(exitThreadStatus, 1);
}
#endif  // ifdef NITRO_UTEST_H__

/*---------------------------------------------------------------------------*
  Name:         osDestroyThread

  Description:  destroy specified thread.

  Arguments:    thread: thread to be destroyed

  Returns:      None
 *---------------------------------------------------------------------------*/
void osDestroyThread(OSThread *thread)
{
    OSIntrMode enabled = osDisableInterrupts();

    SDK_ASSERT(thread);

    //---- check current thread
    if (osGetCurrentThread() == thread)
    {
        i_osExitThread_Destroy();
        // not reached
    }

#ifdef SDK_THREAD_INFINITY
    (void)osDisableScheduler();
#endif

    //---- Release all the locked mutexed by specified thread
    i_osUnlockAllMutex(thread);

    //---- cancel alarm for sleep
    i_osCancelThreadAlarmForSleep(thread);

    //---- remove from thread queue list
    if (thread->queue)
    {
        (void)i_osRemoveSpecifiedLinkFromQueue(thread->queue, thread);
    }

    //---- remove from thread list
    i_osRemoveThreadFromList(thread);

    //---- delete thread
#ifndef SDK_THREAD_INFINITY
    i_osThreadInfo.entry[thread->id] = NULL;
#endif
    thread->state = OS_THREAD_STATE_TERMINATED;

    //---- wakeup thread waiting this thread terminated
    osWakeupThread(&thread->joinQueue);

#ifdef SDK_THREAD_INFINITY
    (void)osEnableScheduler();
#endif
    (void)osRestoreInterrupts(enabled);

    osRescheduleThread();
}

/*---------------------------------------------------------------------------*
  Name:         osKillThread

  Description:  switch PC to thread destructor to finalize thread

  Arguments:    thread : thread to wait to finish
                flag   : argument for destructor

  Returns:      None
 *---------------------------------------------------------------------------*/
#ifdef SDK_THREAD_INFINITY
// TESTEE
static inline void i_osKillThreadWithPriority(OSThread *thread, void *arg, u32 prio);
static inline void i_osKillThreadWithPriority(OSThread *thread, void *arg, u32 prio)
{
    SDK_ASSERT(thread);

    {
        u32     stack = (i_osStackForDestructor) ? (u32)i_osStackForDestructor : thread->stackBottom - sizeof(u32)*2; // minus for stack CheckNumber and padding

        osInitContext(&thread->context, (u32)i_osExitThread, stack);
    }
    thread->context.r[0] = (u32)arg;
    thread->context.cpsr |= HW_PSR_IRQ_DISABLE; // デストラクタ内では IRQ 割り込みを禁止する

    thread->state = OS_THREAD_STATE_READY;

    (void)osDisableScheduler();
    (void)osSetThreadPriority(thread, prio);
    (void)osEnableScheduler();
}

// BODY
void osKillThread(OSThread *thread, void *arg)
{
    osKillThreadWithPriority(thread, arg, osGetThreadPriority(thread));
}

void osKillThreadWithPriority(OSThread *thread, void *arg, u32 prio)
{
    OSIntrMode enabled = osDisableInterrupts();

    if (thread == osGetCurrentThread())
    {
        i_osExitThread_ArgSpecified(thread, arg);
        // Never returns
    }

    //---- cancel alarm for sleep
    i_osCancelThreadAlarmForSleep(thread);

    i_osKillThreadWithPriority(thread, arg, prio);

    i_osRescheduleThread();
    (void)osRestoreInterrupts(enabled);
}

// TEST ---------------------------------------------------------------------
#ifdef NITRO_UTEST_H__
// test1
void    UTEST_osKillThread_1(void);
void UTEST_osKillThread_1(void)
{
    OSThread thread;
    OSThread *t = &thread;
    u32     flag;

    osInit();
    osInitThread();

    t->stackBottom = 0x6789abcd;
    t->state = OS_THREAD_STATE_TERMINATED;
    i_osKillThreadWithPriority(t, (void *)0x12345678, 16);

    // コンテクスト内の主要レジスタチェック
    UT_AssertEq(t->context.pc_plus4, (u32)i_osExitThread + 4);  // osExitThread
    UT_AssertEq(t->context.r[0], (u32)0x12345678);      // arg
    flag = ((u32)osExitThread & 1) ? (u32)HW_PSR_THUMB_STATE : (u32)HW_PSR_ARM_STATE;
    UT_AssertEq(t->context.cpsr, (u32)HW_PSR_IRQ_DISABLE | HW_PSR_SYS_MODE | flag);
    UT_AssertEq(t->context.sp, (u32)0x6789abcd - HW_SVC_STACK_SIZE);

    // ステートチェック
    UT_AssertEq(t->state, OS_THREAD_STATE_READY);
    UT_AssertAsserted(i_osKillThreadWithPriority(0, 0, 16));
}

// test2
static vu32 killThreadStatus = 0;

static void killThreadDtor(void *arg)
{
    killThreadStatus = 666;
    (void)arg;
}

static void killThreadFunc(void *arg)
{
    osSetThreadDestructor(osGetCurrentThread(), killThreadDtor);

    killThreadStatus = 1;
    while (1)
    {
        osSleepThread(NULL);
        killThreadStatus++;
    }
    (void)arg;
}

void    UTEST_osKillThread_2(void);
void UTEST_osKillThread_2(void)
{
    OSThread thread;
    u32     stack[1024];

    osInit();
    osInitThread();

    osCreateThread(&thread,
                    killThreadFunc,
                    NULL,
                    stack + 1024, sizeof(stack), osGetThreadPriority(osGetCurrentThread()) - 1);
    UT_AssertEq(killThreadStatus, 0);

    osWakeupThreadDirect(&thread);
    UT_AssertEq(killThreadStatus, 1);
    UT_AssertEq(thread.destructor, killThreadDtor);

    osWakeupThreadDirect(&thread);
    UT_AssertEq(killThreadStatus, 2);

    osKillThread(&thread, 0);
    osJoinThread(&thread);
    UT_AssertEq(killThreadStatus, 666);
}
#endif  // ifdef NITRO_UTEST_H__
#endif  // ifdef SDK_THREAD_INFINITY

/*---------------------------------------------------------------------------*
  Name:         i_osCancelThreadAlarmForSleep

  Description:  Cancel alarm used to sleep thread.
                If sleeping alarm is not set, do nothing.

  Arguments:    thread : thread to calcel alarm

  Returns:      None
 *---------------------------------------------------------------------------*/
static void i_osCancelThreadAlarmForSleep(OSThread *thread)
{
    OSAlarm *alarm = thread->alarmForSleep;

    if (alarm)
    {
        osCancelAlarm(alarm);
    }
}

/*---------------------------------------------------------------------------*
  Name:         osJoinThread

  Description:  wait for specified thread to terminated

  Arguments:    thread : thread to wait to finish

  Returns:      None
 *---------------------------------------------------------------------------*/
void osJoinThread(OSThread *thread)
{
    OSIntrMode enabled = osDisableInterrupts();

    SDK_ASSERT(thread);

    //---- skip if thread is terminated already
    while(thread->state != OS_THREAD_STATE_TERMINATED)
    {
        osSleepThread(&thread->joinQueue);
    }

    (void)osRestoreInterrupts(enabled);
}

/*---------------------------------------------------------------------------*
  Name:         osIsThreadTerminated

  Description:  check thread status whether it's terminated

  Arguments:    thread : pointer to thread to be examined

  Returns:      TRUE if the thread is terminated. FALSE if not
 *---------------------------------------------------------------------------*/
BOOL osIsThreadTerminated(const OSThread *thread)
{
	SDK_ASSERT(thread);
    return (thread->state == OS_THREAD_STATE_TERMINATED) ? TRUE : FALSE;
}

/*---------------------------------------------------------------------------*
  Name:         osGetThreadStatus

  Description:  get thread status

  Arguments:    thread : pointer to thread

  Returns:      
 *---------------------------------------------------------------------------*/
OSThreadState  osGetThreadStatus(const OSThread *thread)
{
	SDK_ASSERT(thread);
	return thread->state;
}

/*---------------------------------------------------------------------------*
  Name:         osSleepThreadDirect

  Description:  Gets the thread into sleep status directly

  Arguments:    thread            thread to sleep
                queue             waiting list queue (or NULL)

  Returns:      none
 *---------------------------------------------------------------------------*/
void osSleepThreadDirect(OSThread *thread, OSThreadQueue *queue)
{
    SDK_ASSERT(thread);
    SDK_ASSERT(thread->state != OS_THREAD_STATE_TERMINATED);

	{
        OSIntrMode bak_intr = osDisableInterrupts();

		if ( thread->state == OS_THREAD_STATE_READY )
		{
			if (queue)
			{
#ifndef SDK_THREAD_INFINITY
				*queue |= (OSThreadQueue)(1UL << thread->id);
#else
				thread->queue = queue;
				i_osInsertLinkToQueue(queue, thread);
#endif
			}
			thread->state = OS_THREAD_STATE_WAITING;
			i_osRescheduleThread();
		}
        (void)osRestoreInterrupts(bak_intr);
    }
}

/*---------------------------------------------------------------------------*
  Name:         osSleepThread

  Description:  Gets the current thread into sleep status

  Arguments:    waiting list queue

  Returns:      none
 *---------------------------------------------------------------------------*/
void osSleepThread(OSThreadQueue *queue)
{
    OSIntrMode enable;
    OSThread *currentThread;

    enable = osDisableInterrupts();
#ifndef SDK_THREAD_INFINITY
    {
        currentThread = i_osGetCurrentThread();
        SDK_ASSERT(currentThread);

        if (queue)
        {
            *queue |= (OSThreadQueue)(1UL << currentThread->id);
        }

        currentThread->state = OS_THREAD_STATE_WAITING;
        i_osRescheduleThread();
    }
#else
    {
        currentThread = i_osGetCurrentThread();
        SDK_ASSERT(currentThread);

        if (queue)
        {
            currentThread->queue = queue;
            i_osInsertLinkToQueue(queue, currentThread);
        }

        currentThread->state = OS_THREAD_STATE_WAITING;
        i_osRescheduleThread();
    }
#endif
    (void)osRestoreInterrupts(enable);
}

/*---------------------------------------------------------------------------*
  Name:         osWakeupThread

  Description:  Gets the threads out of sleep status

  Arguments:    none

  Returns:      none
 *---------------------------------------------------------------------------*/
void osWakeupThread(OSThreadQueue *queue)
{
    OSIntrMode enable;
#ifndef SDK_THREAD_INFINITY
    u32     mask;
#else
    BOOL    isNeedRescheduling = FALSE;
#endif
    SDK_ASSERT(queue);
    enable = osDisableInterrupts();

#ifndef SDK_THREAD_INFINITY
    mask = (u32)*queue;
    if (mask)
    {
        //---- wakeup threads
        OSThread *t = i_osThreadInfo.list;
        while (t)
        {
            if (mask & (1UL << t->id))
            {
                t->state = OS_THREAD_STATE_READY;
            }

            t = t->next;
        }

        osInitThreadQueue(queue);
        i_osRescheduleThread();
    }
#else
    if (queue->head)
    {
        while (queue->head)
        {
            OSThread *thread = i_osRemoveLinkFromQueue(queue);

            thread->state = OS_THREAD_STATE_READY;
            thread->queue = NULL;
            thread->link.prev = thread->link.next = NULL;
        }

        osInitThreadQueue(queue);
        i_osRescheduleThread();
    }
#endif

    (void)osRestoreInterrupts(enable);
}


/*---------------------------------------------------------------------------*
  Name:         osWakeupThreadDirect

  Description:  Gets the threads out of sleep status directly

  Arguments:    none

  Returns:      none
 *---------------------------------------------------------------------------*/
void osWakeupThreadDirect(OSThread *thread)
{
    OSIntrMode enable;

    SDK_ASSERT(thread);
    SDK_ASSERT(thread->state != OS_THREAD_STATE_TERMINATED);
    enable = osDisableInterrupts();
    {
        thread->state = OS_THREAD_STATE_READY;
        i_osRescheduleThread();

    }
    (void)osRestoreInterrupts(enable);
}


/*---------------------------------------------------------------------------*
  Name:         osSelectThread

  Description:  Select the runnable thread highest priority

  Arguments:    None

  Returns:      Pointer to thread must run
                NULL if no thread candidate to run
 *---------------------------------------------------------------------------*/
OSThread *osSelectThread(void)
{
    OSThread *t = i_osThreadInfo.list;

    while (t && !osIsThreadRunnable(t))
    {
        t = t->next;
    }

    return t;
}

/*---------------------------------------------------------------------------*
  Name:         osRescheduleThread

  Description:  Switch to the runnable thread highest priority

  Arguments:    None

  Returns:      None or Never return
 *---------------------------------------------------------------------------*/
void osRescheduleThread(void)
{
    OSIntrMode bak_intr = osDisableInterrupts();
    i_osRescheduleThread();
    (void)osRestoreInterrupts(bak_intr);
}

/*---------------------------------------------------------------------------*
  Name:         osYieldThread

  Description:  do thread rescheduling. current thread relinquish CPU
                to give chance of running to other threads which has same
                priority.

  Arguments:    None

  Returns:      None
 *---------------------------------------------------------------------------*/
void osYieldThread(void)
{
    OSThread *current = osGetCurrentThread();
    OSThread *pre = NULL;
    OSThread *lastThread = NULL;
    int     samePriorityThread = 0;
    OSIntrMode enable = osDisableInterrupts();

    {
        OSThread *t = i_osThreadInfo.list;
        OSThread *tPre = NULL;

        while (t)
        {
            if (t == current)
            {
                pre = tPre;
            }

            if (current->priority == t->priority)
            {
                lastThread = t;
                samePriorityThread++;
            }

            tPre = t;
            t = t->next;
        }
    }

    //---- no thread of same priority with current  or  needless to arrange list
    if (samePriorityThread <= 1 || lastThread == current)
    {
        (void)osRestoreInterrupts(enable);
        return;
    }

    //---- remove thread from list
    if (!pre)
    {
        i_osThreadInfo.list = current->next;
    }
    else
    {
        pre->next = current->next;
    }

    //---- insert thread after 'lastThread'
    current->next = lastThread->next;
    lastThread->next = current;

    //---- re-schedule
    i_osRescheduleThread();

    (void)osRestoreInterrupts(enable);
}


/*---------------------------------------------------------------------------*
  Name:         osDumpThreadList

  Description:  Dump thread list

  Arguments:    None

  Returns:      None
 *---------------------------------------------------------------------------*/
void osDumpThreadList(void)
{
#ifndef SDK_FINALROM
#ifndef SDK_THREAD_INFINITY
    int     i;
#endif

    osPrintf("thread list top %08x\n", i_osThreadInfo.list);

#ifndef SDK_THREAD_INFINITY
    osPrintf("No:  address  prio     next\n");
    for (i = 0; i < OS_THREAD_MAX_NUM; i++)
    {
        OSThread *thread = i_osThreadInfo.entry[i];
        osPrintf("%02d: %08x %5d %08x\n", i, thread, (thread) ? thread->priority : 0,
                  (thread) ? thread->next : 0);
    }
#else
    //    osPrintf("Id:  address  prio     next\n");
    osPrintf("Id:  address  prio     next st  queue.h  queue.t   link.p   link.n\n");
    {
        OSThread *thread = i_osThreadInfo.list;
        while (thread)
        {
            //                  osPrintf("%02d: %08x %5d %08x\n", thread->id, thread, thread->priority, thread->next );
            osPrintf("%02d: %08x %5d %08x  %d %8x %8x %8x %8x\n", thread->id, thread,
                      thread->priority, thread->next, thread->state,
                      (thread->queue) ? thread->queue->head : (OSThread *)1,
                      (thread->queue) ? thread->queue->tail : (OSThread *)1, thread->link.prev,
                      thread->link.next);
            thread = thread->next;

        }
    }
#endif
#endif
}

/*---------------------------------------------------------------------------*
  Name:         osGetNumberOfThread

  Description:  get number of thread whch exists in system

  Arguments:    None

  Returns:      number of thread which exists in system
 *---------------------------------------------------------------------------*/
int osGetNumberOfThread(void)
{
    OSIntrMode enabled = osDisableInterrupts();
    int     threads = 0;

#ifndef SDK_THREAD_INFINITY
    int     i;

    for (i = 0; i < OS_THREAD_MAX_NUM; i++)
    {
        if (i_osThreadInfo.entry[i])
        {
            threads++;
        }
    }
#else
    OSThread *thread = i_osThreadInfo.list;
    while (thread)
    {
        threads++;
        thread = thread->next;
    }
#endif

    (void)osRestoreInterrupts(enabled);
    return threads;
}

/*---------------------------------------------------------------------------*
  Name:         osGetStackStatus

  Description:  check thread stack. check each CheckNUM.
                return result.

  Arguments:    thread   thread checked

  Returns:      0 (OS_STACK_NO_ERROR)        no error
                OS_STACK_OVERFLOW            overflow
                OS_STACK_ABOUT_TO_OVERFLOW   about to overflow
                OS_STACK_UNDERFLOW           underflow
 *---------------------------------------------------------------------------*/
OSStackStatus osGetStackStatus(const OSThread *thread)
{
    //---- Check if overflow
    if (*(u32 *)(thread->stackTop) != OSi_STACK_CHECKNUM_TOP)
    {
        return OS_STACK_OVERFLOW;
    }
    //---- Check if about to overflow
    else if (thread->stackWarningOffset
             && *(u32 *)(thread->stackTop + thread->stackWarningOffset) != OSi_STACK_CHECKNUM_WARN)
    {
        return OS_STACK_ABOUT_TO_OVERFLOW;
    }
    //---- Check if underFlow
    else if (*(u32 *)(thread->stackBottom - sizeof(u32)*2) != OSi_STACK_CHECKNUM_BOTTOM)
    {
        return OS_STACK_UNDERFLOW;
    }
    //---- No Error, return.
    else
    {
        return OS_STACK_NO_ERROR;
    }
}

/*---------------------------------------------------------------------------*
  Name:         i_osCheckStack

  Description:  check thread stack. check each CheckNUM.
                if changed, display warning and halt.

  Arguments:    file     file name displayed when stack overflow
                line     line number displayed when stack overflow
                thread   thread checked

  Returns:      None
                ( if error occurred, never return )
 *---------------------------------------------------------------------------*/
static char *i_osCheckStack_mesg[] = {
    "overflow", "about to overflow", "underflow"
};

#ifndef SDK_FINALROM
#ifndef SDK_NO_MESSAGE
void i_osCheckStack(const char *file, int line, const OSThread *thread)
{
    OSStackStatus st = osGetStackStatus(thread);

    if (st == OS_STACK_NO_ERROR)
    {
        return;
    }

    i_osPanic(file, line, " stack %x(id:%d) %s.\nstack area: %08x-%08x, warning offset: %x",
              thread,
              thread->id,
              i_osCheckStack_mesg[(int)st - 1],
              thread->stackTop, thread->stackBottom, thread->stackWarningOffset);
    // Never return
}
#endif
#endif

/*---------------------------------------------------------------------------*
  Name:         i_osGetSystemStackPointer

  Description:  Get system mode stack pointer at svc/irq mode

  Arguments:    None

  Returns:      Stack Pointer
 *---------------------------------------------------------------------------*/
static u32 i_osSystemStackBuffer;

#include  <brom/code32.h>
asm u32 i_osGetSystemStackPointer( void )
{
    INASM_EXTERN( i_osSystemStackBuffer )

    ldr   r0, =i_osSystemStackBuffer
    stmia r0, { sp }^
    ldr   r0, [ r0 ]
    bx    lr
}
#include  <brom/codereset.h>

/*---------------------------------------------------------------------------*
  Name:         i_osGetCurrentStackPointer

  Description:  Get current mode stack pointer

  Arguments:    None

  Returns:      Stack Pointer
 *---------------------------------------------------------------------------*/
asm u32 i_osGetCurrentStackPointer( void )
{
    mov   r0, sp
    bx    lr
}

/*---------------------------------------------------------------------------*
  Name:         osSetThreadStackWarningOffset

  Description:  Set warning level for stack checker

  Arguments:    thread     thread to set
                offset     offset from stack top. must be multiple of 4

  Returns:      None
 *---------------------------------------------------------------------------*/
void osSetThreadStackWarningOffset(OSThread *thread, u32 offset)
{
    SDK_ASSERTMSG((offset & 3) == 0, "Offset must be aligned by 4");
    SDK_ASSERTMSG(osGetThreadContext(thread)->sp > thread->stackTop + offset,
                  "Cannot set warning level below current sp.");

    //---- remember warning offset
    thread->stackWarningOffset = offset;

    //---- set Stack CheckNum
    if (offset != 0)
    {
        *(u32 *)(thread->stackTop + offset) = OSi_STACK_CHECKNUM_WARN;
    }
}

/*---------------------------------------------------------------------------*
  Name:         osSetThreadPriority

  Description:  change priority of thread

  Arguments:    thread     thread to set priority
                prio       new priority to be set

  Returns:      TRUE if success
 *---------------------------------------------------------------------------*/
BOOL osSetThreadPriority(OSThread *thread, u32 prio)
{
    OSThread *t = i_osThreadInfo.list;
    OSThread *pre = NULL;
    OSIntrMode enable;

    SDK_ASSERTMSG(OS_THREAD_PRIORITY_MIN <= prio
                  && prio <= OS_THREAD_PRIORITY_MAX, "invalid priority");
    SDK_ASSERTMSG(thread != &i_osIdleThread, "cannot change idle thread priority.");

    enable = osDisableInterrupts();

    while (t && t != thread)
    {
        pre = t;
        t = t->next;
    }

    //---- thread not found or thread is idle
    if (!t || t == &i_osIdleThread)
    {
        (void)osRestoreInterrupts(enable);
        return FALSE;
    }

    if (t->priority != prio)
    {
        //---- remove thread from list
        if (!pre)
        {
            i_osThreadInfo.list = thread->next;
        }
        else
        {
            pre->next = thread->next;
        }

        //---- set priority and insert proper position
        thread->priority = prio;
        i_osInsertThreadToList(thread);

        //---- re-schedule
        i_osRescheduleThread();
    }

    (void)osRestoreInterrupts(enable);

    return TRUE;
}

/*---------------------------------------------------------------------------*
  Name:         osGetThreadPriority

  Description:  get priority of thread

  Arguments:    thread     thread to get priority

  Returns:      priority
 *---------------------------------------------------------------------------*/
u32 osGetThreadPriority(const OSThread *thread)
{
    SDK_ASSERTMSG(thread, "osGetThreadPriority: bad thread");

    return thread->priority;
}


/*---------------------------------------------------------------------------*
  Name:         osSleep

  Description:  sleep specified period

  Arguments:    msec      sleeping period. ( milliseconds )

  Returns:      None.
 *---------------------------------------------------------------------------*/
void osSleep(u32 msec)
{
    OSAlarm alarm;

    SDK_ASSERTMSG(osIsTickAvailable()
                  && osIsAlarmAvailable(), "osSleep: need to start Tick and Alarm beforehand.");
    SDK_ASSERTMSG(i_osIsThreadInitialized, "osSleep: thread system not initialized.");

    osCreateAlarm(&alarm);
    {
        OSThread *volatile p_thread = i_osGetCurrentThread();
        OSIntrMode bak_cpsr = osDisableInterrupts();

        // ---- remember alarm
        p_thread->alarmForSleep = &alarm;

        osSetAlarm(&alarm, OS_MSEC_TO_TICK(msec), &i_osSleepAlarmCallback,
                    (void *)&p_thread);
        while (p_thread != NULL)
        {
            osSleepThread(NULL);
        }
        (void)osRestoreInterrupts(bak_cpsr);
    }
}

//---------------- callback to wakeup sleeping thread
static void i_osSleepAlarmCallback(void *arg)
{
    OSThread **pp_thread = (OSThread **)arg;
    OSThread *p_thread = *pp_thread;
    *pp_thread = NULL;

    //---- clear remembrance of alarm
    p_thread->alarmForSleep = NULL;

    osWakeupThreadDirect(p_thread);
}


/*---------------------------------------------------------------------------*
  Name:         osSetSwitchThreadCallback

  Description:  set callback called at switching thread

  Arguments:    callback      callback function

  Returns:      previous callback function before set callback now
 *---------------------------------------------------------------------------*/
OSSwitchThreadCallback osSetSwitchThreadCallback(OSSwitchThreadCallback callback)
{
    OSSwitchThreadCallback prev;
    OSIntrMode enabled;

    enabled = osDisableInterrupts();
    prev = (OSSwitchThreadCallback)i_osThreadInfo.switchCallback;
    i_osThreadInfo.switchCallback = (void*)callback;

    (void)osRestoreInterrupts(enabled);
    return prev;
}

/*---------------------------------------------------------------------------*
  Name:         i_osIdleThreadProc

  Description:  procedure of idle thread which system creates

  Arguments:    None

  Returns:      None (never return)
 *---------------------------------------------------------------------------*/
static void i_osIdleThreadProc(void *arg)
{
    (void)osEnableInterrupts();
    while (1)
    {
        osHalt();
    }
    // never return
}

/*---------------------------------------------------------------------------*
  Name:         i_osGetIdleThread

  Description:  get pointer to idle thread structure

  Arguments:    None

  Returns:      pointer to idle thread structure
 *---------------------------------------------------------------------------*/
OSThread *i_osGetIdleThread(void)
{
    OSThread *t = NULL;
    if (i_osIsThreadInitialized)
    {
        t = &i_osIdleThread;
    }
    return t;
}

/*---------------------------------------------------------------------------*
  Name:         osDisableScheduler

  Description:  disable scheduler

  Arguments:    None

  Returns:      Previous scheduler suspend count.
                Suspended if value >= 0.
 *---------------------------------------------------------------------------*/
u32 osDisableScheduler(void)
{
    OSIntrMode enabled = osDisableInterrupts();
    u32     count = 0;

    if (i_osRescheduleCount < (u32)(0 - 1) /* u32 max value -1 */ )
    {
        count = i_osRescheduleCount++;
    }
    (void)osRestoreInterrupts(enabled);

    return count;
}

/*---------------------------------------------------------------------------*
  Name:         osEnableScheduler

  Description:  enable scheduler

  Arguments:    None

  Returns:      Previous scheduler suspend count.
                Suspended if value >= 0.
 *---------------------------------------------------------------------------*/
u32 osEnableScheduler(void)
{
    OSIntrMode enabled = osDisableInterrupts();
    u32     count = 0;

    if (i_osRescheduleCount > 0)
    {
        count = i_osRescheduleCount--;
    }
    (void)osRestoreInterrupts(enabled);

    return count;
}

#ifdef SDK_THREAD_INFINITY
/*---------------------------------------------------------------------------*
  Name:         osGetThread

  Description:  Gets pointer to thread which id is specified

  Arguments:    id : thread id to get thread

  Returns:      pointer to thread which id is specified
 *---------------------------------------------------------------------------*/
OSThread *osGetThread(u32 id)
{
    OSThread *retval = NULL;
    OSThread *t = i_osThreadInfo.list;

    while (t)
    {
        if (t->id == id)
        {
            retval = t;
            break;
        }

        t = t->next;
    }

    return retval;
}
#endif


#ifdef SDK_THREAD_INFINITY
/*---------------------------------------------------------------------------*
  Name:         osSetThreadDestructor

  Description:  set thread destructor, which is called when that thread exits.

  Arguments:    thread : thread pointer
  				dtor   : destructor function

  Returns:      None
 *---------------------------------------------------------------------------*/
void osSetThreadDestructor(OSThread *thread, OSThreadDestructor dtor)
{
    SDK_ASSERT(thread);
    thread->destructor = dtor;
}

/*---------------------------------------------------------------------------*
  Name:         osGetThreadDestructor

  Description:  get thread destructor which is set

  Arguments:    thread : thread pointer

  Returns:      destructor function
 *---------------------------------------------------------------------------*/
OSThreadDestructor osGetThreadDestructor(const OSThread *thread)
{
    SDK_ASSERT(thread);
    return thread->destructor;
}

/*---------------------------------------------------------------------------*
  Name:         osSetThreadParameter

  Description:  set user parameter which is allowed to use freely.

  Arguments:    thread : thread pointer
				parameter : user parameter

  Returns:      None
 *---------------------------------------------------------------------------*/
void osSetThreadParameter(OSThread *thread, void *parameter)
{
    SDK_ASSERT(thread);
    thread->userParameter = parameter;
}

/*---------------------------------------------------------------------------*
  Name:         osGetThreadParameter

  Description:  get user parameter which is set

  Arguments:    thread : thread pointer

  Returns:      user parameter which is set
 *---------------------------------------------------------------------------*/
void   *osGetThreadParameter(const OSThread *thread)
{
    SDK_ASSERT(thread);
    return thread->userParameter;
}



/*---------------------------------------------------------------------------*
  Name:         i_osSetSystemErrno

  Description:  set system error number.

  Arguments:    thread : thread to set error number
  				errno  : error number to set

  Returns:      None
 *---------------------------------------------------------------------------*/
void i_osSetSystemErrno(OSThread *thread, int errno)
{
    SDK_ASSERT(thread);
    thread->systemErrno = errno;
}

/*---------------------------------------------------------------------------*
  Name:         i_osGetSystemErrno

  Description:  get system error number.

  Arguments:    thread : thread to set error number

  Returns:      error number
 *---------------------------------------------------------------------------*/
int i_osGetSystemErrno(const OSThread *thread)
{
    SDK_ASSERT(thread);
    return thread->systemErrno;
}

/*---------------------------------------------------------------------------*
  Name:         osGetErrno

  Description:  get system error number.

  Arguments:    None.

  Returns:      error number
 *---------------------------------------------------------------------------*/
int osGetErrno(void)
{
    OSThread *thread = i_osGetCurrentThread();
    return i_osGetSystemErrno(thread);
}
#endif

/*---------------------------------------------------------------------------*
  Name:         osIsThreadInList

  Description:  check if the specified thread is in the thread list

  Arguments:    thread : thread

  Returns:      TRUE if thread is in the thread list
 *---------------------------------------------------------------------------*/
BOOL osIsThreadInList(const OSThread *thread)
{
    BOOL    r = FALSE;
    OSThread *t = i_osThreadInfo.list;
    OSIntrMode enabled = osDisableInterrupts();

    while (t)
    {
        if (t == thread)
        {
            r = TRUE;
            break;
        }

        t = t->next;
    }

    (void)osRestoreInterrupts(enabled);
    return r;
}

/*---------------------------------------------------------------------------*
  Name:         osSetThreadDestructorStack

  Description:  specify stack area to call thread destructor

  Arguments:    stack       stack bottom address
                stackSize   stack size (byte. must be aligned by 4)

  Returns:      None
 *---------------------------------------------------------------------------*/
void osSetThreadDestructorStack(void *stack)
{
    SDK_ASSERT(stack);
    SDK_ASSERT((u32)stack % STACK_ALIGN == 0);

    i_osStackForDestructor = stack;
}

//================================================================================
//    for DEBUG
//================================================================================
/*---------------------------------------------------------------------------*
  Name:         osGetThreadResource

  Description:  store resources of thread to specified pointer

  Arguments:    resource       pointer to store thread resources

  Returns:      TRUE  ... success (always return this now)
                FALSE ... fail
 *---------------------------------------------------------------------------*/
BOOL osGetThreadResource(OSThreadResource *resource)
{
	resource->num = osGetNumberOfThread();

	return TRUE;
}


#if defined(SDK_TCM_APPLY) && defined(SDK_ARM9)
#include    <brom/itcm_end.h>
#endif