#include "../mipsi-include/main.h"
#include "minithread.h"
#include <setjmp.h>
#include <string.h>

/*
   Reverse-engineered from lib/switch.o
   
*/

static void minithread_swtch(stack_pointer_t *old_thread_sp,
			     stack_pointer_t *new_thread_sp);

    
/*
 * minithread_switch(stack_pointer_t *old_thread_sp, stack_pointer_t *new_thread_sp);
 *      Context switch code.
 *      Save caller's state on old_thread's stack.
 *      Resume new_thread's stack and continue executing where
 *      new_thread left off.
 */

void minithread_switch(stack_pointer_t *old_thread_sp,
		       stack_pointer_t *new_thread_sp)

{
    minithread_swtch(old_thread_sp, new_thread_sp);
}

void minithread_swtch(stack_pointer_t *old_thread_sp,
		       stack_pointer_t *new_thread_sp)
{

    /* We save all of MIPSI's state in these variables: */
    unsigned int saved_R[32]; /* CPU registers */
    unsigned int saved_CpCond[4], saved_CCR[4][32], saved_CPR[4][32];
    float saved_FGR[16];      /* FPU registers */
    unsigned int saved_pc, saved_npc, saved_nnpc, saved_HI, saved_LO;
    unsigned int saved_ownedfp;  
    jmp_buf saved_inrun;

    extern jmp_buf inrun;
    
    int i, j;
    
    /* Save the current state of MIPSI onto the stack ------------------- */

    /* save GPRs */
    for (i = 0; i < 32; i++) {
	saved_R[i] = R[i];
    }

    /* save CpCond[4] to the stack */
    for (i = 0; i < 4; i++) {
	saved_CpCond[i] = CpCond[i];
    }
    
    /* copy CCR[4][32], CPR[4][32] to the stack */
    for (i = 0; i < 4; i++)
	for (j = 0; j < 32; j++) {
	    saved_CCR[i][j] = CCR[i][j];
	    saved_CPR[i][j] = CPR[i][j];
	}
   
    /* copy FGR[16] from globals to stack */
    for (i = 0; i < 16; i++) {
	saved_FGR[i] = FGR[i];
    }

    /* save 6 other state variables */
    saved_pc      = pc;
    saved_npc     = npc;
    saved_nnpc    = nnpc;
    saved_HI      = HI;
    saved_LO      = LO;
    saved_ownedfp = ownedfp;

    /* copy the jumpbuf to save it for later */
    memcpy(&saved_inrun, &inrun, sizeof(inrun));

    /* switch to other thread */
    context_switch(old_thread_sp, new_thread_sp);

    /* Restore the state of MIPSI from the stack ------------------- */

    /* restore GPRs */
    for (i = 0; i < 32; i++) {
	R[i] = saved_R[i];
    }

    /* restore CpCond[4] from the stack */
    for (i = 0; i < 4; i++) {
	CpCond[i] = saved_CpCond[i];
    }
    
    /* copy CCR[4][32], CPR[4][32] from the stack */
    for (i = 0; i < 4; i++)
	for (j = 0; j < 32; j++) {
	    CCR[i][j] = saved_CCR[i][j];
	    CPR[i][j] = saved_CPR[i][j];
	}
   
    /* restore FGR[16] */
    for (i = 0; i < 16; i++) {
	FGR[i] = saved_FGR[i];
    }

    /* restore 6 other state variables */
    pc      = saved_pc;
    npc     = saved_npc;
    nnpc    = saved_nnpc;
    HI      = saved_HI;
    LO      = saved_LO;
    ownedfp = saved_ownedfp;

    /* restore the jumpbuf */
    memcpy(&inrun, &saved_inrun, sizeof(inrun));    
}