/********************************************************************************/
/*	File : reset.S							        	*/
/*	Author : Alain Greiner & mohamed karaoui		    */
/*	Date : 26/04/2012						        	*/
/********************************************************************************/
/* This file contain a function wich is called by init.s*/
/* after initialising the peripherals and the shedulers.*/
/* this _task_init function write the (procid+1) at the */
/* first word of the mapping_base_info, to "wakeup" the */
/* the processors wich id correspond to (procid+1)      */
/* ...TODO                                              */
/********************************************************************************/
		
#include <giet_config.h>
#include <mips32_registers.h>

	.section .kinitentry,"ax",@progbits
	.align	2
   	.set noreorder

    /* get the procid */
    mfc0    k0, CP0_PROCID
    andi    k0, k0, 0x3FF   /* no more than 1024 processors... */

    /* All processors jump to _task_init excepte the processor 0 */
    bne k0, zero,   _task_init
    nop

    /* plat-form initialisation */
	jal	_init
    nop

_task_init:

    /* get the scheduler address */
    li	    k0,	256
    li	    k1,	GIET_NB_TASKS_MAX
    mul	    s2,	k1,	k0		    /* s2 <= sizeof(context_array) */
    addiu	k0,	s2,	8		    /* k0 <= sizeof(scheduler_t) */
    mul	    k0,	k0,	s0		    /* k0 <= proc_id*sizeof(scheduler_t) */
    la	    k1,	_scheduler
    addu	s1,	k1,	k0		    /* s1 <= &_scheduler[proc_id] */

    /* test number of tasks */
    addu	k1,	s1,	s2		    /* k1 <= &tasks */
	lw	    k1,	0(k1)			/* k1 <= tasks */
    beq	    k1, zero,	_task_exit
	nop

    /* load SP */
	li	    k1,	CTX_SP_ID
    sll	    k1,	k1, 	2
	addu	k1,	s1,	k1
    lw	    k1,	0(k1)	
    move	sp,	k1 			    /* sp <= ctx[SP]  */

    /* load SR */
	li	    k1,	CTX_SR_ID
    sll	    k1,	k1, 	2
	addu	k1,	s1,	k1
    lw	    k1,	0(k1)	
    mtc0	k1,	CP0_SR 			/* sr <= ctx[SR]  */

    /* load PTPR, MMU already activeted */
	li	    k1,	CTX_PTPR_ID
    sll	    k1,	k1, 	2
	addu	k1,	s1,	k1
    lw	    k1,	0(k1)	
    mtc2	k1,	CP2_PTPR 		/* ptpr <= ctx[PTPR]  */

    /* load EPC */
	li	    k1,	CTX_EPC_ID
    sll	    k1,	k1, 	2
	addu	k1,	s1,	k1
    lw	    k1,	0(k1)	
    mtc0	k1,	CP0_EPC 		/* epc <= ctx[EPC]  */

	/* jump to user's code in user mode */
	eret #jr  ra 
    nop         

_task_exit:
    j _task_exit

	.set	reorder