os_cpu_c.c

os源代码 os源代码 os源代码 os源代码 os源代码 os源代码

*                                            Context Switch
*
* Description: This function is called to initiate a context switch from task level
*
* Arguments  : none
*
* Note		 : You MUST be in the critical state to call this function			
*********************************************************************************************************
*/
void OSCtxSw(void)
{
	/* This stack pointer will point at the NEXT EMPTY address */
	/* Load current stack pointer to SPRG2 */
	SaveContext();						
	/* Call OSTaskSWHook() */
	OSTaskSwHook();					
	/* Copy High Ready Priority to Current Priority */
	OSPrioCur = OSPrioHighRdy;
	/* Copy High Ready Stack Pointer to Current Task */
	*OSTCBCur = *OSTCBHighRdy;
	MyContext = OSTCBCur->OSTCBStkPtr;
	HoldMSR=MSRAllEn;
	RestoreContext();
	return;
}

/*
*********************************************************************************************************
*                                            CONTEXT SWITCH FROM ISR
*
* Description: This function is called to initiate a context switch from ISR level
*
* Arguments  : none
*
*********************************************************************************************************
*/
void	OSIntCtxSw(void)
{
	/* Note: We are about to go into critical mode with this call */
	OSCtxSw();
	ReturnFromInterrupt();
	return;
}

/*
*********************************************************************************************************
*                                         DECREMENTER INTERRUPT
*
* Description: This function is called to handle the decrementer ISR.
*
* Arguments  : none
*
*********************************************************************************************************
*/
void DecIntr (void)
{
	/* Save context */
	SaveContext();
	OS_ENTER_CRITICAL();
	/* Call Decrement Handler */
	DEC_Hdlr();	
	OSIntNesting++; /* Moved from OSIntEnter to eliminate function call */ 
	/* Note: the increment of OSIntNesting MUST be atomic */ 
	OSTimeTick ();
	/* Restore context */
	RestoreContext();
	/* RestoreContext will exit critical, but might be interrupted here */
	OS_EXIT_CRITICAL();
	OSIntExit ();
	return;
}


/*
*********************************************************************************************************
*                                            SAVE CONTEXT
*
* Description: This function is called to save the context
*
* Arguments  : none
*
* NOTE: The pointer points to the top of the NEXT AVAILABLE EMPTY stack space.
*
*********************************************************************************************************
*/
asm void SaveContext(void)
{
	/* You should be in critical mode if you're here */
	mtspr	sprg3, gpr4
	/* GPR #1 is also called SP, and is ALWAYS the pointer to the current stack. */
	mtspr sprg2, gpr2				/* Save GPR2 in SPRG2 now: GPR2 is scratchpad, GPR1 is SP */
	lis SP, MyContext@ha			/* Copy CURRENT stack pointer from memory */
	lwz SP, MyContext@l(SP) 	/* Now current SP is in GPR2 */
	mfspr gpr4, sprg3				/* Give original contents back to GPR4 */

	/* Begin to store registers */
	mfspr gpr2, srr0
	stw gpr2, OFFSET_SRR0(SP)	
	mfspr gpr2, srr1
	stw gpr2, OFFSET_SRR1(SP)	
	mfspr gpr2, xer
	stw gpr2, OFFSET_XER(SP)	
	mfspr gpr2, dar
	stw gpr2, OFFSET_DAR(SP)	
	stw gpr0, OFFSET_GPR0(SP)	
	stw gpr3, OFFSET_GPR3(SP)	
	stw gpr4, OFFSET_GPR4(SP)	
	stw gpr5, OFFSET_GPR5(SP)	
	stw gpr6, OFFSET_GPR6(SP)	
	stw gpr7, OFFSET_GPR7(SP)	
	stw gpr8, OFFSET_GPR8(SP)	
	stw gpr9, OFFSET_GPR9(SP)	
	stw gpr10, OFFSET_GPR10(SP)	
	stw gpr11, OFFSET_GPR11(SP)	
	stw gpr12, OFFSET_GPR12(SP)
	lis gpr13, Register13@ha
	lwz gpr13, Register13@l(gpr13)
	stw gpr13, OFFSET_GPR13(SP)	
	stw gpr14, OFFSET_GPR14(SP)	
	stw gpr15, OFFSET_GPR15(SP)	
	stw gpr16, OFFSET_GPR16(SP)	
	stw gpr17, OFFSET_GPR17(SP)	
	stw gpr18, OFFSET_GPR18(SP)	
	stw gpr19, OFFSET_GPR19(SP)	
	stw gpr20, OFFSET_GPR20(SP)	
	stw gpr21, OFFSET_GPR21(SP)	
	stw gpr22, OFFSET_GPR22(SP)	
	stw gpr23, OFFSET_GPR23(SP)	
	stw gpr24, OFFSET_GPR24(SP)	
	stw gpr25, OFFSET_GPR25(SP)	
	stw gpr26, OFFSET_GPR26(SP)	
	stw gpr27, OFFSET_GPR27(SP)	
	stw gpr28, OFFSET_GPR28(SP)	
	stw gpr29, OFFSET_GPR29(SP)	
	stw gpr30, OFFSET_GPR30(SP)	
	stw gpr31, OFFSET_GPR31(SP)
	stfd fp0, OFFSET_FP0(SP)	
	stfd fp1, OFFSET_FP1(SP)	
	stfd fp2, OFFSET_FP2(SP)	
	stfd fp3, OFFSET_FP3(SP)	
	stfd fp4, OFFSET_FP4(SP)	
	stfd fp5, OFFSET_FP5(SP)	
	stfd fp6, OFFSET_FP6(SP)	
	stfd fp7, OFFSET_FP7(SP)	
	stfd fp8, OFFSET_FP8(SP)	
	stfd fp9, OFFSET_FP9(SP)	
	stfd fp10, OFFSET_FP10(SP)	
	stfd fp11, OFFSET_FP11(SP)	
	stfd fp12, OFFSET_FP12(SP)	
	stfd fp13, OFFSET_FP13(SP)	
	stfd fp14, OFFSET_FP14(SP)	
	stfd fp15, OFFSET_FP15(SP)	
	stfd fp16, OFFSET_FP16(SP)	
	stfd fp17, OFFSET_FP17(SP)	
	stfd fp18, OFFSET_FP18(SP)	
	stfd fp19, OFFSET_FP19(SP)	
	stfd fp20, OFFSET_FP20(SP)	
	stfd fp21, OFFSET_FP21(SP)	
	stfd fp22, OFFSET_FP22(SP)	
	stfd fp23, OFFSET_FP23(SP)	
	stfd fp24, OFFSET_FP24(SP)	
	stfd fp25, OFFSET_FP25(SP)	
	stfd fp26, OFFSET_FP26(SP)	
	stfd fp27, OFFSET_FP27(SP)	
	stfd fp28, OFFSET_FP28(SP)	
	stfd fp29, OFFSET_FP29(SP)	
	stfd fp30, OFFSET_FP30(SP)	
	stfd fp31, OFFSET_FP31(SP)	
	mfspr gpr2, sprg2					/* Restore GPR2 contents from SPRG2 */
	stw gpr2, OFFSET_GPR2(SP)	
	/* Update Current task's stack ptr */
	stw SP, OFFSET_GPR1(SP)			/* Store the stack pointer */
	lis gpr11,OSTCBCur@ha
	lwz gpr11,OSTCBCur@l(gpr11)
	stw SP,0(r11)
	lwz gpr2, 0(r11)
	stw SP, MyContext
	mflr gpr2
	stw gpr2, OFFSET_LR(SP)
	/* Come back from critical to whatever state of criticalness you had */
	mtspr	sprg3, gpr4
	lis gpr4, HoldMSR@ha
	lwz gpr4, HoldMSR@l(gpr4)
	mtmsr	gpr4
	mfspr gpr4, sprg3				/* SPRG3 now free */
	/* Done */
	blr
}

/*
*********************************************************************************************************
*                                            RESTORE CONTEXT
*
* Description: This function is called to restore the context
*
* Arguments  : none
*
* NOTE: The register SPRG2 should contain the stack pointer before this function is called.
*
*********************************************************************************************************
*/
asm void RestoreContext(void)
{
	nofralloc
	lis r20, MyContext@ha					/* High address portion */
	lwz r20, MyContext@l(r20)				/*	r20 now points to start of context */
	or sp,r20,r20							// Transfer cleverly to SP

	/* Go critical if you're not there already */
	mtspr	sprg3, gpr4
	mfmsr gpr4
	stw gpr4, HoldMSR				/* Save MSR */
	lis gpr4, MSRCritical@ha
	lwz gpr4, MSRCritical@l(gpr4)
	mtmsr	gpr4
	/* Begin to stuff registers */
	lwz gpr0, OFFSET_GPR0(SP)	
	lwz gpr3, OFFSET_GPR3(SP)	
	lwz gpr4, OFFSET_GPR4(SP)	
	lwz gpr5, OFFSET_GPR5(SP)	
	lwz gpr6, OFFSET_GPR6(SP)	
	lwz gpr7, OFFSET_GPR7(SP)	
	lwz gpr8, OFFSET_GPR8(SP)	
	lwz gpr9, OFFSET_GPR9(SP)	
	lwz gpr10, OFFSET_GPR10(SP)	
	lwz gpr11, OFFSET_GPR11(SP)	
	lwz gpr12, OFFSET_GPR12(SP)	
	lwz gpr13, OFFSET_GPR13(SP)	
	lwz gpr14, OFFSET_GPR14(SP)	
	lwz gpr15, OFFSET_GPR15(SP)	
	lwz gpr16, OFFSET_GPR16(SP)	
	lwz gpr17, OFFSET_GPR17(SP)	
	lwz gpr18, OFFSET_GPR18(SP)	
	lwz gpr19, OFFSET_GPR19(SP)	
	lwz gpr20, OFFSET_GPR20(SP)	
	lwz gpr21, OFFSET_GPR21(SP)	
	lwz gpr22, OFFSET_GPR22(SP)	
	lwz gpr23, OFFSET_GPR23(SP)	
	lwz gpr24, OFFSET_GPR24(SP)	
	lwz gpr25, OFFSET_GPR25(SP)	
	lwz gpr26, OFFSET_GPR26(SP)	
	lwz gpr27, OFFSET_GPR27(SP)	
	lwz gpr28, OFFSET_GPR28(SP)	
	lwz gpr29, OFFSET_GPR29(SP)	
	lwz gpr30, OFFSET_GPR30(SP)	
	lwz gpr31, OFFSET_GPR31(SP)	
	lfd fp0, OFFSET_FP0(SP)	
	lfd fp1, OFFSET_FP1(SP)	
	lfd fp2, OFFSET_FP2(SP)	
	lfd fp3, OFFSET_FP3(SP)	
	lfd fp4, OFFSET_FP4(SP)	
	lfd fp5, OFFSET_FP5(SP)	
	lfd fp6, OFFSET_FP6(SP)	
	lfd fp7, OFFSET_FP7(SP)	
	lfd fp8, OFFSET_FP8(SP)	
	lfd fp9, OFFSET_FP9(SP)	
	lfd fp10, OFFSET_FP10(SP)	
	lfd fp11, OFFSET_FP11(SP)	
	lfd fp12, OFFSET_FP12(SP)	
	lfd fp13, OFFSET_FP13(SP)	
	lfd fp14, OFFSET_FP14(SP)	
	lfd fp15, OFFSET_FP15(SP)	
	lfd fp16, OFFSET_FP16(SP)	
	lfd fp17, OFFSET_FP17(SP)	
	lfd fp18, OFFSET_FP18(SP)	
	lfd fp19, OFFSET_FP19(SP)	
	lfd fp20, OFFSET_FP20(SP)	
	lfd fp21, OFFSET_FP21(SP)	
	lfd fp22, OFFSET_FP22(SP)	
	lfd fp23, OFFSET_FP23(SP)	
	lfd fp24, OFFSET_FP24(SP)	
	lfd fp25, OFFSET_FP25(SP)	
	lfd fp26, OFFSET_FP26(SP)	
	lfd fp27, OFFSET_FP27(SP)	
	lfd fp28, OFFSET_FP28(SP)	
	lfd fp29, OFFSET_FP29(SP)	
	lfd fp30, OFFSET_FP30(SP)	
	lfd fp31, OFFSET_FP31(SP)
	lwz gpr2, OFFSET_XER(SP)	
	mtspr xer, gpr2
	lwz gpr2, OFFSET_DAR(SP)
	mtspr dar, gpr2
	lwz gpr2, OFFSET_LR(SP)			/* Update link register to return to new high point */
	mtlr gpr2
   /* Update Current task's stack ptr */
	/* Use the copy that is in SPRG2, it has already been adjusted */
	lis gpr11, ContextSize@ha
	lwz gpr11, ContextSize@l(gpr11)
	addc SP, r11, SP
	lis gpr11,OSTCBCur@ha
	lwz gpr11,OSTCBCur@l(gpr11)
	stw SP,0(r11)
	lwz gpr2, 0(r11)
	/* Come back from critical to whatever state of criticalness you had */
	mtspr	sprg3, gpr4
	lis gpr4, HoldMSR@ha
	lwz gpr4, HoldMSR@l(gpr4)
	mtmsr	gpr4
	mfspr gpr4, sprg3				/* SPRG3 now free */
	/* Done */
	blr
}

/*
*********************************************************************************************************
*                                        RETURN FROM INTERRUPT
*
* Description: This function is called to get an interupt return
*
* Arguments  : none
*
*********************************************************************************************************
*/
asm void ReturnFromInterrupt(void)
{
	rfi
	blr
}

asm void ReportRegister13(void)
{
	stw		r13,Register13
	blr
}

asm void SetDecrementer(void)
{
	/* First clear decrementer exception */
	lis gpr5, 0x3           /* 0x00030000 */
	addi gpr5, gpr5, 0x0D40 /* 0x30000 + 0xD40 = 0x30D40 = 200,000 */
	mtdec	gpr5              /* Re-load decrementer */
	blr
}