windalib.s

VxWorks BSP框架源代码包含头文件和驱动

	 *
	 * return OK
	 */
	
	MOV	r0, #0
#if	(ARM_THUMB)
	LDMFD	sp!, {lr}
	BX	lr
#else
	LDMFD	sp!, {pc}
#endif	/* (ARM_THUMB) */

/******************************************************************************/

#ifdef	WV_INSTRUMENTATION

	/* windview instrumentation - BEGIN
	 * exit windExit with no dispatch; point 4 in the windExit diagram.
	 */

instrumentDispatch4:

	STMFD	sp!, {lr}		/* save link */

        LDR	lr, L$_wvEvtClass		/* is instrumentation on? */
	LDR	lr, [lr]
        AND	lr, lr, #WV_CLASS_1_ON	
        TEQS	lr, #WV_CLASS_1_ON	
	BNE	trgInst4			/* branch if not */

	/* 
	 * try to determine if the task is running at an inherited priority
	 * and log a different event if so
	 */

	LDR	r12, L$_taskIdCurrent
	LDR	r12, [r12]			/* r12 -> TCB */
	LDR	r1, [r12, #WIND_TCB_PRIORITY]
	LDR	r2, [r12, #WIND_TCB_PRI_NORMAL]
	CMPS	r2, r1
	MOVHI	r0, #EVENT_WIND_EXIT_NODISPATCH_PI
	MOVLS	r0, #EVENT_WIND_EXIT_NODISPATCH

	LDR	r12, L$__func_evtLogTSched	/* get address of fn pointer */

#if	(ARM_THUMB)
	LDR	r12, [r12]			/* get function address */
	BL	FUNC(arm_call_via_r12)		/* and call it */
#else
	MOV	lr, pc				/* call function */
	LDR	pc, [r12]
#endif	/* (ARM_THUMB) */

trgInst4: 
        LDR	lr, L$_trgEvtClass	/* is instrumentation on? */
	LDR	lr, [lr]
        AND	lr, lr, #TRG_CLASS_1_ON	
        TEQS	lr, #TRG_CLASS_1_ON	
	BNE	instTidy4		/* branch if not */

	/*
	 * There are 8 parameters to the trgCheck function:
	 * r0	 <- eventId
	 * r1 	 <- index = TRG_CLASS1_INDEX
	 * r2 	 <- obj (NULL)
	 * r3 	 <- arg1 (NULL - unused)
	 * stack <- arg2 (NULL - unused)
	 * stack <- arg3 (NULL - unused)
	 * stack <- arg4 (NULL - unused)
	 * stack <- arg5 (NULL - unused)
	 */

	MOV	r3, #0				/* NULL parms for stack */
	MOV	r2, #0
	MOV	r1, #0
	MOV	r0, #0

	STMFD	sp!, {r0-r3}			/* push stack-based parms */

	/* 
	 * try to determine if the task is running at an inherited priority
	 * and log a different event if so
	 */

	LDR	r12, L$_taskIdCurrent
	LDR	r12, [r12]			/* r12 -> TCB */
	LDR	r0, [r12, #WIND_TCB_PRIORITY]
	LDR	r1, [r12, #WIND_TCB_PRI_NORMAL]
	CMPS	r1, r0
	MOVHI	r0, #EVENT_WIND_EXIT_NODISPATCH_PI 
	MOVLS	r0, #EVENT_WIND_EXIT_NODISPATCH	/* r0 <- eventId */
	MOV   	r1, #TRG_CLASS1_INDEX		/* r1 <- TRG_CLASS1_INDEX */
						/* r2 <- NULL (from above) */
						/* r3 <- NULL (from above) */
	LDR	r12,L$__func_trgCheck 		/* trigCheck routine */

#if	(ARM_THUMB)
	LDR	r12, [r12]			/* get function address */
	BL	FUNC(arm_call_via_r12)		/* and call it */
#else
	MOV	lr, pc				/* call function */
	LDR	pc, [r12]
#endif	/* (ARM_THUMB) */

	ADD	sp, sp, #16		/* strip 4 parameters from stack */

instTidy4:
	LDMFD	sp!, {lr}		/* restore link */

	B	resumeDispatch4

	/* windview instrumentation - END */
#endif	/* WV_INSTRUMENTATION */

/*******************************************************************************
*
* doWorkPreSave - empty the work queue with current context not saved
*
* We try to empty the work queue here, rather than let reschedule
* perform the work because there is a strong chance that the
* work we do will not preempt the calling task.  If this is the case, then
* saving the entire context just to restore it in reschedule is a waste of
* time.  Once the work has been emptied, the ready queue must be checked to
* see if reschedule must be called; the check of the ready queue is done by
* branching to checkTaskSwitch.
*/

doWorkPreSave:

	/* enable interrupts */

	MRS	r0, cpsr
	BIC	r0, r0, #I_BIT
	MSR	cpsr, r0

	/* INTERRUPTS ENABLED */

	STMFD	sp!, {lr}

#if	(ARM_THUMB)
	LDR	r12, L$_workQDoWork	/* empty the work queue */
	BL	FUNC(arm_call_via_r12)	/* returns in ARM state */
#else
	BL	FUNC(workQDoWork)		/* empty the work queue */
#endif	/* (ARM_THUMB) */

	LDMFD	sp!, {lr}
	B	checkTaskSwitch		/* go and test if tasks switched */

/******************************************************************************
*
* windExit - task level exit from kernel
*
* Release kernel mutual exclusion (kernelState) and dispatch any new task if
* necessary.  If a higher priority task than the current task has been made
* ready, then we invoke the rescheduler.  Before releasing mutual exclusion,
* the work queue is checked and emptied if necessary.
*
* If rescheduling is necessary, the context of the calling task is saved in its
* associated TCB with the PC pointing at the next instruction after the BL to
* this routine. Thus the context saved is as if this routine was never called.
*
* Only the volatile registers r0-r3 are safe to use until the context
* is saved in saveTaskContext.
*
* At the call to reschedule the value of taskIdCurrent must be in r0.
*
* RETURNS: OK or
*          ERROR if semaphore timeout occurs.
*
* NOMANUAL

* STATUS windExit ()

*/

_ARM_FUNCTION_CALLED_FROM_C(windExit)

	/* if (intCnt != 0), exiting interrupt code */

	LDR	r0, L$_intCnt
	LDR	r0, [r0]
	TEQS	r0, #0
	BNE	windExitInt

	/*
	 * exiting task level code
	 *
	 * FALL THROUGH TO checkTaskSwitch
	 */

/*******************************************************************************
*
* checkTaskSwitch - check to see if taskIdCurrent is still highest task
*
* We arrive at this code either as the result of falling thru from
* windExit, or if we have finished emptying the work queue. We compare
* taskIdCurrent with the highest ready task on the ready queue. If they
* are same we go to a routine to check the work queue. If they are
* different and preemption is NOT [original comment incorrect] allowed
* we branch to a routine to make sure that taskIdCurrent is really ready
* (it may have blocked with preemption disabled). Otherwise we save the
* context of taskIdCurrent and fall thru to reschedule.
*
* REGISTERS
*   Entry: lr = task link
*   Uses r0, r1
*/

checkTaskSwitch:

	/* check if current task is highest ready task */

	LDR	r0, L$_taskIdCurrent
	LDR	r0, [r0]
	LDR	r1, L$_readyQHead
	LDR	r1, [r1]
	TEQS	r0, r1
	BEQ	checkWorkQ		/* branch if it is */

	/*
	 * current task is NOT highest ready task
	 * check if we can preempt it
	 * r0 -> TCB
	 */

	LDR	r1, [r0, #WIND_TCB_LOCK_CNT]
	TEQS	r1, #0			/* can preempt? */
	BNE	checkTaskReady		/* branch if not */

	/* we CAN preempt this task */

saveTaskContext:

	/*
	 * Save return address as PC so that when this task is resumed,
	 * it resumes at the exit from its windExit() call.
	 * NOTE: unlike on 68K and i86, no adjustment of task SP is necessary
	 */

	STR	lr, [r0, #WIND_TCB_PC]

	/* store a brand new PSR */

#if	(ARM_THUMB)
	MOV	r1, #MODE_SVC32 | T_BIT		/* interrupts enabled, Thumb */
#else
	MOV	r1, #MODE_SVC32			/* interrupts enabled */
#endif	/* (ARM_THUMB) */

	STR	r1, [r0, #WIND_TCB_CPSR]

	/* set r0 in TCB to return OK */

	MOV	r1, #0
	STR	r1, [r0, #WIND_TCB_R0]

	/*
	 * save registers: no need to save r0-r3 as they are volatile
	 * i.e. caller of windExit() does not expect them to be
	 * preserved (APCS)
	 */

	ADD	r1, r0, #WIND_TCB_R4
	STMIA	r1, {r4-r12,sp}

	/* save _fpStatus in TCB */

	LDR	r1, L$__fpStatus
	LDR	r1, [r1]
	STR	r1, [r0, #WIND_TCB_FPSTATUS]

	/* save errno in TCB */

	LDR	r1, L$_errno
	LDR	r1, [r1]
	STR	r1, [r0, #WIND_TCB_ERRNO]

#ifndef	PORTABLE

	/* FALL THROUGH to reschedule */

/*******************************************************************************
*
* reschedule - fast rescheduler for VxWorks kernel
*
* This routine is called when either intExit, or windExit, thinks the
* context might change.  All of the contexts of all of the tasks have been
* accurately stored in the task control blocks when entering this function.
* The status register is set to SVC32 mode with interrupts enabled.
*
* The register r0 must contain the value of FUNC(taskIdCurrent) at the entrance to
* this routine.
*
* At the conclusion of this routine, taskIdCurrent will equal the highest
* priority task eligible to run, and the kernel work queue will be empty.
* If a context switch to a different task is to occur, then the installed
* switch hooks are called.
*
* NOMANUAL

* void reschedule ()

* INTERNAL
* This routine can use whatever registers it likes since it does not
* return to its caller but enters a new task, loading all its registers
* to do so.
*
* The following non-volatile registers are used:
*    r4 -> TCB of current task
*    r5 -> TCB of task at top of readyQ ('highest task')
*    r6 = swap mask
*    r8 -> taskIdCurrent i.e. the pointer to the TCB pointer
*/

_ARM_FUNCTION(reschedule)

	MOV	r4, r0				/* r4 -> current task */
	LDR	r8, L$_taskIdCurrent		/* keep r8 -> taskIdCurrent */
	LDR	r5, L$_readyQHead
	LDR	r5, [r5]			/* r5 -> highest task */
	TEQS	r5, #0				/* readyQ empty? */
	BEQ	idle				/* branch if so */

switchTasks:

	/*
	 * r4 -> current task
	 * r5 -> highest task
	 * update taskIdCurrent so that it points to the highest task
	 */

	STR	r5, [r8]			/* taskIdCurrent -> highest */

	/* swap current/highest pointers */

	MOV	r2, r5				/* r2 -> highest task */
	MOV	r5, r4				/* r5 -> previous task */
	MOV	r4, r2				/* r4 -> highest task */

	/*
	 * check the swap masks (16 bit fields)
	 * r4 -> highest task
	 * r5 -> previous task
	 */

#if	((WIND_TCB_SWAP_IN & 3) == 0) && ((WIND_TCB_SWAP_OUT & 3) == 2)
	LDR	r3, [r4, #WIND_TCB_SWAP_IN]	/* get swap masks */
	LDR	r2, [r5, #WIND_TCB_SWAP_OUT & ~3]
#if (_BYTE_ORDER == _BIG_ENDIAN)
	ORR	r3, r2, r3, LSR #16		/* b0..15 = OR of masks */
#else
	ORR	r3, r3, r2, LSR #16		/* b0..15 = OR of masks */
#endif /* (_BYTE_ORDER == _BIG_ENDIAN) */
	MOVS	r6, r3, LSL #16			/* r6[31..16] = OR of masks */
	BNE	doSwapHooks			/* branch if swap hooks to do */
#else
#	error	WIND_TCB_SWAP_IN/OUT alignment incorrect for code
#endif	/* WIND_TCB_SWAP_IN */

	/* check for global hooks */

	LDR	r7, L$_taskSwitchTable		/* any global hooks? */
	LDR	r2, [r7], #4
	TEQS	r2, #0
	BNE	doSwitchHooks			/* branch if so */

dispatch:

	/*
	 * despatch the new task
	 * r4 -> TCB
	 * NOTE: MIPS code reloads from taskIdCurrent here but MC68K does
	 * not - it should not be necessary
	 *
	 * first restore _fpStatus from TCB
	 */

	LDR	r1, [r4, #WIND_TCB_FPSTATUS]
	LDR	r2, L$__fpStatus
	STR	r1, [r2]

	/* restore errno from TCB */

	LDR	r1, [r4, #WIND_TCB_ERRNO]
	LDR	r2, L$_errno
	STR	r1, [r2]

	/* lock interrupts */

	MRS	r0, cpsr
	ORR	r0, r0, #I_BIT
	MSR	cpsr, r0

	/*
	 * INTERRUPTS DISABLED
	 *
	 * see if workQ is empty
	 */

	LDR	r2, L$_workQIsEmpty
	LDR	r2, [r2]
	TEQS	r2, #0			/* queue empty? */
	BEQ	doWorkUnlock		/* branch if not */

	/* nothing in workQ so continue despatch */

#ifdef	WV_INSTRUMENTATION
 
	/* windview instrumentation - BEGIN
	 * exit windExit with dispatch
	 */

	LDR	lr, L$_evtAction	/* is instrumentation on? */
	LDR	lr, [lr]
	TEQS	lr, #0
	BNE	instrumentDispatch3	/* branch if so */

	/* instrumentation currently disabled */

resumeDispatch3:

	/* windview instrumentation - END */
#endif	/* WV_INSTRUMENTATION */

	LDR	r2, L$_kernelState		/* exit kernel state */
	MOV	r0, #0
	STR	r0, [r2]

	/* get task's saved status and put it in svc_spsr */

	LDR	r1, [r4, #WIND_TCB_CPSR]	/* get status */
	MSR	spsr, r1			/* and put it in place */

	/* load all regs and reenter task */

	ADD	r4, r4, #WIND_TCB_REGS		/* r4 -> task regs */
	LDMIA	r4, {r0-r12,sp,lr,pc}^

/******************************************************************************/

#ifdef	WV_INSTRUMENTATION

	/* windview instrumentation - BEGIN
	 * exit windExit with dispatch
	 */