/*---------------------------------------------------------------------*
        Copyright (C) 1998 Nintendo. (Originated by SGI)
        
        $RCSfile: stand.c,v $
        $Revision: 1.1.1.1 $
        $Date: 2002/05/02 03:27:13 $
 *---------------------------------------------------------------------*/
#include <os.h>
#include <ultra64.h>
#include <string.h>
#include <bstring.h>
/*
#include <setjmp.h>
*/
#include "os_internal.h"
#include "cpu.h"

/*
#define _DUMP_THREAD
*/

/**************************************************************************
 *
 * Definitions
 *
 */


/**************************************************************************
 *
 * Global variables
 *
 */
char		bootStack[STACKSIZE];

static	OSThread	tlbThread;
static	char		tlbThreadStack[STACKSIZE];
static	OSMesgQueue	tlbMesgQueue;
static	OSMesg		tlbMesgBuf[5];

OSFault		__osFault = { 0 };


/**************************************************************************
 *
 * Local variables
 *
 */
static OSThread	idleThread;
static char	idleThreadStack[STACKSIZE];

static OSThread	mainThread;
static char	mainThreadStack[STACKSIZE];

static OSThread	faultThread;
static char	faultThreadStack[STACKSIZE];
static OSMesgQueue	faultMesgQueue;
static OSMesg		faultMesgBuf[5];


/**************************************************************************
 *
 * Extern references
 *
 */
extern char 		_appSegmentRomStart[];
extern char 		_appSegmentRomEnd[];
extern char		_appSegmentStart[];
extern char		_appSegmentTextStart[];
extern char		_appSegmentTextEnd[];
extern char		_appSegmentDataStart[];
extern char		_appSegmentDataEnd[];
extern char		_appSegmentBssStart[];
extern char		_appSegmentBssEnd[];
extern char		_bufferSegmentBssStart[];
extern char		_bufferSegmentBssEnd[];

extern OSThread 	userThread;
extern char		userThreadStack[];
extern void		userMain(void);
extern void		userCleanupThread(void);
extern OSThread 	slaveThread;

extern OSThread 	schedThread;
extern char		schedThreadStack[];
extern void		schedMain(void);

extern OSMesgQueue	kernMesgQueue;
extern u32		stat[];
extern u32		userThread1Stat;
extern u32		userThread2Stat;
extern u32		high, low, average;

extern u32		__osSetJmp(u32 *);
extern void		__osRestoreJmp(u32 *, OSThread *);

extern s32		tlbTest(void);
extern s32		fpTest(void);


/**************************************************************************
 *
 * Function prototypes
 *
 */
static void	idle(void *);
static void	main(void *);

void dumpThread(char *, OSThread *);
void faultMain(void *arg);
void tlbMain(void *arg);


/**************************************************************************
 *
 * Boot program
 *
 */
OSPiHandle	*handler;

boot(void)
{

    osInitialize();

    handler = osCartRomInit();

    /*
     * Map TLB
     */
    osMapTLB(0, OS_PM_256K, _appSegmentStart, APP_PADDR, APP_PADDR+_256K, -1);

    osInvalICache(_appSegmentTextStart,
                  _appSegmentTextEnd-_appSegmentTextStart);
    osInvalDCache(_appSegmentDataStart,
                  _appSegmentDataEnd-_appSegmentDataStart);
    osPiRawStartDma(OS_READ, (u32)_appSegmentRomStart, _appSegmentStart, 
                   _appSegmentRomEnd-_appSegmentRomStart);

    while(osPiGetStatus() & PI_STATUS_DMA_BUSY);

    bzero(_appSegmentBssStart, _appSegmentBssEnd-_appSegmentBssStart);

    bzero(_bufferSegmentBssStart, _bufferSegmentBssEnd-_bufferSegmentBssStart);

    osCreateThread(&mainThread, 1, main, (void *)0, mainThreadStack+STACKSIZE,
		  (OSPri)MAIN_THREAD_PRI);
    osStartThread(&mainThread);
}

static void
main(void *arg)
{

    OSMesg	mesg;
    int		i;
    u32		saveMask;

    /*
     * Create idle thread
     */
    osCreateThread(&idleThread, 2, idle, (void *)0, idleThreadStack+STACKSIZE,
		  (OSPri)IDLE_THREAD_PRI);
    osStartThread(&idleThread);


    /* Start VI manager */
    osCreateViManager((OSPri)OS_PRIORITY_VIMGR);

    /*
     * Create fault handler thread
     */
    osCreateThread(&faultThread, 3, faultMain, (void *)0,
		faultThreadStack+STACKSIZE, (OSPri)FAULT_THREAD_PRI);
    osStartThread(&faultThread);
    
    /*
     * Create scheduler thread
     */
    osCreateThread(&schedThread, 4, schedMain, (void *)0,
		schedThreadStack+STACKSIZE, (OSPri)SCHED_THREAD_PRI);
    osStartThread(&schedThread);
    
    /*
     * Now, we turn off interrupts to replace exception handler 
     */
    saveMask = __osDisableInt();

    osWritebackDCache((void*)UT_VEC,
                  E_VEC - UT_VEC + sizeof(__osExceptionVector));
    osInvalICache((void*)UT_VEC,
                  E_VEC - UT_VEC + sizeof(__osExceptionVector));

    *(__osExceptionVector *)UT_VEC =
            *(__osExceptionVector *)__osExceptionPreamble2;
    *(__osExceptionVector *)XUT_VEC =
            *(__osExceptionVector *)__osExceptionPreamble2;
    *(__osExceptionVector *)ECC_VEC =
            *(__osExceptionVector *)__osExceptionPreamble2;
    *(__osExceptionVector *)E_VEC =
            *(__osExceptionVector *)__osExceptionPreamble2;

    osWritebackDCache((void*)UT_VEC,
                  E_VEC - UT_VEC + sizeof(__osExceptionVector));
    osInvalICache((void*)UT_VEC,
                  E_VEC - UT_VEC + sizeof(__osExceptionVector));

    __osRestoreInt(saveMask);

    /*
     * Create user app thread
     */
    XDEBUG(PRINTF("Creating user thread!\n"));

    osCreateThread(&userThread, 6, userMain, (void *)0,
		userThreadStack+STACKSIZE, (OSPri)USER_THREAD_PRI);
    userThread.context.sr |= (SR_CU0| SR_KSU_USR);
    userThread.context.ra = (u64)userCleanupThread;

#if 0
    dumpThread("mainThread", &mainThread);
    dumpThread("userThread", &userThread);
#endif
    osStartThread(&userThread);

    /*
     * Create tlb handler thread
     */
    XDEBUG(PRINTF("Creating tlb thread!\n"));

    osCreateThread(&tlbThread, 5, tlbMain, (void *)0,
		tlbThreadStack+STACKSIZE, (OSPri)TLB_THREAD_PRI);
    
    osStartThread(&tlbThread);

    /*
     * Register event handler for TLB miss
     */

    XDEBUG(PRINTF("Get control back!\n"));

#if 0
    for (i=0; i < 10; i++) {
        XDEBUG(PRINTF("Stat[%2d]: 0x%x\n", i, stat[i]));
    }
#endif

    PRINTF("userThread1Stat: 0x%x\n", userThread1Stat);
    PRINTF("userThread2Stat: 0x%x\n", userThread2Stat);

    PRINTF("interrupt latency (cycles) average %d high %d low %d\n",
        average, high, low);
    PRINTF("interrupt latency (ns)     average %lld high %lld low %lld\n",
        OS_CYCLES_TO_NSEC(average),
        OS_CYCLES_TO_NSEC(high),
        OS_CYCLES_TO_NSEC(low));

#if 0
    for (i=0; i<20; i++) {
	mesg = 0x0;
        XDEBUG(PRINTF("Waiting on kernel message queue: #%d!\n", i));
	(void)osRecvMesg(&kernMesgQueue, &mesg, OS_MESG_BLOCK);
        XDEBUG(PRINTF("Receiving: 0x%x\n", mesg));

    }
#endif


}  /* main */


void
faultMain(void *arg)
{

    OSThread	*tptr;
    OSMesg	mesg;
    int		i;

    XDEBUG(PRINTF("faultMain: Creating mesg queue...\n"));

    osCreateMesgQueue(&faultMesgQueue, faultMesgBuf, 5);
    osSetEventMesg(OS_EVENT_FAULT, &faultMesgQueue, (OSMesg)MSG_FAULT);

    while (1) {
/*
        XDEBUG(PRINTF("faultMain: Listening on FAULT mesg queue...\n"));
*/
        (void)osRecvMesg(&faultMesgQueue, (OSMesg *)&mesg, OS_MESG_BLOCK);

        /* If nofault is defined via a setjmp call */
        /* 
         * Search for faulted thread 
         * If faulted thread is same as nofault thread
         *     set thread pc to jmp_buffer pc
         *     set thread to OS_STATE_RUNNABLE
         *     reset thread state flag
         *     clear nofault
	 *	   enqueue thread again
         */
        tptr = FindFaultedThread();
        if (tptr) {
/*
*/
            XDEBUG(PRINTF("==> FAULT: mesg=0x%x\n", mesg));
            dumpThread("faulted thread", tptr);
/*
            XDEBUG(PRINTF("==> FAULT: dumping thread is completed!\n"));
*/

        }
	if (__osFault.nofault) {
/*
            XDEBUG(PRINTF("==> nofault is set and thread is 0x%x!\n", __osFault.thread));
*/
            if (__osFault.thread == tptr) {
                /* Assume that caller has called __osSetJmp */
                __osRestoreJmp(__osFault.nofault, tptr);
                tptr->state = OS_STATE_STOPPED;
                tptr->flags = 0;
                tptr->context.sr |= SR_EXL;
                tptr->next = (OSThread *)NULL;
                tptr->queue = (OSThread **)NULL;
/*
                XDEBUG(PRINTF("==> Restarting thread 0x%x!\n", tptr));
*/
                dumpThread("faulted thread", tptr);
/*
                XDEBUG(PRINTF("==> Thread 0x%x has been started!\n", tptr));
*/

                osStartThread(tptr);
            } 
	}

#if 0
        XDEBUG(PRINTF("==> FAULT: 0x%x...dumping threads...\n", mesg));

        dumpThread("mainThread", &mainThread);
        dumpThread("userThread", &userThread);
        dumpThread("slaveThread", &slaveThread);

        XDEBUG(PRINTF("==> FAULT: dumping threads is completed!\n"));
#endif
    }

}  /* faultMain */


void
tlbMain(void *arg)
{

    OSMesg	mesg;
    int		i, error;

    XDEBUG(PRINTF("tlbMain: Creating mesg queue...\n"));

    osCreateMesgQueue(&tlbMesgQueue, tlbMesgBuf, 5);

/*
    error = tlbTest(); 
*/
    error = fpTest(); 
/*
    error = tlb0(); 
    error += tlb1(); 

    XDEBUG(PRINTF("tlbMain: error=%d\n", error));
*/


#if 0
    osSetEventMesg(OS_EVENT_TLB_MOD, &tlbMesgQueue, (OSMesg)MSG_TLB_MOD);
    osSetEventMesg(OS_EVENT_TLB_REFILL, &tlbMesgQueue, (OSMesg)MSG_TLB_REFILL);
    osSetEventMesg(OS_EVENT_TLB_INV, &tlbMesgQueue, (OSMesg)MSG_TLB_INV);

    while (1) {
        XDEBUG(PRINTF("tlbMain: Listening on TLB mesg queue...\n"));
        (void)osRecvMesg(&tlbMesgQueue, (OSMesg *)&mesg, OS_MESG_BLOCK);
	XDEBUG(PRINTF("tlbMain: received mesg 0x%x\n", (u32)mesg));

	switch ((u32)mesg) {
	    case MSG_TLB_MOD:
		PRINTF("tlbMain: received TLB_MOD mesg\n");
		break;
	    case MSG_TLB_REFILL:
		PRINTF("tlbMain: received TLB_REFILL mesg\n");
		break;
	    case MSG_TLB_INV:
		PRINTF("tlbMain: received TLB_INV mesg\n");
		break;
	    default: {
		PRINTF("tlbMain: received unknown mesg 0x%x\n", mesg);
		break;
	    }
	}

        XDEBUG(PRINTF("==> TLB: 0x%x...dumping threads...\n", mesg));

        dumpThread("mainThread", &mainThread);
        dumpThread("userThread", &userThread);
        dumpThread("slaveThread", &slaveThread);

        XDEBUG(PRINTF("==> TLB: dumping threads is completed!\n"));
    }
#endif

}  /* tlbMain */


static void
idle(void *arg)
{
	for (;;);
}


void
dumpThread(char *name, OSThread *t)
{
#ifdef _DUMP_THREAD
    PRINTF("******** Thread `%s`: 0x%x ********\n", name, t);
    PRINTF("\tid\t= %d\n", t->id);
    PRINTF("\tpri\t= %d\n", t->priority);
    PRINTF("\tnext\t= 0x%x\n", t->next);
    PRINTF("\tqueue\t= 0x%x\n", t->queue);
    PRINTF("\tpc\t= 0x%x\n", t->context.pc);
    PRINTF("\ta0\t= 0x%llx\n", t->context.a0);
    PRINTF("\tsp\t= 0x%llx\n", t->context.sp);
    PRINTF("\tra\t= 0x%llx\n", t->context.ra);
    PRINTF("\tsr\t= 0x%x\n", t->context.sr);
    PRINTF("\tcause\t= 0x%x\n", t->context.cause);
    PRINTF("\tbadvaddr= 0x%x\n", t->context.badvaddr);
    PRINTF("\trcp\t= 0x%x\n", t->context.rcp);
    PRINTF("\tfpcsr\t= 0x%x\n", t->context.fpcsr);
    PRINTF("\tfp\t= 0x%x\n", t->fp);
    PRINTF("\tstate\t= 0x%x\n", t->state);
    PRINTF("\tflags\t= 0x%x\n", t->flags);
/*
    PRINTF("\tksp\t= 0x%llx\n", t->ksp);
*/
    PRINTF("******** END *******\n");
#endif
}